Stageros: how to programmatically set the pose of a simulated robot

edit

I have modified the stageros.cpp file to make it subscribe to the topic "pose" which receives geometry_msgs/Pose2D and then sets the robot's pose to the desired one. The following are the lines that I had to add to stageros.cpp:

#include <geometry_msgs/Pose2D.h>

#define POSE "pose" 

std::vector<ros::Subscriber> pose_subs_;

void
StageNode::poseReceived(int idx, const boost::shared_ptr<geometry_msgs::Pose2D const>& msg)
{
  boost::mutex::scoped_lock lock(msg_lock);
    Stg::Pose pose;
    pose.x = msg->x;
    pose.y = msg->y;
    pose.z =0;
    pose.a = msg->theta;
    this->positionmodels[idx]->SetPose(pose);
}


pose_subs_.push_back(n_.subscribe<geometry_msgs::Pose2D>(mapName(POSE,r), 10, boost::bind(&StageNode::poseReceived, this, r, _1)));