Merge pull request #49 in SRR/franka_ros2 from PRCUN-2201-align-msg-t…

…ypes-to-ros-types to humble

* commit '64a3878a1f924e21f9822803ca173a1d82c4fa2c':
  refactor: Moved more of the assign functionality into functions
  refactor: Reverted to change to use twist -> now using Eigen::Vector3
  test: Added a unit test for the frame setting
  refactor: Replaced the Cartesian velocity commands with twist
  refactor: Adapted the franka robot state to publish with ROS 2 types

.devcontainer/Dockerfile

@@ -32,6 +32,8 @@ RUN apt-get update && \
     ros-humble-controller-manager \
     ros-humble-joint-state-publisher-gui \
     ros-humble-control-msgs \
+    ros-humble-geometry-msgs \
+    ros-humble-sensor-msgs \
     ros-humble-gazebo-ros \
     ros-humble-control-toolbox \
     ros-humble-ackermann-msgs \

.devcontainer/devcontainer.json

@@ -12,8 +12,13 @@
 				"ms-python.python",
 				"bungcip.better-toml",
 				"ms-vscode.cpptools",
-				"ms-vscode.cpptools-extension-pack"
+				"ms-vscode.cpptools-extension-pack",
+				"ms-vscode.cmake-tools",
+				"xaver.clang-format",
+				"josetr.cmake-language-support-vscode",
+				"twxs.cmake",
+				"cheshirekow.cmake-format"
 			]
 		}
 	}
-}
+}

CHANGELOG.md

@@ -1,5 +1,12 @@
 # Changelog
 
+## 0.1.10 - 2023-12-04
+
+Requires libfranka >= 0.13.0, required ROS 2 Humble
+
+* Adapted the franka robot state broadcaster to use ROS 2 message types
+* Adapted the Cartesian velocity command interface to use Eigen types
+
 ## 0.1.9 - 2023-12-04
 
 Requires libfranka >= 0.13.0, required ROS 2 Humble
@@ -95,4 +102,4 @@ Requires libfranka >= 0.10.0, required ROS 2 Humble
 
 ### Fixed
 
-* franka\_hardware Fix the mismatched joint state interface type logger error message.
+* franka\_hardware Fix the mismatched joint state interface type logger error message.

franka_example_controllers/src/cartesian_velocity_example_controller.cpp

@@ -54,9 +54,11 @@ controller_interface::return_type CartesianVelocityExampleController::update(
   double v_x = std::cos(k_angle_) * v;
   double v_z = -std::sin(k_angle_) * v;
 
-  std::array<double, 6> cartesian_velocity_command = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}};
+  Eigen::Vector3d cartesian_linear_velocity(v_x, 0.0, v_z);
+  Eigen::Vector3d cartesian_angular_velocity(0.0, 0.0, 0.0);
 
