From f1533f54bb62a93f232ba41038d8516f8ef2e50c Mon Sep 17 00:00:00 2001 From: tappan-at-git Date: Sun, 13 Dec 2015 12:12:07 -0500 Subject: [PATCH 1/3] Parameterized diff_drive_controller's odom_frame_id --- .../include/diff_drive_controller/diff_drive_controller.h | 3 +++ diff_drive_controller/src/diff_drive_controller.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index 781c21976..9ae6fc447 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -141,6 +141,9 @@ namespace diff_drive_controller{ /// Frame to use for the robot base: std::string base_frame_id_; + /// Frame to use for odometry and odom tf: + std::string odom_frame_id_; + /// Whether to publish odometry to tf or not: bool enable_odom_tf_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index b581b0467..98a2711f2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -116,6 +116,7 @@ namespace diff_drive_controller{ , wheel_radius_multiplier_(1.0) , cmd_vel_timeout_(0.5) , base_frame_id_("base_link") + , odom_frame_id_("odom") , enable_odom_tf_(true) , wheel_joints_size_(0) { @@ -184,6 +185,9 @@ namespace diff_drive_controller{ controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_); + controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_); + controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled")); @@ -530,7 +534,7 @@ namespace diff_drive_controller{ // Setup odometry realtime publisher + odom message constant fields odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odom", 100)); - odom_pub_->msg_.header.frame_id = "odom"; + odom_pub_->msg_.header.frame_id = odom_frame_id_; odom_pub_->msg_.child_frame_id = base_frame_id_; odom_pub_->msg_.pose.pose.position.z = 0; odom_pub_->msg_.pose.covariance = boost::assign::list_of @@ -555,7 +559,7 @@ namespace diff_drive_controller{ tf_odom_pub_->msg_.transforms.resize(1); tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0; tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_; - tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom"; + tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_; } } // namespace diff_drive_controller From e451b87d5452563184a2b19c7800fefe012a279e Mon Sep 17 00:00:00 2001 From: Eric Tappan Date: Mon, 14 Dec 2015 11:08:35 -0500 Subject: [PATCH 2/3] Added tests for the odom_frame_id parameter. --- diff_drive_controller/CMakeLists.txt | 8 ++ .../test/diff_drive_default_odom_frame.test | 19 ++++ .../diff_drive_default_odom_frame_test.cpp | 79 ++++++++++++++++ .../test/diff_drive_odom_frame.test | 20 ++++ .../test/diff_drive_odom_frame_test.cpp | 93 +++++++++++++++++++ 5 files changed, 219 insertions(+) create mode 100644 diff_drive_controller/test/diff_drive_default_odom_frame.test create mode 100644 diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp create mode 100644 diff_drive_controller/test/diff_drive_odom_frame.test create mode 100644 diff_drive_controller/test/diff_drive_odom_frame_test.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b0389cad5..e180d2902 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -66,6 +66,14 @@ if (CATKIN_ENABLE_TESTING) test/diff_drive_odom_tf.test test/diff_drive_odom_tf_test.cpp) target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES}) + add_rostest_gtest(diff_drive_default_odom_frame_test + test/diff_drive_default_odom_frame.test + test/diff_drive_default_odom_frame_test.cpp) + target_link_libraries(diff_drive_default_odom_frame_test ${catkin_LIBRARIES}) + add_rostest_gtest(diff_drive_odom_frame_test + test/diff_drive_odom_frame.test + test/diff_drive_odom_frame_test.cpp) + target_link_libraries(diff_drive_odom_frame_test ${catkin_LIBRARIES}) add_rostest(test/diff_drive_open_loop.test) add_rostest(test/skid_steer_controller.test) add_rostest(test/skid_steer_no_wheels.test) diff --git a/diff_drive_controller/test/diff_drive_default_odom_frame.test b/diff_drive_controller/test/diff_drive_default_odom_frame.test new file mode 100644 index 000000000..decd9fc0b --- /dev/null +++ b/diff_drive_controller/test/diff_drive_default_odom_frame.test @@ -0,0 +1,19 @@ + + + + + + + + + + + + + diff --git a/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp b/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp new file mode 100644 index 000000000..d1d5b9992 --- /dev/null +++ b/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp @@ -0,0 +1,79 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// 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 PAL Robotics, 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 Eric Tappan + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the original odom frame doesn't exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +TEST_F(DiffDriveControllerTest, testOdomTopic) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get an odom message + nav_msgs::Odometry odom_msg = getLastOdom(); + // check its frame_id + ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom"); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_default_odom_frame_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/diff_drive_controller/test/diff_drive_odom_frame.test b/diff_drive_controller/test/diff_drive_odom_frame.test new file mode 100644 index 000000000..76117d402 --- /dev/null +++ b/diff_drive_controller/test/diff_drive_odom_frame.test @@ -0,0 +1,20 @@ + + + + + + + diffbot_controller: + odom_frame_id: new_odom + enable_odom_tf: True + + + + + + + + diff --git a/diff_drive_controller/test/diff_drive_odom_frame_test.cpp b/diff_drive_controller/test/diff_drive_odom_frame_test.cpp new file mode 100644 index 000000000..c1c68adbb --- /dev/null +++ b/diff_drive_controller/test/diff_drive_odom_frame_test.cpp @@ -0,0 +1,93 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// 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 PAL Robotics, 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 Eric Tappan + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNoOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the original odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); +} + +TEST_F(DiffDriveControllerTest, testNewOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the new_odom frame does exist + EXPECT_TRUE(listener.frameExists("new_odom")); +} + +TEST_F(DiffDriveControllerTest, testOdomTopic) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get an odom message + nav_msgs::Odometry odom_msg = getLastOdom(); + // check its frame_id + ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom"); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_odom_frame_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} From 3c4f5d75a78cbe9d25129a6f36f6d08b57020788 Mon Sep 17 00:00:00 2001 From: Eric Tappan Date: Tue, 15 Dec 2015 10:15:03 -0500 Subject: [PATCH 3/3] Reduced pedantry, redundancy. --- .../test/diff_drive_default_odom_frame_test.cpp | 8 +------- diff_drive_controller/test/diff_drive_odom_frame.test | 1 - diff_drive_controller/test/diff_drive_odom_frame_test.cpp | 8 +------- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp b/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp index d1d5b9992..bd2130388 100644 --- a/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp +++ b/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp @@ -1,5 +1,5 @@ /////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2014, PAL Robotics S.L. +// Copyright (C) 2015, Locus Robotics Corp. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -53,12 +53,6 @@ TEST_F(DiffDriveControllerTest, testOdomTopic) ros::Duration(0.1).sleep(); } - // zero everything before test - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.angular.z = 0.0; - publish(cmd_vel); - ros::Duration(0.1).sleep(); // get an odom message nav_msgs::Odometry odom_msg = getLastOdom(); // check its frame_id diff --git a/diff_drive_controller/test/diff_drive_odom_frame.test b/diff_drive_controller/test/diff_drive_odom_frame.test index 76117d402..235f8069d 100644 --- a/diff_drive_controller/test/diff_drive_odom_frame.test +++ b/diff_drive_controller/test/diff_drive_odom_frame.test @@ -6,7 +6,6 @@ diffbot_controller: odom_frame_id: new_odom - enable_odom_tf: True diff --git a/diff_drive_controller/test/diff_drive_odom_frame_test.cpp b/diff_drive_controller/test/diff_drive_odom_frame_test.cpp index c1c68adbb..735f57609 100644 --- a/diff_drive_controller/test/diff_drive_odom_frame_test.cpp +++ b/diff_drive_controller/test/diff_drive_odom_frame_test.cpp @@ -1,5 +1,5 @@ /////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2014, PAL Robotics S.L. +// Copyright (C) 2015, Locus Robotics Corp. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -67,12 +67,6 @@ TEST_F(DiffDriveControllerTest, testOdomTopic) ros::Duration(0.1).sleep(); } - // zero everything before test - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.angular.z = 0.0; - publish(cmd_vel); - ros::Duration(0.1).sleep(); // get an odom message nav_msgs::Odometry odom_msg = getLastOdom(); // check its frame_id