Merge branch 'master' into fix/reconfigure

ros-controls · Aug 22, 2024 · e915412 · e915412
2 parents 9c7c733 + 4ab22a5
commit e915412
Show file tree

Hide file tree

Showing 26 changed files with 342 additions and 85 deletions.
diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt
@@ -58,10 +58,8 @@ if(BUILD_TESTING)
   find_package(ros2_control_test_assets REQUIRED)
 
 
-  add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller
-    test/test_load_ackermann_steering_controller.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml
-  )
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp)
   ament_target_dependencies(test_load_ackermann_steering_controller
     controller_manager
     hardware_interface

diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp
@@ -29,12 +29,15 @@ TEST(TestLoadAckermannSteeringController, load_controller)
 
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/ackermann_steering_controller_params.yaml";
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_ackermann_steering_controller",
-      "ackermann_steering_controller/AckermannSteeringController"),
-    nullptr);
+  cm.set_parameter({"test_ackermann_steering_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_ackermann_steering_controller.type",
+     "ackermann_steering_controller/AckermannSteeringController"});
+
+  ASSERT_NE(cm.load_controller("test_ackermann_steering_controller"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt
@@ -68,10 +68,8 @@ if(BUILD_TESTING)
   find_package(kinematics_interface_kdl REQUIRED)
 
   # test loading admittance controller
-  add_rostest_with_parameters_gmock(test_load_admittance_controller
-    test/test_load_admittance_controller.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
-  )
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp)
   ament_target_dependencies(test_load_admittance_controller
     controller_manager
     hardware_interface

diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp
@@ -30,10 +30,13 @@ TEST(TestLoadAdmittanceController, load_controller)
 
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
+  const std::string test_file_path = std::string(TEST_FILES_DIRECTORY) + "/test_params.yaml";
 
-  ASSERT_EQ(
-    cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"),
-    nullptr);
+  cm.set_parameter({"load_admittance_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"load_admittance_controller.type", "admittance_controller/AdmittanceController"});
+
+  ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt
@@ -57,10 +57,8 @@ if(BUILD_TESTING)
   find_package(hardware_interface REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller
-    test/test_load_bicycle_steering_controller.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml
-  )
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp)
   ament_target_dependencies(test_load_bicycle_steering_controller
     controller_manager
     hardware_interface

diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp
@@ -29,11 +29,15 @@ TEST(TestLoadBicycleSteeringController, load_controller)
 
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/bicycle_steering_controller_params.yaml";
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"),
-    nullptr);
+  cm.set_parameter({"test_bicycle_steering_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_bicycle_steering_controller.type",
+     "bicycle_steering_controller/BicycleSteeringController"});
+
+  ASSERT_NE(cm.load_controller("test_bicycle_steering_controller"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
@@ -70,10 +70,8 @@ if(BUILD_TESTING)
     tf2_msgs
   )
 
-  add_rostest_with_parameters_gmock(test_load_diff_drive_controller
-    test/test_load_diff_drive_controller.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml
-  )
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp)
   ament_target_dependencies(test_load_diff_drive_controller
     controller_manager
     ros2_control_test_assets

diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp
@@ -29,10 +29,14 @@ TEST(TestLoadDiffDriveController, load_controller)
 
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager");
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller.yaml";
 
-  ASSERT_NE(
-    cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"),
-    nullptr);
+  cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"});
+
+  ASSERT_NE(cm.load_controller("test_diff_drive_controller"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt
@@ -53,9 +53,8 @@ if(BUILD_TESTING)
   find_package(hardware_interface REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster
-    test/test_load_force_torque_sensor_broadcaster.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml)
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp)
   target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include)
   target_link_libraries(test_load_force_torque_sensor_broadcaster
     force_torque_sensor_broadcaster

diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp
@@ -34,11 +34,15 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller)
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_force_torque_sensor_broadcaster",
-      "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"),
-    nullptr);
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/force_torque_sensor_broadcaster_params.yaml";
+
+  cm.set_parameter({"test_force_torque_sensor_broadcaster.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_force_torque_sensor_broadcaster.type",
+     "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"});
+
+  ASSERT_NE(cm.load_controller("test_force_torque_sensor_broadcaster"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt
@@ -53,9 +53,8 @@ if(BUILD_TESTING)
   find_package(hardware_interface REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster
-    test/test_load_imu_sensor_broadcaster.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml)
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp)
   target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include)
   target_link_libraries(test_load_imu_sensor_broadcaster
     imu_sensor_broadcaster

diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp
@@ -33,11 +33,14 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller)
 
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/imu_sensor_broadcaster_params.yaml";
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"),
-    nullptr);
+  cm.set_parameter({"test_imu_sensor_broadcaster.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_imu_sensor_broadcaster.type", "imu_sensor_broadcaster/IMUSensorBroadcaster"});
+
+  ASSERT_NE(cm.load_controller("test_imu_sensor_broadcaster"), nullptr);
 }
 
 int main(int argc, char ** argv)

diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt
@@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   realtime_tools
   sensor_msgs
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)
@@ -72,6 +73,7 @@ if(BUILD_TESTING)
   )
   ament_target_dependencies(test_joint_state_broadcaster
     hardware_interface
+    ros2_control_test_assets
   )
 endif()
 

diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml
@@ -22,6 +22,7 @@
   <depend>rcutils</depend>
   <depend>realtime_tools</depend>
   <depend>sensor_msgs</depend>
+  <depend>urdf</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp
@@ -25,6 +25,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/time.hpp"
 #include "std_msgs/msg/header.hpp"
+#include "urdf/model.h"
 
 namespace rclcpp_lifecycle
 {
@@ -242,14 +243,30 @@ bool JointStateBroadcaster::init_joint_data()
     name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue;
   }
 
+  const std::string & urdf = get_robot_description();
+
+  urdf::Model model;
+  const bool is_model_loaded = !urdf.empty() && model.initString(urdf);
+  if (!is_model_loaded)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'",
+      HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT);
+  }
   // filter state interfaces that have at least one of the joint_states fields,
   // the rest will be ignored for this message
   for (const auto & name_ifv : name_if_value_mapping_)
   {
     const auto & interfaces_and_values = name_ifv.second;
     if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}))
     {
-      joint_names_.push_back(name_ifv.first);
+      if (
+        !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded ||
+        model.getJoint(name_ifv.first))
+      {
+        joint_names_.push_back(name_ifv.first);
+      }
     }
   }
 

diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml
@@ -39,3 +39,10 @@ joint_state_broadcaster:
       type: string,
       default_value: "effort",
     }
+  use_urdf_to_filter: {
+    type: bool,
+    default_value: true,
+    description: "Uses the robot_description to filter the ``joint_states`` topic.
+    If true, the broadcaster will publish the data of the joints present in the URDF alone.
+    If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``."
+  }