[Example 14] Modular robot with actuators not providing states, with additional sensors

README.md

@@ -412,8 +412,8 @@ Useful launch-file options:
 ### Example 3: "Industrial robot with integrated sensor"
 
 - Launch file: [rrbot_system_with_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py)
-- URDF: [rrbot_system_with_sensor.urdf.xacro](ros2_control_demo_bringup/config/rrbot_with_sensor_controllers.yaml)
-- ros2_control URDF: [rrbot_system_with_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro)
+- Controllers yaml: [rrbot_system_with_sensor.yaml](ros2_control_demo_bringup/config/rrbot_with_sensor_controllers.yaml)
+- `ros2_control` URDF tag: [rrbot_system_with_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro)
 
 - Command interfaces:
   - joint1/position
@@ -443,8 +443,8 @@ ros2 topic echo /fts_broadcaster/wrench
 ### Example 4: "Industrial Robots with externally connected sensor"
 
 - Launch file: [rrbot_system_with_external_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py)
-- URDF: [rrbot_with_external_sensor_controllers.urdf.xacro](ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml)
-- ros2_control URDF: [external_rrbot_force_torque_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro)
+- Controllers yaml: [rrbot_with_external_sensor_controllers.yaml](ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml)
+- `ros2_control` URDF tag: [external_rrbot_force_torque_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro)
 
 - Command interfaces:
   - joint1/position
@@ -475,8 +475,8 @@ ros2 topic echo /fts_broadcaster/wrench
 ### Example 5: "Modular Robots with separate communication to each actuator"
 
 - Launch file: [rrbot_modular_actuators.launch.py](ros2_control_demo_bringup/launch/rrbot_modular_actuators.launch.py)
-- URDF: [rrbot_modular_actuators.urdf.xacro](ros2_control_demo_bringup/config/rrbot_modular_actuators.yaml)
-- ros2_control URDF: [rrbot_modular_actuators.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro)
+- Controllers yaml: [rrbot_modular_actuators.yaml](ros2_control_demo_bringup/config/rrbot_modular_actuators.yaml)
+- `ros2_control` URDF tag: [rrbot_modular_actuators.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro)
 
 - Command interfaces:
   - joint1/position
@@ -492,6 +492,30 @@ Available controllers:
 Commanding the robot: see the commands below.
 
 
+### Example 6: "Modular Robots with actuators not providing states and with additional sensors"
+
+- Launch file: [rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py](ros2_control_demo_bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py)
+- Controllers yaml: [rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml](ros2_control_demo_bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml)
+- `ros2_control` URDF tag: [rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro)
+
+- Command interfaces:
+  - joint1/velocity
+  - joint2/velocity
+- State interfaces:
+  - joint1/position
+  - joint2/position
+
+Available controllers:
+- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
+- `forward_velocity_controller[forward_command_controller/ForwardCommandController]`
+
+Moving the robot:
+  ```
+  ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data:
+  - 5
+  - 5"
+  ```
+
 ## Controllers and moving hardware
 
 To move the robot you should load and start controllers.

ros2_control_demo_bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml

@@ -0,0 +1,17 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    forward_velocity_controller:
+      type: forward_command_controller/ForwardCommandController
+
+
+forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+    interface_name: velocity

ros2_control_demo_bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py

