[Documentation] [TitleIndex] [WordIndex

  Show EOL distros: 

!!! !!! !!!

The ARIAC 2017 competition is complete. If you are interested in competing in an active ARIAC competition you are probably in the wrong place: this page is only available for archival reasons.

!!! !!! !!!

The purpose of this tutorial is to introduce you to how to programmatically interface with the ROS interface that is used to interact with the ARIAC competition simulation. See the GEAR Interface tutorial for examples of interfacing with GEAR through the command line (GEAR is the software used to implement the ARIAC competition).

Prerequisites

You should have already completed the GEAR interface tutorial.

Creating a Competition Package

We provide a template ROS Package to help you get started with the competition.

Note: this tutorial will only work with ROS indigo (Ubuntu Trusty 14.04) or ROS kinetic (Ubuntu Xenial 16.04). ROS indigo is recommended.

Please select the appropriate ROS version from the tabs at the top of this page.

Setting up a Catkin Workspace

Before creating your own package from the template package, we'll want to setup a Catkin Workspace in which you will build your package after it is setup.

A workspace is essentially just a set of folders with a conventional structure. Start by creating a folder like this (you can choose a different location for the workspace, but the src subfolder should be unchanged):

Next we'll setup the workspace so it is ready to be built once you get your package created:

At this point you can build you workspace, but nothing will really happen because you have not put your new package into the src folder yet.

Creating a New Package Using the Template

The template package comes with the build system files, some sample configurations, an example C++ node, and an example Python node. The example package can be found in the same repository as the competition's code:

https://bitbucket.org/osrf/ariac/src/ariac_2017/ariac_example

This tutorial will go over several of the files in that repository and help you create them locally. However, it will also limit itself to the C++ example and only one example configuration file.

Now you should select a name for your package. You can pick what ever you want, but the name should follow our package name guidelines if you don't want warnings. The package name should be all lowercase with underscores, e.g. my_package.

In this tutorial we'll use ariac_example as the name of the package. Anywhere this is being used, you can replace it with your package's name.

Now that we have the package name, create a folder in the src by the same name:

All of the files in your package need to go into, or under, this folder.

Package Manifest

Next we will create, what we call in ROS, the package manifest. The purpose of the package manifest is to capture essential meta information about your package in a machine readable format. Additionally, it is used to mark the root of your package, so any files in your package need to be peers of this file or in folders under it.

Many of the entries in the package manifest are used for packaging and releasing of your package. Since you probably will not be releasing this package, many of them can be neglected or filled with placeholders.

Let's create the package.xml in the root of your package's folder, i.e. ~/helloworld_ws/src/ariac_example/package.xml:

https://bitbucket.org/osrf/ariac/raw/ariac_2017/ariac_example/package.xml

   1 <?xml version="1.0"?>
   2 <package format="2">
   3   <name>ariac_example</name>
   4   <version>0.1.0</version>
   5   <description>An example of an ARIAC competitor's package.</description>
   6   <maintainer email="william@osrfoundation.org">William Woodall</maintainer>
   7 
   8   <license>Apache 2.0</license>
   9 
  10   <buildtool_depend>catkin</buildtool_depend>
  11 
  12   <depend>osrf_gear</depend>
  13   <depend>roscpp</depend>
  14   <depend>sensor_msgs</depend>
  15   <depend>std_srvs</depend>
  16   <depend>trajectory_msgs</depend>
  17 
  18 </package>

You will want to change a few things from this example in your package:

Finally you come to the dependencies. The initial list of dependencies will get you started, but you can add dependencies as necessary using the <depend> tag.

CMakeLists.txt

Next we'll setup a basic CMake build file. ROS packages use CMake as the build system, create the cmake file in the root of your package folder, next to the package manifest, named as ~/helloworld_ws/src/ariac_example/CMakeLists.txt:

https://bitbucket.org/osrf/ariac/raw/no_catkin_python_setup/ariac_example/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(ariac_example)

find_package(catkin REQUIRED COMPONENTS
  osrf_gear
  roscpp
  sensor_msgs
  std_srvs
  trajectory_msgs
)

## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

catkin_package()

###########
## Build ##
###########

include_directories(include ${catkin_INCLUDE_DIRS})

## Declare a C++ executable
add_executable(ariac_example_node src/ariac_example_node.cpp)
add_dependencies(ariac_example_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(ariac_example_node ${catkin_LIBRARIES})

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
install(PROGRAMS script/ariac_example_node.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark executables and/or libraries for installation
install(TARGETS ariac_example_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
 config/sample_gear_conf.yaml
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
)

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ariac_example.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

The only thing you need to change in this file is:

You can look at the file's comments for some information about what it is doing, but this tutorial will not dive deeply into the CMake file.

Competition Configuration Example

In addition to writing your own competition code, you have the ability to control some of the aspects of the competition's simulation environment, like which robot arm to use, which sensors to use, and where to place them.

This is done using a YAML file, which is explained in another tutorial.

Create a folder for your example configuration:

Then create the config file from our example as ~/helloworld_ws/src/ariac_example/config/team_conf.yaml:

https://bitbucket.org/osrf/ariac/raw/ariac_2017/ariac_example/config/sample_gear_conf.yaml

arm:
  type: ur10
sensors:
  break_beam_1:
    type: break_beam
    pose:
      xyz: [1.6, 2.25, 0.91]
      rpy: [0, 0, 'pi']
  proximity_sensor_1:
    type: proximity_sensor
    pose:
      xyz: [0.9, 2.6, 0.914]
      rpy: [0, 0, 0]
  logical_camera_1:
    type: logical_camera
    pose:
      xyz: [-0.66, 0.22, 1.36]
      rpy: ['pi/2', 1.32, 0]
  logical_camera_2:
    type: logical_camera
    pose:
      xyz: [0.3, 3.15, 1.5]
      rpy: [0, 'pi/2', 0]
  laser_profiler_1:
    type: laser_profiler
    pose:
      xyz: [1.21, 4, 1.64]
      rpy: ['-pi', 'pi/2', 'pi/2']

You can reference the above linked tutorial for more information about how to modify this configuration file.

C++ Example Node

Finally, we will add the C++ example file and discuss what it is doing.

First create the folder that will hold the C++ source file:

Remember to replace ariac_example with your package's name.

Then create the C++ file in the src folder of you package, i.e. ~/helloworld_ws/src/ariac_example/src/ariac_example_node.cpp

https://bitbucket.org/osrf/ariac/raw/ariac_2017/ariac_example/src/ariac_example_node.cpp

   1 #include <algorithm>
   2 #include <vector>
   3 
   4 #include <ros/ros.h>
   5 
   6 #include <osrf_gear/LogicalCameraImage.h>
   7 #include <osrf_gear/Order.h>
   8 #include <osrf_gear/Proximity.h>
   9 #include <sensor_msgs/JointState.h>
  10 #include <sensor_msgs/LaserScan.h>
  11 #include <sensor_msgs/Range.h>
  12 #include <std_msgs/Float32.h>
  13 #include <std_msgs/String.h>
  14 #include <std_srvs/Trigger.h>
  15 #include <trajectory_msgs/JointTrajectory.h>
  16 
  17 
  18 
  19 /// Start the competition by waiting for and then calling the start ROS Service.
  20 void start_competition(ros::NodeHandle & node) {
  21   // Create a Service client for the correct service, i.e. '/ariac/start_competition'.
  22   ros::ServiceClient start_client =
  23     node.serviceClient<std_srvs::Trigger>("/ariac/start_competition");
  24   // If it's not already ready, wait for it to be ready.
  25   // Calling the Service using the client before the server is ready would fail.
  26   if (!start_client.exists()) {
  27     ROS_INFO("Waiting for the competition to be ready...");
  28     start_client.waitForExistence();
  29     ROS_INFO("Competition is now ready.");
  30   }
  31   ROS_INFO("Requesting competition start...");
  32   std_srvs::Trigger srv;  // Combination of the "request" and the "response".
  33   start_client.call(srv);  // Call the start Service.
  34   if (!srv.response.success) {  // If not successful, print out why.
  35     ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message);
  36   } else {
  37     ROS_INFO("Competition started!");
  38   }
  39 }
  40 
  41 
  42 /// Example class that can hold state and provide methods that handle incoming data.
  43 class MyCompetitionClass
  44 {
  45 public:
  46   explicit MyCompetitionClass(ros::NodeHandle & node)
  47   : current_score_(0), has_been_zeroed_(false)
  48   {
  49 
  50     joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
  51       "/ariac/arm/command", 10);
  52 
  53   }
  54 
  55   /// Called when a new message is received.
  56   void current_score_callback(const std_msgs::Float32::ConstPtr & msg) {
  57     if (msg->data != current_score_)
  58     {
  59       ROS_INFO_STREAM("Score: " << msg->data);
  60     }
  61     current_score_ = msg->data;
  62   }
  63 
  64   /// Called when a new message is received.
  65   void competition_state_callback(const std_msgs::String::ConstPtr & msg) {
  66     if (msg->data == "done" && competition_state_ != "done")
  67     {
  68       ROS_INFO("Competition ended.");
  69     }
  70     competition_state_ = msg->data;
  71   }
  72 
  73   /// Called when a new Order message is received.
  74   void order_callback(const osrf_gear::Order::ConstPtr & order_msg) {
  75     ROS_INFO_STREAM("Received order:\n" << *order_msg);
  76     received_orders_.push_back(*order_msg);
  77   }
  78 
  79 
  80   /// Called when a new JointState message is received.
  81   void joint_state_callback(
  82     const sensor_msgs::JointState::ConstPtr & joint_state_msg)
  83   {
  84     ROS_INFO_STREAM_THROTTLE(10,
  85       "Joint States (throttled to 0.1 Hz):\n" << *joint_state_msg);
  86     // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
  87     current_joint_states_ = *joint_state_msg;
  88     if (!has_been_zeroed_) {
  89       has_been_zeroed_ = true;
  90       ROS_INFO("Sending arm to zero joint positions...");
  91       send_arm_to_zero_state();
  92     }
  93   }
  94 
  95 
  96 
  97   /// Create a JointTrajectory with all positions set to zero, and command the arm.
  98   void send_arm_to_zero_state() {
  99     // Create a message to send.
 100     trajectory_msgs::JointTrajectory msg;
 101 
 102     // Fill the names of the joints to be controlled.
 103     // Note that the vacuum_gripper_joint is not controllable.
 104     msg.joint_names.clear();
 105     msg.joint_names.push_back("elbow_joint");
 106     msg.joint_names.push_back("linear_arm_actuator_joint");
 107     msg.joint_names.push_back("shoulder_lift_joint");
 108     msg.joint_names.push_back("shoulder_pan_joint");
 109     msg.joint_names.push_back("wrist_1_joint");
 110     msg.joint_names.push_back("wrist_2_joint");
 111     msg.joint_names.push_back("wrist_3_joint");
 112     // Create one point in the trajectory.
 113     msg.points.resize(1);
 114     // Resize the vector to the same length as the joint names.
 115     // Values are initialized to 0.
 116     msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
 117     // How long to take getting to the point (floating point seconds).
 118     msg.points[0].time_from_start = ros::Duration(0.001);
 119     ROS_INFO_STREAM("Sending command:\n" << msg);
 120     joint_trajectory_publisher_.publish(msg);
 121   }
 122 
 123 
 124   /// Called when a new LogicalCameraImage message is received.
 125   void logical_camera_callback(
 126     const osrf_gear::LogicalCameraImage::ConstPtr & image_msg)
 127   {
 128     ROS_INFO_STREAM_THROTTLE(10,
 129       "Logical camera: '" << image_msg->models.size() << "' objects.");
 130   }
 131 
 132   /// Called when a new Proximity message is received.
 133   void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) {
 134     if (msg->object_detected) {  // If there is an object in proximity.
 135       ROS_INFO("Break beam triggered.");
 136     }
 137   }
 138 
 139 private:
 140   std::string competition_state_;
 141   double current_score_;
 142   ros::Publisher joint_trajectory_publisher_;
 143   std::vector<osrf_gear::Order> received_orders_;
 144   sensor_msgs::JointState current_joint_states_;
 145   bool has_been_zeroed_;
 146 };
 147 
 148 void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) {
 149   if ((msg->max_range - msg->range) > 0.01) {  // If there is an object in proximity.
 150     ROS_INFO_THROTTLE(1, "Proximity sensor sees something.");
 151   }
 152 }
 153 
 154 void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) {
 155   size_t number_of_valid_ranges = std::count_if(
 156     msg->ranges.begin(), msg->ranges.end(), std::isfinite<float>);
 157   if (number_of_valid_ranges > 0) {
 158     ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
 159   }
 160 }
 161 
 162 
 163 int main(int argc, char ** argv) {
 164   // Last argument is the default name of the node.
 165   ros::init(argc, argv, "ariac_example_node");
 166 
 167   ros::NodeHandle node;
 168 
 169   // Instance of custom class from above.
 170   MyCompetitionClass comp_class(node);
 171 
 172   // Subscribe to the '/ariac/current_score' topic.
 173   ros::Subscriber current_score_subscriber = node.subscribe(
 174     "/ariac/current_score", 10,
 175     &MyCompetitionClass::current_score_callback, &comp_class);
 176 
 177   // Subscribe to the '/ariac/competition_state' topic.
 178   ros::Subscriber competition_state_subscriber = node.subscribe(
 179     "/ariac/competition_state", 10,
 180     &MyCompetitionClass::competition_state_callback, &comp_class);
 181 
 182 
 183   // Subscribe to the '/ariac/orders' topic.
 184   ros::Subscriber orders_subscriber = node.subscribe(
 185     "/ariac/orders", 10,
 186     &MyCompetitionClass::order_callback, &comp_class);
 187 
 188 
 189   // Subscribe to the '/ariac/joint_states' topic.
 190   ros::Subscriber joint_state_subscriber = node.subscribe(
 191     "/ariac/joint_states", 10,
 192     &MyCompetitionClass::joint_state_callback, &comp_class);
 193 
 194 
 195   // Subscribe to the '/ariac/proximity_sensor_1' topic.
 196   ros::Subscriber proximity_sensor_subscriber = node.subscribe(
 197     "/ariac/proximity_sensor_1", 10, proximity_sensor_callback);
 198 
 199 
 200   // Subscribe to the '/ariac/break_beam_1_change' topic.
 201   ros::Subscriber break_beam_subscriber = node.subscribe(
 202     "/ariac/break_beam_1_change", 10,
 203     &MyCompetitionClass::break_beam_callback, &comp_class);
 204 
 205   // Subscribe to the '/ariac/logical_camera_1' topic.
 206   ros::Subscriber logical_camera_subscriber = node.subscribe(
 207     "/ariac/logical_camera_1", 10,
 208     &MyCompetitionClass::logical_camera_callback, &comp_class);
 209 
 210   // Subscribe to the '/ariac/laser_profiler_1' topic.
 211   ros::Subscriber laser_profiler_subscriber = node.subscribe(
 212     "/ariac/laser_profiler_1", 10, laser_profiler_callback);
 213 
 214   ROS_INFO("Setup complete.");
 215   start_competition(node);
 216   ros::spin();  // This executes callbacks on new data until ctrl-c.
 217 
 218   return 0;
 219 }

