Ros2 control

.devcontainer/Dockerfile

@@ -1,6 +1,7 @@
 ARG ROS_DISTRO=humble
 FROM ros:${ROS_DISTRO}
 
+ARG ROS_DISTRO
 ARG DEBIAN_FRONTEND=noninteractive
 ARG USERNAME=flatland
 
@@ -12,17 +13,28 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     openssh-client \
     python3-colcon-common-extensions \
     python3-rosdep \
+    python3-vcstool \
+    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
     sudo \
     && rm -rf /var/lib/apt/lists/*
 
 COPY . /tmp/flatland
-RUN rosdep update && \
+RUN mkdir -p /opt/flatland_upstream_ws/src && \
+        vcs import /opt/flatland_upstream_ws/src < /tmp/flatland/flatland-not-released.repos && \
+        rosdep update && \
     apt-get update && \
-    rosdep install --from-paths /tmp/flatland --ignore-src --rosdistro "$ROS_DISTRO" -y && \
+        rosdep install --from-paths /tmp/flatland /opt/flatland_upstream_ws/src \
+            --ignore-src --rosdistro "$ROS_DISTRO" -y && \
+        . "/opt/ros/$ROS_DISTRO/setup.sh" && \
+        colcon --log-base /opt/flatland_upstream_ws/log build \
+            --base-paths /opt/flatland_upstream_ws/src \
+            --build-base /opt/flatland_upstream_ws/build \
+            --install-base /opt/flatland_upstream_ws/install && \
     rm -rf /tmp/flatland /var/lib/apt/lists/*
 
 ENV CCACHE_DIR=/tmp/.ccache
 ENV QT_QPA_PLATFORM=xcb
+ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 WORKDIR /opt/overlay_ws
 RUN if id -u ubuntu >/dev/null 2>&1; then \
         if [ "$USERNAME" != ubuntu ]; then \

.devcontainer/post-create-command.sh

@@ -1,7 +1,7 @@
 #!/usr/bin/env bash
 set -euo pipefail
 
-setup_line='if [ -f /opt/overlay_ws/install/setup.bash ]; then source /opt/overlay_ws/install/setup.bash; else for setup_file in /opt/ros/*/setup.bash; do if [ -f "$setup_file" ]; then source "$setup_file"; break; fi; done; fi'
+setup_line='source /opt/flatland_upstream_ws/install/setup.bash; if [ -f /opt/overlay_ws/install/setup.bash ]; then source /opt/overlay_ws/install/setup.bash; fi'
 if grep -Fxq 'source /opt/overlay_ws/install/setup.bash' "$HOME/.bashrc"; then
     sed -i '\|^source /opt/overlay_ws/install/setup.bash$|d' "$HOME/.bashrc"
 fi

.github/workflows/industrial_ci_action.yml

@@ -16,13 +16,34 @@ jobs:
     strategy:
       matrix:
         env:
