Welcome to the second lab session of the Care-O-bot Experimentation Days

The challenge of the day will be to step into one service robot technology in more detail. As an example we will have a look at the navigation capabilities of the robot following these steps:

• Learn about the architecture and different layers of base control and navigation
• Move the robot through code
• Explore different navigation strategies

Login to the robot

On the robot each group has the same user as on the desktop machines, e.g. expdays-group1. You can login to that account using the password first.

ssh -X expdays-group1@cob3-3-pc1

Learn about layers of base control and navigation

Message used to command the base

To move the robot, messages of type geometry_msgs/Twist containing linear (m/s) and angular (rad/s) velocities (in robot coordinate-system) are sent to the undercarriage controller via /base_controller/command topic.

rostopic echo /base_controller/command

Create a expdays_base_test ROS package

Create a new ROS package in ~/git/care-o-bot.

cd ~/git/care-o-bot
roscreate-pkg expdays_base_test std_msgs rospy roscpp geometry_msgs nav_msgs

Open the generated CMakeLists.txt with an editor.

cd ~/git/care-o-bot/expdays_base_test
gedit CMakeLists.txt

Add the following line at the end of the the document.

rosbuild_add_executable(expdays_base_test src/expdays_base_test.cpp)

This will generate an executable if you build the package. Now edit the sourcefile.

gedit ~/git/care-o-bot/expdays_base_test/src/expdays_base_test.cpp

Publish your own message

Now as you learned what kind of message needs to be commanded to move the base, it's time to create and publish such a message on your own. Here you get an code sample where you can insert your code.

   1 // ROS includes
   2 #include <ros/ros.h>
   3 #include <geometry_msgs/Twist.h>
   4 
   5 int main(int argc, char** argv)
   6 {
   7   // initialize ROS, specify name of node
   8   ros::init(argc, argv, "cob_simple_drive");
   9 
  10   // create a handle for this node, initialize node
  11   ros::NodeHandle nh;
  12 
  13   // create a ROS publisher
  14   ros::Publisher command_publisher;
  15 
  16   // Set ros publisher to publish velocity command messages 
  17   // to the "base_controller/command" topic
  18   command_publisher 
  19     = nh.advertise<geometry_msgs::Twist>("/base_controller/command", 1);
  20 
  21   // create new geometry_msgs::Twist that holds 
  22   // linear and angular velocities
  23   geometry_msgs::Twist twist_msg;
  24   
  25   // TODO: fill in velocities; reasonable velocities are 0.05 [m/s]
  26   
  27   // publish twist_msg until node is killed
  28   ros::Rate loop_rate(10); //Hz
  29   while( nh.ok() )
  30   {
  31     command_publisher.publish(twist_msg);
  32     ros::spinOnce();
  33     loop_rate.sleep();
  34   }
  35 
  36   // TODO: publish stop message
  37 
  38   command_publisher.publish(twist_msg);
  39     
  40   return 0;
  41 }

To build your package do a

rosmake expdays_base_test

You can start your node with

rosrun expdays_base_test expdays_base_test

Make the robot move forward for 3 seconds

For this task it's convenient to use the Timer. Extend the while-loop of the previous example to fulfill the requirements.

   1 ros::Time t0 = ros::Time::now();
   2 ros::Time some_future_time = ros::Time::now() + ros::Duration(3.0);

Make the robot move forward for 1 meter