You can change the location or the name of this file, but remember to update the add_executable() call in the CMakeLists.txt if you do.

C++ Code Explained

In this section of the tutorial, sections of the C++ example will be explained in more detail. If you want to continue with the tutorial you can skip to the section about the trying the example out.

First, let's look at the C++ header files we are including and why.

   1 #include <algorithm>
   2 #include <vector>
   3 
   4 #include <ros/ros.h>
   5 
   6 #include <osrf_gear/LogicalCameraImage.h>
   7 #include <osrf_gear/Order.h>
   8 #include <osrf_gear/Proximity.h>
   9 #include <sensor_msgs/JointState.h>
  10 #include <sensor_msgs/LaserScan.h>
  11 #include <sensor_msgs/Range.h>
  12 #include <std_msgs/Float32.h>
  13 #include <std_msgs/String.h>
  14 #include <std_srvs/Trigger.h>
  15 #include <trajectory_msgs/JointTrajectory.h>
  16 

The first couple of lines are C++ standard library includes which allows you to use some built in types like std::vector.

Next is the include for ROS, which lets you use ROS types like NodeHandle and call functions like ros::init().

The last block of include statements are all includes for specific ROS message types. You need to include a file for each ROS message or service type you intend to use in your program. The fully qualified type of the message is the package containing the message plus the message name, and the same is true with the include statements. Take the #include <sensor_msgs/LaserScan.h> statement, for example, the message is called LaserScan and it is from the sensor_msgs package, which is a built-in message package in ROS.

