From 701ee54b65597efd7b2a2a832cebe9290ddaaa24 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Fri, 31 May 2024 10:10:46 +0900 Subject: [PATCH 01/14] [add] check velocity command exists to publish command [change] separate position/velocity/effort command [change] use mimic joint name to publish command --- .../topic_based_system.hpp | 2 + src/topic_based_system.cpp | 154 ++++++++++++++---- 2 files changed, 123 insertions(+), 33 deletions(-) diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index cf67d61..f300eb0 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -80,6 +80,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface struct MimicJoint { + std::string joint_name; + std::string mimicked_joint_name; std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index 67a9ec9..a4b8a18 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -114,6 +114,8 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); } MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; + mimic_joint.mimicked_joint_name = mimicked_joint_it->name; mimic_joint.joint_index = i; mimic_joint.mimicked_joint_index = static_cast(std::distance(info_.joints.begin(), mimicked_joint_it)); @@ -271,64 +273,150 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); - if (diff <= trigger_joint_command_threshold_) + + bool exist_velocity_command = false; + static bool exist_velocity_command_old = false; // Use old state to publish zero velocities + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) + { + exist_velocity_command = true; + } + } + + if (diff <= trigger_joint_command_threshold_ && (exist_velocity_command == false && exist_velocity_command_old == false)) { return hardware_interface::return_type::OK; } + + exist_velocity_command_old = exist_velocity_command; - sensor_msgs::msg::JointState joint_state; - for (std::size_t i = 0; i < info_.joints.size(); ++i) + // For Position Joint { - joint_state.name.push_back(info_.joints[i].name); + sensor_msgs::msg::JointState joint_state; joint_state.header.stamp = node_->now(); - // only send commands to the interfaces that are defined for this joint - for (const auto& interface : info_.joints[i].command_interfaces) + for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (interface.name == hardware_interface::HW_IF_POSITION) + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_POSITION) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); + } } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_POSITION) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]); + } + } + } } - else if (interface.name == hardware_interface::HW_IF_EFFORT) + } + + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } + } + + // For Velocity Joint + { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->now(); + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_VELOCITY) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + } } - else + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", - info_.joints[i].name.c_str(), interface.name.c_str()); + if (interface.name == hardware_interface::HW_IF_VELOCITY) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.position.push_back(mimic_joint.multiplier * joint_state.velocity[index]); + } + } + } } } - } - for (const auto& mimic_joint : mimic_joints_) + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } + } + + // For Effort Joint { - for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->now(); + for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (interface.name == hardware_interface::HW_IF_POSITION) + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.position[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) - { - joint_state.velocity[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index]; + if (interface.name == hardware_interface::HW_IF_EFFORT) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); + } + else + { + RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", + info_.joints[i].name.c_str(), interface.name.c_str()); + } } - else if (interface.name == hardware_interface::HW_IF_EFFORT) + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - joint_state.effort[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index]; + if (interface.name == hardware_interface::HW_IF_EFFORT) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.position.push_back(mimic_joint.multiplier * joint_state.effort[index]); + } + } + } } } - } - if (rclcpp::ok()) - { - topic_based_joint_commands_publisher_->publish(joint_state); + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } } return hardware_interface::return_type::OK; From 0f491af553a35da490dcf636cdcc4b8e1b2afdee Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 10 Jun 2024 19:28:55 +0900 Subject: [PATCH 02/14] [fix] fixed bug about mimic joint only publish position --- src/topic_based_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index a4b8a18..d99c369 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -360,7 +360,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (joint_state.name[index] == mimic_joint.mimicked_joint_name) { joint_state.name.push_back(mimic_joint.joint_name); - joint_state.position.push_back(mimic_joint.multiplier * joint_state.velocity[index]); + joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]); } } } @@ -406,7 +406,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (joint_state.name[index] == mimic_joint.mimicked_joint_name) { joint_state.name.push_back(mimic_joint.joint_name); - joint_state.position.push_back(mimic_joint.multiplier * joint_state.effort[index]); + joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]); } } } From 34cef9abf6b1a0cdb1e811d6515aba14fdde630f Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 07:14:10 +0900 Subject: [PATCH 03/14] [remove] remove unnecessary warining --- src/topic_based_system.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index d99c369..a006d7e 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -387,11 +387,6 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti joint_state.name.push_back(info_.joints[i].name); joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); } - else - { - RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", - info_.joints[i].name.c_str(), interface.name.c_str()); - } } } From e186585bdd7a9142db7b54328fa98a0beb68ef73 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 07:19:24 +0900 Subject: [PATCH 04/14] allow hardware parameter to allow initialising joint commands with robot starting state --- .../topic_based_system.hpp | 2 ++ src/topic_based_system.cpp | 22 +++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index f300eb0..1a3b07f 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -71,6 +71,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface rclcpp::Node::SharedPtr node_; sensor_msgs::msg::JointState latest_joint_state_; bool sum_wrapped_joint_states_{ false }; + bool initial_states_as_initial_cmd_{ false }; + bool ready_to_send_cmds_{ false }; /// Use standard interfaces for joints because they are relevant for dynamic behavior std::array standard_interfaces_ = { hardware_interface::HW_IF_POSITION, diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index a006d7e..ce17f95 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -97,6 +97,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& } } } + ready_to_send_cmds_ = true; // Search for mimic joints for (auto i = 0u; i < info_.joints.size(); ++i) @@ -159,6 +160,11 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& { sum_wrapped_joint_states_ = true; } + if (get_hardware_parameter("use_initial_states_as_initial_commands", "false") == "true") + { + initial_states_as_initial_cmd_ = true; + ready_to_send_cmds_ = false; + } return CallbackReturn::SUCCESS; } @@ -247,6 +253,18 @@ hardware_interface::return_type TopicBasedSystem::read(const rclcpp::Time& /*tim } } + if (!ready_to_send_cmds_ && initial_states_as_initial_cmd_) + { + for (std::size_t i = 0; i < joint_states_.size(); ++i) + { + for (std::size_t j = 0; j < joint_states_[i].size(); ++j) + { + joint_commands_[i][j] = joint_states_[i][j]; + } + } + ready_to_send_cmds_ = true; + } + return hardware_interface::return_type::OK; } @@ -267,6 +285,10 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string& hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + if (!ready_to_send_cmds_) + { + return hardware_interface::return_type::ERROR; + } // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. const auto diff = std::transform_reduce( From 747eee85d95ca69daafb80505a4841f38bdf0461 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 23:11:41 +0900 Subject: [PATCH 05/14] [add] add wait_for_reaching_initial_values parameter --- .../topic_based_system.hpp | 2 ++ src/topic_based_system.cpp | 23 ++++++++++++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index 1a3b07f..9fd6c41 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -73,6 +73,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface bool sum_wrapped_joint_states_{ false }; bool initial_states_as_initial_cmd_{ false }; bool ready_to_send_cmds_{ false }; + bool initial_cmd_reached_{ false }; /// Use standard interfaces for joints because they are relevant for dynamic behavior std::array standard_interfaces_ = { hardware_interface::HW_IF_POSITION, @@ -93,6 +94,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; + std::vector> initial_joint_commands_; // If the difference between the current joint state and joint command is less than this value, // the joint command will not be published. diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index ce17f95..c9ea98a 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -78,6 +78,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& } // Initial command values + initial_joint_commands_ = joint_commands_; for (auto i = 0u; i < info_.joints.size(); i++) { const auto& component = info_.joints[i]; @@ -92,7 +93,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& if (!interface.initial_value.empty()) { joint_states_[index][i] = std::stod(interface.initial_value); - joint_commands_[index][i] = std::stod(interface.initial_value); + initial_joint_commands_[index][i] = std::stod(interface.initial_value); } } } @@ -165,6 +166,10 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& initial_states_as_initial_cmd_ = true; ready_to_send_cmds_ = false; } + if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false") + { + initial_cmd_reached_ = true; + } return CallbackReturn::SUCCESS; } @@ -289,6 +294,22 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti { return hardware_interface::return_type::ERROR; } + + if (initial_cmd_reached_ == false) + { + const auto diff = std::transform_reduce( + joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), + initial_joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, + [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); + if (diff < trigger_joint_command_threshold_) + { + initial_cmd_reached_ = true; + } + else + { + joint_commands_ = initial_joint_commands_; + } + } // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. const auto diff = std::transform_reduce( From 8a03d7bb50c988e0d7008e67cd8853f219e6e207 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 5 Sep 2024 23:38:06 +0900 Subject: [PATCH 06/14] [fix] Fixed a bug that caused joints to return to their original position after being moved to the initial position. --- src/topic_based_system.cpp | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index c9ea98a..9037639 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -295,33 +295,37 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti return hardware_interface::return_type::ERROR; } + std::vector> target_joint_commands; if (initial_cmd_reached_ == false) { - const auto diff = std::transform_reduce( - joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), - initial_joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, - [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); - if (diff < trigger_joint_command_threshold_) + double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, [](double sum, double val) { + return sum + std::abs(val); + }); + if (abs_sum >= trigger_joint_command_threshold_) { initial_cmd_reached_ = true; } - else - { - joint_commands_ = initial_joint_commands_; - } + } + if (initial_cmd_reached_ == false) + { + target_joint_commands = initial_joint_commands_; + } + else + { + target_joint_commands = joint_commands_; } // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. const auto diff = std::transform_reduce( joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), - joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, + target_joint_commands[POSITION_INTERFACE_INDEX].cbegin(), 0.0, [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); bool exist_velocity_command = false; static bool exist_velocity_command_old = false; // Use old state to publish zero velocities for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) + if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) { exist_velocity_command = true; } @@ -346,7 +350,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_POSITION) { joint_state.name.push_back(info_.joints[i].name); - joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); + joint_state.position.push_back(target_joint_commands[POSITION_INTERFACE_INDEX][i]); } } } @@ -387,7 +391,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_VELOCITY) { joint_state.name.push_back(info_.joints[i].name); - joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + joint_state.velocity.push_back(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]); } } } @@ -428,7 +432,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_EFFORT) { joint_state.name.push_back(info_.joints[i].name); - joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); + joint_state.effort.push_back(target_joint_commands[EFFORT_INTERFACE_INDEX][i]); } } } From 1825c1366cb9b092c486402fff6788127b633ec8 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Tue, 8 Oct 2024 23:03:12 +0900 Subject: [PATCH 07/14] Revert "allow hardware parameter to allow initialising joint commands with robot starting state" This reverts commit 2b23e6c9279c7c43c576443b6f9b208d66500ae6. --- .../topic_based_system.hpp | 2 - src/topic_based_system.cpp | 46 +++++-------------- 2 files changed, 12 insertions(+), 36 deletions(-) diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index 9fd6c41..a31fccd 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -71,8 +71,6 @@ class TopicBasedSystem : public hardware_interface::SystemInterface rclcpp::Node::SharedPtr node_; sensor_msgs::msg::JointState latest_joint_state_; bool sum_wrapped_joint_states_{ false }; - bool initial_states_as_initial_cmd_{ false }; - bool ready_to_send_cmds_{ false }; bool initial_cmd_reached_{ false }; /// Use standard interfaces for joints because they are relevant for dynamic behavior diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index 9037639..70a3bc4 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -98,7 +98,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& } } } - ready_to_send_cmds_ = true; // Search for mimic joints for (auto i = 0u; i < info_.joints.size(); ++i) @@ -161,11 +160,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& { sum_wrapped_joint_states_ = true; } - if (get_hardware_parameter("use_initial_states_as_initial_commands", "false") == "true") - { - initial_states_as_initial_cmd_ = true; - ready_to_send_cmds_ = false; - } if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false") { initial_cmd_reached_ = true; @@ -258,18 +252,6 @@ hardware_interface::return_type TopicBasedSystem::read(const rclcpp::Time& /*tim } } - if (!ready_to_send_cmds_ && initial_states_as_initial_cmd_) - { - for (std::size_t i = 0; i < joint_states_.size(); ++i) - { - for (std::size_t j = 0; j < joint_states_[i].size(); ++j) - { - joint_commands_[i][j] = joint_states_[i][j]; - } - } - ready_to_send_cmds_ = true; - } - return hardware_interface::return_type::OK; } @@ -290,17 +272,12 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string& hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (!ready_to_send_cmds_) - { - return hardware_interface::return_type::ERROR; - } - std::vector> target_joint_commands; if (initial_cmd_reached_ == false) { - double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, [](double sum, double val) { - return sum + std::abs(val); - }); + double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), + joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, + [](double sum, double val) { return sum + std::abs(val); }); if (abs_sum >= trigger_joint_command_threshold_) { initial_cmd_reached_ = true; @@ -322,7 +299,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); bool exist_velocity_command = false; - static bool exist_velocity_command_old = false; // Use old state to publish zero velocities + static bool exist_velocity_command_old = false; // Use old state to publish zero velocities for (std::size_t i = 0; i < info_.joints.size(); ++i) { if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) @@ -331,11 +308,12 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti } } - if (diff <= trigger_joint_command_threshold_ && (exist_velocity_command == false && exist_velocity_command_old == false)) + if (diff <= trigger_joint_command_threshold_ && + (exist_velocity_command == false && exist_velocity_command_old == false)) { return hardware_interface::return_type::OK; } - + exist_velocity_command_old = exist_velocity_command; // For Position Joint @@ -366,7 +344,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (joint_state.name[index] == mimic_joint.mimicked_joint_name) { joint_state.name.push_back(mimic_joint.joint_name); - joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]); + joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]); } } } @@ -378,7 +356,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti topic_based_joint_commands_publisher_->publish(joint_state); } } - + // For Velocity Joint { sensor_msgs::msg::JointState joint_state; @@ -407,7 +385,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (joint_state.name[index] == mimic_joint.mimicked_joint_name) { joint_state.name.push_back(mimic_joint.joint_name); - joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]); + joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]); } } } @@ -419,7 +397,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti topic_based_joint_commands_publisher_->publish(joint_state); } } - + // For Effort Joint { sensor_msgs::msg::JointState joint_state; @@ -448,7 +426,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (joint_state.name[index] == mimic_joint.mimicked_joint_name) { joint_state.name.push_back(mimic_joint.joint_name); - joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]); + joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]); } } } From 353dcaeff5b7d9de62d672bf1cfc8c7abef4bd02 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 31 Oct 2024 16:52:48 +0900 Subject: [PATCH 08/14] [fix] Modifications to pass test --- src/topic_based_system.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index 70a3bc4..91fa656 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -93,6 +93,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& if (!interface.initial_value.empty()) { joint_states_[index][i] = std::stod(interface.initial_value); + joint_commands_[index][i] = std::stod(interface.initial_value); initial_joint_commands_[index][i] = std::stod(interface.initial_value); } } From 7050f7280ee87381427b92dea7571a2f17324636 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 31 Oct 2024 16:54:35 +0900 Subject: [PATCH 09/14] [add] Ignore test temporary files --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..40776c2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +test/__pycache__/ From 99faf3aa1f95b858670cf943da44ed4e267c4eff Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 31 Oct 2024 16:56:18 +0900 Subject: [PATCH 10/14] [add] add diffbot_with_rrr to test velocity and effort joint --- test/control.launch.py | 4 +- test/diffbot_with_rrr.urdf.xacro | 275 +++++++++++++++++++++++++++++++ test/ros2_controllers.yaml | 52 ++++++ 3 files changed, 329 insertions(+), 2 deletions(-) create mode 100755 test/diffbot_with_rrr.urdf.xacro diff --git a/test/control.launch.py b/test/control.launch.py index 7b44508..ab33e2e 100644 --- a/test/control.launch.py +++ b/test/control.launch.py @@ -41,9 +41,9 @@ def generate_launch_description(): ros2_controllers_file = Path(SCRIPT_PATH / "ros2_controllers.yaml") robot_description = { - "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), + "robot_description": xacro.process_file(SCRIPT_PATH / "diffbot_with_rrr.urdf.xacro").toxml(), } - controllers = ["joint_state_broadcaster", "joint_trajectory_controller"] + controllers = ["joint_state_broadcaster", "joint_trajectory_controller", "diff_drive_controller", "effort_controller"] return LaunchDescription( [ Node( diff --git a/test/diffbot_with_rrr.urdf.xacro b/test/diffbot_with_rrr.urdf.xacro new file mode 100755 index 0000000..1932667 --- /dev/null +++ b/test/diffbot_with_rrr.urdf.xacro @@ -0,0 +1,275 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 50 + + + hardware_interface/VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + topic_based_ros2_control/TopicBasedSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + + + + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + + + + diff --git a/test/ros2_controllers.yaml b/test/ros2_controllers.yaml index acdf09c..708b714 100644 --- a/test/ros2_controllers.yaml +++ b/test/ros2_controllers.yaml @@ -5,6 +5,10 @@ controller_manager: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + effort_controller: + type: effort_controllers/JointGroupEffortController joint_trajectory_controller: ros__parameters: @@ -23,3 +27,51 @@ joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.0 goal_time: 0.0 + +diff_drive_controller: + ros__parameters: + left_wheel_names : ['left_wheel_joint'] + right_wheel_names : ['right_wheel_joint'] + + wheel_separation : 0.46 + wheel_radius : 0.19 + wheel_separation_multiplier : 1.0 + left_wheel_radius_multiplier : 1.0 + right_wheel_radius_multiplier : 1.0 + + odom_frame_id: odom + base_frame_id: base_link + + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + enable_odom_tf: true + + cmd_vel_timeout: 3.0 + publish_limited_velocity: true + velocity_rolling_window_size: 10 + + # limits + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: false + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 0.4 + linear.x.min_acceleration: -0.4 + linear.x.max_jerk: 0.5 + linear.x.min_jerk: -0.5 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: false + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.5 + angular.z.min_velocity: -1.5 + angular.z.max_acceleration: 0.8 + angular.z.min_acceleration: -0.8 + angular.z.max_jerk: 0.5 + angular.z.min_jerk: -0.5 + +effort_controller: + ros__parameters: + joints: ['rrr_base_joint'] From 5d894a3c9324d892eb00513c846863226d7035f9 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 31 Oct 2024 16:58:29 +0900 Subject: [PATCH 11/14] [add] add velocity and effort test --- test/test_topic_based_robot.py | 20 +++- test/topic_based_robot.py | 182 +++++++++++++++++++++++++++++---- 2 files changed, 180 insertions(+), 22 deletions(-) diff --git a/test/test_topic_based_robot.py b/test/test_topic_based_robot.py index 7f2b600..e5e7339 100755 --- a/test/test_topic_based_robot.py +++ b/test/test_topic_based_robot.py @@ -35,16 +35,28 @@ rclpy.init() -robot = TopicBasedRobot(["joint_1", "joint_2", "joint_3"]) +robot = TopicBasedRobot(["joint_1", "joint_2", "joint_3"], ["left_wheel_joint", "right_wheel_joint"], ["rrr_base_joint"]) # By default the joint_states should have the values from initial_value from rrr.urdf.xacro -current_joint_state = robot.get_current_joint_state() +current_joint_state = robot.get_current_position_joint_state() urdf_initial_values = [0.2, 0.3, 0.1] assert ( current_joint_state == urdf_initial_values ), f"{current_joint_state=} != {urdf_initial_values=}" -# Test setting the robot joint states +# Test setting the robot joint position states joint_state = [0.1, 0.2, 0.3] robot.set_joint_positions(joint_state) -current_joint_state = robot.get_current_joint_state() +current_joint_state = robot.get_current_position_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" + +# Test setting the robot joint velocity states +joint_state = [0.4, 0.5] +robot.set_joint_velocities(joint_state) +current_joint_state = robot.get_current_velocity_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" + +# Test setting the robot joint effort states +joint_state = [0.6] +robot.set_joint_efforts(joint_state) +current_joint_state = robot.get_current_effort_joint_state() assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" diff --git a/test/topic_based_robot.py b/test/topic_based_robot.py index fc33a83..cefa66b 100644 --- a/test/topic_based_robot.py +++ b/test/topic_based_robot.py @@ -42,9 +42,11 @@ class TopicBasedRobot(Node): """Topic Based Robot to test topic_based_ros2_control interface""" - def __init__(self, joint_names: list[str]) -> None: + def __init__(self, position_joint_names: list[str], velocity_joint_names = [], effort_joint_names = []) -> None: super().__init__("topic_based_robot") - self.joint_names = joint_names.copy() + self.position_joint_names = position_joint_names.copy() + self.velocity_joint_names = velocity_joint_names.copy() + self.effort_joint_names = effort_joint_names.copy() self.last_joint_command = [] self.current_joint_state = [] self.callback_group = ReentrantCallbackGroup() @@ -72,46 +74,154 @@ def __init__(self, joint_names: list[str]) -> None: callback_group=self.callback_group, ) - def filter_joint_state_msg(self, msg: JointState): + def filter_joint_position_state_msg(self, msg: JointState): joint_states = [] - for joint_name in self.joint_names: + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.position_joint_names: try: index = msg.name.index(joint_name) except ValueError: - msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - raise ValueError(msg) from None + continue + # + # Commented out because the joints that the message outputs were sometimes missing + # + #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + #raise ValueError(msg) from None joint_states.append(msg.position[index]) return joint_states + def filter_joint_velocity_state_msg(self, msg: JointState): + joint_states = [] + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.velocity_joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + continue + # + # Commented out because the joints that the message outputs were sometimes missing + # + #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + #raise ValueError(msg) from None + joint_states.append(msg.velocity[index]) + return joint_states + + def filter_joint_effort_state_msg(self, msg: JointState): + joint_states = [] + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.effort_joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + continue + # + # Commented out because the joints that the message outputs were sometimes missing + # + #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + #raise ValueError(msg) from None + joint_states.append(msg.effort[index]) + return joint_states + def command_callback(self, msg: JointState): - self.last_joint_command = self.filter_joint_state_msg(msg) + # Output by position, velocity, and force, so the number of joint names of each type is matched + if self.position_joint_names[0] in msg.name: + assert len(self.position_joint_names) == len(msg.name), f"{self.position_joint_names=} != {msg.name=}" + self.last_position_joint_command = self.filter_joint_position_state_msg(msg) + + if not len(self.velocity_joint_names) == 0: + if self.velocity_joint_names[0] in msg.name: + assert len(self.velocity_joint_names) == len(msg.name), f"{self.velocity_joint_names=} != {msg.name=}" + self.last_velocity_joint_command = self.filter_joint_velocity_state_msg(msg) + + if not len(self.effort_joint_names) == 0: + if self.effort_joint_names[0] in msg.name: + assert len(self.effort_joint_names) == len(msg.name), f"{self.effort_joint_names=} != {msg.name=}" + self.last_effort_joint_command = self.filter_joint_effort_state_msg(msg) def joint_states_callback(self, msg: JointState): - self.current_joint_state = self.filter_joint_state_msg(msg) + self.current_position_joint_state = self.filter_joint_position_state_msg(msg) + if not len(self.velocity_joint_names) == 0: + self.current_velocity_joint_state = self.filter_joint_velocity_state_msg(msg) + if not len(self.effort_joint_names) == 0: + self.current_effort_joint_state = self.filter_joint_effort_state_msg(msg) - def get_current_joint_command(self) -> OrderedDict[str, float]: + def get_current_position_joint_command(self) -> OrderedDict[str, float]: """Get the last joint command sent to the robot.""" - self.last_joint_command = [] - while len(self.last_joint_command) == 0: + self.last_position_joint_command = [] + while len(self.last_position_joint_command) == 0: self.get_logger().warning( f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", throttle_duration_sec=2.0, skip_first=True, ) rclpy.spin_once(self, timeout_sec=0.0) - return self.last_joint_command + return self.last_position_joint_command - def get_current_joint_state(self) -> OrderedDict[str, float]: + def get_current_velocity_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_velocity_joint_command = [] + while len(self.last_velocity_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_velocity_joint_command + + def get_current_effort_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_effort_joint_command = [] + while len(self.last_effort_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_effort_joint_command + + def get_current_position_joint_state(self) -> OrderedDict[str, float]: """Get the current joint state reported by ros2_control on joint_states topic.""" - self.current_joint_state = [] - while len(self.current_joint_state) == 0: + self.current_position_joint_state = [] + while len(self.current_position_joint_state) == 0: self.get_logger().warning( f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", throttle_duration_sec=2.0, skip_first=True, ) rclpy.spin_once(self, timeout_sec=0.0) - return self.current_joint_state + return self.current_position_joint_state + + def get_current_velocity_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_velocity_joint_state = [] + while len(self.current_velocity_joint_state) == 0: + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.current_velocity_joint_state + + def get_current_effort_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_effort_joint_state = [] + while len(self.current_effort_joint_state) == 0: + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.current_effort_joint_state def set_joint_positions( self, @@ -120,13 +230,49 @@ def set_joint_positions( """Set the joint positions of the robot.""" self.actual_joint_state_publisher.publish( JointState( - name=list(self.joint_names), + name=list(self.position_joint_names), position=joint_positions, ), ) while not np.allclose( - self.get_current_joint_state(), + self.get_current_position_joint_state(), joint_positions, atol=1e-3, ): rclpy.spin_once(self, timeout_sec=0.0) + + def set_joint_velocities( + self, + joint_velocities: list[float] | np.ndarray, + ) -> None: + """Set the joint velocities of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.velocity_joint_names), + velocity=joint_velocities, + ), + ) + while not np.allclose( + self.get_current_velocity_joint_state(), + joint_velocities, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0) + + def set_joint_efforts( + self, + joint_efforts: list[float] | np.ndarray, + ) -> None: + """Set the joint efforts of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.effort_joint_names), + effort=joint_efforts, + ), + ) + while not np.allclose( + self.get_current_effort_joint_state(), + joint_efforts, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0) From dd83d5aab2b405f2877e446cf11fe4bbfe465618 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 31 Oct 2024 17:04:41 +0900 Subject: [PATCH 12/14] [fix] Tested with Isaac Sim and confirmed that it was an unnecessary process, so it was removed.Tested with Isaac Sim and removed as it was an unnecessary process. --- .../topic_based_system.hpp | 2 -- src/topic_based_system.cpp | 35 +++---------------- 2 files changed, 5 insertions(+), 32 deletions(-) diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index a31fccd..f300eb0 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -71,7 +71,6 @@ class TopicBasedSystem : public hardware_interface::SystemInterface rclcpp::Node::SharedPtr node_; sensor_msgs::msg::JointState latest_joint_state_; bool sum_wrapped_joint_states_{ false }; - bool initial_cmd_reached_{ false }; /// Use standard interfaces for joints because they are relevant for dynamic behavior std::array standard_interfaces_ = { hardware_interface::HW_IF_POSITION, @@ -92,7 +91,6 @@ class TopicBasedSystem : public hardware_interface::SystemInterface /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; - std::vector> initial_joint_commands_; // If the difference between the current joint state and joint command is less than this value, // the joint command will not be published. diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index 91fa656..274765b 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -78,7 +78,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& } // Initial command values - initial_joint_commands_ = joint_commands_; for (auto i = 0u; i < info_.joints.size(); i++) { const auto& component = info_.joints[i]; @@ -94,7 +93,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& { joint_states_[index][i] = std::stod(interface.initial_value); joint_commands_[index][i] = std::stod(interface.initial_value); - initial_joint_commands_[index][i] = std::stod(interface.initial_value); } } } @@ -161,10 +159,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& { sum_wrapped_joint_states_ = true; } - if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false") - { - initial_cmd_reached_ = true; - } return CallbackReturn::SUCCESS; } @@ -273,37 +267,18 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string& hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - std::vector> target_joint_commands; - if (initial_cmd_reached_ == false) - { - double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), - joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, - [](double sum, double val) { return sum + std::abs(val); }); - if (abs_sum >= trigger_joint_command_threshold_) - { - initial_cmd_reached_ = true; - } - } - if (initial_cmd_reached_ == false) - { - target_joint_commands = initial_joint_commands_; - } - else - { - target_joint_commands = joint_commands_; - } // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. const auto diff = std::transform_reduce( joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), - target_joint_commands[POSITION_INTERFACE_INDEX].cbegin(), 0.0, + joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); bool exist_velocity_command = false; static bool exist_velocity_command_old = false; // Use old state to publish zero velocities for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) + if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) { exist_velocity_command = true; } @@ -329,7 +304,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_POSITION) { joint_state.name.push_back(info_.joints[i].name); - joint_state.position.push_back(target_joint_commands[POSITION_INTERFACE_INDEX][i]); + joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); } } } @@ -370,7 +345,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_VELOCITY) { joint_state.name.push_back(info_.joints[i].name); - joint_state.velocity.push_back(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]); + joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); } } } @@ -411,7 +386,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti if (interface.name == hardware_interface::HW_IF_EFFORT) { joint_state.name.push_back(info_.joints[i].name); - joint_state.effort.push_back(target_joint_commands[EFFORT_INTERFACE_INDEX][i]); + joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); } } } From c50453419034deeb6dcdf7e4cca938a1af3424a6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 21 Apr 2025 13:55:49 +0100 Subject: [PATCH 13/14] Remove rrr.urdf.xacro --- test/rrr.urdf.xacro | 140 -------------------------------------------- 1 file changed, 140 deletions(-) delete mode 100644 test/rrr.urdf.xacro diff --git a/test/rrr.urdf.xacro b/test/rrr.urdf.xacro deleted file mode 100644 index 26a9c13..0000000 --- a/test/rrr.urdf.xacro +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - topic_based_ros2_control/TopicBasedSystem - /topic_based_joint_commands - /topic_based_joint_states - - - - - 0.2 - - - - - - - 0.3 - - - - - - - 0.1 - - - - - From 5cf7dacf4a9ac246f8ccf96ea5271fee2d7c1724 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 21 Apr 2025 13:57:07 +0100 Subject: [PATCH 14/14] Remove commented out code --- test/topic_based_robot.py | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/test/topic_based_robot.py b/test/topic_based_robot.py index cefa66b..9267160 100644 --- a/test/topic_based_robot.py +++ b/test/topic_based_robot.py @@ -84,11 +84,6 @@ def filter_joint_position_state_msg(self, msg: JointState): index = msg.name.index(joint_name) except ValueError: continue - # - # Commented out because the joints that the message outputs were sometimes missing - # - #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - #raise ValueError(msg) from None joint_states.append(msg.position[index]) return joint_states @@ -102,11 +97,6 @@ def filter_joint_velocity_state_msg(self, msg: JointState): index = msg.name.index(joint_name) except ValueError: continue - # - # Commented out because the joints that the message outputs were sometimes missing - # - #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - #raise ValueError(msg) from None joint_states.append(msg.velocity[index]) return joint_states @@ -120,11 +110,6 @@ def filter_joint_effort_state_msg(self, msg: JointState): index = msg.name.index(joint_name) except ValueError: continue - # - # Commented out because the joints that the message outputs were sometimes missing - # - #msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - #raise ValueError(msg) from None joint_states.append(msg.effort[index]) return joint_states