fix: start torque controller in perform_command_mode_switch

CHANGELOG.md

@@ -1,5 +1,11 @@
 # Changelog
 
+## 0.1.3 - 2023-08-24
+
+Requires libfranka >= 0.11.0, required ROS 2 Humble
+
+* franka\_hardware: hotfix start controller when user claims the command interface
+
 ## 0.1.2 - 2023-08-21
 
 Requires libfranka >= 0.11.0, required ROS 2 Humble

franka_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_bringup</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_description/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_description</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_example_controllers/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_example_controllers</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_gripper/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_gripper</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_hardware/include/franka_hardware/franka_hardware_interface.hpp

@@ -71,6 +71,8 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface {
   Model* hw_franka_model_ptr_ = nullptr;
 
   bool effort_interface_claimed_ = false;
+  bool effort_interface_running_ = false;
+
   static rclcpp::Logger getLogger();
 
   const std::string k_robot_name{"panda"};

franka_hardware/include/franka_hardware/robot.hpp

@@ -60,11 +60,11 @@ class Robot {
   /// Starts a read / write communication with the connected robot
   virtual void initializeReadWriteInterface();
 
-  /// stops the read / write communication with the connected robot
+  /// stops the read continous communication with the connected robot
   virtual void stopRobot();
 
   /**
-   * Get the current robot state in a thread-safe way.
+   * Get the current robot state
    * @return current robot state.
    */
   virtual franka::RobotState readOnce();
@@ -192,13 +192,22 @@ class Robot {
   Robot() = default;
 
  private:
+  /**
+   * Get the current robot state, when the controller is active
+   * @return current robot state.
+   */
+  virtual franka::RobotState readOnceActiveControl();
+
   std::mutex write_mutex_;
+  std::mutex control_mutex_;
 
   std::unique_ptr<franka::Robot> robot_;
   std::unique_ptr<franka::ActiveControl> active_control_;
   std::unique_ptr<franka::Model> model_;
   std::unique_ptr<Model> franka_hardware_model_;
 
   std::array<double, 7> last_desired_torque_ = {0, 0, 0, 0, 0, 0, 0};
+  bool control_loop_active_{false};
+  franka::RobotState current_state_;
 };
 }  // namespace franka_hardware

franka_hardware/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_hardware</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_hardware/src/franka_hardware_interface.cpp

@@ -69,7 +69,6 @@ std::vector<CommandInterface> FrankaHardwareInterface::export_command_interfaces
 
 CallbackReturn FrankaHardwareInterface::on_activate(
     const rclcpp_lifecycle::State& /*previous_state*/) {
-  robot_->initializeReadWriteInterface();
   hw_commands_.fill(0);
   read(rclcpp::Time(0),
        rclcpp::Duration(0, 0));  // makes sure that the robot state is properly initialized.
@@ -105,8 +104,13 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim
                   [](double hw_command) { return !std::isfinite(hw_command); })) {
     return hardware_interface::return_type::ERROR;
   }
-
-  robot_->writeOnce(hw_commands_);
+  if (effort_interface_running_) {
+    robot_->writeOnce(hw_commands_);
+  } else {
+    RCLCPP_FATAL(getLogger(),
+                 "Effort interface is not running. Did you claim the command interface?");
+    return hardware_interface::return_type::ERROR;
+  }
   return hardware_interface::return_type::OK;
 }
 