@@ -0,0 +1,63 @@
+# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed, joint names in the controllers' configuration \
+        also have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "slowdown", default_value="50.0", description="Slowdown factor of the RRbot."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_controller",
+            default_value="forward_velocity_controller",
+            description="Robot controller to start.",
+        )
+    )
+
+    # Initialize Arguments
+    prefix = LaunchConfiguration("prefix")
+    slowdown = LaunchConfiguration("slowdown")
+    robot_controller = LaunchConfiguration("robot_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]),
+        launch_arguments={
+            "controllers_file": "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml",
+            "description_file": "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro",
+            "prefix": prefix,
+            "use_fake_hardware": "false",
+            "slowdown": slowdown,
+            "robot_controller": robot_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="rrbot_modular_actuators_without_feedback_sensors_for_position_feedback"
+    params="name prefix use_sim:=^|false slowdown:=2.0">
+
+    <!-- NOTE generally is the order of hardware definition not relevant. But in this case the
+    sensors are listening on socket to which the actuators are connecting. Therefore, the sensors
+    has to be initialized and started first. -->
+
+    <ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotSensorPositionFeedback</plugin>
+        <param name="example_param_hw_start_duration_sec">1.0</param>
+        <param name="example_param_hw_stop_duration_sec">2.0</param>
+        <param name="example_param_hw_slowdown">${slowdown}</param>
+        <param name="example_param_socket_port">23286</param>
+      </hardware>
+      <joint name="joint1">
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+    <ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotSensorPositionFeedback</plugin>
+        <param name="example_param_hw_start_duration_sec">1.0</param>
+        <param name="example_param_hw_stop_duration_sec">2.0</param>
+        <param name="example_param_hw_slowdown">${slowdown}</param>
+        <param name="example_param_socket_port">23287</param>
+      </hardware>
+      <joint name="joint2">
+        <state_interface name="position"/>
+      </joint>
+      </ros2_control>
+
+    <ros2_control name="RRBotModularJoint1" type="actuator">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotActuatorWithoutFeedback</plugin>
+        <param name="example_param_hw_start_duration_sec">2.0</param>
+        <param name="example_param_hw_stop_duration_sec">3.0</param>
+        <param name="example_param_socket_port">23286</param>
+      </hardware>
+      <joint name="joint1">
+        <command_interface name="velocity">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+      </joint>
+    </ros2_control>
+    <ros2_control name="RRBotModularJoint2" type="actuator">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotActuatorWithoutFeedback</plugin>
+        <param name="example_param_hw_start_duration_sec">2.0</param>
+        <param name="example_param_hw_stop_duration_sec">3.0</param>
+        <param name="example_param_socket_port">23287</param>
+      </hardware>
+      <joint name="joint2">
+        <command_interface name="velocity">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+      </joint>
+      </ros2_control>
+
+  </xacro:macro>
+
+</robot>

ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<!-- Revolute-Revolute Manipulator -->
+<!--
+Copied and modified from ROS1 example -
+https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
+  <xacro:arg name="use_sim" default="false" />
+
+  <!-- Enable setting arguments from the launch file -->
+  <xacro:arg name="prefix" default="" />
+  <xacro:arg name="slowdown" default="2.0" />
+
+  <!-- Import RRBot macro -->
+  <xacro:include filename="$(find rrbot_description)/urdf/rrbot_description.urdf.xacro" />
+
+  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
+  <xacro:include filename="$(find rrbot_description)/gazebo/rrbot.gazebo.xacro" />
+
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find rrbot_description)/gazebo/rrbot.materials.xacro" />
+
+  <!-- Import RRBot ros2_control description -->
+  <xacro:include filename="$(find rrbot_description)/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro" />
+
+  <!-- Used for fixing robot -->
+  <link name="world"/>
+  <gazebo reference="world">
+    <static>true</static>
+  </gazebo>
+
+  <xacro:rrbot parent="world" prefix="$(arg prefix)">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:rrbot>
+
+  <xacro:rrbot_gazebo prefix="$(arg prefix)" />
+
+  <xacro:rrbot_modular_actuators_without_feedback_sensors_for_position_feedback
+    name="RRBotModularJoint" prefix="$(arg prefix)"
+    slowdown="$(arg slowdown)" />
+
+</robot>

ros2_control_demo_hardware/CMakeLists.txt

@@ -27,6 +27,8 @@ add_library(
   src/rrbot_system_multi_interface.cpp
   src/rrbot_system_with_sensor.cpp
   src/rrbot_actuator.cpp
+  src/rrbot_actuator_without_feedback.cpp
+  src/rrbot_sensor_for_position_feedback.cpp
   src/external_rrbot_force_torque_sensor.cpp
 )
 target_include_directories(

ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator_without_feedback.hpp

@@ -0,0 +1,84 @@
+// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+//
+// Authors: Denis Stogl
+//
+
+#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_
+#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_
+
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "ros2_control_demo_hardware/visibility_control.h"
+
+namespace ros2_control_demo_hardware
+{
+class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(RRBotActuatorWithoutFeedback);
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type read() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type write() override;
+
+private:
+  // Parameters for the RRBot simulation
+  double hw_start_sec_;
+  double hw_stop_sec_;
+
+  // Store the command for the simulated robot
+  double hw_joint_command_;
+
+  // Fake "mechanical connection" between actuator and sensor using sockets
+  struct sockaddr_in address_;
+  int socket_port_;
+  int sockoptval_ = 1;
+  int sock_;
+};
+
+}  // namespace ros2_control_demo_hardware
+
+#endif  // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_