Merge pull request #38 in SRR/franka_ros2 from feat/non-realtime-para…

…m-setting to humble

* commit 'c7c4aa2467bd1b283793b3af7ab476f2b11d99bc':
  changelod update for v0.1.2
  feat: non-realtime parameter services

.clang-tidy

@@ -1,4 +1,23 @@
-Checks: 'cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,clang-diagnostic-*,clang-analyzer-*,-clang-analyzer-optin.cplusplus.VirtualCall,-clang-analyzer-alpha*,google-*,misc-*,readability-*,-readability-function-cognitive-complexity,modernize-*,-modernize-use-trailing-return-type,performance-*,-clang-diagnostic-deprecated-declarations,-modernize-pass-by-value,-clang-diagnostic-reinterpret-base-class,-clang-diagnostic-return-type,-clang-diagnostic-switch,-bugprone-lambda-function-name,-cppcoreguidelines-avoid-magic-numbers, -readability-magic-numbers'
+Checks: 'cppcoreguidelines-*,
+        -cppcoreguidelines-pro-type-vararg,
+        -cppcoreguidelines-pro-bounds-array-to-pointer-decay,
+        clang-diagnostic-*,
+        clang-analyzer-*,
+        -clang-analyzer-optin.cplusplus.VirtualCall,
+        -clang-analyzer-alpha*,
+        google-*,
+        misc-*,
+        -readability-*,
+        -readability-function-cognitive-complexity,modernize-*,
+        -modernize-use-trailing-return-type,performance-*,
+        -clang-diagnostic-deprecated-declarations,
+        -modernize-pass-by-value,
+        -clang-diagnostic-reinterpret-base-class,
+        -clang-diagnostic-return-type,
+        -clang-diagnostic-switch,
+        -bugprone-lambda-function-name,
+        -cppcoreguidelines-avoid-magic-numbers,
+        -readability-magic-numbers'
 HeaderFilterRegex: '^franka_.*'
 CheckOptions:
 # Classes, structs, ...

CHANGELOG.md

@@ -1,5 +1,12 @@
 # Changelog
 
+## 0.1.2 - 2023-08-21
+
+Requires libfranka >= 0.11.0, required ROS 2 Humble
+
+* franka\_hardware: implement non-realtime parameter services
+
+
 ## 0.1.1 - 2023-08-21
 
 Requires libfranka >= 0.11.0, required ROS 2 Humble
@@ -11,7 +18,7 @@ Requires libfranka >= 0.11.0, required ROS 2 Humble
 Requires libfranka >= 0.10.0, required ROS 2 Humble
 
 * franka\_bringup: franka_robot_state broadcaster added to franka.launch.py.
-* franka\_example\_controllers: model printing read onyl controller implemented
+* franka\_example\_controllers: model printing read only controller implemented
 * franka\_robot\_model: semantic component to access robot model parameters.
 * franka\_msgs: franka robot state msg added
 * franka\_robot\_state: broadcaster publishes robot state.

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.1</version>
+  <version>0.1.2</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.1</version>
+  <version>0.1.2</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.1</version>
+  <version>0.1.2</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.1</version>
+  <version>0.1.2</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/CMakeLists.txt

@@ -19,17 +19,22 @@ find_package(franka_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(Franka 0.11.0 REQUIRED)
-
+find_package(rclcpp_components REQUIRED)
 
 add_library(franka_hardware
         SHARED
         src/franka_hardware_interface.cpp
-        src/robot.cpp)
+        src/robot.cpp
+        src/franka_param_service_server.cpp
+        src/franka_executor.cpp
+)
+
 target_include_directories(
         franka_hardware
         PRIVATE
         include
 )
