How to replace a robot in stage_ros

edit

Hi, it is currently not implemented in the stage_ros node. But it can be implemented rather easily using the stage API. (If you do, please make a pull request, I was planning to do something similar ;) ). I already made a pull request for something similar, namely resetting the robots to their initial position when stage started: https://github.com/ros-simulation/sta...

You would need to implement a "MoveRobot" service or something similar, i.e. create a new service in which has the robot name/number and the target position and then move to corresponding robot.

This is the code snippet I used to replace the robots (the initial poses are saved at startup, see here):

StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response){       ROS_INFO("Resetting stage!"); 
for (size_t r = 0; r < this->positionmodels.size(); r++) {
    this->positionmodels[r]->SetPose(this->initial_poses[r]);
    this->positionmodels[r]->SetStall(false); }
return true; }