last
update: December 14th, 2011.
How to link OMNET++/Castalia with ROS (Robot Operating System)
------------------------------------------------------------
Author: Congduc
Pham, LIUPPA labs, University of Pau, France
------------------------------------------------------------
See
Congduc's page on wireless sensor networks research
Objectives
This page describes the early experiments in linking the
OMNET++/Castalia simulation framework (oriented towards wireless sensor
networks) with the ROS middleware (using
roscpp) for
interacting with robot simulators in order to get within the
OMNET++/Castalia simulation a robot's position (x,y) which is
accurately simulated by an external simulator based on ROS (MORSE for
instance). The motivation is to used well-tested and realistic robot
simulators for handling all the robot navigation tasks (obstacle
avoidance, navigation towards goals, velocity, ...) and to only get the
robot's position in OMNET++/Castalia for interacting with the deployed
sensors.
Requirements
Before going further with this page, you should have:
- a working install of OMNET++ 4.1 (see www.omnetpp.org)
- a working install of Castalia 3.2 (see http://castalia.npc.nicta.com.au/),
preferably a brand new installation. I also wrote this page "Understanding
Castalia"
to provide additional information on Castalia that are complementary to
those found on the Castalia web site.
- a working install of ROS (see http://www.ros.org)
- optionally a working install of MORSE (see http://www.openrobots.org) to
have a simple robot simulator and a nice graphical environment based on
Blender3D.
- optionally a simple robot example from the ANR PROTEUS project developed on
top of MORSE.
First step: beginner_tutorial
of ROS tutorial
ROS provides the roscpp
(see the roscpp
overview) C++ API implementation that can be used to write C++
program
interacting with ROS. They provide a powerfull, but quite complex,
build chain to develop C++ applications with roscpp. On the other hand
OMNET++/Castalia uses their own build procedure based on make that
is quite difficult to make it working with the roscpp build procedure.
The first step for being able to link an OMNET++/Castalia simulation
with ROS support is to be able to use the OMNET++/Castalia build
procedure and linking "manually" the various ROS components (librairies
and header files mainly).
The solution is to use the ROS
procedure for creating a simple package such as the beginner_tutorial
package described in the roscpp tutorial.
Then use the rospack
command tool to get the header file dependencies and the dynamic
librairies dependencies as described in the "Creating
a Package by Hand" page:
> cd
~/ros_workspace
> roscreate-pkg beginner_tutorials std_msgs rospy roscpp
> rospack
export --lang=cpp --attrib=cflags beginner_tutorial
> rospack
export --lang=cpp --attrib=lflags beginner_tutorial
The first rospack
command prints the header file include directory paths in the
form of '-I'
separated item paths that can directly be used in a Makefile or
as input
parameters to the gcc/g++ compiler. The second rospack command
prints both the '-l' and '-L'
informations that are respectively the list of libraries (mostly .so
dynamic libraries) to be linked and the list of library paths.
Here is an example of the outputs.
>
rospack export --lang=cpp
--attrib=cflags beginner_tutorials
-I/opt/ros/electric/stacks/common_msgs/nav_msgs/msg_gen/cpp/include
-I/opt/ros/electric/stacks/common_msgs/nav_msgs/srv_gen/cpp/include
-I/opt/ros/electric/stacks/geometry/tf/include
-I/opt/ros/electric/stacks/geometry/tf/msg_gen/cpp/include
-I/opt/ros/electric/stacks/geometry/tf/srv_gen/cpp/include
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/include
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/msg_gen/cpp/include
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/srv_gen/cpp/include
-I/opt/ros/electric/stacks/common_msgs/geometry_msgs/msg_gen/cpp/include
-I/opt/ros/electric/stacks/bullet/include -DBT_USE_DOUBLE_PRECISION
-DBT_EULER_DEFAULT_ZYX
-I/opt/ros/electric/stacks/geometry/angles/include
-I/opt/ros/electric/stacks/ros_comm/tools/rosbag/include
-I/opt/ros/electric/stacks/ros_comm/tools/topic_tools/include
-I/opt/ros/electric/stacks/ros_comm/tools/topic_tools/srv_gen/cpp/include
-I/opt/ros/electric/stacks/ros_comm/utilities/message_filters/include
-I/opt/ros/electric/stacks/ros_comm/tools/rostest/include
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/include
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/msg_gen/cpp/include
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/srv_gen/cpp/include
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_serialization/include
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_traits/include
-I/opt/ros/electric/stacks/ros_comm/utilities/xmlrpcpp/src
-I/opt/ros/electric/stacks/ros_comm/tools/rosconsole/include
-I/opt/ros/electric/stacks/ros_comm/utilities/rostime/include
-I/opt/ros/electric/stacks/ros_comm/utilities/cpp_common/include
-I/opt/ros/electric/stacks/ros_comm/messages/rosgraph_msgs/msg_gen/cpp/include
-I/opt/ros/electric/stacks/ros_comm/messages/std_msgs/include
-I/opt/ros/electric/stacks/ros_comm/messages/std_msgs/msg_gen/cpp/include
-I/opt/ros/electric/ros/core/roslib/msg_gen/cpp/include
-I/opt/ros/electric/ros/core/roslib/include
-I/opt/ros/electric/ros/tools/rospack
-I/opt/ros/electric/ros/tools/rospack/include
>
rospack export --lang=cpp
--attrib=lflags beginner_tutorials
-Wl,-rpath,/opt/ros/electric/stacks/geometry/tf/lib
-L/opt/ros/electric/stacks/geometry/tf/lib -ltf -lboost_thread-mt
-L/opt/ros/electric/stacks/common_msgs/sensor_msgs/lib -lsensor_msgs
-Wl,-rpath,/opt/ros/electric/stacks/common_msgs/sensor_msgs/lib
-L/opt/ros/electric/stacks/bullet/lib
-Wl,-rpath,/opt/ros/electric/stacks/bullet/lib -lBulletDynamics
-lBulletCollision
-lLinearMath
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/tools/rosbag/lib
-L/opt/ros/electric/stacks/ros_comm/tools/rosbag/lib -lrosbag
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/tools/topic_tools/lib
-L/opt/ros/electric/stacks/ros_comm/tools/topic_tools/lib
-ltopic_tools
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/utilities/message_filters/lib
-L/opt/ros/electric/stacks/ros_comm/utilities/message_filters/lib
-lmessage_filters -lboost_thread-mt
-lboost_signals-mt
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/lib
-L/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/lib -lros
-lboost_thread-mt -lboost_signals-mt
-L/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_serialization/lib
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_serialization/lib
-lroscpp_serialization
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/utilities/xmlrpcpp/lib
-L/opt/ros/electric/stacks/ros_comm/utilities/xmlrpcpp/lib
-lXmlRpc
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/tools/rosconsole/lib
-L/opt/ros/electric/stacks/ros_comm/tools/rosconsole/lib -lrosconsole
-lboost_thread-mt -llog4cxx
-L/opt/ros/electric/stacks/ros_comm/utilities/rostime/lib
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/utilities/rostime/lib
-lrostime
-L/opt/ros/electric/stacks/ros_comm/utilities/cpp_common/lib
-Wl,-rpath,/opt/ros/electric/stacks/ros_comm/utilities/cpp_common/lib
-lcpp_common
-Wl,-rpath,/opt/ros/electric/ros/core/roslib/lib
-L/opt/ros/electric/ros/core/roslib/lib -lroslib
-L/opt/ros/electric/ros/tools/rospack/lib
-Wl,-rpath,/opt/ros/electric/ros/tools/rospack/lib -lrospack -lrosstack
The next step is to incorporate these outputs in a Makefile to
perform a "manual" compilation of a ROS-enabled C++ program such as the
listener.cpp
program described in the "Writing
a Simple Publisher and Subscriber (C++)" tutorial.
Here is a simple Makefile
that can be used to compile the talker.cpp
and the listener.cpp
programs of the tutorial. The example assumes that you downloaded the talker.cpp
and the listener.cpp
files from the tutorial into the src
directory of your newly created package beginner_tutorial
and that you also saved the Makefile
into this src
directory.
> cd
~/ros_workspace/beginner_tutorial/src
> make talker listener
You can test both programs as explained in the tutorial by running roscore, talker and listener in
3 different shells terminal. You can also only run listener
and use the rostopic
command instead of the talker
program.
> rostopic pub /chatter std_msgs/String "Standalone compilation with
make"
Second step: adding more dependencies
ROS has many dependencies and when you create a package (your
own C++
program) you have to indicate the list of dependencies. Please refer to
the ROS
procedure for creating a simple package for more details. We are
going to add to the basic listener.cpp
program proposed in the roscpp tutorial the possibility to subscribe to
the position (x,y,z) of the robot provided that the robot has the
corresponding hardware (Pose sensor with GPS for instance).
We need to add to listener.cpp
the following header file:
#include
<tf/transform_broadcaster.h>
#include
<nav_msgs/Odometry.h>
but this is not enough as we also need to tell the roscpp build system
that the beginner_tutorial package has new dependencies. This is done
by adding information in the package manifest.xml
file as explained in the "Creating
a Package by Hand" page. In our case, this file has a content
similar to:
<package>
<description brief="beginner_tutorials">
beginner_tutorials
</description>
<author>serge</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/beginner_tutorials</url>
<depend package="std_msgs"/>
<depend package="rospy"/>
<depend package="roscpp"/>
</package>
We are going to add 2 dependencies that roughly correspond to the new
header files that we need.
....
<depend package="roscpp"/>
<depend package="nav_msgs"/>
<depend package="tf"/>
</package>
Saving the manifest.xml
file and running:
> rospack
export --lang=cpp --attrib=cflags beginner_tutorial
> rospack
export --lang=cpp --attrib=lflags beginner_tutorial
should give you the '-I', '-l' and '-L' flags used in our
custom Makefile
shown previously.
Here is the listener_proteus.cpp
file where the listener is going to subscribe to the nav_msgs/Odometry.msg
publish by a Pose sensor. The 2 main modifications are:
ros::Subscriber
sub_odom = n.subscribe("/ATRV/Pose_sensor", 1000, odomCallback);
and
void
odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
ROS_INFO("I received odom: [%f,%f]", odom->pose.pose.position.x,
odom->pose.pose.position.y);
}
where we suppose that the topic comes from an ATRV robot. The example
we use here is provided by the PROTEUS
project. Please have a look at these demo slides
written by Pierrick Koch.
Saving listener_proteus.cpp
in the src
directory and running:
>
cd
~/ros_workspace/beginner_tutorial/src
> make listener_proteus
produces the listener_proteus
executable. If you have MORSE and Proteus installed, you could launch
the AvoidObstacleLaser scenario, start it and then run the listener_proteus
executable.
Once you run the AvoidObstacleLaser
scenario, you could also use the following rostopic
commands to display the information you want. Check that the last rostopic command shows the
same information than the running listener_proteus
program.
> rostopic
list
> rostopic
echo -c /ATRV/Pose_sensor
> rostopic
echo -c /ATRV/Pose_sensor/pose
> rostopic
echo -c /ATRV/Pose_sensor/pose/pose/position
Here is a screenshoot that shows the rostopic list
command and the published topics by the robot (Proteus example)
The following screenshoot shows the rostopic
echo -c /ATRV/Pose_sensor/pose/pose/position (top window) and
the listener_proteus
program (bottom window).
Here is the tgz archive
with all the source file and the Makefile
that you can copy into your package's src folder.
Third step: first step in OMNET++/Castalia integration
This third step is the first step for OMNET++/Castalia integration
which is not fully operational yet but it is a first step!
Now that we know how to compile without the ROS build chain, we can
easily modify the Castalia's Makefile to
be able to produce an executable that can make roscpp
calls to subscribe to a robot's position for instance.
Normally, you should have a working install of Castalia 3.2 over
OMNET++ 4.1. You should have build Castalia by issuing:
> cd
Castalia-3.2
>
./makemake
> make
We are going to modify the Makefile to
include additional compiling information in order to support linkage
with roscpp
API and libraries. Here is the modified Makefile of my own
Castalia-3.2 installation tree which should not be very different from
yours.
Basically, we added in the Castalia's standard Makefile
the following information:
INCLUDE_PATH_ROS
= \
-I/opt/ros/electric/stacks/common_msgs/nav_msgs/msg_gen/cpp/include \
-I/opt/ros/electric/stacks/common_msgs/nav_msgs/srv_gen/cpp/include \
-I/opt/ros/electric/stacks/geometry/tf/include \
-I/opt/ros/electric/stacks/geometry/tf/msg_gen/cpp/include \
-I/opt/ros/electric/stacks/geometry/tf/srv_gen/cpp/include \
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/include \
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/msg_gen/cpp/include \
-I/opt/ros/electric/stacks/common_msgs/sensor_msgs/srv_gen/cpp/include \
-I/opt/ros/electric/stacks/common_msgs/geometry_msgs/msg_gen/cpp/include
\
-I/opt/ros/electric/stacks/bullet/include \
-I/opt/ros/electric/stacks/geometry/angles/include \
-I/opt/ros/electric/stacks/ros_comm/tools/rosbag/include \
-I/opt/ros/electric/stacks/ros_comm/tools/topic_tools/include \
-I/opt/ros/electric/stacks/ros_comm/tools/topic_tools/srv_gen/cpp/include
\
-I/opt/ros/electric/stacks/ros_comm/utilities/message_filters/include \
-I/opt/ros/electric/stacks/ros_comm/tools/rostest/include \
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/include \
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/msg_gen/cpp/include
\
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/srv_gen/cpp/include
\
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_serialization/include
\
-I/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_traits/include \
-I/opt/ros/electric/stacks/ros_comm/utilities/xmlrpcpp/src \
-I/opt/ros/electric/stacks/ros_comm/tools/rosconsole/include \
-I/opt/ros/electric/stacks/ros_comm/utilities/rostime/include \
-I/opt/ros/electric/stacks/ros_comm/utilities/cpp_common/include \
-I/opt/ros/electric/stacks/ros_comm/messages/rosgraph_msgs/msg_gen/cpp/include
\
-I/opt/ros/electric/stacks/ros_comm/messages/std_msgs/include \
-I/opt/ros/electric/stacks/ros_comm/messages/std_msgs/msg_gen/cpp/include
\
-I/opt/ros/electric/ros/core/roslib/msg_gen/cpp/include \
-I/opt/ros/electric/ros/core/roslib/include \
-I/opt/ros/electric/ros/tools/rospack \
-I/opt/ros/electric/ros/tools/rospack/include
which is more or less the output of the previous rospack
command line:
>
rospack
export --lang=cpp --attrib=cflags beginner_tutorial
then we modified INCLUDE_PATH to take into account INCLUDE_PATH_ROS:
# C++
include
paths (with -I)
INCLUDE_PATH
= $(INCLUDE_PATH_ROS) \
-I. \
-Isrc \
-Isrc/helpStructures \
-Isrc/node \
-Isrc/node/application \
....
We then also add:
ROS_LIB_DIR
=
\
-L/opt/ros/electric/stacks/geometry/tf/lib \
-L/opt/ros/electric/stacks/common_msgs/sensor_msgs/lib \
-L/opt/ros/electric/stacks/bullet/lib \
-L/opt/ros/electric/stacks/ros_comm/tools/rosbag/lib \
-L/opt/ros/electric/stacks/ros_comm/tools/topic_tools/lib \
-L/opt/ros/electric/stacks/ros_comm/utilities/message_filters/lib
\
-L/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp/lib \
-L/opt/ros/electric/stacks/ros_comm/clients/cpp/roscpp_serialization/lib
\
-L/opt/ros/electric/stacks/ros_comm/utilities/xmlrpcpp/lib \
-L/opt/ros/electric/stacks/ros_comm/tools/rosconsole/lib \
-L/opt/ros/electric/stacks/ros_comm/utilities/rostime/lib \
-L/opt/ros/electric/stacks/ros_comm/utilities/cpp_common/lib \
-L/opt/ros/electric/ros/core/roslib/lib \
-L/opt/ros/electric/ros/tools/rospack/lib
which again is more
or less the output of the previous rospack
command line:
>
rospack
export --lang=cpp --attrib=lflags beginner_tutorial
and of course modified the OMNETPP_LIBS with:
OMNETPP_LIBS
= -L"$(OMNETPP_LIB_SUBDIR)" -L"$(OMNETPP_LIB_DIR)" $(ROS_LIB_DIR)
$(USERIF_LIBS) $(KERNEL_LIBS) $(SYS_LIBS)
to take into account ROS_LIB_DIR. Then, we indicate the extra libraries:
#
Additional
libraries (-L, -l options)
LIBS = -ltf
-lsensor_msgs -lBulletDynamics -lBulletCollision -lLinearMath -lrosbag
-ltopic_tools -lmessage_filters -lros -lboost_thread-mt
-lboost_signals-mt -lroscpp_serialization -lXmlRpc -lrosconsole
-llog4cxx -lrostime -lcpp_common -lroslib -lrospack -lrosstack
and we are ready to build Castalia with roscpp support! There are
certainly other ways to do it but this is the way we chose to have a
quick roscpp support.
Fourth step: testing with Castalia's valueReporting example
We are going to modify the Castalia's valueReporting
example to give it
the basic same feature than our listener_proteus
program. First of all,
we have to add the header files:
#include
"ValueReporting.h"
#include
"ros/ros.h"
#include
"std_msgs/String.h"
#include
<tf/transform_broadcaster.h>
#include
<nav_msgs/Odometry.h>
then add a callback function (note that a class method could also be
used, see "Using
Class Methods as Callbacks")
void
odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
ROS_INFO("I received odom: [%f,%f]", odom->pose.pose.position.x,
odom->pose.pose.position.y);
}
then add in the ValueReporting::startup()
method:
int
dummy_argc=0;
ros::init((int&)dummy_argc,
NULL, "listener");
ros::NodeHandle
n;
ros::Subscriber
sub_odom = n.subscribe("/ATRV/Pose_sensor", 1000, odomCallback);
ros::spin();
we can now rebuild Castalia:
> cd
Castalia-3.2
> make
and test the new valueReporting
example:
> cd
Simulations/valueReporting
>
../../CastaliaBin
Remember to run roscore
first in a terminal! If you test with the previous Proteus example,
your program should keep showing the robot's (x,y) position just like
the listener_proteus
program.
The following screenshoot shows the valueReporting
Castalia program
(bottom window)
Fifth step: non blocking callback with Castalia
This step, probably the most useful step with Castalia since currently
the simulation is blocked, is not yet finalized. Actually, ros::spin()
should not be used but probably ros::spinOnce()
or our own timer method using AsyncSpinner
(see "Callbacks
and Spinning").
If you have any feedback, they are
welcome!
Sixth step: handle time synchronization between simulators
This step is also not finalized at all. The main objective of this
project is to get robot's position from a robot simulator in order to
benefit from a realistic robot navigation stack with navigating
towards goals features. If deployed sensors have to interact with
the robot, at least by knowing the robot's position, time
synchronization is a concern. How time is handle in ROS is currently
not clear for me and this issue should be investigated further.
There have been of course previous efforts for making cooperative
simulations built from various simulators. HLA is one of these efforts
(there is an HLA support for MORSE by the way, check the MORSE web
page). Hopefully, in our case, the complexity will be smaller :-)
Once again, if you have any feedback,
they are welcome!
Useful link
- the rospack
command tool
- the rostopic
command tool, YAML on the ROS
Commandline for test purpose
- description of the nav_msgs/Odometry Message
- example
of sending simple goals with ROS
- a thread
in answers.ros.org
regarding how to get robot's position
- resources
from the ANR Proteus project (S. Stinckwich and Pierrick Koch)
- videos posted by Pierrick
Koch on MORSE, ROS,...