[ros-users] Displaying visualization_msgs::Marker in Stage

[Gonçalo Cabrita]

Hi everyone!

I've recently been trying to modify stageros to draw
visualization_msgs::Marker. After taking a look at Stage's API this is what
I came up with:

// Our node
class StageNode
{
private:

    (...)

    ros::Subscriber markers_sub_;
    Stg::Model * plume_model;

    (...)
}

void
StageNode::markersReceived(const
boost::shared_ptr<visualization_msgs::Marker const>& msg)
{
boost::mutex::scoped_lock lock(msg_lock);
 plume_model->SetColor(Stg::Color(msg->color.r, msg->color.g, msg->color.b,
msg->color.a));
plume_model->ClearBlocks();
for(int i=0 ; i<msg->points.size() ; i++)
{
plume_model->AddBlockRect(msg->points[i].x, msg->points[i].y, 0.1, 0.1,
0.1);
}
plume_model->NeedRedraw();
}

StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
{
(...)

markers_sub_ = n_.subscribe<visualization_msgs::Marker>("plumesim_markers",
10, boost::bind(&StageNode::markersReceived, this, _1));

plume_model = new Stg::Model(world);
plume_model->ClearBlocks();
plume_model->SetObstacleReturn(0);
}

So I am creating a Stg::Model and I am also creating a callback for
the visualization_msgs::Marker, and my idea was to push the incoming markers
as Blocks into my Stg:Model and redraw it (not worrying for now how they
look, just want them to show up).
However so far I have had no luck. Does anyone have any experience with
libstage? Am I missing something or is this entirely wrong?

Thanks for the help,

Gonçalo Cabrita
ISR - University of Coimbra
Portugal
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20100608/31ea02ae/attachment-0002.html>