diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index cf10e60..516fbab 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -297,9 +297,9 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti 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 +321,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 +330,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 +366,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 +378,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 +407,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 +419,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 +448,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]); } } }