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