Merge pull request #120 from pal-robotics/diff_drive_param_enable_odo…

…m_tf

Add default-true parameter "~enable_odom_tf"

diff_drive_controller/CMakeLists.txt

@@ -31,15 +31,17 @@ install(FILES diff_drive_controller_plugins.xml
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 if (CATKIN_ENABLE_TESTING)
-  find_package(catkin COMPONENTS rostest controller_manager)
+  find_package(catkin COMPONENTS rostest controller_manager tf)
   include_directories(include ${catkin_INCLUDE_DIRS})
 
   add_executable(diffbot test/diffbot.cpp)
   target_link_libraries(diffbot ${catkin_LIBRARIES})
 
   add_dependencies(tests diffbot)
 
-  add_rostest_gtest(diff_drive_test test/diff_drive_controller.test test/diff_drive_test.cpp)
+  add_rostest_gtest(diff_drive_test
+    test/diff_drive_controller.test
+    test/diff_drive_test.cpp)
   target_link_libraries(diff_drive_test ${catkin_LIBRARIES})
   add_rostest_gtest(diff_drive_limits_test
     test/diff_drive_controller_limits.test
@@ -55,5 +57,9 @@ if (CATKIN_ENABLE_TESTING)
     test/diff_drive_fail_test.cpp)
   target_link_libraries(diff_drive_fail_test ${catkin_LIBRARIES})
   add_rostest(test/diff_drive_bad_urdf.test)
+  add_rostest_gtest(diff_drive_odom_tf_test
+    test/diff_drive_odom_tf.test
+    test/diff_drive_odom_tf_test.cpp)
+  target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES})
 
 endif()

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_;
 
+    /// Whether to publish odometry to tf or not:
+    bool enable_odom_tf_;
+
     // speed limiters
     Commands last_cmd_;
     SpeedLimiter limiter_lin_;

diff_drive_controller/package.xml

@@ -30,8 +30,8 @@
   <!--Tests-->
   <build_depend>rostest</build_depend>
 
-  <build_depend>controller_manager</build_depend>
-  <build_depend>xacro</build_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>xacro</test_depend>
 
   <export>
     <controller_interface plugin="${prefix}/diff_drive_controller_plugins.xml"/>

diff_drive_controller/src/diff_drive_controller.cpp

@@ -113,6 +113,7 @@ namespace diff_drive_controller{
     , wheel_radius_multiplier_(1.0)
     , cmd_vel_timeout_(0.5)
     , base_frame_id_("base_link")
+    , enable_odom_tf_(true)
   {
   }
 
@@ -160,6 +161,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("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
+
     // Velocity and acceleration limits:
     controller_nh.param("linear/x/has_velocity_limits"    , limiter_lin_.has_velocity_limits    , limiter_lin_.has_velocity_limits    );
     controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
@@ -219,7 +223,7 @@ namespace diff_drive_controller{
       }
 
       // Publish tf /odom frame
-      if(tf_odom_pub_->trylock())
+      if (enable_odom_tf_ && tf_odom_pub_->trylock())
       {
         odom_frame_.header.stamp = time;
         odom_frame_.transform.translation.x = odometry_.getX();

diff_drive_controller/test/diff_drive_odom_tf.test

@@ -0,0 +1,19 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Make sure to disable odom tf -->
+  <rosparam>
+    diffbot_controller:
+      enable_odom_tf: False
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_odom_tf_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_odom_tf_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

diff_drive_controller/test/diff_drive_odom_tf_test.cpp

@@ -0,0 +1,59 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 Bence Magyar
+
+#include "test_common.h"
+#include <tf/transform_listener.h>
+
+// 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 odom frame doesn't exist
+  EXPECT_FALSE(listener.frameExists("odom"));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_odom_tf_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

diff_drive_controller/test/diff_drive_test.cpp

@@ -28,6 +28,7 @@
 /// \author Bence Magyar
 
 #include "test_common.h"
+#include <tf/transform_listener.h>
 
 // TEST CASES
 TEST_F(DiffDriveControllerTest, testForward)
@@ -121,6 +122,20 @@ TEST_F(DiffDriveControllerTest, testTurn)
   EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS);
 }
 
+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 odom frame exist
+  EXPECT_TRUE(listener.frameExists("odom"));
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);