To get the robot's actual movement one can use nav_msgs/Odometry published by undercarriage controller on topic /base_controller/odometry. This message provides measurements of the velocity and the integrated position since startup (fixed coordinate-system).

   1 // ROS includes
   2 #include <ros/ros.h>
   3 #include <geometry_msgs/Twist.h>
   4 #include <nav_msgs/Odometry.h>
   5 
   6 // callback function for incomming messages
   7 void topicCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
   8 
   9 // flag for received odometry message
  10 bool has_odometry_msg;
  11 // stores last received odometry message
  12 nav_msgs::Odometry last_odometry_msg;
  13 
  14 int main(int argc, char** argv)
  15 {
  16   // initialize ROS, spezify name of node
  17   ros::init(argc, argv, "cob_simple_drive");
  18 
  19   // create a handle for this node, initialize node
  20   ros::NodeHandle nh;
  21 
  22   // create publisher and subscriber
  23   ros::Publisher command_publisher;
  24   ros::Subscriber odometry_subscriber;  
  25   
  26   // Set ros publisher to publish velocity command messages 
  27   // to the "base_controller/command" topic
  28   command_publisher = nh.advertise<geometry_msgs::Twist>
  29     ("base_controller/command", 1);
  30   // Set ros subscriber to listen to odometry measurement messages 
  31   // from the "base_controller/odometry" topic
  32   odometry_subscriber = nh.subscribe<nav_msgs::Odometry>
  33     ("base_controller/odometry", 1, &topicCallbackOdometry);
  34     
  35   has_odometry_msg = false;
  36   
  37   // wait for first odometry measurement message
  38   while(!has_odometry_msg && nh.ok()) 
  39   {
  40     ros::spinOnce();
  41   }
  42   
  43   // Save start position
  44   double x0 = last_odometry_msg.pose.pose.position.x;
  45   double y0 = last_odometry_msg.pose.pose.position.y;
  46   double dist_traveled = 0.0;
  47   
  48   // create new geometry_msgs::Twist that holds linear and angular velocities
  49   geometry_msgs::Twist twist_msg;
  50  
  51   // TODO: fill in velocities  
  52 
  53   // publish velocity messages until the robot moved a defined distance 
  54   ros::Rate loop_rate(10);
  55   while( dist_traveled < 1.0 && nh.ok() ) 
  56   {
  57     command_publisher.publish(twist_msg);
  58     ros::spinOnce();
  59     loop_rate.sleep();
  60    
  61     // TODO: update dist_traveled
  62   }
  63   
  64   // TODO: publish stop message
  65 
  66   command_publisher.publish(twist_msg);
  67     
  68   return 0;
  69 }
  70 
  71 // Callback method for received odometry messages
  72 void topicCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) 
  73 {
  74   last_odometry_msg = *msg;
  75   has_odometry_msg = true;
  76 }

Every received odometry message executes the callback function topicCallbackOdometry which stores the last message for further processing. The flag has_odometry_msg makes sure at least one message is available.

So insert your code and try to solve the task!

Different navigation strategies

start navigation

roslaunch cob_2dnav 2dnav_ipa.launch

linear nav

approach:

This controller tries to reach the Cartesian goal position without path planning using a linear movement. During the Cartesian control of the linear path the environment is checked for obstacles and the robot stops before a collision occurs. As an example this controller if often used when approaching a table. The robot will stop at a certain distance to the table independent of the global localization of the robot.

pro:

• Fast and predictable movements

contra:

• No active obstacle avoidance. Robot will stop movement when to close to obstacles.

Usage example: move the robot through code

sss.move("base","home",mode="linear")

For using this mode in rviz contact a member of the Care-O-bot team.

diff

approach:

By only allowing the base to turn and move forward this navigation mode emulates the behaviour of a differential driven platform. The base will turn in place into the direction of the goal and will use global and reactive path planning strategies to navigate to the goal position. Afterwards it will turn in place to the goal angle. During motion it will avoid obstacles or temporary pause motion if no path can be found.

pro:

• Very natural motion because the robot always moves forward.
• Dynamic obstacle avoidance.

contra:

• Doesn't use omnidirectional capabilities of Care-O-bot 3 base.

Usage example: move the robot through code

sss.move("base","home",mode="diff")

For using this mode in rviz contact a member of the Care-O-bot team.

omni

approach:

This navigation method uses the full capabilities of the mobile base. Path planning is done on all 3 DOF directly from start to goal position. A reactive path planner avoids obstacles during the motion or pauses the task if no path can be found temporarly.

pro:

• Uses full flexibility of mobile base to navigate in narrow environments.
• Most effective method of navigation.

contra:

• Sometimes unnatural unexpected movements.
• More CPU expensive path planning.

Usage example: move the robot through code

sss.move("base","home",mode="omni")

For using this mode in rviz contact a member of the Care-O-bot team.

Different navigation strategies

Try to modify your scenario in order to experiment with the different navigation strategies. Find out what approach makes most sense in the different parts of the scenario.

Integrate your own navigation into scenario

Implement a service call in your base controller code in order to call the controller from the scenario script (see http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29). Test the service call using the rosservice tool on the shell. Add a call to the service to the scenario script where it makes sense (e.g. during turning of the robot) and test the scenario on the robot.