ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Placing permanent visual marker in rviz

asked 2012-07-30 23:22:41 -0500

popolvar gravatar image

Hi there,

I would like to place a visual marker representing the goal line (through which the robot should pass) in rviz. IMHO, this code should do the trick

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "goal_marker");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::ARROW;

  visualization_msgs::Marker marker;

  // Set the frame ID and timestamp.  See the TF tutorials for information on these.
  marker.header.frame_id = "/map";
  marker.header.stamp = ros::Time::now();

  // set other marker properties
  ...
  // set other marker properties end

  marker.lifetime = ros::Duration();

  // Publish the marker
  marker_pub.publish(marker);

}

However the marker does not appear in rviz. So in practice I end up doing this (based on Markers:Sending Basic Shapes)

int main( int argc, char** argv )
{
  ros::init(argc, argv, "goal_marker");
  ros::NodeHandle n;
  ros::Rate r(100);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::ARROW;

 for (int x=0; x<2; x++){
    visualization_msgs::Marker marker;

    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/map";
    marker.header.stamp = ros::Time::now();

    // set other marker properties
    ...
    // set other marker properties end

    marker.lifetime = ros::Duration();

    // Publish the marker
    marker_pub.publish(marker);

    r.sleep();
  }
}

Both the two iterations and the r.sleep are necessary, after their removal the marker does not appear in rviz. Any ideas why the first approach doesn't work and second does?

Would appreciate any help. Cheers!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-07-31 01:22:40 -0500

dornhege gravatar image

updated 2012-07-31 01:24:27 -0500

I'd suspect your first program quickly returns and doesn't run a loop?

In that case, the publisher just hasn't connected yet, when you try to send the message and then your program quits, so it can never resend messages.

You should wait until publisher.getNumSubscribers() > 0, before you publish.

Here is some code snippet that I use:

bool waitForSubscribers(ros::Publisher & pub, ros::Duration timeout)
{
    if(pub.getNumSubscribers() > 0)
        return true;
    ros::Time start = ros::Time::now();
    ros::Rate waitTime(0.5);
    while(ros::Time::now() - start < timeout) {
        waitTime.sleep();
        if(pub.getNumSubscribers() > 0)
            break;
    }
    return pub.getNumSubscribers() > 0;
}
edit flag offensive delete link more

Question Tools

Stats

Asked: 2012-07-30 23:22:41 -0500

Seen: 2,426 times

Last updated: Jul 31 '12