Next let's skip down to the main function, which is the entry point for C++ programs.

   1 int main(int argc, char ** argv) {
   2   // Last argument is the default name of the node.
   3   ros::init(argc, argv, "ariac_example_node");
   4 
   5   ros::NodeHandle node;
   6 
   7   // Instance of custom class from above.
   8   MyCompetitionClass comp_class(node);
   9 
  10   // Subscribe to the '/ariac/current_score' topic.
  11   ros::Subscriber current_score_subscriber = node.subscribe(
  12     "/ariac/current_score", 10,
  13     &MyCompetitionClass::current_score_callback, &comp_class);
  14 
  15   // Subscribe to the '/ariac/competition_state' topic.
  16   ros::Subscriber competition_state_subscriber = node.subscribe(
  17     "/ariac/competition_state", 10,
  18     &MyCompetitionClass::competition_state_callback, &comp_class);
  19 
  20 
  21   // Subscribe to the '/ariac/orders' topic.
  22   ros::Subscriber orders_subscriber = node.subscribe(
  23     "/ariac/orders", 10,
  24     &MyCompetitionClass::order_callback, &comp_class);
  25 
  26 
  27   // Subscribe to the '/ariac/joint_states' topic.
  28   ros::Subscriber joint_state_subscriber = node.subscribe(
  29     "/ariac/joint_states", 10,
  30     &MyCompetitionClass::joint_state_callback, &comp_class);
  31 
  32 
  33   // Subscribe to the '/ariac/proximity_sensor_1' topic.
  34   ros::Subscriber proximity_sensor_subscriber = node.subscribe(
  35     "/ariac/proximity_sensor_1", 10, proximity_sensor_callback);
  36 
  37 
  38   // Subscribe to the '/ariac/break_beam_1_change' topic.
  39   ros::Subscriber break_beam_subscriber = node.subscribe(
  40     "/ariac/break_beam_1_change", 10,
  41     &MyCompetitionClass::break_beam_callback, &comp_class);
  42 
  43   // Subscribe to the '/ariac/logical_camera_1' topic.
  44   ros::Subscriber logical_camera_subscriber = node.subscribe(
  45     "/ariac/logical_camera_1", 10,
  46     &MyCompetitionClass::logical_camera_callback, &comp_class);
  47 
  48   // Subscribe to the '/ariac/laser_profiler_1' topic.
  49   ros::Subscriber laser_profiler_subscriber = node.subscribe(
  50     "/ariac/laser_profiler_1", 10, laser_profiler_callback);
  51 
  52   ROS_INFO("Setup complete.");
  53   start_competition(node);
  54   ros::spin();  // This executes callbacks on new data until ctrl-c.
  55 
  56   return 0;
  57 }

