Merge pull request #47 in SRR/franka_ros2 from tests/robot-class to h…

…umble

* commit '9f58e2a3966e70f49c7eac8c4901db97314547a0':
  bump up version 0.1.8
  hotfix: include patched JTC back
  tests: implement franka hardware robot class tests

.devcontainer/Dockerfile

@@ -43,7 +43,6 @@ RUN apt-get update && \
     ros-humble-joint-state-broadcaster \
     ros-humble-ament-cmake-clang-tidy \
     libignition-gazebo6-dev \
-    ros-humble-joint-trajectory-controller \
     libpoco-dev \
     libeigen3-dev \
     && rm -rf /var/lib/apt/lists/*

.devcontainer/devcontainer.json

@@ -13,15 +13,7 @@
 				"bungcip.better-toml",
 				"ms-vscode.cpptools",
 				"ms-vscode.cpptools-extension-pack"
-			],
-			"settings": {
-				"terminal.integrated.defaultProfile.linux": "bash",
-				"terminal.integrated.profiles.linux": {
-					"bash": {
-						"path": "/bin/bash"
-					}
-				}
-			}
+			]
 		}
 	}
 }

CHANGELOG.md

@@ -1,10 +1,17 @@
 # Changelog
 
+## 0.1.8 - 2023-11-16
+
+Requires libfranka >= 0.13.0, required ROS 2 Humble
+
+* franka\_hardware: add unit tests for robot class.
+* joint\_trajectory\_controller: hotfix add joint patched old JTC back.
+
 ## 0.1.7 - 2023-11-10
 
 Requires libfranka >= 0.12.1, required ROS 2 Humble
 
-* franka\_hardware: joint position command inteface supported
+* franka\_hardware: joint position command interface supported
 * franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode
 * franka\_example\_controllers: joint position example controller provided
 * franka\_example\_controllers: fix second start bug with the example controllers

Dockerfile

@@ -59,7 +59,7 @@ RUN python3 -m pip install -U \
 RUN mkdir ~/source_code    
 RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
     && cd libfranka \
-    && git checkout 0.12.1 \
+    && git checkout 0.13.0 \
     && git submodule init \
     && git submodule update \
     && mkdir build && cd build \

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.7</version>
+  <version>0.1.8</version>
   <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">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.7</version>
+  <version>0.1.8</version>
   <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
   <maintainer email="support@franka.de">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.7</version>
+  <version>0.1.8</version>
   <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_gripper/CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 
-find_package(Franka REQUIRED)
+find_package(Franka 0.13.0 REQUIRED)
 
 add_library(gripper_server SHARED
         src/gripper_action_server.cpp)

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.7</version>
+  <version>0.1.8</version>
   <description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_hardware/CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rclcpp REQUIRED)
 find_package(franka_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(Franka 0.12.1 REQUIRED)
+find_package(Franka 0.13.0 REQUIRED)
 find_package(rclcpp_components REQUIRED)
 
 add_library(franka_hardware

franka_hardware/include/franka_hardware/robot.hpp

@@ -23,6 +23,7 @@
 #include <thread>
 
 #include <franka/active_control.h>
+#include <franka/active_control_base.h>
 #include <franka/active_motion_generator.h>
 #include <franka/active_torque_control.h>
 
@@ -44,6 +45,13 @@ namespace franka_hardware {
 
 class Robot {
  public:
+  /**
+   * @brief Construct a new Robot object for tests purposes
+   *
+   * @param robot mocked libfranka robot
+   * @param model mocked model object
+   */
+  explicit Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model);
   /**
    * Connects to the robot. This method can block for up to one minute if the robot is not
    * responding. An exception will be thrown if the connection cannot be established.
@@ -262,17 +270,16 @@ class Robot {
   std::mutex control_mutex_;
 
   std::unique_ptr<franka::Robot> robot_;
-  std::unique_ptr<franka::ActiveControl> active_control_;
+  std::unique_ptr<franka::ActiveControlBase> active_control_ = nullptr;
   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 effort_interface_active_{false};
   bool joint_velocity_interface_active_{false};
   bool joint_position_interface_active_{false};
   bool cartesian_velocity_interface_active_{false};
 
+  bool torque_command_rate_limiter_active_{true};
   bool velocity_command_rate_limit_active_{false};
 
   bool cartesian_velocity_command_rate_limit_active_{false};

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.7</version>
+  <version>0.1.8</version>
   <description>franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_hardware/src/robot.cpp

@@ -24,6 +24,9 @@
 
 namespace franka_hardware {
 
+Robot::Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model)
+    : robot_(std::move(robot)), franka_hardware_model_(std::move(model)) {}
+
 Robot::Robot(const std::string& robot_ip, const rclcpp::Logger& logger) {
   franka::RealtimeConfig rt_config = franka::RealtimeConfig::kEnforce;
   if (!franka::hasRealtimeKernel()) {
@@ -65,6 +68,9 @@ void Robot::stopRobot() {
 }
 
 void Robot::writeOnce(const std::array<double, 7>& joint_commands) {
+  if (!active_control_) {
+    throw std::runtime_error("Control hasn't been started");
+  }
   if (effort_interface_active_) {
     writeOnceEfforts(joint_commands);
   } else if (joint_velocity_interface_active_) {
@@ -78,10 +84,10 @@ void Robot::writeOnceEfforts(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_);
-  last_desired_torque_ = torque_command.tau_J;
-
+  if (torque_command_rate_limiter_active_) {
+    torque_command.tau_J =
+        franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, current_state_.tau_J_d);
+  }
   active_control_->writeOnce(torque_command);
 }
 
@@ -162,6 +168,10 @@ void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_
 }
 
 void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
+  if (!active_control_) {
+    throw std::runtime_error("Control hasn't been started");
+  }
+
   std::lock_guard<std::mutex> lock(control_mutex_);
 
   auto velocity_command = franka::CartesianVelocities(cartesian_velocities);
@@ -172,6 +182,10 @@ void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
 
 void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities,
                       const std::array<double, 2>& elbow_command) {
+  if (!active_control_) {
+    throw std::runtime_error("Control hasn't been started");
+  }
+
   std::lock_guard<std::mutex> lock(control_mutex_);
 
   auto velocity_command = franka::CartesianVelocities(cartesian_velocities, elbow_command);

franka_hardware/test/CMakeLists.txt

@@ -10,3 +10,8 @@ ament_add_gmock(${PROJECT_NAME}_command_interface_test franka_hardware_cartesian
 target_include_directories(${PROJECT_NAME}_command_interface_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)
 
 target_link_libraries(${PROJECT_NAME}_command_interface_test  ${PROJECT_NAME})
+
+ament_add_gmock(${PROJECT_NAME}_robot_test franka_robot_test.cpp)
+target_include_directories(${PROJECT_NAME}_robot_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)
+
+target_link_libraries(${PROJECT_NAME}_robot_test  ${PROJECT_NAME})

franka_hardware/test/franka_robot_test.cpp

@@ -0,0 +1,149 @@
+#include "franka_robot_test.hpp"
+
+TEST_F(FrankaRobotTests, whenInitializeTorqueInterfaceCalled_thenStartTorqueControlCalled) {
+  EXPECT_CALL(*mock_libfranka_robot, startTorqueControl()).Times(1);
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  robot.initializeTorqueInterface();
+}
+
+TEST_F(FrankaRobotTests, whenInitializeJointVelocityInterfaceCalled_thenStartJointVelocityControl) {
+  EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_)).Times(1);
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  robot.initializeJointVelocityInterface();
+}
+
+TEST_F(FrankaRobotTests,
+       whenInitializeJointtPositionInterfaceCalled_thenStartJointPositionControl) {
+  EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_)).Times(1);
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  robot.initializeJointPositionInterface();
+}
+
+TEST_F(FrankaRobotTests,
+       whenInitializeCartesianVelocityInterfaceCalled_thenStartCartesianVelocityControl) {
+  EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_)).Times(1);
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  robot.initializeCartesianVelocityInterface();
+}
+
+TEST_F(FrankaRobotTests,
+       givenCartesianVelocityControlIsStarted_whenReadOnceIsCalled_expectCorrectRobotState) {
+  franka::RobotState robot_state;
+  franka::Duration duration;
+  robot_state.q_d = std::array<double, 7>{1, 2, 3, 1, 2, 3, 1};
+  auto active_control_read_return_tuple = std::make_pair(robot_state, duration);
+
+  EXPECT_CALL(*mock_active_control, readOnce())
+      .WillOnce(testing::Return(active_control_read_return_tuple));
+  EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_))
+      .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  robot.initializeCartesianVelocityInterface();
+  auto actual_state = robot.readOnce();
+  ASSERT_EQ(robot_state.q_d, actual_state.q_d);
+}
+
+TEST_F(FrankaRobotTests,
+       givenJointControlIsNotStarted_whenReadOnceIsCalled_expectCorrectRobotState) {
+  franka::RobotState robot_state;
+  robot_state.q_d = std::array<double, 7>{1, 2, 3, 1, 2, 3, 1};
+
+  EXPECT_CALL(*mock_libfranka_robot, readOnce()).WillOnce(testing::Return(robot_state));
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+  auto actual_state = robot.readOnce();
+
+  ASSERT_EQ(robot_state.q_d, actual_state.q_d);
+}
+
+TEST_F(
+    FrankaRobotTests,
+    givenCartesianVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
+  const std::array<double, 6>& cartesian_velocities{1, 0, 0, 0, 0, 0};
+  const franka::CartesianVelocities expected_cartesian_velocities(cartesian_velocities);
+
+  auto expectCallFunction = [this]() {
+    EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_))
+        .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
+  };
+
+  testWriteOnce<void (franka_hardware::Robot::*)(), franka::CartesianVelocities,
+                std::array<double, 6>>(
+      &franka_hardware::Robot::initializeCartesianVelocityInterface, expectCallFunction,
+      cartesian_velocities, expected_cartesian_velocities);
+}
+
+TEST_F(
+    FrankaRobotTests,
+    givenJointPositionControlIsControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
+  const std::array<double, 7>& joint_positions{1, 0, 0, 0, 0, 0, 0};
+  const franka::JointPositions expected_joint_positions(joint_positions);
+
+  auto expectCallFunction = [this]() {
+    EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_))
+        .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
+  };
+
+  testWriteOnce<void (franka_hardware::Robot::*)(), franka::JointPositions, std::array<double, 7>>(
+      &franka_hardware::Robot::initializeJointPositionInterface, expectCallFunction,
+      joint_positions, expected_joint_positions);
+}
+
+TEST_F(
+    FrankaRobotTests,
+    givenJointVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
+  const std::array<double, 7>& joint_velocities{1, 0, 0, 0, 0, 0, 0};
+  const franka::JointVelocities expected_joint_velocities(joint_velocities);
+
+  const auto expectCallFunction = [this]() {
+    EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_))
+        .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
+  };
+
+  testWriteOnce<void (franka_hardware::Robot::*)(), franka::JointVelocities, std::array<double, 7>>(
+      &franka_hardware::Robot::initializeJointVelocityInterface, expectCallFunction,
+      joint_velocities, expected_joint_velocities);
+}
+
+TEST_F(FrankaRobotTests,
+       givenEffortControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
+  const std::array<double, 7>& joint_torques{1, 0, 0, 0, 0, 0, 0};
+  // Torque rate limiter defaulted to active
+  const franka::Torques expected_joint_torques{0.999999, 0, 0, 0, 0, 0, 0};
+
+  auto expectCallFunction = [this]() {
+    EXPECT_CALL(*mock_libfranka_robot, startTorqueControl())
+        .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
+  };
+
+  testWriteOnce<void (franka_hardware::Robot::*)(), franka::Torques, std::array<double, 7>>(
+      &franka_hardware::Robot::initializeTorqueInterface, expectCallFunction, joint_torques,
+      expected_joint_torques);
+}
+
+TEST_F(FrankaRobotTests,
+       givenControlIsNotStart_whenWriteOnceIsCalled_expectRuntimeExceptionToBeThrown) {
+  const std::array<double, 7>& joint_torques{1, 0, 0, 0, 0, 0, 0};
+  const franka::Torques joint_torques_franka(joint_torques);
+
+  const std::array<double, 6>& cartesian_velocities{1, 0, 0, 0, 0, 0};
+  const franka::CartesianVelocities cartesian_franka(cartesian_velocities);
+
+  EXPECT_CALL(*mock_active_control, writeOnce(joint_torques_franka)).Times(0);
+  EXPECT_CALL(*mock_active_control, writeOnce(cartesian_franka)).Times(0);
+
+  franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
+
+  EXPECT_THROW(robot.writeOnce(joint_torques), std::runtime_error);
+  EXPECT_THROW(robot.writeOnce(cartesian_velocities), std::runtime_error);
+}