Merge pull request #54 in SRR/franka_ros2 from feat/visualize-rviz-fr…

…anka-state to humble

* commit 'e2bb29cc5f07a62d1f88940a6a51677513975268':
  bump version
  format: test robot description
  tests:  update state publisher tests with mocked franka robot state
  feat: visualize robot state messages

.devcontainer/docker-compose.yml

@@ -13,6 +13,7 @@ services:
       - /tmp/.X11-unix:/tmp/.X11-unix
       - $XAUTHORITY:$XAUTHORITY
       - ./limits.conf:/etc/security/limits.conf
+      - ~/.gitconfig:/etc/gitconfig:ro
     environment:
       QT_X11_NO_MITSHM: 1
       DISPLAY: $DISPLAY
@@ -22,4 +23,4 @@ services:
     ulimits:
       rtprio: 99
       rttime: -1
-      memlock: 8428281856
+      memlock: 8428281856

CHANGELOG.md

@@ -1,5 +1,12 @@
 # Changelog
 
+## 0.1.12 - 2024-01-12
+
+Requires libfranka >= 0.13.2, requires ROS 2 Humble
+
+* franka\_semantic\_component: Read robot state from urdf robot description.
+* franka\_state\_broadcaster: Publish visualizable topics seperately.
+
 ## 0.1.11 - 2023-12-20
 
 Requires libfranka >= 0.13.2, requires 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.11</version>
+  <version>0.1.12</version>
   <description>Package with launch files and run-time configurations for using Franka Robotics research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Robotics 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.11</version>
+  <version>0.1.12</version>
   <description>franka_description contains URDF files and meshes ofFranka Robotics robots</description>
   <maintainer email="[email protected]">Franka Robotics 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.11</version>
+  <version>0.1.12</version>
   <description>franka_example_controllers provides example code for controllingFranka Robotics research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Robotics 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.11</version>
+  <version>0.1.12</version>
   <description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
   <maintainer email="[email protected]">Franka Robotics GmbH</maintainer>
   <license>Apache 2.0</license>

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.11</version>
+  <version>0.1.12</version>
   <description>franka_hardware provides hardware interfaces for using Franka Robotics research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Robotics GmbH</maintainer>
   <license>Apache 2.0</license>

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.11</version>
+  <version>0.1.12</version>
   <description>Contains Moveit2 configuration files for Franka Robotics research robots</description>
   <maintainer email="[email protected]">Franka Robotics 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.11</version>
+  <version>0.1.12</version>
   <description>franka_msgs provides messages and actions specific to Franka Robotics research robots</description>
   <maintainer email="[email protected]">Franka Robotics GmbH</maintainer>
   <license>Apache 2.0</license>

franka_robot_state_broadcaster/CMakeLists.txt

@@ -24,7 +24,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     rclcpp_lifecycle
     rcutils
     realtime_tools
-    sensor_msgs)
+    sensor_msgs
+    visualization_msgs)
 
 
 # find dependencies

franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp

@@ -20,6 +20,7 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
+#include "franka_msgs/msg/franka_robot_state.hpp"
 #include "franka_robot_state_broadcaster_parameters.hpp"
 #include "franka_semantic_components/franka_robot_state.hpp"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
