Add pose subscriber (instead of #26)

[gerkey]

Added @slremy's changes from #26 to add a Pose subscriber. Then I:

• changed the topic name from 'pose' to 'cmd_pose', for parallelism with 'cmd_vel' (yes, that should probably be 'cmd_twist' instead, but we're not changing that now)
• added tests for the effects of publishing to cmd_vel and cmd_pose (with a new test dep on rospy)

[gerkey]

@wjwwood, you OK if I merge this?

[wjwwood]

+1 with CI

[daenny]

I had already answered previously in my first feature request to @slremy.
Would it not make sense to wrap the pose subscriber as a service instead of a subscriber, then the lock would not be needed, right?
We would need to define a new "teleport.srv" or something then though.
However, I do see the advantage of also having the message callback, as you can then configure rviz to teleport the robots, by sending "navigation" goals, but then the message type would need to be a PoseStamped.
My last question is, if we want to allow "collision moves", i.e. teleporting the robot to "non free" space in the map, or out of map bounds. I do not know if that is easily accessable in the stage API.

[gerkey]

@daenny We could also add a service interface if you think that's useful. But I like the simplicity of the topic interface. Good point about accepting PoseStamped for interaction with rviz. Should we add both (service and PoseStamped topic)?

[gerkey]

@wjwwood Regarding CI, the Travis build failed on the new test, apparently because the import of tf.transformations in the new test script failed:

Traceback (most recent call last):
  File "/home/travis/build/ros-simulation/stage_ros/test/cmdpose_tests.py", line 45, in <module>
    import tf.transformations
  File "/home/travis/build/ros-simulation/stage_ros/deps/install_isolated/lib/python2.7/dist-packages/tf/__init__.py", line 28, in <module>
    from _tf import *
ImportError: libconsole_bridge.so: cannot open shared object file: No such file or directory

Perhaps related, rosout complains repeatedly during the tests about libconsole_bridge.so:

/home/travis/build/ros-simulation/stage_ros/deps/install_isolated/lib/rosout/rosout: error while loading shared libraries: libconsole_bridge.so: cannot open shared object file: No such file or directory

Did I miss a dependency somewhere?

[wjwwood]

No I think some recent changes have broken the travis config I made for this repository. I'm going to try to update it in a new pr.

[daenny]

@gerkey So for convenience PoseStamped makes more sense, to be able to set the poses via rviz directly. The service call might be useful if we add the collision check, so that we can get a result whether the teleport was successful or not. If that check is not there, I think only having the message interface is fine, since we can assume that the move always works.

[wjwwood]

@gerkey Now that I have merged #28, if you rebase this pr against master then the CI should be working again.

[wjwwood]

I just realized the branch was on this repo, so I went a head and rebased it.

[wjwwood]

I rebased again after another travis-ci fixup pull request.

[gerkey]

In f94cb5a I added a PoseStamped subscriber (with a test), as suggested by @daenny. In doing so, I had to decide what to do with the frame_id in the incoming message. Following a discussion with @wjwwood and further inspection of the code, I decided to do nothing with it, meaning that every pose is taken to be in Stage's world frame, which is how the original Pose subscriber works. My reasoning:

• Stage's world frame isn't currently represented in tf.
• To add Stage's world frame to tf would result in having a tf tree with two roots: you can determine a robot's pose either according to ground truth or according to odometry (plus maybe amcl).
• What Stage is already publishing as base_pose_ground_truth is in Stage's world frame, but the outgoing message is tagged with odom as the frame_id. That's not really correct, but I'd rather not change it.

In summary, the operations on Stage's ground truth information, whether published or subscribed, are operating outside of tf entirely and require the user to have implicit knowledge of that frame. That's not the perfect solution, but it should still be very useful.

Thoughts?

[wjwwood]

Based on that, my conclusion would be to only accept Pose and not PoseStamped, since you're ignoring all the information in the header (time stamp and frame id).

Is the argument for accepting a PoseStamped so that you could use the UI of rviz to set the position of the robot? If so, what fixed frame should the user use to send the PoseStamped such that a valid value is given to Stage? If the ground truth in Stage is not provided as a tf frame, then there is no coordinate system that rviz can publish in which matches the coordinate system that Stage will use (ignoring the frame id rviz put in the message). So it sounds like it will always be the wrong coordinate frame. So this use case seems dangerous (because it works but isn't correct and the user doesn't know that), as it allows Stage to ignore the frame id in the PoseStamped as if it were just a Pose, even though the producing tool is relying on the fact that the frame id should be considered.

Ignoring the time stamp has similar issues, but unlike frame id, I think the amount of time that passes between is small enough and the robots slow enough for that not to matter much. In contrast, ignoring the frame id could result large errors in position and also invalid orientations due to transforms.

[gerkey]

@wjwwood That's a fair point. I hadn't thought all the way through what the rviz interaction would look like. Maybe we should just leave it with the Pose subscriber. @daenny, @slremy, what do you think?

[daenny]

@wjwwood, @gerkey: Good points, I also did not think that through completely. I think it indeed makes sense then not to use PoseStamped. I would then prefer to just use Pose2D as message and not Pose, since then you do not need to set quaternions, i.e. when using the commandline.

[slremy]

Hi @gerkey, I'm not much of an RViz user but I can understand @wjwwood's point.

[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.