There are a few things to point out in this function. First, the call to ros::init() which sets the node's name (node names must be unique, but can be changed at runtime).

Next you create a node and an instance of the custom class which will be explained in more detail later.

The largest section of the main function is where you subscribe to the various streams of data available to you over topics, and assign them callbacks to be called when there is new data to be processed.

Let's take a closer look at two kinds of these subscribe calls:

   1   // Subscribe to the '/ariac/orders' topic.
   2   ros::Subscriber orders_subscriber = node.subscribe(
   3     "/ariac/orders", 10,
   4     &MyCompetitionClass::order_callback, &comp_class);

Here you are subscribing to the /ariac/orders topic and giving it a "class method" callback that is defined in the custom class. The first argument is the name of the topic. The second argument is the queue size which is how many messages to save if they arrive while your callback is being run on a previously received message. The last two arguments are the class method to call, which takes the form of &ClassName::MethodName, and then a pointer to the class instance to call it on, in this case the instance we created earlier in the main function.

Next let's look at another version of subscribing which uses a normal, "free" function:

   1   // Subscribe to the '/ariac/proximity_sensor_1' topic.
   2   ros::Subscriber proximity_sensor_subscriber = node.subscribe(
   3     "/ariac/proximity_sensor_1", 10, proximity_sensor_callback);