@@ -29,6 +30,9 @@
 namespace franka_robot_state_broadcaster {
 class FrankaRobotStateBroadcaster : public controller_interface::ControllerInterface {
  public:
+  explicit FrankaRobotStateBroadcaster(
+      std::unique_ptr<franka_semantic_components::FrankaRobotState> franka_robot_state = nullptr)
+      : franka_robot_state(std::move(franka_robot_state)){};
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
@@ -55,6 +59,32 @@ class FrankaRobotStateBroadcaster : public controller_interface::ControllerInter
   std::shared_ptr<rclcpp::Publisher<franka_msgs::msg::FrankaRobotState>> franka_state_publisher;
   std::shared_ptr<realtime_tools::RealtimePublisher<franka_msgs::msg::FrankaRobotState>>
       realtime_franka_state_publisher;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>>
+      current_pose_stamped_publisher_;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>>
+      last_desired_pose_stamped_publisher_;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>>
+      desired_end_effector_twist_stamped_publisher_;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>>
+      external_wrench_in_base_frame_publisher_;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>>
+      external_wrench_in_stiffness_frame_publisher_;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>>
+      external_joint_torques_publisher_;
+
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> measured_joint_states_publisher_;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> desired_joint_states_publisher_;
+
+  const std::string kCurrentPoseTopic = "~/current_pose";
+  const std::string kLastDesiredPoseTopic = "~/last_desired_pose";
+  const std::string kDesiredEETwist = "~/desired_end_effector_twist";
+  const std::string kMeasuredJointStates = "~/measured_joint_states";
+  const std::string kExternalWrenchInStiffnessFrame = "~/external_wrench_in_stiffness_frame";
+  const std::string kExternalWrenchInBaseFrame = "~/external_wrench_in_base_frame";
+  const std::string kExternalJointTorques = "~/external_joint_torques";
+  const std::string kDesiredJointStates = "~/desired_joint_states";
+
+  franka_msgs::msg::FrankaRobotState franka_robot_state_msg_;
   std::unique_ptr<franka_semantic_components::FrankaRobotState> franka_robot_state;
 };
 

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.11</version>
+  <version>0.1.12</version>
   <description>Broadcaster to publish robot states</description>
   <maintainer email="[email protected]">Franka Robotics GmbH</maintainer>
   <license>Apache 2.0</license>

franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp

@@ -63,10 +63,36 @@ FrankaRobotStateBroadcaster::state_interface_configuration() const {
 controller_interface::CallbackReturn FrankaRobotStateBroadcaster::on_configure(
     const rclcpp_lifecycle::State& /*previous_state*/) {
   params = param_listener->get_params();
-
-  franka_robot_state = std::make_unique<franka_semantic_components::FrankaRobotState>(
-      franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name));
-
+  std::string robot_description;
+  if (!get_node()->get_parameter("robot_description", robot_description)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter");
+    return CallbackReturn::ERROR;
+  }
+  if (!franka_robot_state) {
+    franka_robot_state = std::make_unique<franka_semantic_components::FrankaRobotState>(
+        franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name,
+                                                     robot_description));
+  }
+  current_pose_stamped_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
+      kCurrentPoseTopic, rclcpp::SystemDefaultsQoS());
+  last_desired_pose_stamped_publisher_ =
+      get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(kLastDesiredPoseTopic,
+                                                                    rclcpp::SystemDefaultsQoS());
+  desired_end_effector_twist_stamped_publisher_ =
+      get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(kDesiredEETwist,
+                                                                     rclcpp::SystemDefaultsQoS());
+  measured_joint_states_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
+      kMeasuredJointStates, rclcpp::SystemDefaultsQoS());
+  external_wrench_in_stiffness_frame_publisher_ =
+      get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
+          kExternalWrenchInStiffnessFrame, rclcpp::SystemDefaultsQoS());
+  external_wrench_in_base_frame_publisher_ =
+      get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(kExternalWrenchInBaseFrame,
+                                                                      rclcpp::SystemDefaultsQoS());
+  external_joint_torques_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
+      kExternalJointTorques, rclcpp::SystemDefaultsQoS());
+  desired_joint_states_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
+      kDesiredJointStates, rclcpp::SystemDefaultsQoS());
   try {
     franka_state_publisher = get_node()->create_publisher<franka_msgs::msg::FrankaRobotState>(
         "~/" + state_interface_name, rclcpp::SystemDefaultsQoS());
@@ -108,7 +134,27 @@ controller_interface::return_type FrankaRobotStateBroadcaster::update(
       realtime_franka_state_publisher->unlock();
       return controller_interface::return_type::ERROR;
     }
+
     realtime_franka_state_publisher->unlockAndPublish();
+
+    const auto& franka_state_msg = realtime_franka_state_publisher->msg_;
+
+    current_pose_stamped_publisher_->publish(franka_state_msg.o_t_ee);
+
+    last_desired_pose_stamped_publisher_->publish(franka_state_msg.o_t_ee_d);
+
+    desired_end_effector_twist_stamped_publisher_->publish(franka_state_msg.o_dp_ee_d);
+
+    external_wrench_in_base_frame_publisher_->publish(franka_state_msg.o_f_ext_hat_k);
+
+    external_wrench_in_stiffness_frame_publisher_->publish(franka_state_msg.k_f_ext_hat_k);
+
+    measured_joint_states_publisher_->publish(franka_state_msg.measured_joint_state);
+
+    external_joint_torques_publisher_->publish(franka_state_msg.tau_ext_hat_filtered);
+
+    desired_joint_states_publisher_->publish(franka_state_msg.desired_joint_state);
+
     return controller_interface::return_type::OK;
 
   } else {

franka_robot_state_broadcaster/test/test_franka_robot_state_broadcaster.cpp

@@ -16,20 +16,33 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp"
+#include "franka_semantic_components/franka_robot_state.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
 
-using namespace franka_robot_state_broadcaster;
+class MockFrankaRobotState : public franka_semantic_components::FrankaRobotState {
+ public:
+  MockFrankaRobotState(const std::string& name, const std::string& robot_description)
+      : FrankaRobotState(name, robot_description){};
+
+  MOCK_METHOD(void, initialize_robot_state_msg, (franka_msgs::msg::FrankaRobotState&), (override));
+};
 
+using namespace franka_robot_state_broadcaster;
 class TestFrankaRobotStateBroadcaster : public ::testing::Test {
  protected:
   void SetUp() override {
-    broadcaster_ = std::make_unique<FrankaRobotStateBroadcaster>();
+    franka_robot_state_ = std::make_unique<MockFrankaRobotState>(
+        "mock_franka_robot_state", ros2_control_test_assets::minimal_robot_urdf);
+    broadcaster_ = std::make_unique<FrankaRobotStateBroadcaster>(std::move(franka_robot_state_));
     broadcaster_->init("test_broadcaster");
+    broadcaster_->get_node()->set_parameter(
+        {"robot_description", ros2_control_test_assets::minimal_robot_urdf});
   }
-
   std::unique_ptr<FrankaRobotStateBroadcaster> broadcaster_;
+  std::unique_ptr<MockFrankaRobotState> franka_robot_state_;
 };
 
 TEST_F(TestFrankaRobotStateBroadcaster, test_init_return_success) {
@@ -51,7 +64,6 @@ TEST_F(TestFrankaRobotStateBroadcaster, test_activate_return_success) {
 TEST_F(TestFrankaRobotStateBroadcaster, test_deactivate_return_success) {
   EXPECT_EQ(broadcaster_->on_configure(rclcpp_lifecycle::State()),
             controller_interface::CallbackReturn::SUCCESS);
-
   EXPECT_EQ(broadcaster_->on_deactivate(rclcpp_lifecycle::State()),
             controller_interface::CallbackReturn::SUCCESS);
 }

franka_semantic_components/CMakeLists.txt

@@ -12,7 +12,15 @@ endif()
 
 option(CHECK_TIDY "Adds clang-tidy tests" OFF)
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS Franka franka_hardware franka_msgs geometry_msgs sensor_msgs hardware_interface rclcpp_lifecycle)
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+    Franka
+    franka_hardware
+    franka_msgs
+    geometry_msgs
+    sensor_msgs
+    hardware_interface
+    rclcpp_lifecycle
+    urdf)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -23,23 +31,22 @@ endforeach()
 find_package(Franka 0.13.0 REQUIRED)
 find_package(Eigen3 REQUIRED)
 
-add_library(franka_semantic_components SHARED
-            src/franka_robot_state.cpp
-            src/franka_robot_model.cpp
-            src/franka_cartesian_velocity_interface.cpp
-            src/franka_cartesian_pose_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
-  Eigen3::Eigen)
-ament_target_dependencies(franka_semantic_components
-                          Franka
-                          sensor_msgs
-                          geometry_msgs
-                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+add_library(
+  franka_semantic_components SHARED
+  src/franka_robot_state.cpp
+  src/franka_robot_model.cpp
+  src/franka_cartesian_velocity_interface.cpp
+  src/franka_cartesian_pose_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 Eigen3::Eigen)
+
+ament_target_dependencies(franka_semantic_components Franka sensor_msgs
+                          geometry_msgs ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(TARGETS franka_semantic_components DESTINATION lib)
 install(DIRECTORY include/ DESTINATION include)
@@ -148,6 +155,9 @@ if(BUILD_TESTING)
     sensor_msgs
     geometry_msgs
   )
+
+  install(FILES test/robot_description_test.txt
+          DESTINATION share/${PROJECT_NAME})
 endif()
 
 ament_export_include_directories(include)