@@ -185,6 +189,14 @@ rclcpp::Logger FrankaHardwareInterface::getLogger() {
 hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_switch(
     const std::vector<std::string>& /*start_interfaces*/,
     const std::vector<std::string>& /*stop_interfaces*/) {
+  if (!effort_interface_running_ && effort_interface_claimed_) {
+    robot_->stopRobot();
+    robot_->initializeReadWriteInterface();
+    effort_interface_running_ = true;
+  } else if (effort_interface_running_ && !effort_interface_claimed_) {
+    robot_->stopRobot();
+    effort_interface_running_ = false;
+  }
   return hardware_interface::return_type::OK;
 }
 

franka_hardware/src/robot.cpp

@@ -39,7 +39,29 @@ Robot::Robot(const std::string& robot_ip, const rclcpp::Logger& logger) {
   franka_hardware_model_ = std::make_unique<Model>(model_.get());
 }
 
+Robot::~Robot() {
+  stopRobot();
+}
+
+franka::RobotState Robot::readOnce() {
+  std::lock_guard<std::mutex> lock(control_mutex_);
+  if (!control_loop_active_) {
+    return robot_->readOnce();
+  } else {
+    return readOnceActiveControl();
+  }
+}
+
+void Robot::stopRobot() {
+  if (control_loop_active_) {
+    control_loop_active_ = false;
+    active_control_.reset();
+  }
+}
+
 void Robot::writeOnce(const std::array<double, 7>& efforts) {
+  std::lock_guard<std::mutex> lock(control_mutex_);
+
   auto torque_command = franka::Torques(efforts);
   torque_command.tau_J =
       franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, last_desired_torque_);
@@ -48,7 +70,8 @@ void Robot::writeOnce(const std::array<double, 7>& efforts) {
   active_control_->writeOnce(torque_command);
 }
 
-franka::RobotState Robot::readOnce() {
+franka::RobotState Robot::readOnceActiveControl() {
+  // When controller is active use active control to read the robot state
   const auto [robot_state, _] = active_control_->readOnce();
   return robot_state;
 }
@@ -57,11 +80,8 @@ franka_hardware::Model* Robot::getModel() {
   return franka_hardware_model_.get();
 }
 
-void Robot::stopRobot() {
-  active_control_.reset();
-}
-
 void Robot::initializeReadWriteInterface() {
+  control_loop_active_ = true;
   active_control_ = robot_->startTorqueControl();
 }
 
@@ -168,8 +188,4 @@ void Robot::setFullCollisionBehavior(
       lower_force_thresholds_nominal, upper_force_thresholds_nominal);
 }
 
-Robot::~Robot() {
-  stopRobot();
-}
-
 }  // namespace franka_hardware

franka_hardware/test/franka_hardware_interface_test.cpp

@@ -186,6 +186,7 @@ TEST(
 
   EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
   EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state));
+
   franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
 
   const auto hardware_info = createHardwareInfo();
@@ -226,8 +227,8 @@ TEST(
 
   MockModel mock_model;
   MockModel* model_address = &mock_model;
-
   EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state));
+
   EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
   franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot);
 
@@ -258,7 +259,6 @@ TEST(
 
   MockModel mock_model;
   MockModel* model_address = &mock_model;
-
   EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state));
   EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
   franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot);
@@ -366,6 +366,20 @@ TEST(FrankaHardwareIntefaceTest, when_write_called_expect_ok) {
 
   const auto hardware_info = createHardwareInfo();
   franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> start_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    start_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  std::vector<std::string> stop_interface = {};
+
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+  // can call write only after switching to the torque controller
+  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
 
   const auto time = rclcpp::Time(0, 0);
   const auto duration = rclcpp::Duration(0, 0);
@@ -382,7 +396,6 @@ TEST(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) {
   auto mock_robot = std::make_shared<MockRobot>();
   EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state));
   EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
-  EXPECT_CALL(*mock_robot, initializeReadWriteInterface());
 
   franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot);
 
@@ -426,6 +439,21 @@ TEST(FrankaHardwareInterfaceTest,
             hardware_interface::return_type::OK);
 }
 
+TEST(FrankaHardwareInterfaceTest,
+     given_effort_interface_not_claimed_when_write_called_expect_error_response) {
+  auto mock_robot = std::make_shared<MockRobot>();
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot);
+
+  const auto time = rclcpp::Time(0, 0);
+  const auto duration = rclcpp::Duration(0, 0);
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  EXPECT_EQ(franka_hardware_interface.write(time, duration),
+            hardware_interface::return_type::ERROR);
+}
+
 TEST(FrankaHardwareInterfaceTest,
      given_that_effort_control_started_perform_command_mode_switch_stop_expect_ok) {
   auto mock_robot = std::make_shared<MockRobot>();

franka_moveit_config/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_moveit_config</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>Contains Moveit2 configuration files for Franka Emika research robots</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_msgs</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>franka_msgs provides messages and actions specific to Franka Emika research robots</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_robot_state_broadcaster/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>franka_robot_state_broadcaster</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>Broadcaster to publish robot states</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

integration_launch_testing/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>integration_launch_testing</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>Functional integration tests for franka controllers</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>