Merge branch 'master' into fix_set_hold_position

.github/workflows/ci-ros-lint.yml

@@ -32,6 +32,7 @@ jobs:
           joint_state_broadcaster
           joint_trajectory_controller
           position_controllers
+          range_sensor_broadcaster
           ros2_controllers
           ros2_controllers_test_nodes
           rqt_joint_trajectory_controller
@@ -69,6 +70,7 @@ jobs:
           joint_state_broadcaster
           joint_trajectory_controller
           position_controllers
+          range_sensor_broadcaster
           ros2_controllers
           ros2_controllers_test_nodes
           rqt_joint_trajectory_controller

doc/controllers_index.rst

@@ -63,6 +63,7 @@ Available Broadcasters
 .. toctree::
    :titlesonly:
 
-   Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst>
-   Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst>
    Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst>
+   Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst>
+   Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst>
+   Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst>

gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

@@ -126,7 +126,7 @@ class GripperActionController : public controller_interface::ControllerInterface
   bool verbose_ = false;  ///< Hard coded verbose flag to help in debugging
   std::string name_;      ///< Controller name.
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    joint_position_command_interface_;
+    joint_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
     joint_position_state_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -226,20 +226,21 @@ template <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_activate(
   const rclcpp_lifecycle::State &)
 {
-  auto position_command_interface_it = std::find_if(
+  auto command_interface_it = std::find_if(
     command_interfaces_.begin(), command_interfaces_.end(),
     [](const hardware_interface::LoanedCommandInterface & command_interface)
-    { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });
-  if (position_command_interface_it == command_interfaces_.end())
+    { return command_interface.get_interface_name() == HardwareInterface; });
+  if (command_interface_it == command_interfaces_.end())
   {
-    RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface");
+    RCLCPP_ERROR_STREAM(
+      get_node()->get_logger(), "Expected 1 " << HardwareInterface << " command interface");
     return controller_interface::CallbackReturn::ERROR;
   }
-  if (position_command_interface_it->get_prefix_name() != params_.joint)
+  if (command_interface_it->get_prefix_name() != params_.joint)
   {
     RCLCPP_ERROR_STREAM(
-      get_node()->get_logger(), "Position command interface is different than joint name `"
-                                  << position_command_interface_it->get_prefix_name() << "` != `"
+      get_node()->get_logger(), "Command interface is different than joint name `"
+                                  << command_interface_it->get_prefix_name() << "` != `"
                                   << params_.joint << "`");
     return controller_interface::CallbackReturn::ERROR;
   }
@@ -278,12 +279,12 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  joint_position_command_interface_ = *position_command_interface_it;
+  joint_command_interface_ = *command_interface_it;
   joint_position_state_interface_ = *position_state_interface_it;
   joint_velocity_state_interface_ = *velocity_state_interface_it;
 
   // Hardware interface adapter
-  hw_iface_adapter_.init(joint_position_command_interface_, get_node());
+  hw_iface_adapter_.init(joint_command_interface_, get_node());
 
   // Command - non RT version
   command_struct_.position_ = joint_position_state_interface_->get().get_value();
@@ -311,7 +312,7 @@ template <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_deactivate(
   const rclcpp_lifecycle::State &)
 {
-  joint_position_command_interface_ = std::nullopt;
+  joint_command_interface_ = std::nullopt;
   joint_position_state_interface_ = std::nullopt;
   joint_velocity_state_interface_ = std::nullopt;
   release_interfaces();
@@ -324,7 +325,7 @@ GripperActionController<HardwareInterface>::command_interface_configuration() co
 {
   return {
     controller_interface::interface_configuration_type::INDIVIDUAL,
-    {params_.joint + "/" + hardware_interface::HW_IF_POSITION}};
+    {params_.joint + "/" + HardwareInterface}};
 }
 
 template <const char * HardwareInterface>

gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

@@ -131,7 +131,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
   {
     joint_handle_ = joint_handle;
     // Init PID gains from ROS parameter server
-    const std::string prefix = "gains." + joint_handle_->get().get_name();
+    const std::string prefix = "gains." + joint_handle_->get().get_prefix_name();
     const auto k_p = auto_declare<double>(node, prefix + ".p", 0.0);
     const auto k_i = auto_declare<double>(node, prefix + ".i", 0.0);
     const auto k_d = auto_declare<double>(node, prefix + ".d", 0.0);

gripper_controllers/test/test_gripper_controllers.cpp

@@ -32,123 +32,142 @@ using hardware_interface::LoanedStateInterface;
 using GripperCommandAction = control_msgs::action::GripperCommand;
 using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
 
-void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
+template <typename T>
+void GripperControllerTest<T>::SetUpTestCase()
+{
+  rclcpp::init(0, nullptr);
+}
 
-void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); }
+template <typename T>
+void GripperControllerTest<T>::TearDownTestCase()
+{
+  rclcpp::shutdown();
+}
 
