diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89c433d..b33119b 100644
--- a/CMakeLists.txt
+++ b/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()
diff --git a/package.xml b/package.xml
index 8803254..2ef75e2 100644
--- a/package.xml
+++ b/package.xml
@@ -36,4 +36,6 @@
std_msgs
std_srvs
tf
+
+ rospy
diff --git a/src/stageros.cpp b/src/stageros.cpp
index fc3350e..7d72383 100644
--- a/src/stageros.cpp
+++ b/src/stageros.cpp
@@ -49,6 +49,9 @@
#include
#include
+#include
+#include
+#include "tf/LinearMath/Transform.h"
#include
#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,6 +101,8 @@ class StageNode
std::vector laser_pubs; //multiple lasers
ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
+ ros::Subscriber pose_sub;
+ ros::Subscriber posestamped_sub;
};
std::vector robotmodels_;
@@ -103,9 +110,9 @@ class StageNode
// Used to remember initial poses for soft reset
std::vector 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& msg);
+ // Message callback for a cmd_pose message, which sets positions.
+ void poseReceived(int idx, const boost::shared_ptr& 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& 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& msg)
{
@@ -266,6 +279,35 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptrbase_last_cmd = this->sim_time;
}
+void
+StageNode::poseReceived(int idx, const boost::shared_ptr& 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& 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 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(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10);
new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10);
new_robot->cmdvel_sub = n_.subscribe(mapName(CMD_VEL, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
+ new_robot->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1));
+
+ new_robot->posestamped_sub = n_.subscribe(mapName(POSESTAMPED, r, static_cast(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);
}
-
diff --git a/test/cmdpose_tests.py b/test/cmdpose_tests.py
new file mode 100755
index 0000000..988405a
--- /dev/null
+++ b/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)
diff --git a/test/cmdpose_tests.xml b/test/cmdpose_tests.xml
new file mode 100644
index 0000000..fc4c3f7
--- /dev/null
+++ b/test/cmdpose_tests.xml
@@ -0,0 +1,4 @@
+
+
+
+