Add regression tests for cases with 1, 2 links in urdf, with and without fixed joints. Fix bug where 2-link urdf is not accepted #4042 #4185

robot_state_publisher/CMakeLists.txt

@@ -31,8 +31,19 @@ rosbuild_add_executable(test_publisher test/test_publisher.cpp )
 target_link_libraries(test_publisher ${PROJECT_NAME})
 rosbuild_add_gtest_build_flags(test_publisher)
 rosbuild_declare_test(test_publisher)
-
 rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch)
 
+rosbuild_add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp )
+target_link_libraries(test_two_links_fixed_joint ${PROJECT_NAME})
+rosbuild_add_gtest_build_flags(test_two_links_fixed_joint)
+rosbuild_declare_test(test_two_links_fixed_joint)
+rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch)
+
+rosbuild_add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp )
+target_link_libraries(test_two_links_moving_joint ${PROJECT_NAME})
+rosbuild_add_gtest_build_flags(test_two_links_moving_joint)
+rosbuild_declare_test(test_two_links_moving_joint)
+rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch)
+
 # Download needed data file
 rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states_indexed.bag test/joint_states_indexed.bag 793e0b566ebe4698265a936b92fa2bba)

robot_state_publisher/src/joint_state_listener.cpp

@@ -64,7 +64,7 @@ JointStateListener::~JointStateListener()
 
 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
 {
-  if (state->get_name_size() != state->get_position_size()){
+  if (state->name.size() != state->position.size()){
     ROS_ERROR("Robot state publisher received an invalid joint state vector");
     return;
   }

robot_state_publisher/src/robot_state_publisher.cpp

@@ -68,7 +68,7 @@ bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_pos
   // calculate transforms form root to every segment in tree
   map<string, Frame> link_poses;
   solver_->JntToCart(joint_positions, link_poses);
-  if (link_poses.size() < 2){
+  if (link_poses.empty()){
     ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
     return false;
   }

robot_state_publisher/src/treefksolverposfull_recursive.cpp

@@ -54,7 +54,7 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
                                                   const Frame& previous_frame, const SegmentMap::const_iterator this_segment)
 {
   // get pose of this segment
-  Frame this_frame = previous_frame;
+  Frame this_frame;
   double jnt_p = 0;
   if (this_segment->second.segment.getJoint().getType() != Joint::None){
     map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
@@ -64,7 +64,9 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
     }
     jnt_p = jnt_pos->second;
   }
-  this_frame = this_frame * this_segment->second.segment.pose(jnt_p);
+  this_frame = previous_frame * this_segment->second.segment.pose(jnt_p);
+  double r, p, y;
+  this_frame.M.GetRPY(r, p, y);
   if (this_segment->first != tree.getRootSegment()->first)
     p_out.insert(make_pair(this_segment->first, this_frame));
 

robot_state_publisher/test/one_link.urdf

@@ -0,0 +1,3 @@
+<robot name="test_robot">
+  <link name="link1" />
+</robot>

robot_state_publisher/test/test_publisher.cpp

@@ -62,31 +62,11 @@ public:
 protected:
   /// constructor
   TestPublisher()
-  {
-  /*
-    // constructs a robot model from the xml file
-    urdf::Model robot_model;
-    if (g_argc == 2){
-      if (!robot_model.initFile(g_argv[1]))
-        ROS_ERROR("Failed to construct robot model from xml string");
-    }
-    else
-      ROS_ERROR("No robot model as argument given");
-
-    // constructs a kdl tree from the robot model
-    Tree tree;
-    if (!kdl_parser::treeFromUrdfModel(robot_model, tree))
-      ROS_ERROR("Failed to extract kdl tree from robot model");
-
-    publisher = new JointStateListener(tree);
-  */
-  }
+  {}
 
   /// Destructor
   ~TestPublisher()
-  {
-    //    delete publisher;
-  }
+  {}
 };
 
 

robot_state_publisher/test/test_two_links_fixed_joint.cpp

@@ -0,0 +1,120 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, 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 the Willow Garage 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: Wim Meeussen */
+
+#include <string>
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <boost/thread/thread.hpp>
+#include <urdf/model.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include "robot_state_publisher/joint_state_listener.h"
+
+
+using namespace ros;
+using namespace tf;
+using namespace robot_state_publisher;
+
+
+int g_argc;
+char** g_argv;
+
+#define EPS 0.01
+
+class TestPublisher : public testing::Test
+{
+public:
+  JointStateListener* publisher;
+
+protected:
+  /// constructor
+  TestPublisher()
+  {}
+
+  /// Destructor
+  ~TestPublisher()
+  {}
+};
+
+
+
+
+
+TEST_F(TestPublisher, test)
+{
+  ROS_INFO("Creating tf listener");
+  TransformListener tf;
+
+  ROS_INFO("Publishing joint state to robot state publisher");
+  ros::NodeHandle n;
+  ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("joint_states", 100);
+  sensor_msgs::JointState js_msg;
+  for (unsigned int i=0; i<100; i++){
+    js_msg.header.stamp = ros::Time::now();
+    js_pub.publish(js_msg);
+    ros::Duration(0.1).sleep();
+  }
+
+  ASSERT_TRUE(tf.canTransform("link1", "link2", Time()));
+  ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
+
+  tf::StampedTransform t;
+  tf.lookupTransform("link1", "link2",Time(), t );
+  EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
+  EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
+  EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS);
+
+  SUCCEED();
+}
+
+
+
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_robot_state_publisher");
+  ros::NodeHandle node;
+  boost::thread ros_thread(boost::bind(&ros::spin));
+
+  g_argc = argc;
+  g_argv = argv;
+  int res = RUN_ALL_TESTS();
+  ros_thread.interrupt();
+  ros_thread.join();
+
+  return res;
+}

