<meta http-equiv="content-type" content="text/html; charset=utf-8"><span class="Apple-style-span" style="border-collapse: collapse; font-family: arial, sans-serif; font-size: 13px; ">Hi All,</span><div><span class="Apple-style-span" style="border-collapse: collapse; font-family: arial, sans-serif; font-size: 13px; "> I'm using diamondback on Ubuntu 10.04 x64.</span></div>
<div><span class="Apple-style-span" style="border-collapse: collapse; font-family: arial, sans-serif; font-size: 13px; "><br> 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. <br>
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. </span></div>
<div><span class="Apple-style-span" style="border-collapse: collapse; font-family: arial, sans-serif; font-size: 13px; "><br> 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 fine.</span></div>
<div><span class="Apple-style-span" style="border-collapse: collapse; font-family: arial, sans-serif; font-size: 13px; "><br>I'm adding the recovery behavior file, i that helps:<br><br><b>void BackupRecovery::runBehavior(){<br>
static ros::Time last_invocation;<br>static std::string msg_id("A");<br>ROS_INFO("The backup dwa recovery behavior was just invoked\n");<br> if(!initialized_){<br>   ROS_ERROR("This object must be initialized before runBehavior is called");<br>
   return;<br> }<br><br> if(global_costmap_ == NULL || local_costmap_ == NULL){<br>   ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");<br>   return;<br> }<br><br>
 ros::Rate r(frequency_);<br> ros::Duration interval(0.5);<br> ros::NodeHandle n;<br> ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);<br> ros::Publisher flip_pub = n.advertise<std_msgs::String>("/flip_laser", 1);<br>
 geometry_msgs::PoseStamped backup_goal;<br> std_msgs::String flip_msg;<br> std::vector< geometry_msgs::PoseStamped > backup_path;<br> ros::Time last_valid_control, start_time;<br><br> tf::Stamped<tf::Pose> global_pose, last_pose, start_pose;<br>
 local_costmap_->getRobotPose(global_pose);<br> ROS_INFO( "global_pose frame_id: %s\n", global_pose.frame_id_.c_str() );<br> flip_msg.data = std::string("Start");<br> <br> ROS_INFO( "Sending flip message 1, %s\n", flip_msg.data.c_str() );<br>
 flip_pub.publish( flip_msg );<br>   <br> last_pose = global_pose;<br> start_pose = global_pose;<br> backup_goal.pose.position.x = global_pose.getOrigin().x() - 3*cos( tf::getYaw( global_pose.getRotation() ) );<br> backup_goal.pose.position.y = global_pose.getOrigin().y() - 3*sin( tf::getYaw( global_pose.getRotation() ) );<br>
 backup_goal.pose.position.z = global_pose.getOrigin().z();<br> backup_goal.pose.orientation = tf::createQuaternionMsgFromYaw( tf::getYaw( global_pose.getRotation() ) );<br> backup_goal.header.frame_id = global_pose.frame_id_;<br>
 <br> backup_path.push_back( backup_goal ); <br> <br> dwa_planner_.setPlan( backup_path );<br> <br> double distance_covered = 0;<br>geometry_msgs::Twist cmd_vel;<br> start_time = ros::Time::now();<br> last_valid_control = ros::Time::now();<br>
 <br>cmd_vel.linear.x = -0.1;<br>cmd_vel.linear.y = 0.0;<br>cmd_vel.linear.z = 0.0;<br>cmd_vel.angular.x = 0.0;<br>cmd_vel.angular.y = 0.0;<br>cmd_vel.angular.z = 0.0;<br><br>vel_pub.publish(cmd_vel);<br>interval.sleep();<br>
<br> while(n.ok() && distance_covered < max_backup_distance_ && (ros::Time::now()-start_time).toSec() < 5){<br>   <br>local_costmap_->getRobotPose(global_pose);<br>   distance_covered += (global_pose.getOrigin() - last_pose.getOrigin()).length();<br>
   last_pose = global_pose;<br>   <br>   if( dwa_planner_.computeVelocityCommands( cmd_vel ) ){<br>last_valid_control=ros::Time::now();<br>ROS_INFO( "cmd_vel: %g %g %g\n", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );<br>
}<br>else{<br>cmd_vel.linear.x = 0.0;<br>cmd_vel.linear.y = 0.0;<br>cmd_vel.linear.z = 0.0;<br>cmd_vel.angular.x = 0.0;<br>cmd_vel.angular.y = 0.0;<br>cmd_vel.angular.z = 0.0;       <br>if( (ros::Time::now() - last_valid_control).toSec() > 2.0 ){<br>
vel_pub.publish(cmd_vel);<br>break;<br>}<br>}<br>vel_pub.publish(cmd_vel);<br>r.sleep();<br>}<br><br>//max_backup_distance_ *= 1.5;<br>last_invocation = ros::Time::now();<br> flip_msg.data = std::string("Stop");<br>
 <br> ROS_INFO( "Sending flip message 2, %s\n", flip_msg.data.c_str() );<br> flip_pub.publish( flip_msg );<br><br>}</b></span></div>