-          - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true}
-          # - {ROS_DISTRO: jazzy, ROS_REPO: main, AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true}
-          # - {ROS_DISTRO: kilted, ROS_REPO: main, AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'}
-          - {ROS_DISTRO: lyrical, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:lyrical-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'}
-          - {ROS_DISTRO: rolling, ROS_REPO: main, OS_CODE_NAME: noble, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full-noble', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'}
+          - {ROS_DISTRO: jazzy, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:jazzy-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples', UPSTREAM_WORKSPACE: flatland-not-released.jazzy.repos}
+          # - {ROS_DISTRO: kilted, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:kilted-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples'}
+          - {ROS_DISTRO: lyrical, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:lyrical-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface, ADDITIONAL_DEBS: ros-lyrical-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'}
+          - {ROS_DISTRO: rolling, ROS_REPO: main, OS_CODE_NAME: resolute, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'}
     runs-on: ubuntu-24.04
     steps:
       - uses: actions/checkout@v4
       - uses: 'ros-industrial/industrial_ci@master'
-        env: ${{matrix.env}}
+        env: ${{matrix.env}}
+
+  coverage:
+    name: Coverage (ROS Lyrical)
+    runs-on: ubuntu-24.04
+    steps:
+      - uses: actions/checkout@v4
+      - uses: 'ros-industrial/industrial_ci@master'
+        env:
+          ROS_DISTRO: lyrical
+          ROS_REPO: main
+          DOCKER_IMAGE: osrf/ros:lyrical-desktop-full
+          ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface
+          CC: gcc
+          CXX: g++
+          ADDITIONAL_DEBS: gcovr ros-lyrical-rmw-cyclonedds-cpp
+          DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'
+          TARGET_CMAKE_ARGS: '-DCMAKE_C_FLAGS=--coverage -DCMAKE_CXX_FLAGS=--coverage -DCMAKE_EXE_LINKER_FLAGS=--coverage -DCMAKE_SHARED_LINKER_FLAGS=--coverage'
+          AFTER_SCRIPT: >-
+            gcovr --root "$BASEDIR/${PREFIX}target_ws/src"
+            --filter "$BASEDIR/${PREFIX}target_ws/src"
+            --exclude '.*/test/.*'
+            --print-summary "$BASEDIR/${PREFIX}target_ws/build"

docs/core_functions/models.rst

@@ -63,10 +63,14 @@ shown here. Examples can be found in flatland_server/tests.
           # Does not disable interactions like the laser scan plugin.
           collision: true
 
-          # required, the density of footprint in kg/m^2 in Box2D, used for all
-          # footprint types
+          # required unless mass is given: density in kg/m^2 for this footprint
           density: 1
 
+          # alternative to density: total footprint mass in kg. For circles and
+          # polygons Flatland divides this by the footprint area for Box2D.
+          # If both are given, mass takes precedence and a warning is logged.
+          # mass: 2
+
           # optional, default to 0.0, specifies Box2D fixture friction, used for
           # all footprint types
           friction: 0

docs/included_plugins/drive_wheel.rst

@@ -0,0 +1,52 @@
+DriveWheel
+==========
+
+DriveWheel applies a traction-limited force at a wheel center on a model body.
+Use one plugin per driven wheel; the wheel has no Box2D rotational body. Wheel
+position is integrated from measured forward speed, so slip or external motion
+is reflected in the reported state. A wheel also resists sideways motion.
+
+.. code-block:: yaml
+
+  plugins:
+    - type: DriveWheel
+      name: left_drive
+      body: base
+      offset: [0.0, 0.3, 0.0]
+      radius: 0.1
+      mode: velocity
+      friction: 1.0
+      lateral_resistance: 1.0
+      joint_commands_topic: /robot_joint_commands
+      joint_states_topic: /robot_joint_states
+
+``body`` and a positive ``radius`` (meters) are required. ``offset`` is the
+wheel center (x, y in meters) and forward heading (radians) in body coordinates;
+it defaults to ``[0, 0, 0]``. ``joint_name`` defaults to the plugin's name.
+``mode`` is ``velocity`` (default) or ``effort``; commands are wheel speed in
+rad/s or axle torque in N m. Velocity control accelerates toward the requested
+wheel speed, subject to available traction. ``friction`` defaults
+to 1.0 and limits the combined longitudinal and lateral force to the wheel's
+allocated normal load times the coefficient. ``lateral_resistance`` defaults
+to 1.0 and scales the lateral velocity correction by supported mass and step
+duration. At low slip the wheel resists sideways motion; beyond the friction
+limit it slides. This load-limited approximation does not model a fitted tire
+slip curve or wheel rotational inertia.
+
+Commands are ``control_msgs/msg/JointCommand`` on
+``<joint_commands_topic>/<mode>``. A message must have a matching
+``interface_name`` and a ``joint_names`` entry equal to the plugin's ``name``
+(not ``joint_name``), with a finite corresponding ``values`` entry. Commands
+without that plugin name are ignored, so multiple wheels can share the same
+command topic. Until the first command the wheel applies no drive force. State is
+published after each step as ``sensor_msgs/msg/JointState`` (position,
+velocity, applied effort) on ``joint_states_topic`` with sensor-data QoS.
+The topic defaults match the ros2_control
+``joint_command_topic_hardware_interface/JointCommandTopicSystem``. Absolute
+topics remain global; relative overrides are prefixed by the model namespace.
+
+Flatland assigns each model's weight (total body mass times 9.81 m/s²) across
+its contact-aware plugins each step according to their world-space points and
+the model's center of mass. This is a planar support approximation, not a
+terrain contact sensor; unloaded points outside the support polygon receive
+zero force. Configure at least two separated wheels for load transfer.

docs/included_plugins/free_wheel.rst

@@ -20,9 +20,11 @@ also affects the body's rotation. Use one instance per wheel.
 ``body`` is the required body name. ``offset`` defaults to ``[0, 0, 0]``
 and specifies the wheel center (x, y, in meters) and forward orientation
 (theta, in radians) relative to that body. ``radius`` is required and must be
-positive, in meters. ``lateral_resistance`` defaults to 1.0 N/(m/s); zero
-disables sideways resistance. The force is limited by the wheel body's mass
-and simulation step to avoid reversing lateral velocity in one step.
+positive, in meters. ``lateral_resistance`` defaults to 1.0 and scales lateral
+velocity correction by the wheel's allocated normal load and step duration;
+zero disables sideways resistance. ``friction`` defaults to 1.0 and caps the
+lateral force at the allocated normal load times the coefficient, allowing
+sliding when the required force exceeds the available grip.
 
 ``encoder_topic`` is optional. If provided, it publishes the signed forward
 velocity at the wheel center divided by its radius as a

docs/included_plugins/steering_motor.rst

@@ -0,0 +1,46 @@
+SteeringMotor
+=============
+
+SteeringMotor actuates a model's revolute Box2D joint and publishes its angle,
+angular speed and applied torque as joint state.
+
+.. code-block:: yaml
+
+  plugins:
+    - type: SteeringMotor
+      name: front_steering
+      joint: steering_joint
+      mode: position
+      max_effort: 100.0
+      max_speed: 10.0
+      position_gain: 10.0
+      limit:
+        lower: -0.5
+        upper: 0.5
+      joint_commands_topic: /robot_joint_commands
+      joint_states_topic: /robot_joint_states
+
+``joint`` names the required revolute joint. The plugin's ``name`` is the joint
+name used in ROS messages. ``mode`` is ``position`` (default), ``velocity``
+or ``effort``; commands have units of radians, rad/s or N m respectively.
+``max_effort`` (default 100 N m) limits motor or applied torque. In position
+mode ``position_gain`` (default 10 1/s) turns angle error into motor speed;
+``max_speed`` (default 10 rad/s) caps commanded speed in both position and
+velocity modes. Joint limits configured on the model remain active.
+
+The optional ``limit`` map sets lower and upper joint angles in radians.
+Both values default to NaN, which leaves any model-defined joint limits unchanged.
+If set, both must be finite and within [-0.95*pi, 0.95*pi], with ``upper``
+strictly greater than ``lower``. A valid pair enables the joint limits and
+replaces any model-defined bounds.
+
+The plugin subscribes to ``control_msgs/msg/JointCommand`` on
+``<joint_commands_topic>/<mode>`` and publishes ``sensor_msgs/msg/JointState``
+after each physics step on ``joint_states_topic`` using sensor-data QoS.
+Commands take effect only when ``interface_name`` matches ``mode`` and
+``joint_names`` includes the plugin's ``name``, paired with
+a finite value. Multiple steering joints can share the same command topic;
+each plugin uses only its named value. Published joint states use that same name.
+The default topics are ``/robot_joint_commands`` and ``/robot_joint_states``
+for the ros2_control joint command topic hardware interface. Absolute topic
+overrides remain global; relative overrides use the model namespace.

docs/index.rst

@@ -50,6 +50,8 @@ Class APIs are documented `here <http://flatland-simulator-api.readthedocs.io/>`
    included_plugins/bool_sensor
    included_plugins/diff_drive
    included_plugins/free_wheel
+   included_plugins/drive_wheel
+   included_plugins/steering_motor
    included_plugins/tricycle_drive
    included_plugins/laser
    included_plugins/model_tf_publisher

flatland-not-released.jazzy.repos

@@ -0,0 +1,5 @@
+repositories:
+  control_msgs:
+    type: git
+    url: https://github.com/ros-controls/control_msgs.git
+    version: jazzy

flatland-not-released.repos

@@ -0,0 +1,6 @@
+repositories:
+  # Temporary until joint_command_topic_hardware_interface is released for the supported ROS distros.
+  topic_based_hardware_interfaces:
+    type: git
+    url: https://github.com/ros-controls/topic_based_hardware_interfaces.git
+    version: main

flatland_plugins/CMakeLists.txt

@@ -25,6 +25,8 @@ find_package(ament_cmake REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(interactive_markers REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -71,6 +73,8 @@ add_library(flatland_plugins_lib SHARED
   src/tricycle_drive.cpp
   src/diff_drive.cpp
   src/free_wheel.cpp
+  src/drive_wheel.cpp
+  src/steering_motor.cpp
   src/imu.cpp
   src/dynamics_limits.cpp
   src/model_tf_publisher.cpp
@@ -103,6 +107,8 @@ target_link_libraries(flatland_plugins_lib PUBLIC
   tf2_geometry_msgs::tf2_geometry_msgs
   ${flatland_msgs_TARGETS}
   ${std_msgs_TARGETS}
+  ${control_msgs_TARGETS}
+  ${sensor_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   ${geometry_msgs_TARGETS}
   Boost::headers
@@ -114,7 +120,7 @@ target_link_libraries(flatland_plugins_lib PUBLIC
 
 ament_export_targets(export_flatland_plugins_lib HAS_LIBRARY_TARGET)
 # ament_export_libraries(flatland_lib)
-ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib std_msgs geometry_msgs tf2 tf2_ros tf2_msgs tf2_geometry_msgs yaml_cpp_vendor)
+ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib std_msgs control_msgs sensor_msgs nav_msgs geometry_msgs tf2 tf2_ros tf2_msgs tf2_geometry_msgs yaml_cpp_vendor)
 
 #############
 ## Install ##
@@ -159,6 +165,9 @@ if(BUILD_TESTING)
   ament_add_gtest(free_wheel_test test/free_wheel_test.cpp)
   target_link_libraries(free_wheel_test flatland_plugins_lib)
 
+  ament_add_gtest(joint_actuator_test test/joint_actuator_test.cpp)
+  target_link_libraries(joint_actuator_test flatland_plugins_lib)
+
   ament_add_gtest(dynamics_limits_test test/dynamics_limits_test.cpp)
   target_link_libraries(dynamics_limits_test flatland_plugins_lib)
 

flatland_plugins/include/flatland_plugins/drive_wheel.h

@@ -0,0 +1,40 @@
+#ifndef FLATLAND_PLUGINS_DRIVE_WHEEL_H
+#define FLATLAND_PLUGINS_DRIVE_WHEEL_H
+
+#include <flatland_server/model_plugin.h>
+#include <control_msgs/msg/joint_command.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+namespace flatland_plugins
+{
+
+class DriveWheel : public flatland_server::ModelPlugin
+{
+public:
+  void OnInitialize(const YAML::Node & config) override;
+  bool HasContactPoints() const override { return true; }
+  std::vector<flatland::b2Vec2> GetContactPoints() const override;
+  void UpdateGroundContactForces(const std::vector<double> & forces) override;
+  void BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper) override;
+  void AfterPhysicsStep(const flatland_server::Timekeeper & timekeeper) override;
+
+  flatland_server::Body * body_ = nullptr;
+  flatland::b2Vec2 offset_;
+  double theta_ = 0.0;
+  double radius_ = 0.0;
+  double friction_ = 1.0;
+  double lateral_resistance_ = 1.0;
+  double load_ = 0.0;
+  double position_ = 0.0;
+  double effort_ = 0.0;
+  double command_ = 0.0;
+  bool has_command_ = false;
+  std::string mode_;
+  std::string joint_name_;
+  rclcpp::Subscription<control_msgs::msg::JointCommand>::SharedPtr command_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr state_pub_;
+};
+
+}  // namespace flatland_plugins
+
+#endif  // FLATLAND_PLUGINS_DRIVE_WHEEL_H

flatland_plugins/include/flatland_plugins/free_wheel.h

@@ -11,13 +11,18 @@ class FreeWheel : public flatland_server::ModelPlugin
 {
 public:
   void OnInitialize(const YAML::Node & config) override;
+  bool HasContactPoints() const override { return true; }
+  std::vector<flatland::b2Vec2> GetContactPoints() const override;
+  void UpdateGroundContactForces(const std::vector<double> & forces) override;
   void BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper) override;
 
   flatland_server::Body * body_ = nullptr;
   flatland::b2Vec2 offset_;
   double theta_ = 0.0;
   double radius_ = 0.0;
+  double friction_ = 1.0;
   double lateral_resistance_ = 1.0;
+  double load_ = 0.0;
   rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr encoder_pub_;
 };
 

flatland_plugins/include/flatland_plugins/steering_motor.h

@@ -0,0 +1,31 @@
+#ifndef FLATLAND_PLUGINS_STEERING_MOTOR_H
+#define FLATLAND_PLUGINS_STEERING_MOTOR_H
+
+#include <flatland_server/model_plugin.h>
+#include <control_msgs/msg/joint_command.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+namespace flatland_plugins
+{
+
+class SteeringMotor : public flatland_server::ModelPlugin
+{
+public:
+  void OnInitialize(const YAML::Node & config) override;
+  void BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper) override;
+  void AfterPhysicsStep(const flatland_server::Timekeeper & timekeeper) override;
+
+  flatland::b2RevoluteJoint * joint_ = nullptr;
+  double max_effort_ = 100.0;
+  double max_speed_ = 10.0;
+  double position_gain_ = 10.0;
+  double command_ = 0.0;
+  bool has_command_ = false;
+  std::string mode_;
+  rclcpp::Subscription<control_msgs::msg::JointCommand>::SharedPtr command_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr state_pub_;
+};
+
+}  // namespace flatland_plugins
+
+#endif  // FLATLAND_PLUGINS_STEERING_MOTOR_H

flatland_plugins/package.xml

@@ -18,6 +18,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>control_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>