As you can see, this version has only three arguments, which the first two still being the topic name and the queue size. The third argument is simply a free function to call on new messages. In this case it is to the /ariac/proximity_sensor_1_change topic, the name of which is driven by the proximity_sensor_1 name from the configuration file you created in the previous tutorial section.

The final things you do in the main function are to call a function that starts the competition and then call ros::spin(). The spin call will block (code will not continue past that line) until you stop the program, e.g. with ctrl-c. It is within the spin call that your callbacks are run when new messages are received.

Next let's look at the function that starts the competition:

   1 /// Start the competition by waiting for and then calling the start ROS Service.
   2 void start_competition(ros::NodeHandle & node) {
   3   // Create a Service client for the correct service, i.e. '/ariac/start_competition'.
   4   ros::ServiceClient start_client =
   5     node.serviceClient<std_srvs::Trigger>("/ariac/start_competition");
   6   // If it's not already ready, wait for it to be ready.
   7   // Calling the Service using the client before the server is ready would fail.
   8   if (!start_client.exists()) {
   9     ROS_INFO("Waiting for the competition to be ready...");
  10     start_client.waitForExistence();
  11     ROS_INFO("Competition is now ready.");
  12   }
  13   ROS_INFO("Requesting competition start...");
  14   std_srvs::Trigger srv;  // Combination of the "request" and the "response".
  15   start_client.call(srv);  // Call the start Service.
  16   if (!srv.response.success) {  // If not successful, print out why.
  17     ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message);
  18   } else {
  19     ROS_INFO("Competition started!");
  20   }
  21 }

