Provide ability to programmatically change the pose of a robot.

[kleinma]

I would like to be able to programmatically change the pose of one or multiple robots within stage. I don't want to have to alter the worldfile or drag around the robot in stage with my mouse to do this.

Thus, I added a subscriber to a geometry_msgs\Pose2D message on the topic cmd_pose or \robot_#\cmd_pose. A callback function cmdposeRecieved() is modeled after cmdvelRecieved() and calls SetPose() to set the robot's pose in the stage simulator.

My code is adapted from and answer to the question found here:
http://answers.ros.org/question/61503/stageros-how-to-programmatically-set-the-pose-of-a-simulated-robot/?answer=61612#post-id-61612

I also added a message to be displayed at runtime that informs the of this new change. However, this is for internal use and can be removed.

[wjwwood]

Thanks for the contribution! We'll review it and get back to your asap.

/cc @gerkey fyi

[daenny]

This is very similar to the discussion we started here:
#27

[wjwwood]

Sorry, I accidentally closed this when changing the branch layout of the repository. I've reopened it and updated the target base to be lunar-devel which is the new default branch. You can edit again to point at indigo-devel, which is what is used to release for ROS Indigo and Kinetic, if you like.

[h3ct0r]

Is this pull request going to be merged? This patch is very helpful.

[kleinma]

I just rebased to remove some unnecessary comments from the commit history. Should I squash everything down into one commit. Sorry, I've never actually done this before!