Add pose subscriber (instead of #26)

CMakeLists.txt

@@ -62,4 +62,5 @@ install(DIRECTORY world
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/hztest.xml)
+  add_rostest(test/cmdpose_tests.xml)
 endif()

package.xml

@@ -36,4 +36,6 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>tf</run_depend>
+
+  <test_depend>rospy</test_depend>
 </package>

src/stageros.cpp

@@ -49,6 +49,9 @@
 #include <geometry_msgs/Twist.h>
 #include <rosgraph_msgs/Clock.h>
 
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include "tf/LinearMath/Transform.h"
 #include <std_srvs/Empty.h>
 
 #include "tf/transform_broadcaster.h"
@@ -61,6 +64,8 @@
 #define BASE_SCAN "base_scan"
 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
 #define CMD_VEL "cmd_vel"
+#define POSE "cmd_pose"
+#define POSESTAMPED "cmd_pose_stamped"
 
 // Our node
 class StageNode
@@ -96,16 +101,18 @@ class StageNode
         std::vector<ros::Publisher> laser_pubs; //multiple lasers
 
         ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
+        ros::Subscriber pose_sub;
+        ros::Subscriber posestamped_sub;
     };
 
     std::vector<StageRobot const *> robotmodels_;
 
     // Used to remember initial poses for soft reset
     std::vector<Stg::Pose> initial_poses;
     ros::ServiceServer reset_srv_;
-  
+
     ros::Publisher clock_pub_;
-    
+
     bool isDepthCanonical;
     bool use_model_names;
 
@@ -134,7 +141,7 @@ class StageNode
 
     // Current simulation time
     ros::Time sim_time;
-    
+
     // Last time we saved global position (for velocity calculation).
     ros::Time base_last_globalpos_time;
     // Last published global pose of each robot
@@ -153,14 +160,21 @@ class StageNode
 
     // Our callback
     void WorldCallback();
-    
+
     // Do one update of the world.  May pause if the next update time
     // has not yet arrived.
     bool UpdateWorld();
 
-    // Message callback for a MsgBaseVel message, which set velocities.
+    // Message callback for a cmd_vel message, which set velocities.
     void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);
 
+    // Message callback for a cmd_pose message, which sets positions.
+    void poseReceived(int idx, const boost::shared_ptr<geometry_msgs::Pose const>& msg);
+
+    // Message callback for a cmd_pose_stamped message, which sets positions
+    // with a timestamp (e.g., from rviz).
+    void poseStampedReceived(int idx, const boost::shared_ptr<geometry_msgs::PoseStamped const>& msg);
+
     // Service callback for soft reset
     bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 
@@ -255,7 +269,6 @@ StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Resp
 }
 
 
-
 void
 StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg)
 {
@@ -266,6 +279,35 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist
     this->base_last_cmd = this->sim_time;
 }
 
