diff --git a/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py b/myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py index 45b7eec..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 @@ -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} @@ -116,6 +124,12 @@ 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'], + condition=IfCondition(PythonExpression([extra_status_refresh_rate, ' != 0'])) + ) controllers = PathJoinSubstitution( [ get_package_share_directory('myactuator_rmd_description'), @@ -183,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) @@ -190,6 +205,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..c4c6618 --- /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_code", + "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_code", 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_description/urdf/X12_150.xacro b/myactuator_rmd_description/urdf/X12_150.xacro index ae6d9f1..2d88218 100644 --- a/myactuator_rmd_description/urdf/X12_150.xacro +++ b/myactuator_rmd_description/urdf/X12_150.xacro @@ -6,6 +6,7 @@ - + + velocity_alpha effort_alpha torque_constant + extra_status_refresh_rate"> @@ -17,6 +18,8 @@ ${actuator_id} ${torque_constant} + + ${extra_status_refresh_rate} ${velocity_alpha} diff --git a/myactuator_rmd_description/urdf/myactuator_rmd.xacro b/myactuator_rmd_description/urdf/myactuator_rmd.xacro index 7b4d8ab..05a94cd 100644 --- a/myactuator_rmd_description/urdf/myactuator_rmd.xacro +++ b/myactuator_rmd_description/urdf/myactuator_rmd.xacro @@ -20,6 +20,7 @@ simulation ifname actuator_id velocity_alpha:=0.1 effort_alpha:=0.1 torque_constant + extra_status_refresh_rate limit_effort limit_lower limit_upper limit_velocity visual @@ -109,7 +110,7 @@ + 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 8951cbd..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 @@ -265,9 +265,21 @@ namespace myactuator_rmd_hardware { std::unique_ptr velocity_low_pass_filter_; std::unique_ptr effort_low_pass_filter_; + // 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 async_thread_; std::chrono::milliseconds cycle_time_; + std::chrono::milliseconds extra_cycle_time_; + bool ExtraStatusIsUsed_; // Never accessed by both threads at the same time std::unique_ptr driver_; std::unique_ptr actuator_interface_; @@ -277,6 +289,8 @@ namespace myactuator_rmd_hardware { std::atomic async_position_state_; std::atomic async_velocity_state_; + std::atomicmotor_status1_; + std::atomicmotor_status3_; std::atomic async_effort_state_; std::atomic async_position_command_; std::atomic async_velocity_command_; diff --git a/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp b/myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp index 4805559..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. @@ -97,7 +97,6 @@ namespace myactuator_rmd_hardware { RCLCPP_FATAL(getLogger(), "Failed to start async thread!"); return CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; } @@ -152,7 +151,14 @@ namespace myactuator_rmd_hardware { position_command_ = 0.0; velocity_command_ = 0.0; effort_command_ = 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); @@ -206,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; } @@ -220,6 +239,33 @@ namespace myactuator_rmd_hardware { state_interfaces.emplace_back(hardware_interface::StateInterface( 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_) + ); + 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; } @@ -280,6 +326,16 @@ namespace myactuator_rmd_hardware { position_state_ = async_position_state_.load(); velocity_state_ = async_velocity_state_.load(); effort_state_ = async_effort_state_.load(); + 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_.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; } @@ -301,6 +357,7 @@ namespace myactuator_rmd_hardware { } 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}; @@ -325,6 +382,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; @@ -349,7 +413,6 @@ namespace myactuator_rmd_hardware { } return; } - } // namespace myactuator_rmd_hardware #include 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..942a636 --- /dev/null +++ b/myactuator_rmd_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + myactuator_rmd_interfaces + 0.0.0 + Ros interface message interface for the MyActuator RMD-X-series + ORCIL Lionel + MIT + + ament_cmake + + rosidl_default_generators + + rosidl_default_runtime + std_msgs + + rosidl_interface_packages + + + ament_cmake + +