-void GripperControllerTest::SetUp()
+template <typename T>
+void GripperControllerTest<T>::SetUp()
 {
   // initialize controller
-  controller_ = std::make_unique<FriendGripperController>();
+  controller_ = std::make_unique<FriendGripperController<T::value>>();
 }
 
-void GripperControllerTest::TearDown() { controller_.reset(nullptr); }
+template <typename T>
+void GripperControllerTest<T>::TearDown()
+{
+  controller_.reset(nullptr);
+}
 
-void GripperControllerTest::SetUpController()
+template <typename T>
+void GripperControllerTest<T>::SetUpController()
 {
   const auto result = controller_->init("gripper_controller");
   ASSERT_EQ(result, controller_interface::return_type::OK);
 
   std::vector<LoanedCommandInterface> command_ifs;
-  command_ifs.emplace_back(joint_1_pos_cmd_);
+  command_ifs.emplace_back(this->joint_1_cmd_);
   std::vector<LoanedStateInterface> state_ifs;
-  state_ifs.emplace_back(joint_1_pos_state_);
-  state_ifs.emplace_back(joint_1_vel_state_);
+  state_ifs.emplace_back(this->joint_1_pos_state_);
+  state_ifs.emplace_back(this->joint_1_vel_state_);
   controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
 }
 
-TEST_F(GripperControllerTest, ParametersNotSet)
+using TestTypes = ::testing::Types<
+  std::integral_constant<const char *, HW_IF_POSITION>,
+  std::integral_constant<const char *, HW_IF_EFFORT>>;
+TYPED_TEST_SUITE(GripperControllerTest, TestTypes);
+
+TYPED_TEST(GripperControllerTest, ParametersNotSet)
 {
-  SetUpController();
+  this->SetUpController();
 
   // configure failed, 'joints' parameter not set
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::ERROR);
 }
 
-TEST_F(GripperControllerTest, JointParameterIsEmpty)
+TYPED_TEST(GripperControllerTest, JointParameterIsEmpty)
 {
-  SetUpController();
+  this->SetUpController();
 
-  controller_->get_node()->set_parameter({"joint", ""});
+  this->controller_->get_node()->set_parameter({"joint", ""});
 
   // configure failed, 'joints' is empty
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::ERROR);
 }
 
-TEST_F(GripperControllerTest, ConfigureParamsSuccess)
+TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess)
 {
-  SetUpController();
+  this->SetUpController();
 
-  controller_->get_node()->set_parameter({"joint", "joint_1"});
+  this->controller_->get_node()->set_parameter({"joint", "joint_1"});
 
   // configure successful
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
 }
 
-TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails)
+TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails)
 {
-  SetUpController();
+  this->SetUpController();
 
-  controller_->get_node()->set_parameter({"joint", "unicorn_joint"});
+  this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"});
 
   // activate failed, 'joint4' is not a valid joint name for the hardware
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
   ASSERT_EQ(
-    controller_->on_activate(rclcpp_lifecycle::State()),
+    this->controller_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::ERROR);
 }
 
