Use new joint handles in all controllers

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -32,9 +32,7 @@
 #include "diff_drive_controller/visibility_control.h"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
-#include "hardware_interface/joint_command_handle.hpp"
 #include "hardware_interface/operation_mode_handle.hpp"
-#include "hardware_interface/robot_hardware.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -44,6 +42,12 @@
 #include "sensor_msgs/msg/joint_state.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
+namespace hardware_interface
+{
+class JointHandle;
+class RobotHardware;
+}  // namespace hardware_interface
+
 namespace diff_drive_controller
 {
 class DiffDriveController : public controller_interface::ControllerInterface
@@ -96,8 +100,8 @@ class DiffDriveController : public controller_interface::ControllerInterface
 protected:
   struct WheelHandle
   {
-    const hardware_interface::JointStateHandle * state = nullptr;
-    hardware_interface::JointCommandHandle * command = nullptr;
+    std::shared_ptr<const hardware_interface::JointHandle> state = nullptr;
+    std::shared_ptr<hardware_interface::JointHandle> command = nullptr;
   };
 
   CallbackReturn configure_side(

diff_drive_controller/src/diff_drive_controller.cpp

@@ -23,6 +23,8 @@
 #include <vector>
 
 #include "diff_drive_controller/diff_drive_controller.hpp"
+#include "hardware_interface/joint_handle.hpp"
+#include "hardware_interface/robot_hardware.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "tf2/LinearMath/Quaternion.h"
 
@@ -147,8 +149,8 @@ controller_interface::return_type DiffDriveController::update()
     double left_position_mean = 0.0;
     double right_position_mean = 0.0;
     for (size_t index = 0; index < wheels.wheels_per_side; ++index) {
-      const double left_position = registered_left_wheel_handles_[index].state->get_position();
-      const double right_position = registered_right_wheel_handles_[index].state->get_position();
+      const double left_position = registered_left_wheel_handles_[index].state->get_value();
+      const double right_position = registered_right_wheel_handles_[index].state->get_value();
 
       if (std::isnan(left_position) || std::isnan(right_position)) {
         RCLCPP_ERROR(
@@ -243,8 +245,8 @@ controller_interface::return_type DiffDriveController::update()
 
   // Set wheels velocities:
   for (size_t index = 0; index < wheels.wheels_per_side; ++index) {
-    registered_left_wheel_handles_[index].command->set_cmd(velocity_left);
-    registered_right_wheel_handles_[index].command->set_cmd(velocity_right);
+    registered_left_wheel_handles_[index].command->set_value(velocity_left);
+    registered_right_wheel_handles_[index].command->set_value(velocity_right);
   }
 
   set_op_mode(hardware_interface::OperationMode::ACTIVE);
@@ -532,7 +534,7 @@ void DiffDriveController::halt()
 {
   const auto halt_wheels = [](auto & wheel_handles) {
       for (const auto & wheel_handle : wheel_handles) {
-        wheel_handle.command->set_cmd(0.0);
+        wheel_handle.command->set_value(0.0);
       }
     };
 
@@ -562,17 +564,27 @@ CallbackReturn DiffDriveController::configure_side(
     const auto wheel_name = wheel_names[index].c_str();
     auto & wheel_handle = registered_handles[index];
 
-    auto result = robot_hardware.get_joint_state_handle(wheel_name, &wheel_handle.state);
+    auto joint_state_handle = std::make_shared<hardware_interface::JointHandle>(
+      wheel_name,
+      "position");
+    auto joint_command_handle = std::make_shared<hardware_interface::JointHandle>(
+      wheel_name,
+      "velocity_command");
+
+    auto result = robot_hardware.get_joint_handle(*joint_state_handle);
     if (result != hardware_interface::return_type::OK) {
       RCLCPP_WARN(logger, "unable to obtain joint state handle for %s", wheel_name);
       return CallbackReturn::FAILURE;
     }
 
-    auto ret = robot_hardware.get_joint_command_handle(wheel_name, &wheel_handle.command);
-    if (ret != hardware_interface::return_type::OK) {
+    result = robot_hardware.get_joint_handle(*joint_command_handle);
+    if (result != hardware_interface::return_type::OK) {
       RCLCPP_WARN(logger, "unable to obtain joint command handle for %s", wheel_name);
       return CallbackReturn::FAILURE;
     }
+
+    wheel_handle.state = joint_state_handle;
+    wheel_handle.command = joint_command_handle;
   }
 
   return CallbackReturn::SUCCESS;

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -71,12 +71,27 @@ class TestDiffDriveController : public ::testing::Test
   {
     test_robot = std::make_shared<test_robot_hardware::TestRobotHardware>();
     test_robot->init();
-    left_wheel_names =
-    {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}};
-    right_wheel_names =
-    {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}};
+    left_wheel_names = test_robot->joint_names;
+    right_wheel_names = test_robot->joint_names;
     op_mode = {{test_robot->write_op_handle_name1}};
 
+    // create joint handles
+    auto get_handle = [&](const std::string & joint_name, const std::string & interface_name)
+      {
+        auto joint_handle = std::make_shared<hardware_interface::JointHandle>(
+          joint_name,
+          interface_name);
+        test_robot->get_joint_handle(*joint_handle);
+        return joint_handle;
+      };
+
+    joint1_pos_handle_ = get_handle("joint1", "position");
+    joint2_pos_handle_ = get_handle("joint2", "position");
+    joint3_pos_handle_ = get_handle("joint3", "position");
+    joint1_vel_cmd_handle_ = get_handle("joint1", "velocity_command");
+    joint2_vel_cmd_handle_ = get_handle("joint2", "velocity_command");
+    joint3_vel_cmd_handle_ = get_handle("joint3", "velocity_command");
+
     pub_node = std::make_shared<rclcpp::Node>("velocity_publisher");
     velocity_publisher = pub_node->create_publisher<geometry_msgs::msg::TwistStamped>(
       controller_name + "/cmd_vel",
@@ -145,10 +160,18 @@ class TestDiffDriveController : public ::testing::Test
   std::string controller_name = "test_diff_drive_controller";
 
   std::shared_ptr<test_robot_hardware::TestRobotHardware> test_robot;
+
   std::vector<std::string> left_wheel_names;
   std::vector<std::string> right_wheel_names;
   std::vector<std::string> op_mode;
 
+  std::shared_ptr<hardware_interface::JointHandle> joint1_pos_handle_;
+  std::shared_ptr<hardware_interface::JointHandle> joint2_pos_handle_;
+  std::shared_ptr<hardware_interface::JointHandle> joint3_pos_handle_;
+  std::shared_ptr<hardware_interface::JointHandle> joint1_vel_cmd_handle_;
+  std::shared_ptr<hardware_interface::JointHandle> joint2_vel_cmd_handle_;
+  std::shared_ptr<hardware_interface::JointHandle> joint3_vel_cmd_handle_;
+
   rclcpp::Node::SharedPtr pub_node;
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher;
 };
@@ -180,9 +203,9 @@ TEST_F(TestDiffDriveController, correct_initialization)
 
   auto inactive_state = diff_drive_controller->get_lifecycle_node()->configure();
   EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, inactive_state.id());
-  EXPECT_EQ(1.1, initialized_robot->pos1);
-  EXPECT_EQ(2.2, initialized_robot->pos2);
-  EXPECT_EQ(3.3, initialized_robot->pos3);
+  EXPECT_EQ(1.1, joint1_pos_handle_->get_value());
+  EXPECT_EQ(2.1, joint2_pos_handle_->get_value());
+  EXPECT_EQ(3.1, joint3_pos_handle_->get_value());
 }
 
 TEST_F(TestDiffDriveController, configuration)
@@ -250,8 +273,8 @@ TEST_F(TestDiffDriveController, cleanup)
   test_robot->write();
 
   // shouild be stopped
-  EXPECT_EQ(0, test_robot->cmd1);
-  EXPECT_EQ(0, test_robot->cmd2);
+  EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value());
+  EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value());
 
   executor.cancel();
 }
@@ -286,8 +309,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
 
   auto state = diff_drive_lifecycle_node->configure();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  EXPECT_EQ(1.1, test_robot->cmd1);
-  EXPECT_EQ(2.2, test_robot->cmd2);
+  EXPECT_EQ(1.2, joint1_vel_cmd_handle_->get_value());
+  EXPECT_EQ(2.2, joint2_vel_cmd_handle_->get_value());
 
   state = diff_drive_lifecycle_node->activate();
   ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
@@ -301,8 +324,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
 
   diff_drive_controller->update();
   test_robot->write();
-  EXPECT_EQ(1.0, test_robot->cmd1);
-  EXPECT_EQ(1.0, test_robot->cmd2);
+  EXPECT_EQ(1.0, joint1_vel_cmd_handle_->get_value());
+  EXPECT_EQ(1.0, joint2_vel_cmd_handle_->get_value());
 
   // deactivated
   // wait so controller process the second point when deactivated
@@ -312,15 +335,15 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
   diff_drive_controller->update();
   test_robot->write();
 
-  EXPECT_EQ(0., test_robot->cmd1) << "Wheels are halted on deactivate()";
-  EXPECT_EQ(0., test_robot->cmd2) << "Wheels are halted on deactivate()";
+  EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value()) << "Wheels are halted on deactivate()";
+  EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value()) << "Wheels are halted on deactivate()";
 
   // cleanup
   state = diff_drive_lifecycle_node->cleanup();
   test_robot->write();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
