From 83bcd06c14432ad796150fa1cf20d97de2a950bc Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Wed, 10 Jan 2024 15:33:48 +0100 Subject: [PATCH 1/4] feat: visualize robot state messages --- .devcontainer/docker-compose.yml | 3 +- franka_robot_state_broadcaster/CMakeLists.txt | 3 +- .../franka_robot_state_broadcaster.hpp | 18 +++ .../src/franka_robot_state_broadcaster.cpp | 63 +++++++- franka_semantic_components/CMakeLists.txt | 46 +++--- .../franka_robot_state.hpp | 58 +++++++- .../src/franka_robot_state.cpp | 136 ++++++++++++------ .../test/franka_robot_state_test.cpp | 8 +- .../test/franka_robot_state_test.hpp | 23 ++- .../test/robot_description_test.txt | 1 + 10 files changed, 291 insertions(+), 68 deletions(-) create mode 100644 franka_semantic_components/test/robot_description_test.txt diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml index c260dc91..7e05bde7 100644 --- a/.devcontainer/docker-compose.yml +++ b/.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 \ No newline at end of file + memlock: 8428281856 diff --git a/franka_robot_state_broadcaster/CMakeLists.txt b/franka_robot_state_broadcaster/CMakeLists.txt index dfbe26e7..fec6d56d 100644 --- a/franka_robot_state_broadcaster/CMakeLists.txt +++ b/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 diff --git a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp index 6cbaff9d..bf2b590c 100644 --- a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp +++ b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp @@ -20,6 +20,7 @@ #include #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" @@ -55,6 +56,23 @@ class FrankaRobotStateBroadcaster : public controller_interface::ControllerInter std::shared_ptr> franka_state_publisher; std::shared_ptr> realtime_franka_state_publisher; + std::shared_ptr> + current_pose_stamped_publisher_; + std::shared_ptr> + last_desired_pose_stamped_publisher_; + std::shared_ptr> + desired_end_effector_twist_stamped_publisher_; + std::shared_ptr> + external_wrench_in_base_frame_publisher_; + std::shared_ptr> + external_wrench_in_stiffness_frame_publisher_; + std::shared_ptr> + external_joint_torques_publisher_; + + std::shared_ptr> measured_joint_states_publisher_; + std::shared_ptr> desired_joint_states_publisher_; + + franka_msgs::msg::FrankaRobotState franka_robot_state_msg_; std::unique_ptr franka_robot_state; }; diff --git a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp index 7aa4124e..54422e30 100644 --- a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp +++ b/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(); - + std::string robot_description; + if (!get_node()->get_parameter_or("robot_description", robot_description, std::string(""))) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter"); + return CallbackReturn::ERROR; + } franka_robot_state = std::make_unique( - franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name)); - + franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name, + robot_description)); + current_pose_stamped_publisher_ = get_node()->create_publisher( + "~/current_pose", rclcpp::SystemDefaultsQoS()); + last_desired_pose_stamped_publisher_ = + get_node()->create_publisher("~/last_desired_pose", + rclcpp::SystemDefaultsQoS()); + desired_end_effector_twist_stamped_publisher_ = + get_node()->create_publisher("~/desired_end_effector_twist", + rclcpp::SystemDefaultsQoS()); + measured_joint_states_publisher_ = get_node()->create_publisher( + "~/measured_joint_states", rclcpp::SystemDefaultsQoS()); + external_wrench_in_stiffness_frame_publisher_ = + get_node()->create_publisher( + "~/external_wrench_in_stiffness_frame_expressed_in_stiffness_frame", + rclcpp::SystemDefaultsQoS()); + external_wrench_in_base_frame_publisher_ = + get_node()->create_publisher( + "~/external_wrench_in_stiffness_frame_expressed_in_base_frame", + rclcpp::SystemDefaultsQoS()); + external_joint_torques_publisher_ = get_node()->create_publisher( + "~/external_joint_torques", rclcpp::SystemDefaultsQoS()); + desired_joint_states_publisher_ = get_node()->create_publisher( + "~/desired_joint_states", rclcpp::SystemDefaultsQoS()); try { franka_state_publisher = get_node()->create_publisher( "~/" + state_interface_name, rclcpp::SystemDefaultsQoS()); @@ -108,7 +134,38 @@ controller_interface::return_type FrankaRobotStateBroadcaster::update( realtime_franka_state_publisher->unlock(); return controller_interface::return_type::ERROR; } + realtime_franka_state_publisher->unlockAndPublish(); + + geometry_msgs::msg::PoseStamped pose_stamped_msg; + geometry_msgs::msg::TwistStamped twist_stamped_msg; + geometry_msgs::msg::WrenchStamped wrench_stamped_msg; + sensor_msgs::msg::JointState joint_state_msg; + + pose_stamped_msg = realtime_franka_state_publisher->msg_.o_t_ee; + current_pose_stamped_publisher_->publish(pose_stamped_msg); + + pose_stamped_msg = realtime_franka_state_publisher->msg_.o_t_ee_d; + last_desired_pose_stamped_publisher_->publish(pose_stamped_msg); + + twist_stamped_msg = realtime_franka_state_publisher->msg_.o_dp_ee_d; + desired_end_effector_twist_stamped_publisher_->publish(twist_stamped_msg); + + wrench_stamped_msg = realtime_franka_state_publisher->msg_.o_f_ext_hat_k; + external_wrench_in_base_frame_publisher_->publish(wrench_stamped_msg); + + wrench_stamped_msg = realtime_franka_state_publisher->msg_.k_f_ext_hat_k; + external_wrench_in_stiffness_frame_publisher_->publish(wrench_stamped_msg); + + joint_state_msg = realtime_franka_state_publisher->msg_.measured_joint_state; + measured_joint_states_publisher_->publish(joint_state_msg); + + joint_state_msg = realtime_franka_state_publisher->msg_.tau_ext_hat_filtered; + external_joint_torques_publisher_->publish(joint_state_msg); + + joint_state_msg = realtime_franka_state_publisher->msg_.desired_joint_state; + desired_joint_states_publisher_->publish(joint_state_msg); + return controller_interface::return_type::OK; } else { diff --git a/franka_semantic_components/CMakeLists.txt b/franka_semantic_components/CMakeLists.txt index 3e51d719..16f27717 100644 --- a/franka_semantic_components/CMakeLists.txt +++ b/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) diff --git a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp index 645d1f65..2c078ba4 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp @@ -15,8 +15,10 @@ #pragma once #include +#include #include #include +#include "urdf/model.h" #include "franka/robot_state.h" #include "franka_msgs/msg/franka_robot_state.hpp" @@ -26,7 +28,7 @@ namespace franka_semantic_components { class FrankaRobotState : public semantic_components::SemanticComponentInterface { public: - explicit FrankaRobotState(const std::string& name); + explicit FrankaRobotState(const std::string& name, const std::string& robot_description); virtual ~FrankaRobotState() = default; @@ -45,8 +47,60 @@ class FrankaRobotState franka::RobotState* robot_state_ptr; private: - const std::string robot_name_{"panda"}; + std::string robot_description_; + std::string robot_name_; const std::string state_interface_name_{"robot_state"}; + bool gripper_loaded_{false}; + size_t kEndEffectorLinkIndex{8}; + size_t kTotalAmountOfLinksWithoutEndEffector{8}; + size_t kTotalAmountOfJoints{8}; + // TODO(yazi_ba) update stiffness frame with the user defined transformation + size_t kStiffnessLinkIndex{8}; + std::shared_ptr model_; + std::vector joint_names, link_names; + + /** + * @brief Populate the link_name std::vector with the links from urdf object in order. + * The root link is the first element and tcp is the last element. + * + */ + auto set_links_from_urdf() -> void; + + /** + * @brief Populate the joint_name std::vector with the joints from urdf object in order. + * + */ + auto set_joints_from_urdf() -> void; + + /** + * @brief Recursively sets all child links from a link and assign them to the link_name + * + * @param link root link + */ + auto set_child_links_recursively(const std::shared_ptr& link) -> void; + + /** + * @brief Check if gripper is loaded + * Checks if the robot_name + "_hand_tcp" frame exists + * + * @return true if gripper is loaded + * @return false if gripper is not loaded + */ + auto is_gripper_loaded() -> bool; + /** + * @brief Get the robot name from urdf object + + * @return std::string + */ + auto get_robot_name_from_urdf() -> std::string; + + /** + * @brief Get the link index from link name + * + * @param link_name + * @return size_t + */ + auto get_link_index(const std::string& link_name) -> size_t; }; } // namespace franka_semantic_components \ No newline at end of file diff --git a/franka_semantic_components/src/franka_robot_state.cpp b/franka_semantic_components/src/franka_robot_state.cpp index 2f6f69c8..e6aaa336 100644 --- a/franka_semantic_components/src/franka_robot_state.cpp +++ b/franka_semantic_components/src/franka_robot_state.cpp @@ -24,14 +24,9 @@ namespace { -// TODO(kuhn_an): Are these correct? -const std::string kLinkName = "_link"; -const size_t kTotalAmountOfLinks = 11; const size_t kBaseLinkIndex = 0; -const size_t kFlangeLinkIndex = 7; -const size_t kEndEffectorLinkIndex = 8; -const size_t kLoadLinkIndex = 9; -const size_t kStiffnessLinkIndex = 10; +const size_t kFlangeLinkIndex = 8; +const size_t kLoadLinkIndex = 8; // Example implementation of bit_cast: https://en.cppreference.com/w/cpp/numeric/bit_cast template @@ -52,51 +47,112 @@ bit_cast(const From& src) noexcept { namespace franka_semantic_components { -FrankaRobotState::FrankaRobotState(const std::string& name) : SemanticComponentInterface(name, 1) { +FrankaRobotState::FrankaRobotState(const std::string& name, const std::string& robot_description) + : SemanticComponentInterface(name, 1), model_(std::make_shared()) { interface_names_.emplace_back(name_); - // TODO: Set default values to NaN + robot_description_ = robot_description; + if (!model_->initString(robot_description_)) { + throw std::runtime_error("Failed to parse URDF."); + } + + robot_name_ = get_robot_name_from_urdf(); + gripper_loaded_ = is_gripper_loaded(); + + set_links_from_urdf(); + set_joints_from_urdf(); + + if (gripper_loaded_) { + kEndEffectorLinkIndex = get_link_index(robot_name_ + "_hand_tcp"); + kStiffnessLinkIndex = kEndEffectorLinkIndex; + } else { + kEndEffectorLinkIndex = kFlangeLinkIndex; + kStiffnessLinkIndex = kEndEffectorLinkIndex; + } } -auto FrankaRobotState::initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) +auto FrankaRobotState::get_link_index(const std::string& link_name) -> size_t { + auto link_index = std::find(link_names.cbegin(), link_names.cend(), link_name); + if (link_index != link_names.end()) { + return std::distance(link_names.cbegin(), link_index); + } else { + throw std::runtime_error("Link name not found in URDF."); + } +} + +auto FrankaRobotState::is_gripper_loaded() -> bool { + const auto& links = model_->links_; + bool gripper_loaded = links.find(robot_name_ + "_hand_tcp") != links.end(); + + return gripper_loaded; +} + +auto FrankaRobotState::get_robot_name_from_urdf() -> std::string { + return model_->name_; +} + +auto FrankaRobotState::set_child_links_recursively(const std::shared_ptr& link) -> void { - // The joint states - std::vector joint_names; - // Starting from the base link (aka joint0) up to the end-effector (aka joint8), load (aka joint9) - // and stiffness frame (aka joint10) - for (size_t i = 0; i < kTotalAmountOfLinks; ++i) { - joint_names.push_back(robot_name_ + kLinkName + std::to_string(i)); + for (const auto& child_link : link->child_links) { + link_names.push_back(child_link->name); + set_child_links_recursively(child_link); + } +} + +auto FrankaRobotState::set_links_from_urdf() -> void { + auto root_link = model_->getRoot(); + link_names.push_back(root_link->name); + set_child_links_recursively(root_link); +} + +auto FrankaRobotState::set_joints_from_urdf() -> void { + auto& joints = model_->joints_; + for (const auto& [name, joint] : joints) { + if (joint->type == urdf::Joint::REVOLUTE) { + joint_names.push_back(name); + } } +} - // The joint state - joint 0 is the base while joint 8 is the last joint +auto FrankaRobotState::initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) + -> void { + // The joint state - joint 1 is the first joint while joint 7 is the last revolute joint message.measured_joint_state.name = - std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + std::vector(joint_names.cbegin(), joint_names.cend()); message.desired_joint_state.name = - std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + std::vector(joint_names.cbegin(), joint_names.cend()); message.measured_joint_motor_state.name = - std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + std::vector(joint_names.cbegin(), joint_names.cend()); message.tau_ext_hat_filtered.name = - std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); - - // Active wrenches - message.o_f_ext_hat_k.header.frame_id = joint_names[kBaseLinkIndex]; - message.k_f_ext_hat_k.header.frame_id = joint_names[kStiffnessLinkIndex]; + std::vector(joint_names.cbegin(), joint_names.cend()); - // Different pose frames - message.o_t_ee.header.frame_id = joint_names[kBaseLinkIndex]; - message.o_t_ee_d.header.frame_id = joint_names[kBaseLinkIndex]; - message.o_t_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; + message.measured_joint_state.header.frame_id = link_names[kBaseLinkIndex]; + message.desired_joint_state.header.frame_id = link_names[kBaseLinkIndex]; + message.measured_joint_motor_state.header.frame_id = link_names[kBaseLinkIndex]; + message.tau_ext_hat_filtered.header.frame_id = link_names[kBaseLinkIndex]; - message.f_t_ee.header.frame_id = joint_names[kFlangeLinkIndex]; - message.ee_t_k.header.frame_id = joint_names[kEndEffectorLinkIndex]; - - message.o_dp_ee_d.header.frame_id = joint_names[kBaseLinkIndex]; - message.o_dp_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; - message.o_ddp_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; - - // The inertias - message.inertia_ee.header.frame_id = joint_names[kEndEffectorLinkIndex]; - message.inertia_load.header.frame_id = joint_names[kLoadLinkIndex]; - message.inertia_total.header.frame_id = joint_names[kEndEffectorLinkIndex]; + // Active wrenches + message.o_f_ext_hat_k.header.frame_id = link_names[kBaseLinkIndex]; + message.k_f_ext_hat_k.header.frame_id = link_names[kStiffnessLinkIndex]; + + // Current EE Pose + message.o_t_ee.header.frame_id = link_names[kBaseLinkIndex]; + // Desired EE Pose + message.o_t_ee_d.header.frame_id = link_names[kBaseLinkIndex]; + // Commanded EE Pose + message.o_t_ee_c.header.frame_id = link_names[kBaseLinkIndex]; + + message.f_t_ee.header.frame_id = link_names[kFlangeLinkIndex]; + message.ee_t_k.header.frame_id = link_names[kEndEffectorLinkIndex]; + + message.o_dp_ee_d.header.frame_id = link_names[kBaseLinkIndex]; + message.o_dp_ee_c.header.frame_id = link_names[kBaseLinkIndex]; + message.o_ddp_ee_c.header.frame_id = link_names[kBaseLinkIndex]; + + // The inertias are with respect to the Center Of Mass. + // TODO(yazi_ba) frame ids should be referenced to the Center Of Mass + message.inertia_ee.header.frame_id = link_names[kEndEffectorLinkIndex]; + message.inertia_load.header.frame_id = link_names[kLoadLinkIndex]; + message.inertia_total.header.frame_id = link_names[kEndEffectorLinkIndex]; } auto FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& message) -> bool { diff --git a/franka_semantic_components/test/franka_robot_state_test.cpp b/franka_semantic_components/test/franka_robot_state_test.cpp index 1b92722f..4b223ea8 100644 --- a/franka_semantic_components/test/franka_robot_state_test.cpp +++ b/franka_semantic_components/test/franka_robot_state_test.cpp @@ -90,6 +90,10 @@ TEST_F(FrankaRobotStateTest, givenInitializedRobotStateMsg_thenCorrectFrameIDs) franka_state_friend->initialize_robot_state_msg(franka_robot_state_msg); ASSERT_EQ(franka_robot_state_msg.o_t_ee.header.frame_id, "panda_link0"); - ASSERT_EQ(franka_robot_state_msg.ee_t_k.header.frame_id, "panda_link8"); - ASSERT_EQ(franka_robot_state_msg.measured_joint_state.name[1], "panda_link2"); + ASSERT_EQ(franka_robot_state_msg.ee_t_k.header.frame_id, "panda_hand_tcp"); + ASSERT_EQ(franka_robot_state_msg.measured_joint_state.name[1], "panda_joint2"); + ASSERT_EQ(franka_robot_state_msg.k_f_ext_hat_k.header.frame_id, "panda_hand_tcp"); + ASSERT_EQ(franka_robot_state_msg.o_f_ext_hat_k.header.frame_id, "panda_link0"); + ASSERT_EQ(franka_robot_state_msg.o_dp_ee_c.header.frame_id, "panda_link0"); + ASSERT_EQ(franka_robot_state_msg.o_ddp_ee_c.header.frame_id, "panda_link0"); } diff --git a/franka_semantic_components/test/franka_robot_state_test.hpp b/franka_semantic_components/test/franka_robot_state_test.hpp index 2b79b256..7d043971 100644 --- a/franka_semantic_components/test/franka_robot_state_test.hpp +++ b/franka_semantic_components/test/franka_robot_state_test.hpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include "franka/robot_state.h" #include "franka_semantic_components/franka_robot_state.hpp" #include "gmock/gmock.h" @@ -32,9 +35,27 @@ class FrankaRobotStateTestFriend : public franka_semantic_components::FrankaRobo public: // Use generation of interface names explicit FrankaRobotStateTestFriend(const std::string& name) - : franka_semantic_components::FrankaRobotState(name) {} + : franka_semantic_components::FrankaRobotState( + name, + get_robot_description("franka_semantic_components")) {} virtual ~FrankaRobotStateTestFriend() = default; + + private: + static std::string get_robot_description(const std::string& package_name) { + std::string package_path = ament_index_cpp::get_package_share_directory(package_name); + std::string file_path = package_path + "/robot_description_test.txt"; + + std::ifstream file(file_path); + if (!file) { + throw std::runtime_error("Failed to open robot_description.txt."); + } + + std::stringstream buffer; + buffer << file.rdbuf(); + + return buffer.str(); + } }; class FrankaRobotStateTest : public ::testing::Test { diff --git a/franka_semantic_components/test/robot_description_test.txt b/franka_semantic_components/test/robot_description_test.txt new file mode 100644 index 00000000..a79cc574 --- /dev/null +++ b/franka_semantic_components/test/robot_description_test.txt @@ -0,0 +1 @@ + franka_hardware/FrankaHardwareInterface localhost 0.0 -0.7853981633974483 0.0 -2.356194490192345 0.0 1.5707963267948966 0.7853981633974483 \ No newline at end of file From 45b901884ee48d5914f4e1aa34ff2b2055a14c0b Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 11 Jan 2024 14:38:27 +0100 Subject: [PATCH 2/4] tests: update state publisher tests with mocked franka robot state --- .../franka_robot_state_broadcaster.hpp | 3 +++ .../src/franka_robot_state_broadcaster.cpp | 10 ++++++---- .../test_franka_robot_state_broadcaster.cpp | 20 +++++++++++++++---- .../franka_robot_state.hpp | 4 ++-- .../test/franka_robot_state_test.hpp | 2 +- 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp index bf2b590c..00324c84 100644 --- a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp +++ b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp @@ -30,6 +30,9 @@ namespace franka_robot_state_broadcaster { class FrankaRobotStateBroadcaster : public controller_interface::ControllerInterface { public: + explicit FrankaRobotStateBroadcaster( + std::unique_ptr 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; diff --git a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp index 54422e30..3029a1bc 100644 --- a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp +++ b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp @@ -64,13 +64,15 @@ controller_interface::CallbackReturn FrankaRobotStateBroadcaster::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { params = param_listener->get_params(); std::string robot_description; - if (!get_node()->get_parameter_or("robot_description", robot_description, std::string(""))) { + if (!get_node()->get_parameter("robot_description", robot_description)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter"); return CallbackReturn::ERROR; } - franka_robot_state = std::make_unique( - franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name, - robot_description)); + if (!franka_robot_state) { + franka_robot_state = std::make_unique( + franka_semantic_components::FrankaRobotState(params.arm_id + "/" + state_interface_name, + robot_description)); + } current_pose_stamped_publisher_ = get_node()->create_publisher( "~/current_pose", rclcpp::SystemDefaultsQoS()); last_desired_pose_stamped_publisher_ = diff --git a/franka_robot_state_broadcaster/test/test_franka_robot_state_broadcaster.cpp b/franka_robot_state_broadcaster/test/test_franka_robot_state_broadcaster.cpp index 502697a8..94693af1 100644 --- a/franka_robot_state_broadcaster/test/test_franka_robot_state_broadcaster.cpp +++ b/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(); + franka_robot_state_ = std::make_unique( + "mock_franka_robot_state", ros2_control_test_assets::minimal_robot_urdf); + broadcaster_ = std::make_unique(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 broadcaster_; + std::unique_ptr 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); } diff --git a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp index 2c078ba4..362c0cf3 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp @@ -35,13 +35,13 @@ class FrankaRobotState /** * @param[in/out] message Initializes this message to contain the respective frame_id information */ - auto initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) -> void; + virtual auto initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) -> void; /** * Constructs and return a FrankaRobotState message from the current values. * \return FrankaRobotState message from values; */ - auto get_values_as_message(franka_msgs::msg::FrankaRobotState& message) -> bool; + virtual auto get_values_as_message(franka_msgs::msg::FrankaRobotState& message) -> bool; protected: franka::RobotState* robot_state_ptr; diff --git a/franka_semantic_components/test/franka_robot_state_test.hpp b/franka_semantic_components/test/franka_robot_state_test.hpp index 7d043971..d663d69d 100644 --- a/franka_semantic_components/test/franka_robot_state_test.hpp +++ b/franka_semantic_components/test/franka_robot_state_test.hpp @@ -48,7 +48,7 @@ class FrankaRobotStateTestFriend : public franka_semantic_components::FrankaRobo std::ifstream file(file_path); if (!file) { - throw std::runtime_error("Failed to open robot_description.txt."); + throw std::runtime_error("Failed to open file: " + file_path); } std::stringstream buffer; From 401ccf8ea5ce54839a5798ac948f9088f2417096 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 12 Jan 2024 10:52:56 +0100 Subject: [PATCH 3/4] format: test robot description --- .../franka_robot_state_broadcaster.hpp | 9 + .../src/franka_robot_state_broadcaster.cpp | 49 +- .../src/franka_robot_state.cpp | 5 +- .../test/robot_description_test.txt | 457 +++++++++++++++++- 4 files changed, 486 insertions(+), 34 deletions(-) diff --git a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp index 00324c84..2fb759e7 100644 --- a/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp +++ b/franka_robot_state_broadcaster/include/franka_robot_state_broadcaster/franka_robot_state_broadcaster.hpp @@ -75,6 +75,15 @@ class FrankaRobotStateBroadcaster : public controller_interface::ControllerInter std::shared_ptr> measured_joint_states_publisher_; std::shared_ptr> 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_robot_state; }; diff --git a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp index 3029a1bc..b69e5be6 100644 --- a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp +++ b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp @@ -74,27 +74,25 @@ controller_interface::CallbackReturn FrankaRobotStateBroadcaster::on_configure( robot_description)); } current_pose_stamped_publisher_ = get_node()->create_publisher( - "~/current_pose", rclcpp::SystemDefaultsQoS()); + kCurrentPoseTopic, rclcpp::SystemDefaultsQoS()); last_desired_pose_stamped_publisher_ = - get_node()->create_publisher("~/last_desired_pose", + get_node()->create_publisher(kLastDesiredPoseTopic, rclcpp::SystemDefaultsQoS()); desired_end_effector_twist_stamped_publisher_ = - get_node()->create_publisher("~/desired_end_effector_twist", + get_node()->create_publisher(kDesiredEETwist, rclcpp::SystemDefaultsQoS()); measured_joint_states_publisher_ = get_node()->create_publisher( - "~/measured_joint_states", rclcpp::SystemDefaultsQoS()); + kMeasuredJointStates, rclcpp::SystemDefaultsQoS()); external_wrench_in_stiffness_frame_publisher_ = get_node()->create_publisher( - "~/external_wrench_in_stiffness_frame_expressed_in_stiffness_frame", - rclcpp::SystemDefaultsQoS()); + kExternalWrenchInStiffnessFrame, rclcpp::SystemDefaultsQoS()); external_wrench_in_base_frame_publisher_ = - get_node()->create_publisher( - "~/external_wrench_in_stiffness_frame_expressed_in_base_frame", - rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher(kExternalWrenchInBaseFrame, + rclcpp::SystemDefaultsQoS()); external_joint_torques_publisher_ = get_node()->create_publisher( - "~/external_joint_torques", rclcpp::SystemDefaultsQoS()); + kExternalJointTorques, rclcpp::SystemDefaultsQoS()); desired_joint_states_publisher_ = get_node()->create_publisher( - "~/desired_joint_states", rclcpp::SystemDefaultsQoS()); + kDesiredJointStates, rclcpp::SystemDefaultsQoS()); try { franka_state_publisher = get_node()->create_publisher( "~/" + state_interface_name, rclcpp::SystemDefaultsQoS()); @@ -139,34 +137,23 @@ controller_interface::return_type FrankaRobotStateBroadcaster::update( realtime_franka_state_publisher->unlockAndPublish(); - geometry_msgs::msg::PoseStamped pose_stamped_msg; - geometry_msgs::msg::TwistStamped twist_stamped_msg; - geometry_msgs::msg::WrenchStamped wrench_stamped_msg; - sensor_msgs::msg::JointState joint_state_msg; + const auto& franka_state_msg = realtime_franka_state_publisher->msg_; - pose_stamped_msg = realtime_franka_state_publisher->msg_.o_t_ee; - current_pose_stamped_publisher_->publish(pose_stamped_msg); + current_pose_stamped_publisher_->publish(franka_state_msg.o_t_ee); - pose_stamped_msg = realtime_franka_state_publisher->msg_.o_t_ee_d; - last_desired_pose_stamped_publisher_->publish(pose_stamped_msg); + last_desired_pose_stamped_publisher_->publish(franka_state_msg.o_t_ee_d); - twist_stamped_msg = realtime_franka_state_publisher->msg_.o_dp_ee_d; - desired_end_effector_twist_stamped_publisher_->publish(twist_stamped_msg); + desired_end_effector_twist_stamped_publisher_->publish(franka_state_msg.o_dp_ee_d); - wrench_stamped_msg = realtime_franka_state_publisher->msg_.o_f_ext_hat_k; - external_wrench_in_base_frame_publisher_->publish(wrench_stamped_msg); + external_wrench_in_base_frame_publisher_->publish(franka_state_msg.o_f_ext_hat_k); - wrench_stamped_msg = realtime_franka_state_publisher->msg_.k_f_ext_hat_k; - external_wrench_in_stiffness_frame_publisher_->publish(wrench_stamped_msg); + external_wrench_in_stiffness_frame_publisher_->publish(franka_state_msg.k_f_ext_hat_k); - joint_state_msg = realtime_franka_state_publisher->msg_.measured_joint_state; - measured_joint_states_publisher_->publish(joint_state_msg); + measured_joint_states_publisher_->publish(franka_state_msg.measured_joint_state); - joint_state_msg = realtime_franka_state_publisher->msg_.tau_ext_hat_filtered; - external_joint_torques_publisher_->publish(joint_state_msg); + external_joint_torques_publisher_->publish(franka_state_msg.tau_ext_hat_filtered); - joint_state_msg = realtime_franka_state_publisher->msg_.desired_joint_state; - desired_joint_states_publisher_->publish(joint_state_msg); + desired_joint_states_publisher_->publish(franka_state_msg.desired_joint_state); return controller_interface::return_type::OK; diff --git a/franka_semantic_components/src/franka_robot_state.cpp b/franka_semantic_components/src/franka_robot_state.cpp index e6aaa336..ba23eaca 100644 --- a/franka_semantic_components/src/franka_robot_state.cpp +++ b/franka_semantic_components/src/franka_robot_state.cpp @@ -27,6 +27,7 @@ namespace { const size_t kBaseLinkIndex = 0; const size_t kFlangeLinkIndex = 8; const size_t kLoadLinkIndex = 8; +const std::string kTCPFrameName = "_hand_tcp"; // Example implementation of bit_cast: https://en.cppreference.com/w/cpp/numeric/bit_cast template @@ -62,7 +63,7 @@ FrankaRobotState::FrankaRobotState(const std::string& name, const std::string& r set_joints_from_urdf(); if (gripper_loaded_) { - kEndEffectorLinkIndex = get_link_index(robot_name_ + "_hand_tcp"); + kEndEffectorLinkIndex = get_link_index(robot_name_ + kTCPFrameName); kStiffnessLinkIndex = kEndEffectorLinkIndex; } else { kEndEffectorLinkIndex = kFlangeLinkIndex; @@ -81,7 +82,7 @@ auto FrankaRobotState::get_link_index(const std::string& link_name) -> size_t { auto FrankaRobotState::is_gripper_loaded() -> bool { const auto& links = model_->links_; - bool gripper_loaded = links.find(robot_name_ + "_hand_tcp") != links.end(); + bool gripper_loaded = links.find(robot_name_ + kTCPFrameName) != links.end(); return gripper_loaded; } diff --git a/franka_semantic_components/test/robot_description_test.txt b/franka_semantic_components/test/robot_description_test.txt index a79cc574..c485a932 100644 --- a/franka_semantic_components/test/robot_description_test.txt +++ b/franka_semantic_components/test/robot_description_test.txt @@ -1 +1,456 @@ - franka_hardware/FrankaHardwareInterface localhost 0.0 -0.7853981633974483 0.0 -2.356194490192345 0.0 1.5707963267948966 0.7853981633974483 \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + franka_hardware/FrankaHardwareInterface + localhost + + + 0.0 + + + + + + + + + -0.7853981633974483 + + + + + + + + + 0.0 + + + + + + + + + -2.356194490192345 + + + + + + + + + 0.0 + + + + + + + + + 1.5707963267948966 + + + + + + + + + 0.7853981633974483 + + + + + + + + + \ No newline at end of file From e2bb29cc5f07a62d1f88940a6a51677513975268 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 12 Jan 2024 15:04:59 +0100 Subject: [PATCH 4/4] bump version --- CHANGELOG.md | 7 +++++++ franka_bringup/package.xml | 2 +- franka_description/package.xml | 2 +- franka_example_controllers/package.xml | 2 +- franka_gripper/package.xml | 2 +- franka_hardware/package.xml | 2 +- franka_moveit_config/package.xml | 2 +- franka_msgs/package.xml | 2 +- franka_robot_state_broadcaster/package.xml | 2 +- integration_launch_testing/package.xml | 2 +- 10 files changed, 16 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 42558fab..5115b792 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index acd4d32a..5e6a0188 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.11 + 0.1.12 Package with launch files and run-time configurations for using Franka Robotics research robots with ros2_control Franka Robotics GmbH Apache 2.0 diff --git a/franka_description/package.xml b/franka_description/package.xml index 59c04e80..461da641 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.11 + 0.1.12 franka_description contains URDF files and meshes ofFranka Robotics robots Franka Robotics GmbH Apache 2.0 diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 8317129c..b97f850f 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.11 + 0.1.12 franka_example_controllers provides example code for controllingFranka Robotics research robots with ros2_control Franka Robotics GmbH Apache 2.0 diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index eebf8371..8321ab24 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.11 + 0.1.12 This package implements the franka gripper of type Franka Hand for the use in ROS2 Franka Robotics GmbH Apache 2.0 diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index 51460ba0..1cbac928 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.11 + 0.1.12 franka_hardware provides hardware interfaces for using Franka Robotics research robots with ros2_control Franka Robotics GmbH Apache 2.0 diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index 9a8c1caf..d54dbb24 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.11 + 0.1.12 Contains Moveit2 configuration files for Franka Robotics research robots Franka Robotics GmbH Apache 2.0 diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index 042c9822..338daade 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.11 + 0.1.12 franka_msgs provides messages and actions specific to Franka Robotics research robots Franka Robotics GmbH Apache 2.0 diff --git a/franka_robot_state_broadcaster/package.xml b/franka_robot_state_broadcaster/package.xml index 104599ae..00266027 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.11 + 0.1.12 Broadcaster to publish robot states Franka Robotics GmbH Apache 2.0 diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index 2bc556e3..3d8a75f4 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.11 + 0.1.12 Functional integration tests for franka controllers Franka Robotics GmbH Apache 2.0