robot_state_publisher/test/test_two_links_fixed_joint.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
+  <param name="robot_description" textfile="$(find robot_state_publisher)/test/two_links_fixed_joint.urdf" />
+
+  <test test-name="test_publisher" pkg="robot_state_publisher" type="test_two_links_fixed_joint" />
+</launch>

robot_state_publisher/test/test_two_links_moving_joint.cpp

@@ -0,0 +1,122 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, 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 the Willow Garage 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: Wim Meeussen */
+
+#include <string>
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <boost/thread/thread.hpp>
+#include <urdf/model.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include "robot_state_publisher/joint_state_listener.h"
+
+
+using namespace ros;
+using namespace tf;
+using namespace robot_state_publisher;
+
+
+int g_argc;
+char** g_argv;
+
+#define EPS 0.01
+
+class TestPublisher : public testing::Test
+{
+public:
+  JointStateListener* publisher;
+
+protected:
+  /// constructor
+  TestPublisher()
+  {}
+
+  /// Destructor
+  ~TestPublisher()
+  {}
+};
+
+
+
+
+
+TEST_F(TestPublisher, test)
+{
+  ROS_INFO("Creating tf listener");
+  TransformListener tf;
+
+  ROS_INFO("Publishing joint state to robot state publisher");
+  ros::NodeHandle n;
+  ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("joint_states", 100);
+  sensor_msgs::JointState js_msg;
+  js_msg.name.push_back("joint1");
+  js_msg.position.push_back(M_PI);
+  for (unsigned int i=0; i<100; i++){
+    js_msg.header.stamp = ros::Time::now();
+    js_pub.publish(js_msg);
+    ros::Duration(0.1).sleep();
+  }
+
+  ASSERT_TRUE(tf.canTransform("link1", "link2", Time()));
+  ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
+
+  tf::StampedTransform t;
+  tf.lookupTransform("link1", "link2",Time(), t );
+  EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
+  EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
+  EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS);
+
+  SUCCEED();
+}
+
+
+
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_robot_state_publisher");
+  ros::NodeHandle node;
+  boost::thread ros_thread(boost::bind(&ros::spin));
+
+  g_argc = argc;
+  g_argv = argv;
+  int res = RUN_ALL_TESTS();
+  ros_thread.interrupt();
+  ros_thread.join();
+
+  return res;
+}

robot_state_publisher/test/test_two_links_moving_joint.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
+  <param name="robot_description" textfile="$(find robot_state_publisher)/test/two_links_moving_joint.urdf" />
+
+  <test test-name="test_publisher" pkg="robot_state_publisher" type="test_two_links_moving_joint" />
+</launch>

robot_state_publisher/test/two_links_fixed_joint.urdf

@@ -0,0 +1,10 @@
+<robot name="test_robot">
+  <link name="link1" />
+  <link name="link2" />
+
+  <joint name="joint1" type="fixed">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz="5 0 0" rpy="0 0 1.57" />
+  </joint>
+</robot>

robot_state_publisher/test/two_links_moving_joint.urdf

@@ -0,0 +1,10 @@
+<robot name="test_robot">
+  <link name="link1" />
+  <link name="link2" />
+
+  <joint name="joint1" type="continuous">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz="5 0 0" rpy="0 0 1.57" />
+  </joint>
+</robot>