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).
Note: this tutorial assumes you are using ROS Kinetic (Ubuntu Xenial 16.04), the officially supported ROS distro for ARIAC 2018.
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.
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):
$ mkdir -p ~/helloworld_ws/src $ cd ~/helloworld_ws/src
Next we'll setup the workspace so it is ready to be built once you get your package created:
$ source /opt/ros/kinetic/setup.bash $ catkin_init_workspace
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_2018/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:
$ mkdir -p ~/helloworld_ws/src/ariac_example $ cd ~/helloworld_ws/src/ariac_example
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_2018/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>tf2_geometry_msgs</depend>
17 <depend>trajectory_msgs</depend>
18
19 </package>
You will want to change a few things from this example in your package:
replace the package name, which is in the <name></name> tag, with your package name
- replace the description with something, the content doesn't really matter
- set yourself as the maintainer, using your name and email (these can be bogus as well)
replace the license entry, if you don't want to think about it use CLOSED
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 http://ros.org/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 http://ros.org/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:
replace the project name, given in the project() call on the second line
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:
$ mkdir -p ~/helloworld_ws/src/ariac_example/config
Then create the config file from our example as ~/helloworld_ws/src/ariac_example/config/sample_gear_conf.yaml in the config folder and name it team_conf.yaml:
https://bitbucket.org/osrf/ariac/raw/ariac_2018/ariac_example/config/sample_gear_conf.yaml
sensors:
break_beam_1:
type: break_beam
pose:
xyz: [0.97, 0.61, 0.62]
rpy: [0, 0, 'pi']
proximity_sensor_1:
type: proximity_sensor
pose:
xyz: [0.185, 2.6, 0.62]
rpy: [0, 0, 0]
logical_camera_1:
type: logical_camera
pose:
xyz: [-0.54, 1.14, 1.2]
rpy: [0, 1.16, 'pi']
logical_camera_2:
type: logical_camera
pose:
xyz: [0.58, 2.25, 1.08]
rpy: [0, 'pi/2', '-pi/2']
depth_camera_1:
type: depth_camera
pose:
xyz: [-0.535, 2.01, 1.2]
rpy: [-1.59, 1.07, 'pi']
laser_profiler_1:
type: laser_profiler
pose:
xyz: [0.58, 0.94, 1.10]
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:
$ mkdir -p ~/helloworld_ws/src/ariac_example/src
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_2018/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("iiwa_joint_1");
106 msg.joint_names.push_back("iiwa_joint_2");
107 msg.joint_names.push_back("iiwa_joint_3");
108 msg.joint_names.push_back("iiwa_joint_4");
109 msg.joint_names.push_back("iiwa_joint_5");
110 msg.joint_names.push_back("iiwa_joint_6");
111 msg.joint_names.push_back("iiwa_joint_7");
112 msg.joint_names.push_back("linear_arm_actuator_joint");
113 // Create one point in the trajectory.
114 msg.points.resize(1);
115 // Resize the vector to the same length as the joint names.
116 // Values are initialized to 0.
117 msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
118 // How long to take getting to the point (floating point seconds).
119 msg.points[0].time_from_start = ros::Duration(0.001);
120 ROS_INFO_STREAM("Sending command:\n" << msg);
121 joint_trajectory_publisher_.publish(msg);
122 }
123
124
125 /// Called when a new LogicalCameraImage message is received.
126 void logical_camera_callback(
127 const osrf_gear::LogicalCameraImage::ConstPtr & image_msg)
128 {
129 ROS_INFO_STREAM_THROTTLE(10,
130 "Logical camera: '" << image_msg->models.size() << "' objects.");
131 }
132
133 /// Called when a new Proximity message is received.
134 void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) {
135 if (msg->object_detected) { // If there is an object in proximity.
136 ROS_INFO("Break beam triggered.");
137 }
138 }
139
140 private:
141 std::string competition_state_;
142 double current_score_;
143 ros::Publisher joint_trajectory_publisher_;
144 std::vector<osrf_gear::Order> received_orders_;
145 sensor_msgs::JointState current_joint_states_;
146 bool has_been_zeroed_;
147 };
148
149 void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) {
150 if ((msg->max_range - msg->range) > 0.01) { // If there is an object in proximity.
151 ROS_INFO_THROTTLE(1, "Proximity sensor sees something.");
152 }
153 }
154
155 void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) {
156 size_t number_of_valid_ranges = std::count_if(
157 msg->ranges.begin(), msg->ranges.end(), std::isfinite<float>);
158 if (number_of_valid_ranges > 0) {
159 ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
160 }
161 }
162
163
164 int main(int argc, char ** argv) {
165 // Last argument is the default name of the node.
166 ros::init(argc, argv, "ariac_example_node");
167
168 ros::NodeHandle node;
169
170 // Instance of custom class from above.
171 MyCompetitionClass comp_class(node);
172
173 // Subscribe to the '/ariac/current_score' topic.
174 ros::Subscriber current_score_subscriber = node.subscribe(
175 "/ariac/current_score", 10,
176 &MyCompetitionClass::current_score_callback, &comp_class);
177
178 // Subscribe to the '/ariac/competition_state' topic.
179 ros::Subscriber competition_state_subscriber = node.subscribe(
180 "/ariac/competition_state", 10,
181 &MyCompetitionClass::competition_state_callback, &comp_class);
182
183
184 // Subscribe to the '/ariac/orders' topic.
185 ros::Subscriber orders_subscriber = node.subscribe(
186 "/ariac/orders", 10,
187 &MyCompetitionClass::order_callback, &comp_class);
188
189
190 // Subscribe to the '/ariac/joint_states' topic.
191 ros::Subscriber joint_state_subscriber = node.subscribe(
192 "/ariac/joint_states", 10,
193 &MyCompetitionClass::joint_state_callback, &comp_class);
194
195
196 // Subscribe to the '/ariac/proximity_sensor_1' topic.
197 ros::Subscriber proximity_sensor_subscriber = node.subscribe(
198 "/ariac/proximity_sensor_1", 10, proximity_sensor_callback);
199
200
201 // Subscribe to the '/ariac/break_beam_1_change' topic.
202 ros::Subscriber break_beam_subscriber = node.subscribe(
203 "/ariac/break_beam_1_change", 10,
204 &MyCompetitionClass::break_beam_callback, &comp_class);
205
206 // Subscribe to the '/ariac/logical_camera_1' topic.
207 ros::Subscriber logical_camera_subscriber = node.subscribe(
208 "/ariac/logical_camera_1", 10,
209 &MyCompetitionClass::logical_camera_callback, &comp_class);
210
211 // Subscribe to the '/ariac/laser_profiler_1' topic.
212 ros::Subscriber laser_profiler_subscriber = node.subscribe(
213 "/ariac/laser_profiler_1", 10, laser_profiler_callback);
214
215 ROS_INFO("Setup complete.");
216 start_competition(node);
217 ros::spin(); // This executes callbacks on new data until ctrl-c.
218
219 return 0;
220 }
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:
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:
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 }
There is 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("iiwa_joint_1");
10 msg.joint_names.push_back("iiwa_joint_2");
11 msg.joint_names.push_back("iiwa_joint_3");
12 msg.joint_names.push_back("iiwa_joint_4");
13 msg.joint_names.push_back("iiwa_joint_5");
14 msg.joint_names.push_back("iiwa_joint_6");
15 msg.joint_names.push_back("iiwa_joint_7");
16 msg.joint_names.push_back("linear_arm_actuator_joint");
17 // Create one point in the trajectory.
18 msg.points.resize(1);
19 // Resize the vector to the same length as the joint names.
20 // Values are initialized to 0.
21 msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
22 // How long to take getting to the point (floating point seconds).
23 msg.points[0].time_from_start = ros::Duration(0.001);
24 ROS_INFO_STREAM("Sending command:\n" << msg);
25 joint_trajectory_publisher_.publish(msg);
26 }
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:
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:
$ cd ~/helloworld_ws $ catkin_make
Running the Competition Simulation
Open a new terminal and run the competition, passing your configuration as well as our sample competition settings:
$ rosrun osrf_gear gear.py -f `catkin_find --first-only --share osrf_gear`/config/sample.yaml ~/helloworld_ws/src/ariac_example/config/team_conf.yaml
Make sure to replace the path to the team_conf.yaml if that's not the proper location.
This will use the two configuration files to generate the files necessary for launching the competition with the specified environment configuration. These generated files will be output in the /tmp/ariac directory and then run. If you want to specify a different output location, use the -o option.
If you want to visualize the sensors, add the --visualize-sensor-views option.
Once Gazebo is finished loading, continue by running your example competition node.
Running the Example
Open a new terminal and run your example:
$ source ~/helloworld_ws/devel/setup.bash $ rosrun ariac_example ariac_example_node
This should cause statements to the printed 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/2018/tutorials for more tutorials.