+
 ament_target_dependencies(
         franka_hardware
         hardware_interface
@@ -38,6 +43,7 @@ ament_target_dependencies(
         rclcpp
         franka_msgs
 )
+
 pluginlib_export_plugin_description_file(hardware_interface franka_hardware.xml)
 
 
@@ -50,7 +56,6 @@ install(
         DESTINATION include
 )
 
-
 if(BUILD_TESTING)
     find_package(ament_cmake_clang_format REQUIRED)
     find_package(ament_cmake_copyright REQUIRED)
@@ -81,6 +86,7 @@ endif()
 ament_export_include_directories(
         include
 )
+
 ament_export_libraries(
         franka_hardware
 )

franka_hardware/include/franka_hardware/franka_executor.hpp

@@ -0,0 +1,43 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace franka_hardware {
+
+class FrankaExecutor : public rclcpp::executors::MultiThreadedExecutor {
+ public:
+  // Create an instance and start the internal thread
+  FrankaExecutor();
+  FrankaExecutor(const FrankaExecutor&) = delete;
+  FrankaExecutor(FrankaExecutor&&) = delete;
+
+  FrankaExecutor& operator=(const FrankaExecutor&) = delete;
+  FrankaExecutor& operator=(FrankaExecutor&&) = delete;
+
+  // Stops the internal executor and joins with the internal thread
+  ~FrankaExecutor() override;
+
+ private:
+  std::thread executor_spin_;
+
+  // Executor thread starts spining the multithreadedExecutor
+  void run();
+
+  // Cancel any spinning ROS executor
+  void shutdown();
+};
+}  // namespace franka_hardware

franka_hardware/include/franka_hardware/franka_hardware_interface.hpp

@@ -19,8 +19,6 @@
 #include <vector>
 
 #include <hardware_interface/visibility_control.h>
-#include <franka_hardware/robot.hpp>
-
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
@@ -29,13 +27,17 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 
+#include "franka_hardware/franka_executor.hpp"
+#include "franka_hardware/franka_param_service_server.hpp"
+#include "franka_hardware/robot.hpp"
+
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 namespace franka_hardware {
 
 class FrankaHardwareInterface : public hardware_interface::SystemInterface {
  public:
-  explicit FrankaHardwareInterface(std::unique_ptr<Robot> robot);
+  explicit FrankaHardwareInterface(std::shared_ptr<Robot> robot);
   FrankaHardwareInterface() = default;
 
   hardware_interface::return_type prepare_command_mode_switch(
@@ -56,7 +58,10 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface {
   static const size_t kNumberOfJoints = 7;
 
  private:
-  std::unique_ptr<Robot> robot_;
+  std::shared_ptr<Robot> robot_;
+  std::shared_ptr<FrankaParamServiceServer> node_;
+  std::shared_ptr<FrankaExecutor> executor_;
+
   std::array<double, kNumberOfJoints> hw_commands_{0, 0, 0, 0, 0, 0, 0};
   std::array<double, kNumberOfJoints> hw_positions_{0, 0, 0, 0, 0, 0, 0};
   std::array<double, kNumberOfJoints> hw_velocities_{0, 0, 0, 0, 0, 0, 0};

franka_hardware/include/franka_hardware/franka_param_service_server.hpp

@@ -0,0 +1,151 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+
+#include "franka/exception.h"
+#include "franka_hardware/robot.hpp"
+
+#include <franka_msgs/srv/set_cartesian_stiffness.hpp>
+#include <franka_msgs/srv/set_force_torque_collision_behavior.hpp>
+#include <franka_msgs/srv/set_full_collision_behavior.hpp>
+#include <franka_msgs/srv/set_joint_stiffness.hpp>
+#include <franka_msgs/srv/set_load.hpp>
+#include <franka_msgs/srv/set_stiffness_frame.hpp>
+#include <franka_msgs/srv/set_tcp_frame.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+/**
+ * Node implementing the service server
+ */
+namespace franka_hardware {
+
+class FrankaParamServiceServer : public rclcpp::Node {
+ public:
+  FrankaParamServiceServer(const rclcpp::NodeOptions& options, std::shared_ptr<Robot> robot);
+
+ private:
+  /**
+   * @brief generic templated param setter function
+   *
+   * @param param_setter_function: std::function<void(request_type)> makes the call to the robot
+   * class which take the request type
+   * @param request franka_msgs::srv set parameter request type
+   * @param response franka_msgs::srv set parameter response type
+   */
+  template <typename request_type, typename response_type>
+  void setGenericRobotParam(const std::function<void(request_type)>& param_setter_function,
+                            const request_type& request,
+                            const response_type& response) {
+    try {
+      param_setter_function(request);
+      response->success = true;
+    } catch (const franka::CommandException& command_exception) {
+      RCLCPP_ERROR(this->get_logger(), "Command exception thrown during parameter setting %s",
+                   command_exception.what());
+      response->success = false;
+      response->error = "command exception error";
+    } catch (const franka::NetworkException& network_exception) {
+      RCLCPP_ERROR(this->get_logger(), "Network exception thrown during parameter setting %s",
+                   network_exception.what());
+      response->success = false;
+      response->error = "network exception error";
+    }
+  }
+
+  /**
+   * @brief Callback function for set_joint_stiffness service
+   *
+   * @param request shared_ptr to setJointStiffness service msgs request
+   * @param response shared_ptr to setJointStiffness service msgs response
+   */
+  void setJointStiffnessCallback(
+      const franka_msgs::srv::SetJointStiffness::Request::SharedPtr& request,
+      const franka_msgs::srv::SetJointStiffness::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_cartesian_stiffness service
+   *
+   * @param request shared_ptr to SetCartesianStiffness service msgs request
+   * @param response shared_ptr to SetCartesianStiffness service msgs response
+   */
+  void setCartesianStiffnessCallback(
+      const franka_msgs::srv::SetCartesianStiffness::Request::SharedPtr& request,
+      const franka_msgs::srv::SetCartesianStiffness::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_cartesian_stiffness service
+   *
+   * @param request shared_ptr to SetCartesianStiffness service msgs request
+   * @param response shared_ptr to SetCartesianStiffness service msgs response
+   */
+  void setTCPFrameCallback(const franka_msgs::srv::SetTCPFrame::Request::SharedPtr& request,
+                           const franka_msgs::srv::SetTCPFrame::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_stiffness_frame service
+   *
+   * @param request shared_ptr to SetStiffnessFrame service msgs request
+   * @param response shared_ptr to SetStiffnessFrame service msgs response
+   */
+  void setStiffnessFrameCallback(
+      const franka_msgs::srv::SetStiffnessFrame::Request::SharedPtr& request,
+      const franka_msgs::srv::SetStiffnessFrame::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_force_torque_collision_behavior service
+   *
+   * @param request shared_ptr to SetForceTorqueCollisionBehavior service msgs request
+   * @param response shared_ptr to SetForceTorqueCollisionBehavior service msgs response
+   */
+  void setForceTorqueCollisionBehaviorCallback(
+      const franka_msgs::srv::SetForceTorqueCollisionBehavior::Request::SharedPtr& request,
+      const franka_msgs::srv::SetForceTorqueCollisionBehavior::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_full_collision_behavior service
+   *
+   * @param request shared_ptr to SetFullCollisionBehavior service msgs request
+   * @param response shared_ptr to SetFullCollisionBehavior service msgs response
+   */
+  void setFullCollisionBehaviorCallback(
+      const franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr& request,
+      const franka_msgs::srv::SetFullCollisionBehavior::Response::SharedPtr& response);
+
+  /**
+   * @brief Callback function for set_load_callback service
+   *
+   * @param request shared_ptr to SetLoad service msgs request
+   * @param response shared_ptr to SetLoad service msgs response
+   */
+  void setLoadCallback(const franka_msgs::srv::SetLoad::Request::SharedPtr& request,
+                       const franka_msgs::srv::SetLoad::Response::SharedPtr& response);
+
+  std::shared_ptr<Robot> robot_;
+
+  rclcpp::Service<franka_msgs::srv::SetJointStiffness>::SharedPtr set_joint_stiffness_service_;
+  rclcpp::Service<franka_msgs::srv::SetCartesianStiffness>::SharedPtr
+      set_cartesian_stiffness_service_;
+  rclcpp::Service<franka_msgs::srv::SetLoad>::SharedPtr set_load_service_;
+  rclcpp::Service<franka_msgs::srv::SetTCPFrame>::SharedPtr set_tcp_frame_service_;
+  rclcpp::Service<franka_msgs::srv::SetStiffnessFrame>::SharedPtr set_stiffness_frame_service_;
+  rclcpp::Service<franka_msgs::srv::SetForceTorqueCollisionBehavior>::SharedPtr
+      set_force_torque_collision_behavior_service_;
+  rclcpp::Service<franka_msgs::srv::SetFullCollisionBehavior>::SharedPtr
+      set_full_collision_behavior_service_;
+};
+}  // namespace franka_hardware