Myactuator status broadcast

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,13 +197,15 @@ 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)
     ld.add_action(simulation_parameter_cmd)
     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)

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
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+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()

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.

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 <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#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<ParamListener> param_listener_;
+  Params params_;
+  std::unordered_map<std::string, std::string> map_interface_to_motor_state_;
+  std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
+  std::vector<std::string> motor_names_;
+
+  std::shared_ptr<rclcpp::Publisher<myactuator_rmd_interfaces::msg::MotorStatus>> motor_state_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<myactuator_rmd_interfaces::msg::MotorStatus>> realtime_motor_state_publisher_;
+};
+
+}  // namespace myactuator_rmd_state_broadcaster
+
+#endif  // MYACTUATOR_RMD_STATE_BROADCASTER__MYACTUATOR_RMD_STATE_BROADCASTER_HPP_

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_

myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/myactuator_rmd_state_broadcaster_plugins.xml

@@ -0,0 +1,9 @@
+<library path="myactuator_rmd_state_broadcaster">
+  <class name="myactuator_rmd_state_broadcaster/MyActuatorRmdStateBroadcaster"
+         type="myactuator_rmd_state_broadcaster::MyActuatorRmdStateBroadcaster"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      ros2_control status broadcaster for the MyActuator RMD-X-series
+    </description>
+  </class>
+</library>

myactuator_rmd_controllers/myactuator_rmd_state_broadcaster/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>myactuator_rmd_state_broadcaster</name>
+  <version>0.0.0</version>
+  <description>ros2_control status broadcaster for the MyActuator RMD-X-series</description>
+  <maintainer email="orcil.lionel@gmail.com">ORCIL Lionel</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>myactuator_rmd_interfaces</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>generate_parameter_library</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>