From c6029294fdc0c89937a842baab7e4a0543111db5 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Fri, 31 May 2024 10:10:46 +0900 Subject: [PATCH 1/7] [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 ee506dd..48d395d 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -113,6 +113,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)); @@ -270,64 +272,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 c8c3165403575d028b3fe4f46b911f8d637b58f3 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 10 Jun 2024 19:28:55 +0900 Subject: [PATCH 2/7] [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 48d395d..4eef9eb 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -359,7 +359,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]); } } } @@ -405,7 +405,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 82ae31fdd8805dcb6dfa1522018fcce69c1ad7f2 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 07:14:10 +0900 Subject: [PATCH 3/7] [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 4eef9eb..383a3a5 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -386,11 +386,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 2b23e6c9279c7c43c576443b6f9b208d66500ae6 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 07:19:24 +0900 Subject: [PATCH 4/7] 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 383a3a5..7b7da0a 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -96,6 +96,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) @@ -158,6 +159,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; } @@ -246,6 +252,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; } @@ -266,6 +284,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 6dccdc0ac8fec2fd7106b07f4b29e751b5cf096a Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Mon, 5 Aug 2024 23:11:41 +0900 Subject: [PATCH 5/7] [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 7b7da0a..cc39f6f 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]; @@ -91,7 +92,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& // Check the initial_value param is used if (!interface.initial_value.empty()) { - joint_commands_[index][i] = std::stod(interface.initial_value); + initial_joint_commands_[index][i] = std::stod(interface.initial_value); } } } @@ -164,6 +165,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; } @@ -288,6 +293,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 ffa421f3aa46b3697f1ccc043cb4d3d44ee19571 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Thu, 5 Sep 2024 23:38:06 +0900 Subject: [PATCH 6/7] [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 cc39f6f..cf10e60 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -294,33 +294,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; } @@ -345,7 +349,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]); } } } @@ -386,7 +390,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]); } } } @@ -427,7 +431,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 ab71778725cbe4ba86ecb0de9df972fbe11ab141 Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Tue, 8 Oct 2024 23:03:12 +0900 Subject: [PATCH 7/7] 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 cf10e60..bb3d93a 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -97,7 +97,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) @@ -160,11 +159,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; @@ -257,18 +251,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; } @@ -289,17 +271,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; @@ -321,7 +298,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_) @@ -330,11 +307,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 @@ -365,7 +343,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]); } } } @@ -377,7 +355,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; @@ -406,7 +384,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]); } } } @@ -418,7 +396,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; @@ -447,7 +425,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]); } } }