[ros-users] Messages being dropped only when sent from plugin.

Kartik Babu kartik.babu at gmail.com
Mon May 16 21:13:27 UTC 2011

Hi All,
 I'm using diamondback on Ubuntu 10.04 x64.

 I wrote a custom move_base recovery plugin that sends out a couple of
std_msgs::String messages. This is a recovery behavior that deliberatively
plans an escape route from a stuck configuration. A laser sensor mounted on
a tilt unit is sent a message to flip and face backwards when the routine
starts and told to flip and face forwards again when done.
These "/flip_laser" messages are not getting through to the destination node
consistently, at least one or both of the messages are dropped and there is
no consistent pattern as to which one is dropped.

 My system has pretty heavy message traffic including sensor messages,
costmaps, etc. However a "rostopic pub" command-line will consistently
deliver the messages to the pan tilt with full background traffic flowing.
This message apparently is the only one that is disappearing, as far as I
can tell. The cmd_vel messages published by the same plugin get through just

I'm adding the recovery behavior file, i that helps:

*void BackupRecovery::runBehavior(){
static ros::Time last_invocation;
static std::string msg_id("A");
ROS_INFO("The backup dwa recovery behavior was just invoked\n");
   ROS_ERROR("This object must be initialized before runBehavior is

 if(global_costmap_ == NULL || local_costmap_ == NULL){
   ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot
be NULL. Doing nothing.");

 ros::Rate r(frequency_);
 ros::Duration interval(0.5);
 ros::NodeHandle n;
 ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
 ros::Publisher flip_pub = n.advertise<std_msgs::String>("/flip_laser", 1);
 geometry_msgs::PoseStamped backup_goal;
 std_msgs::String flip_msg;
 std::vector< geometry_msgs::PoseStamped > backup_path;
 ros::Time last_valid_control, start_time;

 tf::Stamped<tf::Pose> global_pose, last_pose, start_pose;
 ROS_INFO( "global_pose frame_id: %s\n", global_pose.frame_id_.c_str() );
 flip_msg.data = std::string("Start");

 ROS_INFO( "Sending flip message 1, %s\n", flip_msg.data.c_str() );
 flip_pub.publish( flip_msg );

 last_pose = global_pose;
 start_pose = global_pose;
 backup_goal.pose.position.x = global_pose.getOrigin().x() - 3*cos(
tf::getYaw( global_pose.getRotation() ) );
 backup_goal.pose.position.y = global_pose.getOrigin().y() - 3*sin(
tf::getYaw( global_pose.getRotation() ) );
 backup_goal.pose.position.z = global_pose.getOrigin().z();
 backup_goal.pose.orientation = tf::createQuaternionMsgFromYaw( tf::getYaw(
global_pose.getRotation() ) );
 backup_goal.header.frame_id = global_pose.frame_id_;

 backup_path.push_back( backup_goal );

 dwa_planner_.setPlan( backup_path );

 double distance_covered = 0;
geometry_msgs::Twist cmd_vel;
 start_time = ros::Time::now();
 last_valid_control = ros::Time::now();

cmd_vel.linear.x = -0.1;
cmd_vel.linear.y = 0.0;
cmd_vel.linear.z = 0.0;
cmd_vel.angular.x = 0.0;
cmd_vel.angular.y = 0.0;
cmd_vel.angular.z = 0.0;


 while(n.ok() && distance_covered < max_backup_distance_ &&
(ros::Time::now()-start_time).toSec() < 5){

   distance_covered += (global_pose.getOrigin() -
   last_pose = global_pose;

   if( dwa_planner_.computeVelocityCommands( cmd_vel ) ){
ROS_INFO( "cmd_vel: %g %g %g\n", cmd_vel.linear.x, cmd_vel.linear.y,
cmd_vel.angular.z );
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.linear.z = 0.0;
cmd_vel.angular.x = 0.0;
cmd_vel.angular.y = 0.0;
cmd_vel.angular.z = 0.0;
if( (ros::Time::now() - last_valid_control).toSec() > 2.0 ){

//max_backup_distance_ *= 1.5;
last_invocation = ros::Time::now();
 flip_msg.data = std::string("Stop");

 ROS_INFO( "Sending flip message 2, %s\n", flip_msg.data.c_str() );
 flip_pub.publish( flip_msg );

-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20110516/56cbf8e3/attachment-0001.html>

More information about the ros-users mailing list