This function starts the competition by making a service call to the /ariac/start_competition service. First it creates a service client (object that lets you make the call) and then checks to see if the service is available yet. Since ROS is a completely asynchronous system, it is possible that this function is run before the service server is available (which is running in a different program). So if the service server (which is provided by our Gazebo plugin for the competition) is not yet ready, we can wait on it.

Once the service server is there, we make the call and check the result to ensure the competition was started successfully. Since the .call() method is blocking, you can be assured that the competition has been synchronously started once it returns.

Finally, let's look at some of the callback functions. First look at a "class method" style callback in our custom class:

   1   /// Called when a new JointState message is received.
   2   void joint_state_callback(
   3     const sensor_msgs::JointState::ConstPtr & joint_state_msg)
   4   {
   5     ROS_INFO_STREAM_THROTTLE(10,
   6       "Joint States (throttled to 0.1 Hz):\n" << *joint_state_msg);
   7     // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
   8     current_joint_states_ = *joint_state_msg;
   9     if (!has_been_zeroed_) {
  10       has_been_zeroed_ = true;
  11       ROS_INFO("Sending arm to zero joint positions...");
  12       send_arm_to_zero_state();
  13     }
  14   }

Nothing special about this function other than that it is a class method of our custom class. The signature of the function is important, however, as it should return void and should take a single parameter of the message pointer type. The type of the parameter is driven by the topic type, in this case it is the JointState message type from the sensor_msgs package. The parameter type follows from that as const <msg pkg>::<msg name>::ConstPtr &.