+void
+StageNode::poseReceived(int idx, const boost::shared_ptr<geometry_msgs::Pose const>& msg)
+{
+    boost::mutex::scoped_lock lock(msg_lock);
+    Stg::Pose pose;
+
+    double roll, pitch, yaw;
+    tf::Matrix3x3 m(tf::Quaternion(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w));
+    m.getRPY(roll, pitch, yaw);
+    pose.x = msg->position.x;
+    pose.y = msg->position.y;
+    pose.z = 0;
+    pose.a = yaw;
+    this->positionmodels[idx]->SetPose(pose);
+}
+
+void
+StageNode::poseStampedReceived(int idx, const boost::shared_ptr<geometry_msgs::PoseStamped const>& msg)
+{
+    // Create a shared pointer to the raw pose and pass it through.
+    // Note that we ignore the header because we're not supporting setting of
+    // poses in an arbitrary frame. Every pose is interpreted to be in Stage's
+    // world frame (which isn't represented anywhere as a tf frame).
+    geometry_msgs::Pose* pose = new geometry_msgs::Pose;
+    *pose = msg->pose;
+    boost::shared_ptr<geometry_msgs::Pose const> pose_ptr(pose);
+    this->poseReceived(idx, pose_ptr);
+}
+
 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
 {
     this->use_model_names = use_model_names;
@@ -354,6 +396,9 @@ StageNode::SubscribeModels()
         new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
         new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
         new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
+        new_robot->pose_sub = n_.subscribe<geometry_msgs::Pose>(mapName(POSE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1));
+
+        new_robot->posestamped_sub = n_.subscribe<geometry_msgs::PoseStamped>(mapName(POSESTAMPED, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseStampedReceived, this, r, _1));
 
         for (size_t s = 0;  s < new_robot->lasermodels.size(); ++s)
         {
@@ -779,4 +824,3 @@ main(int argc, char** argv)
 
     exit(0);
 }
-

test/cmdpose_tests.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2015, Open Source Robotics Foundation, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Brian Gerkey
+
+import sys
+import threading
+import time
+import unittest
+
+from geometry_msgs.msg import Pose, PoseStamped, Twist
+from nav_msgs.msg import Odometry
+import rospy
+import rostest
+import tf.transformations
+
+class TestStageRos(unittest.TestCase):
+
+    def __init__(self, *args):
+        unittest.TestCase.__init__(self, *args)
+        rospy.init_node('pose_tester', anonymous=True)
+
+    def _base_pose_ground_truth_sub(self, msg):
+        self.base_pose_ground_truth = msg
+
+    def _odom_sub(self, msg):
+        self.odom = msg
+
+    def setUp(self):
+        self.odom = None
+        self.base_pose_ground_truth = None
+        self.done = False
+        self.odom_sub = rospy.Subscriber('odom', Odometry, self._odom_sub)
+        self.base_pose_ground_truth_sub = rospy.Subscriber(
+            'base_pose_ground_truth', Odometry, self._base_pose_ground_truth_sub)
+        # Make sure we get base_pose_ground_truth
+        while self.base_pose_ground_truth is None:
+          time.sleep(0.1)
+        # Make sure we get odom and the robot is stopped (not still moving
+        # from the previous test). We can count on stage to return true zeros.
+        while (self.odom is None or
+               self.odom.twist.twist.linear.x != 0.0 or
+               self.odom.twist.twist.linear.y != 0.0 or
+               self.odom.twist.twist.linear.z != 0.0 or
+               self.odom.twist.twist.angular.x != 0.0 or
+               self.odom.twist.twist.angular.y != 0.0 or
+               self.odom.twist.twist.angular.z != 0.0):
+          time.sleep(0.1)
+
+    def _pub_thread(self, pub, msg):
+        while not self.done:
+            pub.publish(msg)
+            time.sleep(0.05)
+
+    # Test that, if we command the robot to drive forward for a while, that it does
+    # so.
+    def test_cmdvel_x(self):
+        odom0 = self.odom
+        pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
+        twist = Twist()
+        twist.linear.x = 1.0
+        # Make a thread to repeatedly publish (to overcome Stage's watchdog)
+        t = threading.Thread(target=self._pub_thread, args=[pub, twist])
+        t.start()
+        time.sleep(3.0)
+        odom1 = self.odom
+        self.done = True
+        t.join()
+        # Now we expect the robot's odometric pose to differ in X but nothing
+        # else
+        self.assertGreater(odom1.header.stamp, odom0.header.stamp)
+        self.assertNotAlmostEqual(odom1.pose.pose.position.x, odom0.pose.pose.position.x)
+        self.assertAlmostEqual(odom1.pose.pose.position.y, odom0.pose.pose.position.y)
+        self.assertAlmostEqual(odom1.pose.pose.position.z, odom0.pose.pose.position.z)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.x, odom0.pose.pose.orientation.x)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.y, odom0.pose.pose.orientation.y)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.z, odom0.pose.pose.orientation.z)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.w, odom0.pose.pose.orientation.w)
+
+    # Test that, if we command the robot to turn in place for a while, that it does
+    # so.
+    def test_cmdvel_yaw(self):
+        odom0 = self.odom
+        pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
+        twist = Twist()
+        twist.angular.z = 0.25
+        # Make a thread to repeatedly publish (to overcome Stage's watchdog)
+        t = threading.Thread(target=self._pub_thread, args=[pub, twist])
+        t.start()
+        time.sleep(3.0)
+        odom1 = self.odom
+        self.done = True
+        t.join()
+        # Now we expect the robot's odometric pose to differ in yaw (which will
+        # appear in the quaternion elements z and w) and not elsewhere
+        self.assertGreater(odom1.header.stamp, odom0.header.stamp)
+        self.assertAlmostEqual(odom1.pose.pose.position.x, odom0.pose.pose.position.x)
+        self.assertAlmostEqual(odom1.pose.pose.position.y, odom0.pose.pose.position.y)
+        self.assertAlmostEqual(odom1.pose.pose.position.z, odom0.pose.pose.position.z)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.x, odom0.pose.pose.orientation.x)
+        self.assertAlmostEqual(odom1.pose.pose.orientation.y, odom0.pose.pose.orientation.y)
+        self.assertNotAlmostEqual(odom1.pose.pose.orientation.z, odom0.pose.pose.orientation.z)
+        self.assertNotAlmostEqual(odom1.pose.pose.orientation.w, odom0.pose.pose.orientation.w)
+
+    # Test that, if we command the robot to jump to a pose, it does so.
+    def test_pose(self):
+        pub = rospy.Publisher('cmd_pose', Pose, queue_size=1)
+        while pub.get_num_connections() == 0:
+            time.sleep(0.1)
+        pose = Pose()
+        pose.position.x = 42.0
+        pose.position.y = -42.0
+        pose.position.z = 142.0
+        roll = 0.2
+        pitch = -0.3
+        yaw = 0.9
+        q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
+        pose.orientation.x = q[0]
+        pose.orientation.y = q[1]
+        pose.orientation.z = q[2]
+        pose.orientation.w = q[3]
+        pub.publish(pose)
+        time.sleep(3.0)
+        # Now we expect the robot's ground truth pose to be what we told, except
+        # for z, roll, and pitch, which should all be zero (Stage is 2-D, after all).
+        bpgt = self.base_pose_ground_truth
+        self.assertAlmostEqual(bpgt.pose.pose.position.x, pose.position.x)
+        self.assertAlmostEqual(bpgt.pose.pose.position.y, pose.position.y)
+        self.assertEqual(bpgt.pose.pose.position.z, 0.0)
+        q = [bpgt.pose.pose.orientation.x,
+             bpgt.pose.pose.orientation.y,
+             bpgt.pose.pose.orientation.z,
+             bpgt.pose.pose.orientation.w]
+        e = tf.transformations.euler_from_quaternion(q)
+        self.assertEqual(e[0], 0.0)
+        self.assertEqual(e[1], 0.0)
+        self.assertAlmostEqual(e[2], yaw)
+
+    # Test that, if we command the robot to jump to a pose (with a header), it does so.
+    def test_pose_stamped(self):
+        pub = rospy.Publisher('cmd_pose_stamped', PoseStamped, queue_size=1)
+        while pub.get_num_connections() == 0:
+            time.sleep(0.1)
+        ps = PoseStamped()
+        ps.header.frame_id = 'ignored_value'
+        ps.header.stamp = rospy.Time.now()
+        ps.pose.position.x = -42.0
+        ps.pose.position.y = 42.0
+        ps.pose.position.z = -142.0
+        roll = -0.2
+        pitch = 0.3
+        yaw = -0.9
+        q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
+        ps.pose.orientation.x = q[0]
+        ps.pose.orientation.y = q[1]
+        ps.pose.orientation.z = q[2]
+        ps.pose.orientation.w = q[3]
+        pub.publish(ps)
+        time.sleep(3.0)
+        # Now we expect the robot's ground truth pose to be what we told, except
+        # for z, roll, and pitch, which should all be zero (Stage is 2-D, after all).
+        bpgt = self.base_pose_ground_truth
+        self.assertAlmostEqual(bpgt.pose.pose.position.x, ps.pose.position.x)
+        self.assertAlmostEqual(bpgt.pose.pose.position.y, ps.pose.position.y)
+        self.assertEqual(bpgt.pose.pose.position.z, 0.0)
+        q = [bpgt.pose.pose.orientation.x,
+             bpgt.pose.pose.orientation.y,
+             bpgt.pose.pose.orientation.z,
+             bpgt.pose.pose.orientation.w]
+        e = tf.transformations.euler_from_quaternion(q)
+        self.assertEqual(e[0], 0.0)
+        self.assertEqual(e[1], 0.0)
+        self.assertAlmostEqual(e[2], yaw)
+
+NAME = 'stage_ros'
+if __name__ == '__main__':
+    rostest.unitrun('stage_ros', NAME, TestStageRos, sys.argv)

test/cmdpose_tests.xml

@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="stage_ros" name="stageros" type="stageros" args="-g $(find stage_ros)/world/willow-erratic.world"/>
+  <test test-name="cmdpose_tests" pkg="stage_ros" type="cmdpose_tests.py" name="cmdpose_tests"/>
+</launch>