-TEST_F(GripperControllerTest, ActivateSuccess)
+TYPED_TEST(GripperControllerTest, ActivateSuccess)
 {
-  SetUpController();
+  this->SetUpController();
 
-  controller_->get_node()->set_parameter({"joint", "joint1"});
+  this->controller_->get_node()->set_parameter({"joint", "joint1"});
 
   // activate successful
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
   ASSERT_EQ(
-    controller_->on_activate(rclcpp_lifecycle::State()),
+    this->controller_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
 }
 
-TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess)
+TYPED_TEST(GripperControllerTest, ActivateDeactivateActivateSuccess)
 {
-  SetUpController();
+  this->SetUpController();
 
-  controller_->get_node()->set_parameter({"joint", "joint1"});
+  this->controller_->get_node()->set_parameter({"joint", "joint1"});
 
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
   ASSERT_EQ(
-    controller_->on_activate(rclcpp_lifecycle::State()),
+    this->controller_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
   ASSERT_EQ(
-    controller_->on_deactivate(rclcpp_lifecycle::State()),
+    this->controller_->on_deactivate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
 
   // re-assign interfaces
   std::vector<LoanedCommandInterface> command_ifs;
-  command_ifs.emplace_back(joint_1_pos_cmd_);
+  command_ifs.emplace_back(this->joint_1_cmd_);
   std::vector<LoanedStateInterface> state_ifs;
-  state_ifs.emplace_back(joint_1_pos_state_);
-  state_ifs.emplace_back(joint_1_vel_state_);
-  controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
+  state_ifs.emplace_back(this->joint_1_pos_state_);
+  state_ifs.emplace_back(this->joint_1_vel_state_);
+  this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
 
   ASSERT_EQ(
-    controller_->on_configure(rclcpp_lifecycle::State()),
+    this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
   ASSERT_EQ(
-    controller_->on_activate(rclcpp_lifecycle::State()),
+    this->controller_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
 }

gripper_controllers/test/test_gripper_controllers.hpp

@@ -26,20 +26,22 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
 using hardware_interface::CommandInterface;
+using hardware_interface::HW_IF_EFFORT;
 using hardware_interface::HW_IF_POSITION;
 using hardware_interface::HW_IF_VELOCITY;
 using hardware_interface::StateInterface;
 
 namespace
 {
-
 // subclassing and friending so we can access member variables
+template <const char * HardwareInterface>
 class FriendGripperController
-: public gripper_action_controller::GripperActionController<HW_IF_POSITION>
+: public gripper_action_controller::GripperActionController<HardwareInterface>
 {
   FRIEND_TEST(GripperControllerTest, CommandSuccessTest);
 };
 
+template <typename T>
 class GripperControllerTest : public ::testing::Test
 {
 public:
@@ -53,7 +55,7 @@ class GripperControllerTest : public ::testing::Test
   void SetUpHandles();
 
 protected:
-  std::unique_ptr<FriendGripperController> controller_;
+  std::unique_ptr<FriendGripperController<T::value>> controller_;
 
   // dummy joint state values used for tests
   const std::string joint_name_ = "joint1";
@@ -62,7 +64,7 @@ class GripperControllerTest : public ::testing::Test
 
   StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]};
   StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]};
-  CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]};
+  CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]};
 };
 
 }  // anonymous namespace

joint_trajectory_controller/CMakeLists.txt

@@ -60,8 +60,7 @@ if(BUILD_TESTING)
   target_link_libraries(test_trajectory joint_trajectory_controller)
 
   ament_add_gmock(test_trajectory_controller
-    test/test_trajectory_controller.cpp
-    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
+    test/test_trajectory_controller.cpp)
   set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
   target_link_libraries(test_trajectory_controller
     joint_trajectory_controller

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -953,7 +953,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     last_commanded_state_ = state;
   }
 
-  // Should the controller start by holding position after on_configure?
+  // Should the controller start by holding position at the beginning of active state?
   if (params_.start_with_holding)
   {
     add_new_trajectory_msg(set_hold_position());