In the callback you can see that the message's contents are being logged, but throttled to about 0.1 Hz. This topic, /ariac/joint_states, is the measured joint values of the arm which come very quickly at hundreds of Hz. So, if you printed every one your console would be very crowded with joint states.

Also this callback function stores the joint state message in the class for use later. If it is the first time this callback function was called it will also call the send_arm_to_zero_state method:

   1   /// Create a JointTrajectory with all positions set to zero, and command the arm.
   2   void send_arm_to_zero_state() {
   3     // Create a message to send.
   4     trajectory_msgs::JointTrajectory msg;
   5 
   6     // Fill the names of the joints to be controlled.
   7     // Note that the vacuum_gripper_joint is not controllable.
   8     msg.joint_names.clear();
   9     msg.joint_names.push_back("elbow_joint");
  10     msg.joint_names.push_back("linear_arm_actuator_joint");
  11     msg.joint_names.push_back("shoulder_lift_joint");
  12     msg.joint_names.push_back("shoulder_pan_joint");
  13     msg.joint_names.push_back("wrist_1_joint");
  14     msg.joint_names.push_back("wrist_2_joint");
  15     msg.joint_names.push_back("wrist_3_joint");
  16     // Create one point in the trajectory.
  17     msg.points.resize(1);
  18     // Resize the vector to the same length as the joint names.
  19     // Values are initialized to 0.
  20     msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
  21     // How long to take getting to the point (floating point seconds).
  22     msg.points[0].time_from_start = ros::Duration(0.001);
  23     ROS_INFO_STREAM("Sending command:\n" << msg);
  24     joint_trajectory_publisher_.publish(msg);
  25   }

This is a good example of publishing a message, and a good example of how to control the arm in the simplest way possible. The function starts off by creating a new JointTrajectory message, which is from the trajectory_msgs package, and then proceeds to fill it with data.

The first thing it does it copy the joint names from the joint state message into the joint trajectory message. It's a bit confusing, but the joint *states* message type is the measured state of the arm, whereas the joint *trajectory* message is a way to represent a desired state for the controller to try and achieve. Next it sets the desired states for each joint to 0. Finally it sets the time_from_start field with a ROS Duration object. This time_from_start field tells the controller by how far into the future this state should be achieved. By setting this value to something small, like a millisecond, you're basically telling the controller to go there as fast as possible. You can play around with these values to see what effect they have.

Then it publishes the message, after logging it, using a publisher created in the class's constructor:

   1     joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
   2       "/ariac/arm/command", 10);

This is done with the advertise function, which requires a message type, a topic name, and a queue size. In this case it is the trajectory_msgs::JointTrajectory message type (given as a template argument), the topic /ariac/arm/command, and a queue size of 10.

The rest of the code is basically one variation or another on the callbacks already covered here.

Trying the Example

There are three main steps to trying out your example: build the example, run the competition simulation, run your example.

Building the Example

Before you can run the example, you need to build it:

Running the Competition Simulation

Open a new terminal and run the competition, passing your configuration as well as our sample competition settings:

Make sure to replace the path to the team_conf.yaml if that's not the proper location.

This should start Gazebo and all of the competition handling code. Once Gazebo is finished loading, continue by running your example competition node.

Running the Example

Open a new terminal and run your example:

This should start the competition, which will cause items to be spawned on the conveyor belt after a short period.

This will also cause print statements to the screen on various events like the competition start request succeeding, new sensor data being received, and an order being received.

Next steps

See https://bitbucket.org/osrf/ariac/wiki/2017/tutorials for more tutorials.


2022-05-28 12:26