-  if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command)) {
+  if (franka_cartesian_velocity_->setCommand(cartesian_linear_velocity,
+                                             cartesian_angular_velocity)) {
     return controller_interface::return_type::OK;
   } else {
     RCLCPP_FATAL(get_node()->get_logger(),
@@ -110,4 +112,4 @@ controller_interface::CallbackReturn CartesianVelocityExampleController::on_deac
 #include "pluginlib/class_list_macros.hpp"
 // NOLINTNEXTLINE
 PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianVelocityExampleController,
-                       controller_interface::ControllerInterface)
+                       controller_interface::ControllerInterface)

franka_example_controllers/src/elbow_example_controller.cpp

@@ -53,11 +53,14 @@ controller_interface::return_type ElbowExampleController::update(
 
   double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
 
-  std::array<double, 6> cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+  Eigen::Vector3d cartesian_linear_velocity(0.0, 0.0, 0.0);
+  Eigen::Vector3d cartesian_angular_velocity(0.0, 0.0, 0.0);
+
   std::array<double, 2> elbow_command = {
       {initial_elbow_configuration_[0] + angle, initial_elbow_configuration_[1]}};
 
-  if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command, elbow_command)) {
+  if (franka_cartesian_velocity_->setCommand(cartesian_linear_velocity, cartesian_angular_velocity,
+                                             elbow_command)) {
     return controller_interface::return_type::OK;
   } else {
     RCLCPP_FATAL(get_node()->get_logger(),
@@ -114,4 +117,4 @@ controller_interface::CallbackReturn ElbowExampleController::on_deactivate(
 #include "pluginlib/class_list_macros.hpp"
 // NOLINTNEXTLINE
 PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController,
-                       controller_interface::ControllerInterface)
+                       controller_interface::ControllerInterface)

franka_msgs/CMakeLists.txt

@@ -4,11 +4,15 @@ project(franka_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "action/Grasp.action"
   "action/Homing.action"
   "action/Move.action"
+  "msg/CollisionIndicators.msg"
+  "msg/Elbow.msg"
   "msg/Errors.msg"
   "msg/GraspEpsilon.msg"
   "msg/FrankaRobotState.msg"
@@ -19,12 +23,12 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetJointStiffness.srv"
   "srv/SetStiffnessFrame.srv"
   "srv/SetLoad.srv"
-  DEPENDENCIES std_msgs builtin_interfaces
+  DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_package()
+ament_package()

franka_msgs/msg/CollisionIndicators.msg

@@ -0,0 +1,8 @@
+### Indicates which dimensions have an active contact/collision flag raised
+geometry_msgs/Vector3 is_cartesian_linear_collision
+geometry_msgs/Vector3 is_cartesian_angular_collision
+geometry_msgs/Vector3 is_cartesian_linear_contact
+geometry_msgs/Vector3 is_cartesian_angular_contact
+
+float64[7] is_joint_collision
+float64[7] is_joint_contact

franka_msgs/msg/Elbow.msg

@@ -0,0 +1,6 @@
+### The state of the elbow
+float64[2] position
+float64[2] desired_position
+float64[2] commanded_position
+float64[2] commanded_velocity
+float64[2] commanded_acceleration

franka_msgs/msg/FrankaRobotState.msg

@@ -1,45 +1,66 @@
+### Default parts of the message
 std_msgs/Header header
-float64[6] cartesian_collision
-float64[6] cartesian_contact
-float64[7] q
-float64[7] q_d
-float64[7] dq
-float64[7] dq_d
+
+### Indicates which dimensions have an active contact/collision flag raised
+CollisionIndicators collision_indicators
+
+### The state of the arm in joint space
+# The joint state consisting out of position (q), velocity (dq) and effort (tau_J)
+sensor_msgs/JointState measured_joint_state
+
+# The desired joint state consisting out of position (q_d), velocity (dq_d) and effort (tau_J_d)
+sensor_msgs/JointState desired_joint_state
+
+# The measured motor state of the joints consisting out of position (theta) and velocity (dtheta)
+sensor_msgs/JointState measured_joint_motor_state
+
+# The desired joint acceleration
 float64[7] ddq_d
-float64[7] theta
-float64[7] dtheta
-float64[7] tau_j
+# The derivative of the measured torque signal
 float64[7] dtau_j
-float64[7] tau_j_d
-float64[6] k_f_ext_hat_k
-float64[2] elbow
-float64[2] elbow_d
-float64[2] elbow_c
-float64[2] delbow_c
-float64[2] ddelbow_c
-float64[7] joint_collision
-float64[7] joint_contact
-float64[6] o_f_ext_hat_k
-float64[6] o_dp_ee_d
-float64[6] o_dp_ee_c
-float64[6] o_ddp_ee_c
-float64[7] tau_ext_hat_filtered
-float64 m_ee
-float64[3] f_x_cee
-float64[9] i_ee
-float64 m_load
-float64[3] f_x_cload
-float64[9] i_load
-float64 m_total
-float64[3] f_x_ctotal
-float64[9] i_total
-float64[16] o_t_ee
-float64[16] o_t_ee_d
-float64[16] o_t_ee_c
-float64[16] f_t_ee
-float64[16] ee_t_k
+# Filtered external torque. The JointState consists out of effort (tau_ext_hat_filtered)
+sensor_msgs/JointState tau_ext_hat_filtered
+
+### The state of the elbow
+Elbow elbow
+
+### The active wrenches acting on the stiffness frame expressed relative to
+# stiffness frame
+geometry_msgs/WrenchStamped k_f_ext_hat_k
+# base frame
+geometry_msgs/WrenchStamped o_f_ext_hat_k
+
+### The different inertias of the arm
+# The end-effector inertia
+geometry_msgs/InertiaStamped inertia_ee
+# The load inertia
+geometry_msgs/InertiaStamped inertia_load
+# The total (end-effector + load) inertia
+geometry_msgs/InertiaStamped inertia_total
+
+### The poses describing the transformations between different frames of the arm
+# Measured end-effector pose in base frame
+geometry_msgs/PoseStamped o_t_ee
+# Last desired end-effector pose of motion generation in base frame
+geometry_msgs/PoseStamped o_t_ee_d
+# Last commanded end-effector pose of motion generation in base frame
+geometry_msgs/PoseStamped o_t_ee_c
+# Flange to end-effector frame
+geometry_msgs/PoseStamped f_t_ee
+# End-effector to stiffness frame
+geometry_msgs/PoseStamped ee_t_k
+
+# Desired end effector twist in base frame
+geometry_msgs/TwistStamped o_dp_ee_d
+# Last commanded end effector twist in base frame
+geometry_msgs/TwistStamped o_dp_ee_c
+# Last commanded end effector acceleration in base frame
+geometry_msgs/AccelStamped o_ddp_ee_c
+
+### Additional information
 float64 time
 float64 control_command_success_rate
+
 uint8 ROBOT_MODE_OTHER=0
 uint8 ROBOT_MODE_IDLE=1
 uint8 ROBOT_MODE_MOVE=2
@@ -48,5 +69,6 @@ uint8 ROBOT_MODE_REFLEX=4
 uint8 ROBOT_MODE_USER_STOPPED=5
 uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6
 uint8 robot_mode
+
 Errors current_errors
-Errors last_motion_errors
+Errors last_motion_errors

franka_msgs/package.xml

@@ -17,10 +17,12 @@
   <depend>builtin_interfaces</depend>
   <depend>std_msgs</depend>
   <depend>action_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp

@@ -73,7 +73,7 @@ controller_interface::CallbackReturn FrankaRobotStateBroadcaster::on_configure(
     realtime_franka_state_publisher =
         std::make_shared<realtime_tools::RealtimePublisher<franka_msgs::msg::FrankaRobotState>>(
             franka_state_publisher);
-    ;
+    franka_robot_state->initialize_robot_state_msg(realtime_franka_state_publisher->msg_);
   } catch (const std::exception& e) {
     fprintf(stderr,
             "Exception thrown during publisher creation at configure stage with message : %s \n",

franka_semantic_components/CMakeLists.txt

@@ -12,7 +12,7 @@ endif()
 
 option(CHECK_TIDY "Adds clang-tidy tests" OFF)
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS Franka franka_hardware franka_msgs hardware_interface rclcpp_lifecycle)
+set(THIS_PACKAGE_INCLUDE_DEPENDS Franka franka_hardware franka_msgs geometry_msgs sensor_msgs hardware_interface rclcpp_lifecycle)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -28,11 +28,17 @@ add_library(franka_semantic_components SHARED
             src/franka_robot_model.cpp
             src/franka_cartesian_velocity_interface.cpp
             src/franka_cartesian_pose_interface.cpp
-            src/franka_semantic_component_interface.cpp)
+            src/franka_semantic_component_interface.cpp
+            src/translation_utils.cpp
+)
 target_include_directories(franka_semantic_components PRIVATE include ${EIGEN3_INCLUDE_DIRS})
-target_link_libraries(franka_semantic_components Franka::Franka)
+target_link_libraries(franka_semantic_components
+  Franka::Franka
+  Eigen3::Eigen)
 ament_target_dependencies(franka_semantic_components
                           Franka
+                          sensor_msgs
+                          geometry_msgs
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(TARGETS franka_semantic_components DESTINATION lib)
@@ -73,6 +79,8 @@ if(BUILD_TESTING)
     controller_interface
     hardware_interface
     franka_msgs
+    sensor_msgs
+    geometry_msgs
     Franka
   )
   ament_add_gmock(test_franka_robot_model test/franka_robot_model_test.cpp)
@@ -85,6 +93,8 @@ if(BUILD_TESTING)
     controller_interface
     hardware_interface
     franka_msgs
+    sensor_msgs
+    geometry_msgs
     Franka
   )
 
@@ -101,6 +111,8 @@ if(BUILD_TESTING)
   ament_target_dependencies(
     franka_semantic_component_interface_test
     hardware_interface
+    sensor_msgs
+    geometry_msgs
   )
 
   ament_add_gmock(
@@ -133,6 +145,8 @@ if(BUILD_TESTING)
   ament_target_dependencies(
     franka_cartesian_pose_interface_test
     hardware_interface
+    sensor_msgs
+    geometry_msgs
   )
 endif()