-  EXPECT_EQ(0, test_robot->cmd1);
-  EXPECT_EQ(0, test_robot->cmd2);
+  EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value());
+  EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value());
 
   state = diff_drive_lifecycle_node->configure();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());

joint_state_controller/include/joint_state_controller/joint_state_controller.hpp

@@ -16,6 +16,7 @@
 #define JOINT_STATE_CONTROLLER__JOINT_STATE_CONTROLLER_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "control_msgs/msg/dynamic_joint_state.hpp"
@@ -27,8 +28,9 @@
 
 namespace hardware_interface
 {
-class JointStateHandle;
+class JointHandle;
 }  // namespace hardware_interface
+
 namespace rclcpp_lifecycle
 {
 class State;
@@ -60,7 +62,12 @@ class JointStateController : public controller_interface::ControllerInterface
   on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
-  std::vector<const hardware_interface::JointStateHandle *> registered_joint_handles_;
+  bool init_joint_data();
+  void init_joint_state_msg();
+  void init_dynamic_joint_state_msg();
+
+protected:
+  std::vector<std::string> joint_names_;
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::JointState>>
   joint_state_publisher_;
@@ -69,6 +76,8 @@ class JointStateController : public controller_interface::ControllerInterface
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::DynamicJointState>>
   dynamic_joint_state_publisher_;
   control_msgs::msg::DynamicJointState dynamic_joint_state_msg_;
+
+  std::vector<std::vector<std::shared_ptr<hardware_interface::JointHandle>>> joint_handles_;
 };
 
 }  // namespace joint_state_controller