Make the change_fixed_joint test more robust. (ros#183)

It may take a bit of time for the updated fixed transforms to propagate through the system, so add in a loop to wait up to 10 seconds for them to appear. Signed-off-by: Chris Lalancette <[email protected]>
AssistiveRoboticsUNH · Nov 12, 2021 · 37aff20 · 37aff20
1 parent 4c9e9e1
commit 37aff20
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 8 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(orocos_kdl REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -33,6 +34,7 @@ ament_target_dependencies(${PROJECT_NAME}_node
   geometry_msgs
   kdl_parser
   orocos_kdl
+  rcl_interfaces
   rclcpp
   rclcpp_components
   sensor_msgs

diff --git a/include/robot_state_publisher/robot_state_publisher.hpp b/include/robot_state_publisher/robot_state_publisher.hpp
@@ -33,6 +33,7 @@
 
 #include <builtin_interfaces/msg/time.hpp>
 #include <kdl/tree.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/string.hpp>

diff --git a/package.xml b/package.xml
@@ -16,6 +16,7 @@
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>kdl_parser</build_depend>
+  <build_depend>rcl_interfaces</build_depend>
   <build_depend>orocos_kdl</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
@@ -28,6 +29,7 @@
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>kdl_parser</exec_depend>
   <exec_depend>orocos_kdl</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>

diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp
@@ -34,6 +34,7 @@
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <kdl/tree.hpp>
 #include <kdl_parser/kdl_parser.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>

diff --git a/test/test_two_links_change_fixed_joint.cpp b/test/test_two_links_change_fixed_joint.cpp
@@ -92,16 +92,19 @@ TEST(test_publisher, test_two_joints)
   ASSERT_EQ(set_parameters_result.size(), 1u);
   ASSERT_TRUE(set_parameters_result[0].successful);
 
-  for (i = 0; i < 100 && !buffer.canTransform("link1", "link2", rclcpp::Time()); ++i) {
+  for (i = 0; i < 100; ++i) {
+    if (buffer.canTransform("link1", "link2", rclcpp::Time())) {
+      t = buffer.lookupTransform("link1", "link2", rclcpp::Time());
+      if (fabs(t.transform.translation.x - 10.0) <= EPS) {
+        ASSERT_NEAR(t.transform.translation.x, 10.0, EPS);
+        ASSERT_NEAR(t.transform.translation.y, 0.0, EPS);
+        ASSERT_NEAR(t.transform.translation.z, 0.0, EPS);
+        break;
+      }
+    }
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
-
-  ASSERT_TRUE(buffer.canTransform("link1", "link2", rclcpp::Time()));
-
-  t = buffer.lookupTransform("link1", "link2", rclcpp::Time());
-  EXPECT_NEAR(t.transform.translation.x, 10.0, EPS);
-  EXPECT_NEAR(t.transform.translation.y, 0.0, EPS);
-  EXPECT_NEAR(t.transform.translation.z, 0.0, EPS);
+  ASSERT_LT(i, 100u);
 }
 
 int main(int argc, char ** argv)