From 981a23c68efb7644bf868f58f2bd035e9b6bdd86 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 24 Aug 2023 10:21:05 +0200 Subject: [PATCH] fix: start torque controller in perform_command_mode_switch --- CHANGELOG.md | 6 ++++ franka_bringup/package.xml | 2 +- franka_description/package.xml | 2 +- franka_example_controllers/package.xml | 2 +- franka_gripper/package.xml | 2 +- .../franka_hardware_interface.hpp | 2 ++ .../include/franka_hardware/robot.hpp | 13 +++++-- franka_hardware/package.xml | 2 +- .../src/franka_hardware_interface.cpp | 18 ++++++++-- franka_hardware/src/robot.cpp | 34 ++++++++++++++----- .../test/franka_hardware_interface_test.cpp | 34 +++++++++++++++++-- franka_moveit_config/package.xml | 2 +- franka_msgs/package.xml | 2 +- franka_robot_state_broadcaster/package.xml | 2 +- integration_launch_testing/package.xml | 2 +- 15 files changed, 99 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d3a02110..2c0888fe 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index 65b9b580..377f9868 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.2 + 0.1.3 Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_description/package.xml b/franka_description/package.xml index be61b839..33bcd887 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.2 + 0.1.3 franka_description contains URDF files and meshes of Franka Emika robots Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 4cea0703..583f7c7a 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.2 + 0.1.3 franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index 1fe11681..90314a80 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.2 + 0.1.3 This package implements the franka gripper of type Franka Hand for the use in ROS2 Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 0c4a6114..9d6d3cd0 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/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"}; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index c9e51bc3..861e2bb3 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/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,7 +192,14 @@ 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 robot_; std::unique_ptr active_control_; @@ -200,5 +207,7 @@ class Robot { std::unique_ptr franka_hardware_model_; std::array last_desired_torque_ = {0, 0, 0, 0, 0, 0, 0}; + bool control_loop_active_{false}; + franka::RobotState current_state_; }; } // namespace franka_hardware diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index 4d7f6fc8..f5f70894 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.2 + 0.1.3 franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index ecccef19..58de2549 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -69,7 +69,6 @@ std::vector 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& /*start_interfaces*/, const std::vector& /*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; } diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index b46ca910..f580c439 100644 --- a/franka_hardware/src/robot.cpp +++ b/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_.get()); } +Robot::~Robot() { + stopRobot(); +} + +franka::RobotState Robot::readOnce() { + std::lock_guard 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& efforts) { + std::lock_guard 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& 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 diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 925422d0..fbe6c05e 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/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 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 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(); 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(); + + 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(); diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index c375b543..d25a560a 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.2 + 0.1.3 Contains Moveit2 configuration files for Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index a6b86470..0ed7ff6d 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.2 + 0.1.3 franka_msgs provides messages and actions specific to Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_robot_state_broadcaster/package.xml b/franka_robot_state_broadcaster/package.xml index 9f87ac9c..08244125 100644 --- a/franka_robot_state_broadcaster/package.xml +++ b/franka_robot_state_broadcaster/package.xml @@ -1,7 +1,7 @@ franka_robot_state_broadcaster - 0.1.2 + 0.1.3 Broadcaster to publish robot states Franka Emika GmbH Apache 2.0 diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index 4eb8f7cb..f17790c5 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.2 + 0.1.3 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0