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

[Tully Foote]

Hi Kartik,

Looking through your code, I notice quickly that you are advertising and
then publishing in very short succession, and if you publish before the link
has been setup the message will not get through.  The advertise call is not
blocking and work continues in the background.  You should move the
publishers to be member variables and the advertise calls into your
initialization.

PS a better forum for this would be answers.ros.org

Tully

On Mon, May 16, 2011 at 2:13 PM, Kartik Babu <kartik.babu at gmail.com> wrote:

> 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
> fine.
>
> 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");
>  if(!initialized_){
>    ROS_ERROR("This object must be initialized before runBehavior is
> called");
>    return;
>  }
>
>  if(global_costmap_ == NULL || local_costmap_ == NULL){
>    ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot
> be NULL. Doing nothing.");
>    return;
>  }
>
>  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;
>  local_costmap_->getRobotPose(global_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;
>
> vel_pub.publish(cmd_vel);
> interval.sleep();
>
>  while(n.ok() && distance_covered < max_backup_distance_ &&
> (ros::Time::now()-start_time).toSec() < 5){
>
> local_costmap_->getRobotPose(global_pose);
>    distance_covered += (global_pose.getOrigin() -
> last_pose.getOrigin()).length();
>    last_pose = global_pose;
>
>    if( dwa_planner_.computeVelocityCommands( cmd_vel ) ){
> last_valid_control=ros::Time::now();
> ROS_INFO( "cmd_vel: %g %g %g\n", cmd_vel.linear.x, cmd_vel.linear.y,
> cmd_vel.angular.z );
> }
> else{
> 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 ){
> vel_pub.publish(cmd_vel);
> break;
> }
> }
> vel_pub.publish(cmd_vel);
> r.sleep();
> }
>
> //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 );
>
> }*
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>


-- 
Tully Foote
Systems Engineer
Willow Garage, Inc.
tfoote at willowgarage.com
(650) 475-2827
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20110517/c92cd6e5/attachment-0003.html>