From 8bce3a17ae2d64419daf82d6a3c57a961022b602 Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Thu, 6 Jun 2024 15:25:36 +0200 Subject: [PATCH 1/6] add MyActuator status broadcaster --- .../launch/myactuator_rmd_control.launch.py | 6 + .../CMakeLists.txt | 75 ++++ .../myactuator_rmd_state_broadcaster/LICENSE | 17 + .../myactuator_rmd_state_broadcaster.hpp | 71 ++++ .../visibility_control.h | 35 ++ ...actuator_rmd_state_broadcaster_plugins.xml | 9 + .../package.xml | 29 ++ .../src/myactuator_rmd_state_broadcaster.cpp | 327 ++++++++++++++++++ ...ator_rmd_state_broadcaster_parameters.yaml | 28 ++ .../config/myactuator_rmd_controllers.yaml | 3 + .../myactuator_rmd_hardware_interface.hpp | 12 + .../src/myactuator_rmd_hardware_interface.cpp | 42 +++ myactuator_rmd_interfaces/CMakeLists.txt | 21 ++ myactuator_rmd_interfaces/LICENSE | 17 + .../msg/CurrentPhase.msg | 2 + myactuator_rmd_interfaces/msg/MotorStatus.msg | 8 + myactuator_rmd_interfaces/package.xml | 22 ++ 17 files changed, 724 insertions(+) create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/CMakeLists.txt create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/LICENSE create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster.hpp create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/visibility_control.h create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster_plugins.xml create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/package.xml create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp create mode 100644 myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster_parameters.yaml create mode 100644 myactuator_rmd_interfaces/CMakeLists.txt create mode 100644 myactuator_rmd_interfaces/LICENSE create mode 100644 myactuator_rmd_interfaces/msg/CurrentPhase.msg create mode 100644 myactuator_rmd_interfaces/msg/MotorStatus.msg create mode 100644 myactuator_rmd_interfaces/package.xml diff --git a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py index 45b7eec..30e2c1f 100644 --- a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py +++ b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py @@ -116,6 +116,11 @@ def generate_launch_description(): executable='spawner', arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'] ) + myactuator_rmd_state_broadcaster_spawner_node = Node( + package='controller_manager', + executable='spawner', + arguments=['myactuator_rmd_state_broadcaster', '--controller-manager', '/controller_manager'] + ) controllers = PathJoinSubstitution( [ get_package_share_directory('myactuator_rmd_description'), @@ -190,6 +195,7 @@ def generate_launch_description(): ld.add_action(xacro_file_parameter_cmd) ld.add_action(description_launch) ld.add_action(joint_state_broadcaster_spawner_node) + ld.add_action(myactuator_rmd_state_broadcaster_spawner_node) ld.add_action(controller_manager_node) ld.add_action(controller_spawner_node) ld.add_action(gazebo_node) diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/CMakeLists.txt b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000..f54bd5d --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.8) +project(myactuator_rmd_state_broadcaster) + + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(myactuator_rmd_interfaces REQUIRED) + + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + myactuator_rmd_interfaces +) + +generate_parameter_library(${PROJECT_NAME}_parameters + src/${PROJECT_NAME}_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp + +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + ${PROJECT_NAME}_parameters +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "MYACTUATOR_RMD_BROADCASTER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface ${PROJECT_NAME}_plugins.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS + ${PROJECT_NAME} + ${PROJECT_NAME}_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/LICENSE b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster.hpp b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster.hpp new file mode 100644 index 0000000..fc99bbd --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster.hpp @@ -0,0 +1,71 @@ +#ifndef MYACTUATOR_RMD_STATE_BROADCASTER__MYACTUATOR_RMD_STATE_BROADCASTER_HPP_ +#define MYACTUATOR_RMD_STATE_BROADCASTER__MYACTUATOR_RMD_STATE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "myactuator_rmd_state_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library +#include "myactuator_rmd_state_broadcaster_parameters.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "myactuator_rmd_interfaces/msg/motor_status.hpp" +#include "semantic_components/semantic_component_interface.hpp" + +namespace myactuator_rmd_state_broadcaster +{ +class MyActuatorRmdStateBroadcaster : public controller_interface::ControllerInterface +{ +public: + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + MyActuatorRmdStateBroadcaster(); + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + bool init_motor_data(); + void init_motor_state_msg(); + bool use_all_available_interfaces() const; + +protected: + // Optional parameters + std::shared_ptr param_listener_; + Params params_; + std::unordered_map map_interface_to_motor_state_; + std::unordered_map> name_if_value_mapping_; + std::vector motor_names_; + + std::shared_ptr> motor_state_publisher_; + std::shared_ptr> realtime_motor_state_publisher_; +}; + +} // namespace myactuator_rmd_state_broadcaster + +#endif // MYACTUATOR_RMD_STATE_BROADCASTER__MYACTUATOR_RMD_STATE_BROADCASTER_HPP_ \ No newline at end of file diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/visibility_control.h b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/visibility_control.h new file mode 100644 index 0000000..52ecf99 --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/include/myactuator_rmd_state_broadcaster/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef MYACTUATOR_RMD_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define MYACTUATOR_RMD_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MYACTUATOR_RMD_STATE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define MYACTUATOR_RMD_STATE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define MYACTUATOR_RMD_STATE_BROADCASTER_EXPORT __declspec(dllexport) +#define MYACTUATOR_RMD_STATE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef MYACTUATOR_RMD_STATE_BROADCASTER_BUILDING_DLL +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC MYACTUATOR_RMD_STATE_BROADCASTER_EXPORT +#else +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC MYACTUATOR_RMD_STATE_BROADCASTER_IMPORT +#endif +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC_TYPE MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC +#define MYACTUATOR_RMD_STATE_BROADCASTER_LOCAL +#else +#define MYACTUATOR_RMD_STATE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define MYACTUATOR_RMD_STATE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define MYACTUATOR_RMD_STATE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC +#define MYACTUATOR_RMD_STATE_BROADCASTER_LOCAL +#endif +#define MYACTUATOR_RMD_STATE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // MYACTUATOR_RMD_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster_plugins.xml b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster_plugins.xml new file mode 100644 index 0000000..ebb74dd --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster_plugins.xml @@ -0,0 +1,9 @@ + + + + ros2_control status broadcaster for the MyActuator RMD-X-series + + + \ No newline at end of file diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/package.xml b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/package.xml new file mode 100644 index 0000000..f4fba0f --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/package.xml @@ -0,0 +1,29 @@ + + + + myactuator_rmd_state_broadcaster + 0.0.0 + ros2_control status broadcaster for the MyActuator RMD-X-series + ORCIL Lionel + MIT + + ament_cmake + + myactuator_rmd_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + generate_parameter_library + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp new file mode 100644 index 0000000..db6b2be --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp @@ -0,0 +1,327 @@ +#include "myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcpputils/split.hpp" +#include "rcutils/logging_macros.h" +#include "std_msgs/msg/header.hpp" + +namespace rclcpp_lifecycle +{ +class State; +} // namespace rclcpp_lifecycle + +namespace myactuator_rmd_state_broadcaster +{ +MyActuatorRmdStateBroadcaster::MyActuatorRmdStateBroadcaster() { +} + +controller_interface::CallbackReturn MyActuatorRmdStateBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +MyActuatorRmdStateBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration MyActuatorRmdStateBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (use_all_available_interfaces()) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::ALL; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & motor : params_.motors) + { + for (const auto & interface : params_.interfaces) + { + state_interfaces_config.names.push_back(motor + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn MyActuatorRmdStateBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (use_all_available_interfaces()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "'motors' or 'interfaces' parameter is empty. " + "All available state interfaces will be published"); + params_.motors.clear(); + params_.interfaces.clear(); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), + "Publishing state interfaces defined in 'motors' and 'interfaces' parameters."); + } + + try + { + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; + + motor_state_publisher_ = get_node()->create_publisher( + topic_name_prefix + "myactuator_rmd_states", rclcpp::SystemDefaultsQoS()); + + realtime_motor_state_publisher_ = + std::make_shared>( + motor_state_publisher_); + } + catch (const std::exception & e) + { + // get_node() may throw, logging raw here + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MyActuatorRmdStateBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!init_motor_data()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "None of requested interfaces exist. Controller will not run."); + return CallbackReturn::ERROR; + } + + init_motor_state_msg(); + + if ( + !use_all_available_interfaces() && + state_interfaces_.size() != (params_.motors.size() * params_.interfaces.size())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Not all requested interfaces exists. " + "Check ControllerManager output for more detailed information."); + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MyActuatorRmdStateBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + motor_names_.clear(); + + return CallbackReturn::SUCCESS; +} + +template +bool has_any_key( + const std::unordered_map & map, const std::vector & keys) +{ + bool found_key = false; + for (const auto & key_item : map) + { + const auto & key = key_item.first; + if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) + { + found_key = true; + break; + } + } + return found_key; +} + +bool MyActuatorRmdStateBroadcaster::init_motor_data() +{ + motor_names_.clear(); + if (state_interfaces_.empty()) + { + return false; + } + + // loop in reverse order, this maintains the order of values at retrieval time + for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) + { + // initialize map if name is new + if (name_if_value_mapping_.count(si->get_prefix_name()) == 0) + { + name_if_value_mapping_[si->get_prefix_name()] = {}; + } + // add interface name + std::string interface_name = si->get_interface_name(); + name_if_value_mapping_[si->get_prefix_name()][interface_name] = std::numeric_limits::quiet_NaN(); + } + + // filter state interfaces that have at least one of the motor_states fields, + // the rest will be ignored for this message + for (const auto & name_ifv : name_if_value_mapping_) + { + const auto & interfaces_and_values = name_ifv.second; + if (has_any_key(interfaces_and_values, {"rmd_error", + "rmd_temperature", + "rmd_brake", + "rmd_voltage", + "rmd_current", + "rmd_phase_a_current", + "rmd_phase_b_current", + "rmd_phase_c_current"})) + { + motor_names_.push_back(name_ifv.first); + } + } + + // Add extra motors from parameters, each motor will be added to motor_names_ and + // name_if_value_mapping_ if it is not already there + rclcpp::Parameter extra_motors; + if (get_node()->get_parameter("extra_motors", extra_motors)) + { + const std::vector & extra_motors_names = extra_motors.as_string_array(); + for (const auto & extra_motor_name : extra_motors_names) + { + if (name_if_value_mapping_.count(extra_motor_name) == 0) + { + name_if_value_mapping_[extra_motor_name] = { + {"rmd_error", 0.0}, + {"rmd_temperature", 0.0}, + {"rmd_brake", 0.0}, + {"rmd_voltage", 0.0}, + {"rmd_current", 0.0}, + {"rmd_phase_a_current", 0.0}, + {"rmd_phase_b_current", 0.0}, + {"rmd_phase_c_current", 0.0}}; + motor_names_.push_back(extra_motor_name); + } + } + } + return true; +} + +void MyActuatorRmdStateBroadcaster::init_motor_state_msg() +{ + const size_t num_motors = motor_names_.size(); + + // default initialization for motor state message + auto & motor_state_msg = realtime_motor_state_publisher_->msg_; + motor_state_msg.name = motor_names_; + motor_state_msg.voltage.resize(num_motors, std::numeric_limits::quiet_NaN()); + motor_state_msg.current.resize(num_motors, std::numeric_limits::quiet_NaN()); + motor_state_msg.temperature.resize(num_motors, std::numeric_limits::quiet_NaN()); + motor_state_msg.error_code.resize(num_motors, std::numeric_limits::quiet_NaN()); + motor_state_msg.brake.resize(num_motors, false); + motor_state_msg.current_phases.resize(num_motors); + for (size_t i = 0; i < num_motors; ++i) { + motor_state_msg.current_phases[i].name.resize(3); + motor_state_msg.current_phases[i].name[0] = "a"; + motor_state_msg.current_phases[i].name[1] = "b"; + motor_state_msg.current_phases[i].name[2] = "c"; + motor_state_msg.current_phases[i].current.resize(3); + motor_state_msg.current_phases[i].current[0] = std::numeric_limits::quiet_NaN(); + motor_state_msg.current_phases[i].current[1] = std::numeric_limits::quiet_NaN(); + motor_state_msg.current_phases[i].current[2] = std::numeric_limits::quiet_NaN(); + } +} + +bool MyActuatorRmdStateBroadcaster::use_all_available_interfaces() const +{ + return params_.motors.empty() || params_.interfaces.empty(); +} + +double get_value( + const std::unordered_map> & map, + const std::string & name, const std::string & interface_name) +{ + const auto & interfaces_and_values = map.at(name); + const auto interface_and_value = interfaces_and_values.find(interface_name); + if (interface_and_value != interfaces_and_values.cend()) + { + return interface_and_value->second; + } + else + { + return std::numeric_limits::quiet_NaN(); + } +} + +controller_interface::return_type MyActuatorRmdStateBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + for (const auto & state_interface : state_interfaces_) + { + std::string interface_name = state_interface.get_interface_name(); + name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = + state_interface.get_value(); + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), + state_interface.get_value()); + } + + if (realtime_motor_state_publisher_ && realtime_motor_state_publisher_->trylock()) + { + auto & motor_state_msg = realtime_motor_state_publisher_->msg_; + + motor_state_msg.header.stamp = time; + + // update motor state message + for (size_t i = 0; i < motor_names_.size(); ++i) + { + motor_state_msg.voltage[i] = get_value(name_if_value_mapping_, motor_names_[i], "rmd_voltage"); + motor_state_msg.current[i] = get_value(name_if_value_mapping_, motor_names_[i], "rmd_current"); + motor_state_msg.temperature[i] = static_cast(get_value(name_if_value_mapping_, motor_names_[i], "rmd_temperature")); + motor_state_msg.error_code[i] = static_cast(get_value(name_if_value_mapping_, motor_names_[i], "rmd_error_code")); + motor_state_msg.brake[i] = static_cast(get_value(name_if_value_mapping_, motor_names_[i], "rmd_brake")); + motor_state_msg.current_phases[i].current[0] = get_value(name_if_value_mapping_, motor_names_[i], "rmd_current_phase_a"); + motor_state_msg.current_phases[i].current[1] = get_value(name_if_value_mapping_, motor_names_[i], "rmd_current_phase_b"); + motor_state_msg.current_phases[i].current[2] = get_value(name_if_value_mapping_, motor_names_[i], "rmd_current_phase_c"); + } + realtime_motor_state_publisher_->unlockAndPublish(); + } + + + return controller_interface::return_type::OK; +} + +} // namespace myactuator_rmd_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + myactuator_rmd_state_broadcaster::MyActuatorRmdStateBroadcaster, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster_parameters.yaml b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster_parameters.yaml new file mode 100644 index 0000000..eaeaa5a --- /dev/null +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster_parameters.yaml @@ -0,0 +1,28 @@ +myactuator_rmd_state_broadcaster: + use_local_topics: { + type: bool, + default_value: false, + description: "Defining if ``motor_states`` and ``dynamic_motor_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/motor_states``." + } + motors: { + type: string_array, + default_value: [], + description: "Parameter to support broadcasting of only specific motors and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + If either ``motors`` or ``interfaces`` is left empty, all available state interfaces will be + published. + Joint state broadcaster asks for access to all defined interfaces on all defined motors." + } + extra_motors: { + type: string_array, + default_value: [], + description: "Names of extra motors to be added to ``motor_states`` and ``dynamic_motor_states`` with state set to 0." + } + interfaces: { + type: string_array, + default_value: [], + description: "Parameter to support broadcasting of only specific motors and interfaces. + It has to be used in combination with the ``motors`` parameter. + If either ``motors`` or ``interfaces`` is left empty, all available state interfaces will be + published." + } \ No newline at end of file diff --git a/myactuator_rmd_description/config/myactuator_rmd_controllers.yaml b/myactuator_rmd_description/config/myactuator_rmd_controllers.yaml index c934da0..ea57d11 100644 --- a/myactuator_rmd_description/config/myactuator_rmd_controllers.yaml +++ b/myactuator_rmd_description/config/myactuator_rmd_controllers.yaml @@ -4,6 +4,9 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + + myactuator_rmd_state_broadcaster: + type: myactuator_rmd_state_broadcaster/MyActuatorRmdStateBroadcaster effort_controller: type: forward_command_controller/ForwardCommandController diff --git a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp index 66bc2dd..c0ac3d0 100755 --- a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp +++ b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp @@ -262,6 +262,16 @@ namespace myactuator_rmd_hardware { double velocity_command_; double effort_command_; + // Buffer status Actuator + double error_code_state_; + double temperature_state_; + double brake_state_; + double voltage_state_; + double current_state_; + double current_phase_a_state_; + double current_phase_b_state_; + double current_phase_c_state_; + // The command thread reads and writes from the actuator cyclically std::thread command_thread_; std::chrono::milliseconds cycle_time_; @@ -271,6 +281,8 @@ namespace myactuator_rmd_hardware { // Shared between the two threads std::atomic stop_command_thread_; std::atomic feedback_; + std::atomicmotor_status1_; + std::atomicmotor_status3_; std::atomic command_thread_position_; std::atomic command_thread_velocity_; std::atomic command_thread_effort_; diff --git a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp index ac68507..6e54e3d 100755 --- a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp +++ b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp @@ -133,6 +133,14 @@ namespace myactuator_rmd_hardware { velocity_command_ = 0.0; effort_command_ = 0.0; + temperature_state_ = 0.0; + voltage_state_ = 0.0; + current_state_ = 0.0; + current_phase_a_state_ = 0.0; + current_phase_b_state_ = 0.0; + current_phase_c_state_ = 0.0; + brake_state_ = 0.0; + position_interface_running_.store(false); velocity_interface_running_.store(false); effort_interface_running_.store(false); @@ -200,6 +208,30 @@ namespace myactuator_rmd_hardware { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints.at(0).name, hardware_interface::HW_IF_EFFORT, &effort_state_) ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_temperature", &temperature_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_voltage", &voltage_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current", ¤t_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_a", ¤t_phase_a_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_b", ¤t_phase_b_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_c", ¤t_phase_c_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_error_code", &error_code_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_brake", &brake_state_) + ); return state_interfaces; } @@ -260,6 +292,14 @@ namespace myactuator_rmd_hardware { position_state_ = degToRad(feedback_.load().shaft_angle); velocity_state_ = degToRad(feedback_.load().shaft_speed); effort_state_ = currentToTorque(feedback_.load().current, torque_constant_); + temperature_state_ = static_cast(feedback_.load().temperature); + error_code_state_ = static_cast(motor_status1_.load().error_code); + brake_state_ = static_cast(motor_status1_.load().is_brake_released); + voltage_state_ = static_cast(motor_status1_.load().voltage); + current_state_ = static_cast(feedback_.load().current); + current_phase_a_state_ = static_cast(motor_status3_.load().current_phase_a); + current_phase_b_state_ = static_cast(motor_status3_.load().current_phase_b); + current_phase_c_state_ = static_cast(motor_status3_.load().current_phase_c); return hardware_interface::return_type::OK; } @@ -291,6 +331,8 @@ namespace myactuator_rmd_hardware { } else if (effort_interface_running_) { feedback_ = actuator_interface_->sendTorqueSetpoint(command_thread_effort_.load(), torque_constant_); } + motor_status1_ = actuator_interface_->getMotorStatus1(); + motor_status3_ = actuator_interface_->getMotorStatus3(); std::this_thread::sleep_until(wakeup_time); } return; diff --git a/myactuator_rmd_interfaces/CMakeLists.txt b/myactuator_rmd_interfaces/CMakeLists.txt new file mode 100644 index 0000000..908f58d --- /dev/null +++ b/myactuator_rmd_interfaces/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(myactuator_rmd_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MotorStatus.msg" + "msg/CurrentPhase.msg" + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/myactuator_rmd_interfaces/LICENSE b/myactuator_rmd_interfaces/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/myactuator_rmd_interfaces/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/myactuator_rmd_interfaces/msg/CurrentPhase.msg b/myactuator_rmd_interfaces/msg/CurrentPhase.msg new file mode 100644 index 0000000..5e376d8 --- /dev/null +++ b/myactuator_rmd_interfaces/msg/CurrentPhase.msg @@ -0,0 +1,2 @@ +string[] name +float64[] current \ No newline at end of file diff --git a/myactuator_rmd_interfaces/msg/MotorStatus.msg b/myactuator_rmd_interfaces/msg/MotorStatus.msg new file mode 100644 index 0000000..49937ce --- /dev/null +++ b/myactuator_rmd_interfaces/msg/MotorStatus.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +string[] name +int64[] temperature +int64[] error_code +float64[] voltage +float64[] current +bool[] brake +CurrentPhase[] current_phases \ No newline at end of file diff --git a/myactuator_rmd_interfaces/package.xml b/myactuator_rmd_interfaces/package.xml new file mode 100644 index 0000000..0335852 --- /dev/null +++ b/myactuator_rmd_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + myactuator_rmd_interfaces + 0.0.0 + TODO: Package description + ORCIL Lionel + MIT + + ament_cmake + + rosidl_default_generators + + rosidl_default_runtime + std_msgs + + rosidl_interface_packages + + + ament_cmake + + From a62fd666bf4f46601f3197eee3a2dbdb51b47619 Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Thu, 6 Jun 2024 16:04:12 +0200 Subject: [PATCH 2/6] fix todo --- myactuator_rmd_interfaces/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/myactuator_rmd_interfaces/package.xml b/myactuator_rmd_interfaces/package.xml index 0335852..942a636 100644 --- a/myactuator_rmd_interfaces/package.xml +++ b/myactuator_rmd_interfaces/package.xml @@ -3,7 +3,7 @@ myactuator_rmd_interfaces 0.0.0 - TODO: Package description + Ros interface message interface for the MyActuator RMD-X-series ORCIL Lionel MIT From 6da01504318f76560c2d2f73c2f66e744908de0a Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Fri, 7 Jun 2024 18:36:37 +0200 Subject: [PATCH 3/6] Implement extra thread for reading 0x9A and 0x9D (currently failing due to concurrent access to CAN resource) --- .../launch/myactuator_rmd_control.launch.py | 12 +- .../src/myactuator_rmd_state_broadcaster.cpp | 4 +- myactuator_rmd_description/urdf/X12_150.xacro | 1 + myactuator_rmd_description/urdf/X8ProV2.xacro | 3 +- .../urdf/myactuator_rmd.ros2_control.xacro | 4 +- .../urdf/myactuator_rmd.xacro | 3 +- .../urdf/standalone.urdf.xacro | 10 +- .../myactuator_rmd_hardware_interface.hpp | 50 ++++- .../src/myactuator_rmd_hardware_interface.cpp | 178 ++++++++++++------ 9 files changed, 189 insertions(+), 76 deletions(-) diff --git a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py index 30e2c1f..ddcffec 100644 --- a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py +++ b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py @@ -26,6 +26,8 @@ def generate_launch_description(): actuator_id = LaunchConfiguration(actuator_id_parameter_name) ifname_parameter_name = 'ifname' ifname = LaunchConfiguration(ifname_parameter_name) + extra_status_refresh_rate_parameter_name = 'extra_status_refresh_rate' + extra_status_refresh_rate = LaunchConfiguration(extra_status_refresh_rate_parameter_name) controller_parameter_name = 'controller' controller = LaunchConfiguration(controller_parameter_name) rqt_controller_manager_parameter_name = 'rqt_controller_manager' @@ -52,6 +54,11 @@ def generate_launch_description(): default_value='can0', description='CAN interface name' ) + extra_status_refresh_rate_cmd = DeclareLaunchArgument( + extra_status_refresh_rate_parameter_name, + default_value='0', + description='Extra status refresh' + ) controller_cmd = DeclareLaunchArgument( controller_parameter_name, choices=['forward_position_controller', 'joint_trajectory_controller'], @@ -92,7 +99,8 @@ def generate_launch_description(): 'actuator:=', actuator, ' ', 'simulation:=', simulation, ' ', 'ifname:=', ifname, ' ', - 'actuator_id:=', actuator_id + 'actuator_id:=', actuator_id, ' ', + 'extra_status_refresh_rate:=', extra_status_refresh_rate ] ) robot_description = {'robot_description': robot_description_content} @@ -132,6 +140,7 @@ def generate_launch_description(): package='controller_manager', executable='ros2_control_node', parameters=[robot_description, controllers], + #arguments=['--ros-args', '--log-level', 'debug'], output='screen', condition=UnlessCondition(simulation) ) @@ -188,6 +197,7 @@ def generate_launch_description(): ld.add_action(actuator_cmd) ld.add_action(actuator_id_cmd) ld.add_action(ifname_cmd) + ld.add_action(extra_status_refresh_rate_cmd) ld.add_action(controller_cmd) ld.add_action(rqt_controller_manager_cmd) ld.add_action(rqt_joint_trajectory_controller_cmd) diff --git a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp index db6b2be..c4c6618 100644 --- a/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp +++ b/myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/src/myactuator_rmd_state_broadcaster.cpp @@ -196,7 +196,7 @@ bool MyActuatorRmdStateBroadcaster::init_motor_data() for (const auto & name_ifv : name_if_value_mapping_) { const auto & interfaces_and_values = name_ifv.second; - if (has_any_key(interfaces_and_values, {"rmd_error", + if (has_any_key(interfaces_and_values, {"rmd_error_code", "rmd_temperature", "rmd_brake", "rmd_voltage", @@ -220,7 +220,7 @@ bool MyActuatorRmdStateBroadcaster::init_motor_data() if (name_if_value_mapping_.count(extra_motor_name) == 0) { name_if_value_mapping_[extra_motor_name] = { - {"rmd_error", 0.0}, + {"rmd_error_code", 0.0}, {"rmd_temperature", 0.0}, {"rmd_brake", 0.0}, {"rmd_voltage", 0.0}, diff --git a/myactuator_rmd_description/urdf/X12_150.xacro b/myactuator_rmd_description/urdf/X12_150.xacro index c845d7e..99b17b2 100644 --- a/myactuator_rmd_description/urdf/X12_150.xacro +++ b/myactuator_rmd_description/urdf/X12_150.xacro @@ -6,6 +6,7 @@ - + - + @@ -16,6 +16,8 @@ ${actuator_id} ${torque_constant} + + ${extra_status_refresh_rate} diff --git a/myactuator_rmd_description/urdf/myactuator_rmd.xacro b/myactuator_rmd_description/urdf/myactuator_rmd.xacro index 17b955a..9e3b535 100644 --- a/myactuator_rmd_description/urdf/myactuator_rmd.xacro +++ b/myactuator_rmd_description/urdf/myactuator_rmd.xacro @@ -19,6 +19,7 @@ + ifname="${ifname}" actuator_id="${actuator_id}" torque_constant="${torque_constant}" extra_status_refresh_rate="${extra_status_refresh_rate}"/> \ No newline at end of file diff --git a/myactuator_rmd_description/urdf/standalone.urdf.xacro b/myactuator_rmd_description/urdf/standalone.urdf.xacro index 15fa137..a883794 100644 --- a/myactuator_rmd_description/urdf/standalone.urdf.xacro +++ b/myactuator_rmd_description/urdf/standalone.urdf.xacro @@ -10,6 +10,8 @@ + + @@ -20,15 +22,17 @@ - + ifname="$(arg ifname)" actuator_id="$(arg actuator_id)" + extra_status_refresh_rate="$(arg extra_status_refresh_rate)"/> + ifname="$(arg ifname)" actuator_id="$(arg actuator_id)" + extra_status_refresh_rate="$(arg extra_status_refresh_rate)"/> diff --git a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp index c0ac3d0..13582b1 100755 --- a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp +++ b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp @@ -248,6 +248,33 @@ namespace myactuator_rmd_hardware { * Stop the asynchronous command thread used to communicate with the hardware */ void stopCommandThread(); + + /**\fn extraThread + * \brief + * The asynchronous extra status thread used to communicate with the hardware + * that performs a combined read and write + * + * \param[in] cycle_time + * The cycle time that the asynchronous thread should run at + */ + void extraThread(std::chrono::milliseconds const& cycle_time); + + /**\fn startExtraThread + * \brief + * Start the asynchronous extra status thread used to communicate with the hardware + * + * \param[in] cycle_time + * The cycle time that the asynchronous thread should run at + * \return + * Boolean variable indicating successful start of the async thread or failure + */ + bool startExtraThread(std::chrono::milliseconds const& cycle_time); + + /**\fn stopExtraThread + * \brief + * Stop the asynchronous extra thread used to communicate with the hardware + */ + void stopExtraThread(); std::string ifname_; std::uint32_t actuator_id_; @@ -262,24 +289,29 @@ namespace myactuator_rmd_hardware { double velocity_command_; double effort_command_; - // Buffer status Actuator - double error_code_state_; - double temperature_state_; - double brake_state_; - double voltage_state_; - double current_state_; - double current_phase_a_state_; - double current_phase_b_state_; - double current_phase_c_state_; + // Buffer Extra status Actuator + double extra_error_code_state_; + double extra_temperature_state_; + double extra_brake_state_; + double extra_voltage_state_; + double extra_current_state_; + double extra_current_phase_a_state_; + double extra_current_phase_b_state_; + double extra_current_phase_c_state_; // The command thread reads and writes from the actuator cyclically std::thread command_thread_; std::chrono::milliseconds cycle_time_; + // The extra thread reads extra status from the actuator cyclically + std::thread extra_thread_; + std::chrono::milliseconds extra_cycle_time_; + bool ExtraThreadIsUsed_; // Never accessed by both threads at the same time std::unique_ptr driver_; std::unique_ptr actuator_interface_; // Shared between the two threads std::atomic stop_command_thread_; + std::atomic stop_extra_thread_; std::atomic feedback_; std::atomicmotor_status1_; std::atomicmotor_status3_; diff --git a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp index 6e54e3d..03af3bf 100755 --- a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp +++ b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp @@ -62,6 +62,18 @@ namespace myactuator_rmd_hardware { max_velocity_ = 720.0; RCLCPP_INFO(getLogger(), "Max velocity not set, defaulting to '%f'", max_velocity_); } + if (info_.hardware_parameters.find("extra_status_refresh_rate") != info_.hardware_parameters.end()) { + extra_cycle_time_ = std::chrono::milliseconds(std::stoi(info_.hardware_parameters["extra_status_refresh_rate"])); + } else { + extra_cycle_time_ = std::chrono::milliseconds::zero(); + RCLCPP_INFO(getLogger(), "Extra status refresh rate not set, defaulting to 0 milliseconds"); + } + if (extra_cycle_time_ != std::chrono::milliseconds::zero()){ + ExtraThreadIsUsed_ = true; + } else { + ExtraThreadIsUsed_ = false; + RCLCPP_INFO(getLogger(), "Extra status refresh rate is 0 milliseconds, it will not be implemented!"); + } cycle_time_ = std::chrono::milliseconds(1); driver_ = std::make_unique(ifname_); @@ -77,11 +89,18 @@ namespace myactuator_rmd_hardware { RCLCPP_FATAL(getLogger(), "Failed to start command thread!"); return CallbackReturn::ERROR; } - + stop_extra_thread_.store(false); + if (ExtraThreadIsUsed_) { + if (!startExtraThread(extra_cycle_time_)) { + RCLCPP_FATAL(getLogger(), "Failed to start extra status thread!"); + return CallbackReturn::ERROR; + } + } return CallbackReturn::SUCCESS; } CallbackReturn MyActuatorRmdHardwareInterface::on_cleanup(rclcpp_lifecycle::State const& /*previous_state*/) { + stopExtraThread(); stopCommandThread(); if (actuator_interface_) { actuator_interface_->shutdownMotor(); @@ -90,6 +109,7 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_shutdown(rclcpp_lifecycle::State const& /*previous_state*/) { + stopExtraThread(); stopCommandThread(); if (actuator_interface_) { actuator_interface_->shutdownMotor(); @@ -103,6 +123,7 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_deactivate(rclcpp_lifecycle::State const& /*previous_state*/) { + stopExtraThread(); stopCommandThread(); if (actuator_interface_) { actuator_interface_->stopMotor(); @@ -112,6 +133,7 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_error(rclcpp_lifecycle::State const& /*previous_state*/) { + stopExtraThread(); stopCommandThread(); if (actuator_interface_) { actuator_interface_->stopMotor(); @@ -132,15 +154,14 @@ namespace myactuator_rmd_hardware { position_command_ = 0.0; velocity_command_ = 0.0; effort_command_ = 0.0; - - temperature_state_ = 0.0; - voltage_state_ = 0.0; - current_state_ = 0.0; - current_phase_a_state_ = 0.0; - current_phase_b_state_ = 0.0; - current_phase_c_state_ = 0.0; - brake_state_ = 0.0; - + extra_temperature_state_ = std::numeric_limits::quiet_NaN(); + extra_voltage_state_ = std::numeric_limits::quiet_NaN(); + extra_current_state_ = std::numeric_limits::quiet_NaN(); + extra_current_phase_a_state_ = std::numeric_limits::quiet_NaN(); + extra_current_phase_b_state_ = std::numeric_limits::quiet_NaN(); + extra_current_phase_c_state_ = std::numeric_limits::quiet_NaN(); + extra_brake_state_ = std::numeric_limits::quiet_NaN(); + extra_error_code_state_= std::numeric_limits::quiet_NaN(); position_interface_running_.store(false); velocity_interface_running_.store(false); effort_interface_running_.store(false); @@ -208,30 +229,32 @@ namespace myactuator_rmd_hardware { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints.at(0).name, hardware_interface::HW_IF_EFFORT, &effort_state_) ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_temperature", &temperature_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_voltage", &voltage_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_current", ¤t_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_current_phase_a", ¤t_phase_a_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_current_phase_b", ¤t_phase_b_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_current_phase_c", ¤t_phase_c_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_error_code", &error_code_state_) - ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints.at(0).name, "rmd_brake", &brake_state_) - ); + if (ExtraThreadIsUsed_) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_temperature", &extra_temperature_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_voltage", &extra_voltage_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current", &extra_current_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_a", &extra_current_phase_a_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_b", &extra_current_phase_b_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_current_phase_c", &extra_current_phase_c_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_error_code", &extra_error_code_state_) + ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints.at(0).name, "rmd_brake", &extra_brake_state_) + ); + } return state_interfaces; } @@ -292,14 +315,16 @@ namespace myactuator_rmd_hardware { position_state_ = degToRad(feedback_.load().shaft_angle); velocity_state_ = degToRad(feedback_.load().shaft_speed); effort_state_ = currentToTorque(feedback_.load().current, torque_constant_); - temperature_state_ = static_cast(feedback_.load().temperature); - error_code_state_ = static_cast(motor_status1_.load().error_code); - brake_state_ = static_cast(motor_status1_.load().is_brake_released); - voltage_state_ = static_cast(motor_status1_.load().voltage); - current_state_ = static_cast(feedback_.load().current); - current_phase_a_state_ = static_cast(motor_status3_.load().current_phase_a); - current_phase_b_state_ = static_cast(motor_status3_.load().current_phase_b); - current_phase_c_state_ = static_cast(motor_status3_.load().current_phase_c); + if (ExtraThreadIsUsed_) { + extra_temperature_state_ = static_cast(feedback_.load().temperature); + extra_error_code_state_ = static_cast(motor_status1_.load().error_code); + extra_brake_state_ = static_cast(motor_status1_.load().is_brake_released); + extra_voltage_state_ = static_cast(motor_status1_.load().voltage); + extra_current_state_ = static_cast(feedback_.load().current); + extra_current_phase_a_state_ = static_cast(motor_status3_.load().current_phase_a); + extra_current_phase_b_state_ = static_cast(motor_status3_.load().current_phase_b); + extra_current_phase_c_state_ = static_cast(motor_status3_.load().current_phase_c); + } return hardware_interface::return_type::OK; } @@ -321,23 +346,25 @@ namespace myactuator_rmd_hardware { } void MyActuatorRmdHardwareInterface::commandThread(std::chrono::milliseconds const& cycle_time) { - while (!stop_command_thread_) { - auto const now {std::chrono::steady_clock::now()}; - auto const wakeup_time {now + cycle_time}; - if (position_interface_running_) { - feedback_ = actuator_interface_->sendPositionAbsoluteSetpoint(command_thread_position_.load(), max_velocity_); - } else if (velocity_interface_running_) { - feedback_ = actuator_interface_->sendVelocitySetpoint(command_thread_velocity_.load()); - } else if (effort_interface_running_) { - feedback_ = actuator_interface_->sendTorqueSetpoint(command_thread_effort_.load(), torque_constant_); + try { + while (!stop_command_thread_) { + auto const now {std::chrono::steady_clock::now()}; + auto const wakeup_time {now + cycle_time}; + if (position_interface_running_) { + feedback_ = actuator_interface_->sendPositionAbsoluteSetpoint(command_thread_position_.load(), max_velocity_); + } else if (velocity_interface_running_) { + feedback_ = actuator_interface_->sendVelocitySetpoint(command_thread_velocity_.load()); + } else if (effort_interface_running_) { + feedback_ = actuator_interface_->sendTorqueSetpoint(command_thread_effort_.load(), torque_constant_); + } + std::this_thread::sleep_until(wakeup_time); + } + } catch (const std::exception& e) { + RCLCPP_ERROR(getLogger(), "Exception in commandThread: %s", e.what()); + } catch (...) { + RCLCPP_ERROR(getLogger(), "Unknown exception in commandThread"); } - motor_status1_ = actuator_interface_->getMotorStatus1(); - motor_status3_ = actuator_interface_->getMotorStatus3(); - std::this_thread::sleep_until(wakeup_time); - } - return; } - bool MyActuatorRmdHardwareInterface::startCommandThread(std::chrono::milliseconds const& cycle_time) { if (!command_thread_.joinable()) { command_thread_ = std::thread(&MyActuatorRmdHardwareInterface::commandThread, this, cycle_time); @@ -347,7 +374,6 @@ namespace myactuator_rmd_hardware { } return true; } - void MyActuatorRmdHardwareInterface::stopCommandThread() { if (command_thread_.joinable()) { stop_command_thread_.store(true); @@ -358,6 +384,42 @@ namespace myactuator_rmd_hardware { return; } + void MyActuatorRmdHardwareInterface::extraThread(std::chrono::milliseconds const& cycle_time) { + try { + while (!stop_extra_thread_) { + auto const now {std::chrono::steady_clock::now()}; + auto const wakeup_time {now + cycle_time}; + motor_status1_ = actuator_interface_->getMotorStatus1(); + motor_status3_ = actuator_interface_->getMotorStatus3(); + std::this_thread::sleep_until(wakeup_time); + } + } catch (const std::exception& e) { + RCLCPP_ERROR(getLogger(), "Exception in extraThread: %s", e.what()); + } catch (...) { + RCLCPP_ERROR(getLogger(), "Unknown exception in extraThread"); + } + } + + bool MyActuatorRmdHardwareInterface::startExtraThread(std::chrono::milliseconds const& cycle_time) { + if (!extra_thread_.joinable()) { + extra_thread_ = std::thread(&MyActuatorRmdHardwareInterface::extraThread, this, cycle_time); + } else { + RCLCPP_WARN(getLogger(), "Could not start extra thread, extra thread already running!"); + return false; + } + return true; + } + + void MyActuatorRmdHardwareInterface::stopExtraThread() { + if (extra_thread_.joinable()) { + stop_extra_thread_.store(true); + extra_thread_.join(); + } else { + RCLCPP_WARN(getLogger(), "Could not stop extra thread: Not running!"); + } + return; + } + } // namespace myactuator_rmd_hardware #include From 978334bdbc7e6e27ec7d412ce6a9d47be83f3c0c Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Sat, 8 Jun 2024 09:44:03 +0200 Subject: [PATCH 4/6] feat: Implement extra wakeup logic in asyncThread function --- .../myactuator_rmd_hardware_interface.hpp | 31 +------- .../src/myactuator_rmd_hardware_interface.cpp | 78 +++++-------------- 2 files changed, 20 insertions(+), 89 deletions(-) diff --git a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp index adaabe7..7878f7c 100755 --- a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp +++ b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp @@ -249,33 +249,6 @@ namespace myactuator_rmd_hardware { * Stop the asynchronous command thread used to communicate with the hardware */ void stopAsyncThread(); - - /**\fn extraThread - * \brief - * The asynchronous extra status thread used to communicate with the hardware - * that performs a combined read and write - * - * \param[in] cycle_time - * The cycle time that the asynchronous thread should run at - */ - void extraThread(std::chrono::milliseconds const& cycle_time); - - /**\fn startExtraThread - * \brief - * Start the asynchronous extra status thread used to communicate with the hardware - * - * \param[in] cycle_time - * The cycle time that the asynchronous thread should run at - * \return - * Boolean variable indicating successful start of the async thread or failure - */ - bool startExtraThread(std::chrono::milliseconds const& cycle_time); - - /**\fn stopExtraThread - * \brief - * Stop the asynchronous extra thread used to communicate with the hardware - */ - void stopExtraThread(); std::string ifname_; std::uint32_t actuator_id_; @@ -305,10 +278,8 @@ namespace myactuator_rmd_hardware { // The command thread reads and writes from the actuator cyclically std::thread async_thread_; std::chrono::milliseconds cycle_time_; - // The extra thread reads extra status from the actuator cyclically - std::thread extra_thread_; std::chrono::milliseconds extra_cycle_time_; - bool ExtraThreadIsUsed_; + bool ExtraStatusIsUsed_; // Never accessed by both threads at the same time std::unique_ptr driver_; std::unique_ptr actuator_interface_; diff --git a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp index db9bd1e..293b0b7 100755 --- a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp +++ b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp @@ -80,21 +80,21 @@ namespace myactuator_rmd_hardware { if (info_.hardware_parameters.find("cycle_time") != info_.hardware_parameters.end()) { cycle_time_ = std::chrono::milliseconds(std::stol(info_.hardware_parameters["cycle_time"])); } else { - if (info_.hardware_parameters.find("extra_status_refresh_rate") != info_.hardware_parameters.end()) { + cycle_time_ = std::chrono::milliseconds(1); + RCLCPP_INFO(getLogger(), "Cycle time not set, defaulting to '%ld' ms.", cycle_time_.count()); + } + if (info_.hardware_parameters.find("extra_status_refresh_rate") != info_.hardware_parameters.end()) { extra_cycle_time_ = std::chrono::milliseconds(std::stoi(info_.hardware_parameters["extra_status_refresh_rate"])); } else { extra_cycle_time_ = std::chrono::milliseconds::zero(); RCLCPP_INFO(getLogger(), "Extra status refresh rate not set, defaulting to 0 milliseconds"); } if (extra_cycle_time_ != std::chrono::milliseconds::zero()){ - ExtraThreadIsUsed_ = true; + ExtraStatusIsUsed_ = true; } else { - ExtraThreadIsUsed_ = false; + ExtraStatusIsUsed_ = false; RCLCPP_INFO(getLogger(), "Extra status refresh rate is 0 milliseconds, it will not be implemented!"); } - cycle_time_ = std::chrono::milliseconds(1); - RCLCPP_INFO(getLogger(), "Cycle time not set, defaulting to '%ld' ms.", cycle_time_.count()); - } driver_ = std::make_unique(ifname_); actuator_interface_ = std::make_unique(*driver_, actuator_id_); @@ -109,18 +109,10 @@ namespace myactuator_rmd_hardware { RCLCPP_FATAL(getLogger(), "Failed to start async thread!"); return CallbackReturn::ERROR; } - stop_extra_thread_.store(false); - if (ExtraThreadIsUsed_) { - if (!startExtraThread(extra_cycle_time_)) { - RCLCPP_FATAL(getLogger(), "Failed to start extra status thread!"); - return CallbackReturn::ERROR; - } - } return CallbackReturn::SUCCESS; } CallbackReturn MyActuatorRmdHardwareInterface::on_cleanup(rclcpp_lifecycle::State const& /*previous_state*/) { - stopExtraThread(); stopAsyncThread(); if (actuator_interface_) { actuator_interface_->shutdownMotor(); @@ -129,7 +121,6 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_shutdown(rclcpp_lifecycle::State const& /*previous_state*/) { - stopExtraThread(); stopAsyncThread(); if (actuator_interface_) { actuator_interface_->shutdownMotor(); @@ -143,7 +134,6 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_deactivate(rclcpp_lifecycle::State const& /*previous_state*/) { - stopExtraThread(); stopAsyncThread(); if (actuator_interface_) { actuator_interface_->stopMotor(); @@ -153,7 +143,6 @@ namespace myactuator_rmd_hardware { } CallbackReturn MyActuatorRmdHardwareInterface::on_error(rclcpp_lifecycle::State const& /*previous_state*/) { - stopExtraThread(); stopAsyncThread(); if (actuator_interface_) { actuator_interface_->stopMotor(); @@ -249,7 +238,7 @@ namespace myactuator_rmd_hardware { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints.at(0).name, hardware_interface::HW_IF_EFFORT, &effort_state_) ); - if (ExtraThreadIsUsed_) { + if (ExtraStatusIsUsed_) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints.at(0).name, "rmd_temperature", &extra_temperature_state_) ); @@ -335,12 +324,12 @@ namespace myactuator_rmd_hardware { position_state_ = async_position_state_.load(); velocity_state_ = async_velocity_state_.load(); effort_state_ = async_effort_state_.load(); - if (ExtraThreadIsUsed_) { - extra_temperature_state_ = static_cast(feedback_.load().temperature); + if (ExtraStatusIsUsed_) { + extra_temperature_state_ = static_cast(motor_status1_.load().temperature); extra_error_code_state_ = static_cast(motor_status1_.load().error_code); extra_brake_state_ = static_cast(motor_status1_.load().is_brake_released); extra_voltage_state_ = static_cast(motor_status1_.load().voltage); - extra_current_state_ = static_cast(feedback_.load().current); + extra_current_state_ = static_cast(feedback_.current); extra_current_phase_a_state_ = static_cast(motor_status3_.load().current_phase_a); extra_current_phase_b_state_ = static_cast(motor_status3_.load().current_phase_b); extra_current_phase_c_state_ = static_cast(motor_status3_.load().current_phase_c); @@ -365,7 +354,8 @@ namespace myactuator_rmd_hardware { return rclcpp::get_logger("MyActuatorRmdHardwareInterface"); } - void MyActuatorRmdHardwareInterface::asyncThread(std::chrono::milliseconds const& cycle_time) { + void MyActuatorRmdHardwareInterface::asyncThread(std::chrono::milliseconds const& cycle_time) { + auto extra_wakeup_time {std::chrono::steady_clock::now() + extra_cycle_time_}; while (!stop_async_thread_) { auto const now {std::chrono::steady_clock::now()}; auto const wakeup_time {now + cycle_time}; @@ -390,6 +380,13 @@ namespace myactuator_rmd_hardware { async_velocity_state_.store(degToRad(velocity_state)); async_effort_state_.store(currentToTorque(current_state, torque_constant_)); + if (ExtraStatusIsUsed_){ + if (extra_wakeup_time <= now){ + motor_status1_ = actuator_interface_->getMotorStatus1(); + motor_status3_ = actuator_interface_->getMotorStatus3(); + extra_wakeup_time = (std::chrono::steady_clock::now() + extra_cycle_time_); + } + } std::this_thread::sleep_until(wakeup_time); } return; @@ -414,43 +411,6 @@ namespace myactuator_rmd_hardware { } return; } - - void MyActuatorRmdHardwareInterface::extraThread(std::chrono::milliseconds const& cycle_time) { - try { - while (!stop_extra_thread_) { - auto const now {std::chrono::steady_clock::now()}; - auto const wakeup_time {now + cycle_time}; - motor_status1_ = actuator_interface_->getMotorStatus1(); - motor_status3_ = actuator_interface_->getMotorStatus3(); - std::this_thread::sleep_until(wakeup_time); - } - } catch (const std::exception& e) { - RCLCPP_ERROR(getLogger(), "Exception in extraThread: %s", e.what()); - } catch (...) { - RCLCPP_ERROR(getLogger(), "Unknown exception in extraThread"); - } - } - - bool MyActuatorRmdHardwareInterface::startExtraThread(std::chrono::milliseconds const& cycle_time) { - if (!extra_thread_.joinable()) { - extra_thread_ = std::thread(&MyActuatorRmdHardwareInterface::extraThread, this, cycle_time); - } else { - RCLCPP_WARN(getLogger(), "Could not start extra thread, extra thread already running!"); - return false; - } - return true; - } - - void MyActuatorRmdHardwareInterface::stopExtraThread() { - if (extra_thread_.joinable()) { - stop_extra_thread_.store(true); - extra_thread_.join(); - } else { - RCLCPP_WARN(getLogger(), "Could not stop extra thread: Not running!"); - } - return; - } - } // namespace myactuator_rmd_hardware #include From 9e750ef775c6d0482190e386845619e330701434 Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Sat, 8 Jun 2024 10:31:40 +0200 Subject: [PATCH 5/6] refactor: Move getParameter Extra_status_refresh_rate to the onInit function --- .../launch/myactuator_rmd_control.launch.py | 2 +- .../myactuator_rmd_hardware_interface.hpp | 1 - .../src/myactuator_rmd_hardware_interface.cpp | 32 ++++++++++--------- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py index ddcffec..65429b7 100644 --- a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py +++ b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py @@ -140,7 +140,7 @@ def generate_launch_description(): package='controller_manager', executable='ros2_control_node', parameters=[robot_description, controllers], - #arguments=['--ros-args', '--log-level', 'debug'], + arguments=['--ros-args', '--log-level', 'debug'], output='screen', condition=UnlessCondition(simulation) ) diff --git a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp index 7878f7c..6ec37a2 100755 --- a/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp +++ b/myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp @@ -287,7 +287,6 @@ namespace myactuator_rmd_hardware { // Shared between the two threads std::atomic stop_async_thread_; - std::atomic stop_extra_thread_; std::atomic async_position_state_; std::atomic async_velocity_state_; std::atomicmotor_status1_; diff --git a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp index 293b0b7..742f8b1 100755 --- a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp +++ b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp @@ -28,7 +28,7 @@ namespace myactuator_rmd_hardware { using CallbackReturn = MyActuatorRmdHardwareInterface::CallbackReturn; - + MyActuatorRmdHardwareInterface::~MyActuatorRmdHardwareInterface() { // If the controller manager is shutdown with Ctrl + C the on_deactivate methods won't be called! // We therefore shut down the actuator here. @@ -80,21 +80,9 @@ namespace myactuator_rmd_hardware { if (info_.hardware_parameters.find("cycle_time") != info_.hardware_parameters.end()) { cycle_time_ = std::chrono::milliseconds(std::stol(info_.hardware_parameters["cycle_time"])); } else { - cycle_time_ = std::chrono::milliseconds(1); + cycle_time_ = std::chrono::milliseconds(1); RCLCPP_INFO(getLogger(), "Cycle time not set, defaulting to '%ld' ms.", cycle_time_.count()); } - if (info_.hardware_parameters.find("extra_status_refresh_rate") != info_.hardware_parameters.end()) { - extra_cycle_time_ = std::chrono::milliseconds(std::stoi(info_.hardware_parameters["extra_status_refresh_rate"])); - } else { - extra_cycle_time_ = std::chrono::milliseconds::zero(); - RCLCPP_INFO(getLogger(), "Extra status refresh rate not set, defaulting to 0 milliseconds"); - } - if (extra_cycle_time_ != std::chrono::milliseconds::zero()){ - ExtraStatusIsUsed_ = true; - } else { - ExtraStatusIsUsed_ = false; - RCLCPP_INFO(getLogger(), "Extra status refresh rate is 0 milliseconds, it will not be implemented!"); - } driver_ = std::make_unique(ifname_); actuator_interface_ = std::make_unique(*driver_, actuator_id_); @@ -224,6 +212,19 @@ namespace myactuator_rmd_hardware { return CallbackReturn::ERROR; } + if (info_.hardware_parameters.find("extra_status_refresh_rate") != info_.hardware_parameters.end()) { + extra_cycle_time_ = std::chrono::milliseconds(std::stoi(info_.hardware_parameters["extra_status_refresh_rate"])); + } else { + extra_cycle_time_ = std::chrono::milliseconds::zero(); + RCLCPP_INFO(getLogger(), "Extra status refresh rate not set, defaulting to 0 milliseconds"); + } + if (extra_cycle_time_ != std::chrono::milliseconds::zero()){ + ExtraStatusIsUsed_ = true; + } else { + ExtraStatusIsUsed_ = false; + RCLCPP_INFO(getLogger(), "Extra status refresh rate is 0 milliseconds, it will not be implemented!"); + } + return CallbackReturn::SUCCESS; } @@ -239,6 +240,7 @@ namespace myactuator_rmd_hardware { info_.joints.at(0).name, hardware_interface::HW_IF_EFFORT, &effort_state_) ); if (ExtraStatusIsUsed_) { + RCLCPP_INFO(getLogger(), "Extra Status set!"); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints.at(0).name, "rmd_temperature", &extra_temperature_state_) ); @@ -354,7 +356,7 @@ namespace myactuator_rmd_hardware { return rclcpp::get_logger("MyActuatorRmdHardwareInterface"); } - void MyActuatorRmdHardwareInterface::asyncThread(std::chrono::milliseconds const& cycle_time) { + void MyActuatorRmdHardwareInterface::asyncThread(std::chrono::milliseconds const& cycle_time) { auto extra_wakeup_time {std::chrono::steady_clock::now() + extra_cycle_time_}; while (!stop_async_thread_) { auto const now {std::chrono::steady_clock::now()}; From 2027435eefac5245447f0e4357759e837969c68a Mon Sep 17 00:00:00 2001 From: ORCIL Lionel Date: Sat, 8 Jun 2024 10:48:41 +0200 Subject: [PATCH 6/6] Add condition to launch myactuator_rmd_state_broadcaster_spawner_node only if extra_status_refresh_rate is different from 0. --- .../launch/myactuator_rmd_control.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py index 65429b7..9f36780 100644 --- a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py +++ b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py @@ -14,7 +14,7 @@ from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( - Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, PythonExpression ) from launch_ros.actions import Node @@ -127,7 +127,8 @@ def generate_launch_description(): myactuator_rmd_state_broadcaster_spawner_node = Node( package='controller_manager', executable='spawner', - arguments=['myactuator_rmd_state_broadcaster', '--controller-manager', '/controller_manager'] + arguments=['myactuator_rmd_state_broadcaster', '--controller-manager', '/controller_manager'], + condition=IfCondition(PythonExpression([extra_status_refresh_rate, ' != 0'])) ) controllers = PathJoinSubstitution( [ @@ -140,7 +141,6 @@ def generate_launch_description(): package='controller_manager', executable='ros2_control_node', parameters=[robot_description, controllers], - arguments=['--ros-args', '--log-level', 'debug'], output='screen', condition=UnlessCondition(simulation) )