From 2d613c4bf15c383f3131cbc76f4efaa5b516d0d1 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Sun, 27 Sep 2026 22:39:00 +0000 Subject: [PATCH 01/13] Added and extended the existing free wheel, plus drive wheel and steering wheel to allow for better vehicle dynamics simulation. Expanded prior work on approximating ground contact forces. --- docs/core_functions/models.rst | 8 +- docs/included_plugins/drive_wheel.rst | 52 +++ docs/included_plugins/free_wheel.rst | 8 +- docs/included_plugins/steering_motor.rst | 46 +++ docs/index.rst | 2 + flatland_plugins/CMakeLists.txt | 11 +- .../include/flatland_plugins/drive_wheel.h | 40 ++ .../include/flatland_plugins/free_wheel.h | 5 + .../include/flatland_plugins/steering_motor.h | 31 ++ flatland_plugins/package.xml | 1 + flatland_plugins/plugin_description.xml | 6 + flatland_plugins/src/drive_wheel.cpp | 114 ++++++ flatland_plugins/src/free_wheel.cpp | 24 +- flatland_plugins/src/steering_motor.cpp | 106 ++++++ flatland_plugins/test/free_wheel_test.cpp | 36 ++ flatland_plugins/test/joint_actuator_test.cpp | 355 ++++++++++++++++++ flatland_server/include/Box2D/Box2D.h | 7 + .../include/flatland_server/model.h | 2 +- .../include/flatland_server/model_body.h | 3 +- .../include/flatland_server/model_plugin.h | 5 + flatland_server/src/model.cpp | 16 +- flatland_server/src/model_body.cpp | 46 ++- flatland_server/src/physics.cpp | 13 + flatland_server/src/plugin_manager.cpp | 85 +++++ flatland_server/test/load_world_test.cpp | 31 ++ flatland_server/test/model_test.cpp | 11 +- 26 files changed, 1028 insertions(+), 36 deletions(-) create mode 100644 docs/included_plugins/drive_wheel.rst create mode 100644 docs/included_plugins/steering_motor.rst create mode 100644 flatland_plugins/include/flatland_plugins/drive_wheel.h create mode 100644 flatland_plugins/include/flatland_plugins/steering_motor.h create mode 100644 flatland_plugins/src/drive_wheel.cpp create mode 100644 flatland_plugins/src/steering_motor.cpp create mode 100644 flatland_plugins/test/joint_actuator_test.cpp diff --git a/docs/core_functions/models.rst b/docs/core_functions/models.rst index f66d1ddc..bb649ac7 100644 --- a/docs/core_functions/models.rst +++ b/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 diff --git a/docs/included_plugins/drive_wheel.rst b/docs/included_plugins/drive_wheel.rst new file mode 100644 index 00000000..ba8c1265 --- /dev/null +++ b/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 +``/``. 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. \ No newline at end of file diff --git a/docs/included_plugins/free_wheel.rst b/docs/included_plugins/free_wheel.rst index 9380a9e0..f923bb12 100644 --- a/docs/included_plugins/free_wheel.rst +++ b/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 diff --git a/docs/included_plugins/steering_motor.rst b/docs/included_plugins/steering_motor.rst new file mode 100644 index 00000000..f45fe079 --- /dev/null +++ b/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 +``/`` 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. \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst index da90d97b..4d6f7f2a 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -50,6 +50,8 @@ Class APIs are documented `here ` 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 diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index e60be544..66c81987 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/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) diff --git a/flatland_plugins/include/flatland_plugins/drive_wheel.h b/flatland_plugins/include/flatland_plugins/drive_wheel.h new file mode 100644 index 00000000..0f50f157 --- /dev/null +++ b/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 +#include +#include + +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 GetContactPoints() const override; + void UpdateGroundContactForces(const std::vector & 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::SharedPtr command_sub_; + rclcpp::Publisher::SharedPtr state_pub_; +}; + +} // namespace flatland_plugins + +#endif // FLATLAND_PLUGINS_DRIVE_WHEEL_H \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/free_wheel.h b/flatland_plugins/include/flatland_plugins/free_wheel.h index ae911f4c..aaa5f02c 100644 --- a/flatland_plugins/include/flatland_plugins/free_wheel.h +++ b/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 GetContactPoints() const override; + void UpdateGroundContactForces(const std::vector & 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::SharedPtr encoder_pub_; }; diff --git a/flatland_plugins/include/flatland_plugins/steering_motor.h b/flatland_plugins/include/flatland_plugins/steering_motor.h new file mode 100644 index 00000000..76e8bead --- /dev/null +++ b/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 +#include +#include + +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::SharedPtr command_sub_; + rclcpp::Publisher::SharedPtr state_pub_; +}; + +} // namespace flatland_plugins + +#endif // FLATLAND_PLUGINS_STEERING_MOTOR_H \ No newline at end of file diff --git a/flatland_plugins/package.xml b/flatland_plugins/package.xml index de7d88fa..bbd94d29 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -18,6 +18,7 @@ pluginlib rclcpp std_msgs + control_msgs nav_msgs sensor_msgs geometry_msgs diff --git a/flatland_plugins/plugin_description.xml b/flatland_plugins/plugin_description.xml index a7d0868e..45e6048d 100644 --- a/flatland_plugins/plugin_description.xml +++ b/flatland_plugins/plugin_description.xml @@ -11,6 +11,12 @@ Passive wheel with lateral resistance and optional encoder + + Traction-limited wheel controlled by joint commands + + + Revolute steering joint controlled by joint commands + Flatland imu plugin diff --git a/flatland_plugins/src/drive_wheel.cpp b/flatland_plugins/src/drive_wheel.cpp new file mode 100644 index 00000000..48fd9a3d --- /dev/null +++ b/flatland_plugins/src/drive_wheel.cpp @@ -0,0 +1,114 @@ +#include +#include +#include + +#include +#include + +namespace flatland_plugins +{ + +void DriveWheel::OnInitialize(const YAML::Node & config) +{ + flatland_server::YamlReader reader(node_, config); + const auto body_name = reader.Get("body"); + const auto offset = reader.GetList("offset", {0.0, 0.0, 0.0}, 3, 3); + offset_ = flatland::b2Vec2(offset[0], offset[1]); + theta_ = offset[2]; + radius_ = reader.Get("radius"); + friction_ = reader.Get("friction", 1.0); + lateral_resistance_ = reader.Get("lateral_resistance", 1.0); + mode_ = reader.Get("mode", "velocity"); + joint_name_ = reader.Get("joint_name", GetName()); + const auto commands_topic = reader.Get("joint_commands_topic", "/robot_joint_commands"); + const auto states_topic = reader.Get("joint_states_topic", "/robot_joint_states"); + reader.EnsureAccessedAllKeys(); + + if (radius_ <= 0.0 || friction_ < 0.0 || lateral_resistance_ < 0.0 || + (mode_ != "velocity" && mode_ != "effort")) { + throw flatland_server::YAMLException("Invalid DriveWheel radius, friction or mode"); + } + body_ = GetModel()->GetBody(body_name); + if (!body_) { + throw flatland_server::YAMLException("DriveWheel body " + body_name + " does not exist"); + } + command_sub_ = node_->create_subscription( + GetModel()->NameSpaceTopic(commands_topic) + "/" + mode_, 1, + [this](control_msgs::msg::JointCommand::ConstSharedPtr message) { + if (message->interface_name != mode_ || message->joint_names.size() != message->values.size()) { + return; + } + for (size_t index = 0; index < message->joint_names.size(); ++index) { + if (message->joint_names[index] == GetName() && std::isfinite(message->values[index])) { + command_ = message->values[index]; + has_command_ = true; + } + } + }); + state_pub_ = node_->create_publisher( + GetModel()->NameSpaceTopic(states_topic), + rclcpp::SensorDataQoS()); +} + +std::vector DriveWheel::GetContactPoints() const +{ + return {body_->physics_body_->GetWorldPoint(offset_)}; +} + +void DriveWheel::UpdateGroundContactForces(const std::vector & forces) +{ + load_ = forces.empty() ? 0.0 : forces[0]; +} + +void DriveWheel::BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper) +{ + const double dt = timekeeper.GetStepSize(); + if (dt <= 0.0) { + return; + } + auto * physics_body = body_->physics_body_; + const auto velocity = physics_body->GetLinearVelocityFromLocalPoint(offset_); + const auto forward = physics_body->GetWorldVector( + flatland::b2Vec2(std::cos(theta_), std::sin(theta_))); + const flatland::b2Vec2 lateral(-forward.y, forward.x); + const double lateral_speed = velocity.x * lateral.x + velocity.y * lateral.y; + const double forward_speed = velocity.x * forward.x + velocity.y * forward.y; + const double traction = friction_ * load_; + const double lateral_force = std::clamp( + -lateral_resistance_ * (load_ / 9.81) * lateral_speed / dt, -traction, traction); + double drive_force = 0.0; + if (has_command_) { + if (mode_ == "effort") { + drive_force = command_ / radius_; + } else { + drive_force = (command_ * radius_ - forward_speed) * (load_ / 9.81) / dt; + } + } + drive_force = std::clamp( + drive_force, -std::sqrt(std::max(0.0, traction * traction - lateral_force * lateral_force)), + std::sqrt(std::max(0.0, traction * traction - lateral_force * lateral_force))); + const auto point = physics_body->GetWorldPoint(offset_); + physics_body->ApplyForce(forward * drive_force + lateral * lateral_force, point); + effort_ = drive_force * radius_; +} + +void DriveWheel::AfterPhysicsStep(const flatland_server::Timekeeper & timekeeper) +{ + const auto * physics_body = body_->physics_body_; + const auto velocity = physics_body->GetLinearVelocityFromLocalPoint(offset_); + const auto forward = physics_body->GetWorldVector( + flatland::b2Vec2(std::cos(theta_), std::sin(theta_))); + const double speed = (velocity.x * forward.x + velocity.y * forward.y) / radius_; + position_ += speed * timekeeper.GetStepSize(); + sensor_msgs::msg::JointState state; + state.header.stamp = timekeeper.GetSimTime(); + state.name = {joint_name_}; + state.position = {position_}; + state.velocity = {speed}; + state.effort = {effort_}; + state_pub_->publish(state); +} + +} // namespace flatland_plugins + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DriveWheel, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/free_wheel.cpp b/flatland_plugins/src/free_wheel.cpp index dda49938..c350b464 100644 --- a/flatland_plugins/src/free_wheel.cpp +++ b/flatland_plugins/src/free_wheel.cpp @@ -16,12 +16,13 @@ void FreeWheel::OnInitialize(const YAML::Node & config) offset_ = flatland::b2Vec2(offset[0], offset[1]); theta_ = offset[2]; radius_ = reader.Get("radius"); + friction_ = reader.Get("friction", 1.0); lateral_resistance_ = reader.Get("lateral_resistance", 1.0); const auto encoder_topic = reader.Get("encoder_topic", ""); reader.EnsureAccessedAllKeys(); - if (radius_ <= 0.0 || lateral_resistance_ < 0.0) { - throw flatland_server::YAMLException("FreeWheel radius must be positive and lateral_resistance nonnegative"); + if (radius_ <= 0.0 || friction_ < 0.0 || lateral_resistance_ < 0.0) { + throw flatland_server::YAMLException("Invalid FreeWheel radius, friction or lateral_resistance"); } body_ = GetModel()->GetBody(body_name); if (body_ == nullptr) { @@ -33,6 +34,16 @@ void FreeWheel::OnInitialize(const YAML::Node & config) } } +std::vector FreeWheel::GetContactPoints() const +{ + return {body_->physics_body_->GetWorldPoint(offset_)}; +} + +void FreeWheel::UpdateGroundContactForces(const std::vector & forces) +{ + load_ = forces.empty() ? 0.0 : forces[0]; +} + void FreeWheel::BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper) { auto * physics_body = body_->physics_body_; @@ -42,11 +53,10 @@ void FreeWheel::BeforePhysicsStep(const flatland_server::Timekeeper & timekeeper const flatland::b2Vec2 lateral(-forward.y, forward.x); const float lateral_speed = velocity.x * lateral.x + velocity.y * lateral.y; const double dt = timekeeper.GetStepSize(); - if (dt > 0.0 && lateral_resistance_ > 0.0 && lateral_speed != 0.0f) { - const float max_force = physics_body->GetMass() * std::abs(lateral_speed) / dt; - const float force = std::clamp( - -lateral_resistance_ * lateral_speed, -static_cast(max_force), - static_cast(max_force)); + if (dt > 0.0 && load_ > 0.0 && lateral_resistance_ > 0.0 && lateral_speed != 0.0f) { + const double grip = friction_ * load_; + const double force = std::clamp( + -lateral_resistance_ * (load_ / 9.81) * lateral_speed / dt, -grip, grip); physics_body->ApplyForce(lateral * force, physics_body->GetWorldPoint(offset_)); } diff --git a/flatland_plugins/src/steering_motor.cpp b/flatland_plugins/src/steering_motor.cpp new file mode 100644 index 00000000..72327b79 --- /dev/null +++ b/flatland_plugins/src/steering_motor.cpp @@ -0,0 +1,106 @@ +#include +#include +#include + +#include +#include +#include +#include + +namespace flatland_plugins +{ + +void SteeringMotor::OnInitialize(const YAML::Node & config) +{ + flatland_server::YamlReader reader(node_, config); + const auto joint_name = reader.Get("joint"); + mode_ = reader.Get("mode", "position"); + max_effort_ = reader.Get("max_effort", 100.0); + max_speed_ = reader.Get("max_speed", 10.0); + position_gain_ = reader.Get("position_gain", 10.0); + const auto commands_topic = reader.Get("joint_commands_topic", "/robot_joint_commands"); + const auto states_topic = reader.Get("joint_states_topic", "/robot_joint_states"); + auto limit_reader = reader.SubnodeOpt("limit", flatland_server::YamlReader::MAP); + const double no_limit = std::numeric_limits::quiet_NaN(); + const double lower = limit_reader.IsNodeNull() ? no_limit : limit_reader.Get("lower", no_limit); + const double upper = limit_reader.IsNodeNull() ? no_limit : limit_reader.Get("upper", no_limit); + if (!limit_reader.IsNodeNull()) { + limit_reader.EnsureAccessedAllKeys(); + } + reader.EnsureAccessedAllKeys(); + if ((mode_ != "position" && mode_ != "velocity" && mode_ != "effort") || + max_effort_ < 0.0 || max_speed_ <= 0.0 || position_gain_ <= 0.0) { + throw flatland_server::YAMLException("Invalid SteeringMotor mode, effort, speed or gain"); + } + if (!(std::isnan(lower) && std::isnan(upper)) && + !(std::isfinite(lower) && std::isfinite(upper) && + lower >= -0.95 * std::numbers::pi && upper <= 0.95 * std::numbers::pi && lower < upper)) { + throw flatland_server::YAMLException("SteeringMotor limits must both be NaN or ordered within +/-0.95*pi"); + } + auto * joint = GetModel()->GetJoint(joint_name); + if (!joint || joint->physics_joint_->GetType() != flatland::e_revoluteJoint) { + throw flatland_server::YAMLException("SteeringMotor joint " + joint_name + " must be revolute"); + } + joint_ = static_cast(joint->physics_joint_); + if (!std::isnan(lower)) { + joint_->SetLimits(lower, upper); + joint_->EnableLimit(true); + } + if (mode_ != "effort") { + joint_->SetMaxMotorTorque(max_effort_); + joint_->EnableMotor(true); + } + command_sub_ = node_->create_subscription( + GetModel()->NameSpaceTopic(commands_topic) + "/" + mode_, 1, + [this](control_msgs::msg::JointCommand::ConstSharedPtr message) { + if (message->interface_name != mode_ || message->joint_names.size() != message->values.size()) { + return; + } + for (size_t index = 0; index < message->joint_names.size(); ++index) { + if (message->joint_names[index] == GetName() && std::isfinite(message->values[index])) { + command_ = message->values[index]; + has_command_ = true; + } + } + }); + state_pub_ = node_->create_publisher( + GetModel()->NameSpaceTopic(states_topic), + rclcpp::SensorDataQoS()); +} + +void SteeringMotor::BeforePhysicsStep(const flatland_server::Timekeeper &) +{ + if (!has_command_) { + return; + } + if (mode_ == "effort") { + const float torque = std::clamp(command_, -max_effort_, max_effort_); + if (joint_->GetBodyA()->GetType() == flatland::b2_dynamicBody) { + joint_->GetBodyA()->ApplyTorque(-torque); + } + if (joint_->GetBodyB()->GetType() == flatland::b2_dynamicBody) { + joint_->GetBodyB()->ApplyTorque(torque); + } + } else { + const double speed = mode_ == "velocity" ? command_ : + position_gain_ * (command_ - joint_->GetJointAngle()); + joint_->SetMotorSpeed(std::clamp(speed, -max_speed_, max_speed_)); + } +} + +void SteeringMotor::AfterPhysicsStep(const flatland_server::Timekeeper & timekeeper) +{ + sensor_msgs::msg::JointState state; + state.header.stamp = timekeeper.GetSimTime(); + state.name = {GetName()}; + state.position = {joint_->GetJointAngle()}; + state.velocity = {joint_->GetJointSpeed()}; + state.effort = {mode_ == "effort" ? + (has_command_ ? std::clamp(command_, -max_effort_, max_effort_) : 0.0) : + joint_->GetMotorTorque()}; + state_pub_->publish(state); +} + +} // namespace flatland_plugins + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::SteeringMotor, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/test/free_wheel_test.cpp b/flatland_plugins/test/free_wheel_test.cpp index cba5288b..7f85b848 100644 --- a/flatland_plugins/test/free_wheel_test.cpp +++ b/flatland_plugins/test/free_wheel_test.cpp @@ -32,6 +32,13 @@ TEST(FreeWheelTest, ResistsLateralMotionAndPublishesEncoder) plugin->Initialize(node, "FreeWheel", "test_wheel", world->models_[0], config); world->plugin_manager_.model_plugins_.push_back(plugin); + const flatland_server::ModelPlugin & contact_plugin = *plugin; + EXPECT_TRUE(contact_plugin.HasContactPoints()); + const auto contact_points = contact_plugin.GetContactPoints(); + ASSERT_EQ(contact_points.size(), 1u); + EXPECT_FLOAT_EQ(contact_points[0].x, 3.0f); + EXPECT_FLOAT_EQ(contact_points[0].y, 3.0f); + std_msgs::msg::Float64::SharedPtr encoder; auto subscription = node->create_subscription( plugin->encoder_pub_->get_topic_name(), 1, @@ -72,6 +79,35 @@ TEST(FreeWheelTest, RejectsNonpositiveRadius) flatland_server::YAMLException); } +TEST(FreeWheelTest, LowSlipGripsAndHighSlipSlides) +{ + auto node = rclcpp::Node::make_shared("test_free_wheel_grip"); + auto world_path = std::filesystem::path(__FILE__).parent_path() / "update_timer_test/world.yaml"; + std::unique_ptr world( + flatland_server::World::MakeWorld(node, world_path.string())); + auto * body = world->models_[0]->GetBody("base")->physics_body_; + body->SetTransform(flatland::b2Vec2(2.0f, 3.0f), 0.0f); + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + config["friction"] = 0.8; + auto wheel = std::make_shared(); + wheel->Initialize(node, "FreeWheel", "grip_test", world->models_[0], config); + wheel->UpdateGroundContactForces({body->GetMass() * 9.81}); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + + body->SetLinearVelocity(flatland::b2Vec2(0.0f, 0.02f)); + wheel->BeforePhysicsStep(timekeeper); + world->Update(timekeeper); + EXPECT_NEAR(body->GetLinearVelocity().y, 0.0, 1e-4); + + body->SetLinearVelocity(flatland::b2Vec2(0.0f, 1.0f)); + wheel->BeforePhysicsStep(timekeeper); + world->Update(timekeeper); + EXPECT_NEAR(body->GetLinearVelocity().y, 1.0 - 0.8 * 9.81 * 0.01, 1e-4); +} + TEST(FreeWheelTest, EncoderIsOptional) { auto node = rclcpp::Node::make_shared("test_free_wheel_no_encoder"); diff --git a/flatland_plugins/test/joint_actuator_test.cpp b/flatland_plugins/test/joint_actuator_test.cpp new file mode 100644 index 00000000..28a86871 --- /dev/null +++ b/flatland_plugins/test/joint_actuator_test.cpp @@ -0,0 +1,355 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +std::unique_ptr MakeWorld(rclcpp::Node::SharedPtr node, bool steering) +{ + const auto root = std::filesystem::path(__FILE__).parent_path(); + const auto path = steering ? root.parent_path().parent_path() / + "flatland_server/test/load_world_tests/simple_test_A/world.yaml" : + root / "update_timer_test/world.yaml"; + return std::unique_ptr(flatland_server::World::MakeWorld(node, path.string())); +} + +template +void SendCommand( + rclcpp::Node::SharedPtr node, rclcpp::executors::SingleThreadedExecutor & executor, + const std::shared_ptr & plugin, const std::string & mode, double value) +{ + auto publisher = node->create_publisher( + plugin->command_sub_->get_topic_name(), 1); + control_msgs::msg::JointCommand command; + command.interface_name = mode; + command.joint_names = {plugin->GetName()}; + command.values = {value}; + rclcpp::WallRate rate(100); + for (int attempt = 0; attempt < 50 && !plugin->has_command_; ++attempt) { + publisher->publish(command); + executor.spin_some(); + rate.sleep(); + } + ASSERT_TRUE(plugin->has_command_); +} +} // namespace + +TEST(JointActuatorTest, LoadsFromPluginlib) +{ + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + EXPECT_NE(loader.createSharedInstance("flatland_plugins::DriveWheel"), nullptr); + EXPECT_NE(loader.createSharedInstance("flatland_plugins::SteeringMotor"), nullptr); +} + +TEST(JointActuatorTest, DistributesLoadAndLimitsWheelEffort) +{ + auto node = rclcpp::Node::make_shared("test_drive_wheel"); + auto world = MakeWorld(node, false); + auto * body = world->models_[0]->GetBody("base")->physics_body_; + std::vector> wheels; + for (double x : {-1.0, 1.0}) { + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + config["offset"] = std::vector{x, 0.0, 0.0}; + config["mode"] = "effort"; + config["friction"] = 0.5; + auto wheel = std::make_shared(); + wheel->Initialize(node, "DriveWheel", x < 0 ? "left" : "right", world->models_[0], config); + world->plugin_manager_.model_plugins_.push_back(wheel); + wheels.push_back(wheel); + } + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + SendCommand(node, executor, wheels.front(), "effort", 1000.0); + EXPECT_FALSE(wheels.back()->has_command_); + sensor_msgs::msg::JointState::SharedPtr state; + auto subscription = node->create_subscription( + wheels.front()->state_pub_->get_topic_name(), rclcpp::SensorDataQoS(), + [&state](sensor_msgs::msg::JointState::SharedPtr message) { + if (message->name[0] == "left") { + state = message; + } + }); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + const double center_x = body->GetWorldCenter().x; + world->Update(timekeeper); + + EXPECT_NEAR(wheels[0]->load_ + wheels[1]->load_, body->GetMass() * 9.81, 1e-3); + EXPECT_NEAR(wheels[0]->load_ / (wheels[0]->load_ + wheels[1]->load_), + std::clamp((1.0 - center_x) / 2.0, 0.0, 1.0), 1e-3); + EXPECT_GT(body->GetLinearVelocity().x, 0.0); + EXPECT_LE(wheels[0]->effort_, wheels[0]->friction_ * wheels[0]->load_ * wheels[0]->radius_ + 1e-5); + rclcpp::WallRate rate(100); + for (int attempt = 0; attempt < 50 && !state; ++attempt) { + executor.spin_some(); + rate.sleep(); + } + ASSERT_NE(state, nullptr); + EXPECT_EQ(state->name[0], "left"); + EXPECT_GT(state->position[0], 0.0); + EXPECT_GT(state->velocity[0], 0.0); + EXPECT_NEAR(state->effort[0], wheels[0]->effort_, 1e-4); +} + +TEST(JointActuatorTest, VelocityWheelAcceleratesTowardCommand) +{ + auto node = rclcpp::Node::make_shared("test_velocity_wheel"); + auto world = MakeWorld(node, false); + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + auto wheel = std::make_shared(); + wheel->Initialize(node, "DriveWheel", "velocity_wheel", world->models_[0], config); + world->plugin_manager_.model_plugins_.push_back(wheel); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + SendCommand(node, executor, wheel, "velocity", 1.0); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + world->Update(timekeeper); + EXPECT_GT(wheel->position_, 0.0); + EXPECT_GT(wheel->effort_, 0.0); +} + +TEST(JointActuatorTest, LateralGripIsLoadLimited) +{ + auto node = rclcpp::Node::make_shared("test_drive_wheel_grip"); + auto world = MakeWorld(node, false); + auto * body = world->models_[0]->GetBody("base")->physics_body_; + body->SetTransform(flatland::b2Vec2(2.0f, 3.0f), 0.0f); + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + config["friction"] = 0.8; + config["mode"] = "effort"; + auto wheel = std::make_shared(); + wheel->Initialize(node, "DriveWheel", "grip_test", world->models_[0], config); + wheel->UpdateGroundContactForces({body->GetMass() * 9.81}); + wheel->command_ = 1000.0; + wheel->has_command_ = true; + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + + body->SetLinearVelocity(flatland::b2Vec2(0.0f, 0.02f)); + wheel->BeforePhysicsStep(timekeeper); + EXPECT_GT(wheel->effort_, 0.0); + world->Update(timekeeper); + EXPECT_NEAR(body->GetLinearVelocity().y, 0.0, 1e-4); + + body->SetLinearVelocity(flatland::b2Vec2(0.0f, 1.0f)); + wheel->BeforePhysicsStep(timekeeper); + EXPECT_NEAR(wheel->effort_, 0.0, 1e-4); + world->Update(timekeeper); + EXPECT_NEAR(body->GetLinearVelocity().y, 1.0 - 0.8 * 9.81 * 0.01, 1e-4); +} + +TEST(JointActuatorTest, DriveWheelRejectsPositionMode) +{ + auto node = rclcpp::Node::make_shared("test_drive_wheel_position_mode"); + auto world = MakeWorld(node, false); + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + config["mode"] = "position"; + auto wheel = std::make_shared(); + EXPECT_THROW( + wheel->Initialize(node, "DriveWheel", "unsupported_wheel", world->models_[0], config), + flatland_server::YAMLException); +} + +TEST(JointActuatorTest, FourContactLoadsBalanceCenterOfMass) +{ + auto node = rclcpp::Node::make_shared("test_four_contact_loads"); + auto world = MakeWorld(node, false); + auto * body = world->models_[0]->GetBody("base")->physics_body_; + const auto center = body->GetWorldCenter(); + std::vector> wheels; + for (double x : {-1.0, 1.0}) { + for (double y : {-1.0, 1.0}) { + YAML::Node config; + config["body"] = "base"; + config["radius"] = 0.1; + config["offset"] = std::vector{x, y, 0.0}; + auto wheel = std::make_shared(); + wheel->Initialize(node, "DriveWheel", "wheel_" + std::to_string(wheels.size()), + world->models_[0], config); + world->plugin_manager_.model_plugins_.push_back(wheel); + wheels.push_back(wheel); + } + } + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + std::vector points; + for (const auto & wheel : wheels) { + points.push_back(wheel->GetContactPoints()[0]); + } + world->Update(timekeeper); + double total = 0.0, moment_x = 0.0, moment_y = 0.0; + for (size_t index = 0; index < wheels.size(); ++index) { + const auto & wheel = wheels[index]; + const auto point = points[index]; + total += wheel->load_; + moment_x += wheel->load_ * point.x; + moment_y += wheel->load_ * point.y; + EXPECT_GE(wheel->load_, 0.0); + } + EXPECT_NEAR(total, body->GetMass() * 9.81, 1e-3); + EXPECT_NEAR(moment_x / total, center.x, 1e-3); + EXPECT_NEAR(moment_y / total, center.y, 1e-3); +} + +TEST(JointActuatorTest, SteeringSupportsAllModes) +{ + for (const std::string mode : {"position", "velocity", "effort"}) { + auto node = rclcpp::Node::make_shared("test_steering_" + mode); + auto world = MakeWorld(node, true); + YAML::Node config; + config["joint"] = "tail_revolute"; + config["mode"] = mode; + auto motor = std::make_shared(); + motor->Initialize(node, "SteeringMotor", "steering", world->models_[0], config); + world->plugin_manager_.model_plugins_.push_back(motor); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + SendCommand(node, executor, motor, mode, mode == "effort" ? 5.0 : 0.5); + sensor_msgs::msg::JointState::SharedPtr state; + auto subscription = node->create_subscription( + motor->state_pub_->get_topic_name(), rclcpp::SensorDataQoS(), + [&state](sensor_msgs::msg::JointState::SharedPtr message) { state = message; }); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + for (int step = 0; step < 10; ++step) { + world->Update(timekeeper); + } + EXPECT_GT(motor->joint_->GetJointAngle(), 0.0) << mode; + rclcpp::WallRate rate(100); + for (int attempt = 0; attempt < 50 && !state; ++attempt) { + executor.spin_some(); + rate.sleep(); + } + ASSERT_NE(state, nullptr); + EXPECT_EQ(state->name[0], motor->GetName()); + EXPECT_GT(state->position[0], 0.0); + EXPECT_GT(state->velocity[0], 0.0); + } +} + +TEST(JointActuatorTest, SteeringRequiresPluginNameInCommand) +{ + auto node = rclcpp::Node::make_shared("test_steering_command_target"); + auto world = MakeWorld(node, true); + YAML::Node config; + config["joint"] = "tail_revolute"; + auto motor = std::make_shared(); + motor->Initialize(node, "SteeringMotor", "steering_plugin", world->models_[0], config); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto publisher = node->create_publisher( + motor->command_sub_->get_topic_name(), 1); + control_msgs::msg::JointCommand command; + command.interface_name = "position"; + command.joint_names = {"tail_revolute", "other_plugin"}; + command.values = {0.5, 0.7}; + rclcpp::WallRate rate(100); + for (int attempt = 0; attempt < 20; ++attempt) { + publisher->publish(command); + executor.spin_some(); + rate.sleep(); + } + EXPECT_FALSE(motor->has_command_); + + command.joint_names = {"other_plugin", motor->GetName()}; + command.values = {0.7, 0.5}; + for (int attempt = 0; attempt < 20 && !motor->has_command_; ++attempt) { + publisher->publish(command); + executor.spin_some(); + rate.sleep(); + } + EXPECT_TRUE(motor->has_command_); + EXPECT_DOUBLE_EQ(motor->command_, 0.5); +} + +TEST(JointActuatorTest, SteeringLimitsRotation) +{ + auto node = rclcpp::Node::make_shared("test_steering_limits"); + auto world = MakeWorld(node, true); + auto * tail = world->models_[0]->GetBody("tail")->physics_body_; + tail->SetTransform(tail->GetPosition(), 0.0f); + YAML::Node config; + config["joint"] = "tail_revolute"; + config["limit"]["lower"] = -0.1; + config["limit"]["upper"] = 0.2; + auto motor = std::make_shared(); + motor->Initialize(node, "SteeringMotor", "limited_steering", world->models_[0], config); + world->plugin_manager_.model_plugins_.push_back(motor); + ASSERT_TRUE(motor->joint_->IsLimitEnabled()); + EXPECT_NEAR(motor->joint_->GetLowerLimit(), -0.1, 1e-6); + EXPECT_NEAR(motor->joint_->GetUpperLimit(), 0.2, 1e-6); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + SendCommand(node, executor, motor, "position", 1.0); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + for (int step = 0; step < 30; ++step) { + world->Update(timekeeper); + EXPECT_GE(motor->joint_->GetJointAngle(), -0.12) << step; + EXPECT_LE(motor->joint_->GetJointAngle(), 0.22) << step; + } + EXPECT_GT(motor->joint_->GetJointAngle(), 0.0); +} + +TEST(JointActuatorTest, SteeringRejectsInvalidLimits) +{ + auto node = rclcpp::Node::make_shared("test_invalid_steering_limits"); + auto world = MakeWorld(node, true); + const double nan = std::numeric_limits::quiet_NaN(); + for (const auto & [lower, upper] : std::vector>{ + {nan, 0.2}, {-0.2, nan}, {0.2, 0.2}, {0.3, 0.2}, + {-std::numbers::pi, 0.2}, {-0.2, std::numbers::pi}, + {-0.2, std::numeric_limits::infinity()}}) { + YAML::Node config; + config["joint"] = "tail_revolute"; + config["limit"]["lower"] = lower; + config["limit"]["upper"] = upper; + auto motor = std::make_shared(); + EXPECT_THROW( + motor->Initialize(node, "SteeringMotor", "invalid_steering", world->models_[0], config), + flatland_server::YAMLException); + } +} + +TEST(JointActuatorTest, SteeringAcceptsNoLimits) +{ + auto node = rclcpp::Node::make_shared("test_unlimited_steering"); + auto world = MakeWorld(node, true); + for (bool explicit_nan : {false, true}) { + YAML::Node config; + config["joint"] = "tail_revolute"; + if (explicit_nan) { + config["limit"]["lower"] = std::numeric_limits::quiet_NaN(); + config["limit"]["upper"] = std::numeric_limits::quiet_NaN(); + } + auto motor = std::make_shared(); + motor->Initialize(node, "SteeringMotor", "unlimited_steering", world->models_[0], config); + EXPECT_FALSE(motor->joint_->IsLimitEnabled()); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/flatland_server/include/Box2D/Box2D.h b/flatland_server/include/Box2D/Box2D.h index 017887c8..c3c07c90 100644 --- a/flatland_server/include/Box2D/Box2D.h +++ b/flatland_server/include/Box2D/Box2D.h @@ -177,6 +177,7 @@ class b2Body void SetLinearVelocity(b2Vec2 velocity); void SetAngularVelocity(float velocity); void ApplyForce(b2Vec2 force, b2Vec2 point); + void ApplyTorque(float torque); void SetAwake(bool awake); void Dump() const; @@ -234,6 +235,12 @@ class b2RevoluteJoint : public b2Joint public: using b2Joint::b2Joint; b2JointType GetType() const override { return e_revoluteJoint; } + float GetJointAngle() const; + float GetJointSpeed() const; + float GetMotorTorque() const; + void EnableMotor(bool enabled); + void SetMotorSpeed(float speed); + void SetMaxMotorTorque(float torque); bool IsLimitEnabled() const; float GetLowerLimit() const; float GetUpperLimit() const; diff --git a/flatland_server/include/flatland_server/model.h b/flatland_server/include/flatland_server/model.h index 06c72985..ac3e879a 100644 --- a/flatland_server/include/flatland_server/model.h +++ b/flatland_server/include/flatland_server/model.h @@ -143,7 +143,7 @@ class Model : public Entity std::string NameSpaceTF(const std::string & frame_id) const; /** - * @return prepend the topic with namespace/ + * @return an absolute topic unchanged, or a relative topic prefixed with namespace/ */ std::string NameSpaceTopic(const std::string & topic) const; diff --git a/flatland_server/include/flatland_server/model_body.h b/flatland_server/include/flatland_server/model_body.h index 2f6846e7..a1ad49ac 100644 --- a/flatland_server/include/flatland_server/model_body.h +++ b/flatland_server/include/flatland_server/model_body.h @@ -104,8 +104,9 @@ class ModelBody : public Body * @param[in] footprint_reader YAML reader for node containing the footprint * parameters * @param[out] fixture_def Box2D fixture definition + * @param[in] area Area of the footprint geometry */ - void ConfigFootprintDef(YamlReader & footprint_reader, flatland::b2FixtureDef & fixture_def); + void ConfigFootprintDef(YamlReader & footprint_reader, flatland::b2FixtureDef & fixture_def, float area); /** * @brief Loads a circle footprint diff --git a/flatland_server/include/flatland_server/model_plugin.h b/flatland_server/include/flatland_server/model_plugin.h index 9fde260c..04961681 100644 --- a/flatland_server/include/flatland_server/model_plugin.h +++ b/flatland_server/include/flatland_server/model_plugin.h @@ -54,6 +54,7 @@ #include #include +#include namespace flatland_server { @@ -83,6 +84,10 @@ class ModelPlugin : public FlatlandPlugin */ virtual void OnInitialize(const YAML::Node & config) = 0; + virtual bool HasContactPoints() const { return false; } + virtual std::vector GetContactPoints() const { return {}; } + virtual void UpdateGroundContactForces(const std::vector &/* forces */) {} + /** * @brief The method to initialize the ModelPlugin, required since Pluginlib * require the class to have a default constructor diff --git a/flatland_server/src/model.cpp b/flatland_server/src/model.cpp index c534c328..0415c6cd 100644 --- a/flatland_server/src/model.cpp +++ b/flatland_server/src/model.cpp @@ -205,17 +205,13 @@ std::string Model::NameSpaceTF(const std::string & frame_id) const std::string Model::NameSpaceTopic(const std::string & topic) const { - // We don't actually want the topic to be "global" in case flatland is itself - // namespaced, so strip leading slash - if (topic.substr(0, 1) == "/") { - return std::string(topic, 1, std::string::npos); // Strip the leading '/' - } else { // case: "local" namespace: prepend namespace/ - if (namespace_.length() > 0) { - return namespace_ + "/" + topic; - } else { - return topic; - } + if (!topic.empty() && topic.front() == '/') { + return topic; + } + if (!namespace_.empty()) { + return namespace_ + "/" + topic; } + return topic; } const std::string & Model::GetName() const { return name_; } diff --git a/flatland_server/src/model_body.cpp b/flatland_server/src/model_body.cpp index 93703f07..95222704 100644 --- a/flatland_server/src/model_body.cpp +++ b/flatland_server/src/model_body.cpp @@ -48,6 +48,8 @@ #include #include +#include +#include #include namespace flatland_server @@ -129,10 +131,24 @@ void ModelBody::LoadFootprints(YamlReader & footprints_reader) } } -void ModelBody::ConfigFootprintDef(YamlReader & footprint_reader, flatland::b2FixtureDef & fixture_def) +void ModelBody::ConfigFootprintDef(YamlReader & footprint_reader, flatland::b2FixtureDef & fixture_def, float area) { // configure physics properties - fixture_def.density = footprint_reader.Get("density"); + if (footprint_reader.Node()["mass"]) { + float mass = footprint_reader.Get("mass"); + if (footprint_reader.Node()["density"]) { + footprint_reader.Subnode("density", YamlReader::NO_CHECK); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("ModelBody"), "Both mass and density set for footprint in " + << footprint_reader.entry_location_ << "; using mass"); + } + if (!std::isfinite(mass) || mass < 0 || !std::isfinite(area) || area <= 0) { + throw YAMLException("Invalid footprint mass or area in " + footprint_reader.entry_location_); + } + fixture_def.density = mass / area; + } else { + fixture_def.density = footprint_reader.Get("density"); + } fixture_def.friction = footprint_reader.Get("friction", 0.0); fixture_def.restitution = footprint_reader.Get("restitution", 0.0); @@ -168,7 +184,7 @@ void ModelBody::LoadCircleFootprint(YamlReader & footprint_reader) double radius = footprint_reader.Get("radius"); flatland::b2FixtureDef fixture_def; - ConfigFootprintDef(footprint_reader, fixture_def); + ConfigFootprintDef(footprint_reader, fixture_def, std::numbers::pi_v * radius * radius); flatland::b2CircleShape shape; shape.m_p.Set(center.x, center.y); @@ -183,11 +199,31 @@ void ModelBody::LoadPolygonFootprint(YamlReader & footprint_reader) std::vector points = footprint_reader.GetList("points", 3, flatland::b2_maxPolygonVertices); flatland::b2FixtureDef fixture_def; - ConfigFootprintDef(footprint_reader, fixture_def); - flatland::b2PolygonShape shape; + bool valid = true; try { shape.Set(points.data(), points.size()); + } catch (const std::invalid_argument & e) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("ModelBody"), "Skipping invalid polygon footprint for body \"" << name_ + << "\" in " + << footprint_reader.entry_location_ + << ": " << e.what()); + valid = false; + } + float area = 1.0f; + if (valid) { + float twice_area = 0.0f; + for (int index = 0; index < shape.m_count; ++index) { + const auto & current = shape.m_vertices[index]; + const auto & next = shape.m_vertices[(index + 1) % shape.m_count]; + twice_area += current.x * next.y - next.x * current.y; + } + area = std::abs(twice_area) / 2.0f; + } + ConfigFootprintDef(footprint_reader, fixture_def, area); + if (!valid) return; + try { fixture_def.shape = &shape; physics_body_->CreateFixture(&fixture_def); } catch (const std::invalid_argument & e) { diff --git a/flatland_server/src/physics.cpp b/flatland_server/src/physics.cpp index b7d1ce69..f7bf2014 100644 --- a/flatland_server/src/physics.cpp +++ b/flatland_server/src/physics.cpp @@ -220,6 +220,7 @@ float b2Body::GetAngularDamping() const { return ::b2Body_GetAngularDamping(id_) void b2Body::SetLinearVelocity(b2Vec2 value) { ::b2Body_SetLinearVelocity(id_, value); } void b2Body::SetAngularVelocity(float value) { ::b2Body_SetAngularVelocity(id_, value); } void b2Body::ApplyForce(b2Vec2 force, b2Vec2 point) { ::b2Body_ApplyForce(id_, force, point, true); } +void b2Body::ApplyTorque(float torque) { ::b2Body_ApplyTorque(id_, torque, true); } void b2Body::SetAwake(bool awake) { ::b2Body_SetAwake(id_, awake); } void b2Body::Dump() const { @@ -247,6 +248,18 @@ void b2Joint::Dump() const b2Log("joint type=%d anchor=(%g, %g) connected=%d\n", GetType(), anchor.x, anchor.y, GetCollideConnected()); } +float b2RevoluteJoint::GetJointAngle() const { return ::b2RevoluteJoint_GetAngle(id_); } +float b2RevoluteJoint::GetJointSpeed() const +{ + return body_b_->GetAngularVelocity() - body_a_->GetAngularVelocity(); +} +float b2RevoluteJoint::GetMotorTorque() const { return ::b2RevoluteJoint_GetMotorTorque(id_); } +void b2RevoluteJoint::EnableMotor(bool enabled) { ::b2RevoluteJoint_EnableMotor(id_, enabled); } +void b2RevoluteJoint::SetMotorSpeed(float speed) { ::b2RevoluteJoint_SetMotorSpeed(id_, speed); } +void b2RevoluteJoint::SetMaxMotorTorque(float torque) +{ + ::b2RevoluteJoint_SetMaxMotorTorque(id_, torque); +} bool b2RevoluteJoint::IsLimitEnabled() const { return ::b2RevoluteJoint_IsLimitEnabled(id_); } float b2RevoluteJoint::GetLowerLimit() const { return ::b2RevoluteJoint_GetLowerLimit(id_); } float b2RevoluteJoint::GetUpperLimit() const { return ::b2RevoluteJoint_GetUpperLimit(id_); } diff --git a/flatland_server/src/plugin_manager.cpp b/flatland_server/src/plugin_manager.cpp index 2e6ded2d..08874a81 100644 --- a/flatland_server/src/plugin_manager.cpp +++ b/flatland_server/src/plugin_manager.cpp @@ -52,6 +52,12 @@ #include #include +#include +#include +#include +#include +#include + namespace flatland_server { @@ -90,6 +96,85 @@ PluginManager::~PluginManager() void PluginManager::BeforePhysicsStep(const Timekeeper & timekeeper_) { + std::map>> contacts; + for (const auto & plugin : model_plugins_) { + if (plugin->HasContactPoints()) { + contacts[plugin->GetModel()].push_back(plugin); + } + } + for (const auto & [model, plugins] : contacts) { + std::vector points; + std::vector counts; + for (const auto & plugin : plugins) { + auto plugin_points = plugin->GetContactPoints(); + counts.push_back(plugin_points.size()); + points.insert(points.end(), plugin_points.begin(), plugin_points.end()); + } + if (points.empty()) { + continue; + } + + double mass = 0.0; + flatland::b2Vec2 center(0.0f, 0.0f); + for (const auto * body : model->GetBodies()) { + const auto * physics_body = body->physics_body_; + mass += physics_body->GetMass(); + center = center + physics_body->GetMass() * physics_body->GetWorldCenter(); + } + if (mass > 0.0) { + center *= 1.0 / mass; + } + + std::vector weights(points.size(), 0.0); + std::vector active(points.size()); + std::iota(active.begin(), active.end(), 0); + while (!active.empty()) { + flatland::b2Vec2 mean(0.0f, 0.0f); + for (auto index : active) { + mean = mean + points[index]; + } + mean *= 1.0 / active.size(); + double xx = 0.0, xy = 0.0, yy = 0.0; + for (auto index : active) { + const auto delta = points[index] - mean; + xx += delta.x * delta.x; + xy += delta.x * delta.y; + yy += delta.y * delta.y; + } + const auto target = center - mean; + double x = 0.0, y = 0.0; + const double determinant = xx * yy - xy * xy; + if (determinant > 1e-10 * (xx + yy) * (xx + yy)) { + x = (yy * target.x - xy * target.y) / determinant; + y = (xx * target.y - xy * target.x) / determinant; + } else if (xx >= yy && xx > 1e-10) { + x = target.x / xx; + } else if (yy > 1e-10) { + y = target.y / yy; + } + for (auto index : active) { + const auto delta = points[index] - mean; + weights[index] = 1.0 / active.size() + delta.x * x + delta.y * y; + } + auto lowest = std::min_element(active.begin(), active.end(), [&](size_t a, size_t b) { + return weights[a] < weights[b]; + }); + if (weights[*lowest] >= 0.0 || active.size() == 1) { + break; + } + weights[*lowest] = 0.0; + active.erase(lowest); + } + size_t start = 0; + for (size_t index = 0; index < plugins.size(); ++index) { + std::vector forces(weights.begin() + start, weights.begin() + start + counts[index]); + for (auto & force : forces) { + force *= mass * 9.81; + } + plugins[index]->UpdateGroundContactForces(forces); + start += counts[index]; + } + } for (const auto & model_plugin : model_plugins_) { model_plugin->BeforePhysicsStep(timekeeper_); } diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp index 5f13dae3..90b8143a 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -49,8 +49,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -759,6 +761,35 @@ TEST_F(LoadWorldTest, simple_test_A) BodyEq(m3->bodies_[0], "body", flatland::b2_kinematicBody, {0, 1, 2}, {0, 0.75, 0.75, 0.25}, 0, 0)); } +TEST_F(LoadWorldTest, footprint_mass) +{ + auto node = rclcpp::Node::make_shared("footprint_mass_node"); + flatland::b2World physics_world({0.0f, 0.0f}); + CollisionFilterRegistry cfr; + ModelBody body( + &physics_world, &cfr, nullptr, "mass_body", Color(1, 1, 1, 1), Pose(0, 0, 0), + flatland::b2_dynamicBody, YAML::Node(), 0, 0); + YamlReader footprints(node, YAML::Load(R"( + - {type: circle, radius: 2, mass: 8} + - {type: polygon, points: [[0, 0], [2, 0], [2, 1], [0, 1]], mass: 6, density: ignored} + - {type: circle, radius: 1, density: 3} + )")); + + testing::internal::CaptureStderr(); + body.LoadFootprints(footprints); + EXPECT_NE(testing::internal::GetCapturedStderr().find("using mass"), std::string::npos); + + auto fixtures = GetBodyFixtures(&body); + ASSERT_EQ(fixtures.size(), 3u); + EXPECT_NEAR(fixtures[0]->GetDensity(), 8.0 / (4.0 * std::numbers::pi), 1e-5); + EXPECT_NEAR(fixtures[1]->GetDensity(), 3.0, 1e-5); + EXPECT_NEAR(fixtures[2]->GetDensity(), 3.0, 1e-5); + EXPECT_NEAR(body.physics_body_->GetMass(), 14.0 + 3.0 * std::numbers::pi, 1e-4); + + YamlReader missing(node, YAML::Load("[{type: circle, radius: 1}]")); + EXPECT_THROW(body.LoadFootprints(missing), YAMLException); +} + /** * This test tries to loads a non-existent world yaml file. It should throw * an exception diff --git a/flatland_server/test/model_test.cpp b/flatland_server/test/model_test.cpp index 807e568b..40ed7668 100644 --- a/flatland_server/test/model_test.cpp +++ b/flatland_server/test/model_test.cpp @@ -75,16 +75,15 @@ TEST(TestSuite, testNameSpaceTopic) { std::shared_ptr node = rclcpp::Node::make_shared("testNameSpaceTopic_node"); flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), std::string("has_ns")); - // namespace "foo" onto tf "bar" => foo_bar + // namespace "foo" onto topic "bar" => foo/bar EXPECT_EQ(has_ns.NameSpaceTopic("bar"), "foo/bar"); - // namespace "foo" onto tf "/bar" => bar - EXPECT_EQ(has_ns.NameSpaceTopic("/bar"), "bar"); + // absolute topics are not namespaced + EXPECT_EQ(has_ns.NameSpaceTopic("/bar"), "/bar"); flatland_server::Model no_ns(node, nullptr, nullptr, std::string(""), std::string("no_ns")); - // namespace "" onto tf "bar" => bar + // namespace "" onto topic "bar" => bar EXPECT_EQ(no_ns.NameSpaceTopic("bar"), "bar"); - // namespace "" onto tf "/bar" => bar - EXPECT_EQ(no_ns.NameSpaceTopic("/bar"), "bar"); + EXPECT_EQ(no_ns.NameSpaceTopic("/bar"), "/bar"); } // Run all the tests that were declared with TEST() From e11f39b0a5f1df1575703e145d0a4f2edc7ed7e3 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Sun, 27 Sep 2026 22:40:29 +0000 Subject: [PATCH 02/13] Added a number of ros2 control examples. 4/6 of them appear to work as intended, but the swerve and articulated examples need some sort of translation. Maybe swerve could be driven pretending to be ackerman but that would be incorrect. --- flatland_ros2_control_examples/CMakeLists.txt | 31 ++++ flatland_ros2_control_examples/README.md | 77 ++++++++ .../config/2910_swerve.yaml | 17 ++ .../config/a300_diff.yaml | 14 ++ .../config/articulated_204g.yaml | 17 ++ .../config/caster_diff.yaml | 14 ++ .../config/front_drive_tricycle.yaml | 15 ++ .../config/rear_drive_ackermann.yaml | 16 ++ .../launch/example.launch.py | 171 ++++++++++++++++++ .../models/2910_swerve.model.yaml | 63 +++++++ .../models/a300_diff.model.yaml | 18 ++ .../models/articulated_204g.model.yaml | 35 ++++ .../models/caster_diff.model.yaml | 32 ++++ .../models/front_drive_tricycle.model.yaml | 28 +++ .../models/rear_drive_ackermann.model.yaml | 41 +++++ flatland_ros2_control_examples/package.xml | 33 ++++ .../test/controller_config_test.py | 146 +++++++++++++++ .../test/worlds_test.cpp | 106 +++++++++++ .../worlds/2910_swerve.world.yaml | 6 + .../worlds/a300_diff.world.yaml | 6 + .../worlds/articulated_204g.world.yaml | 6 + .../worlds/caster_diff.world.yaml | 6 + .../worlds/front_drive_tricycle.world.yaml | 6 + .../worlds/rear_drive_ackermann.world.yaml | 6 + 24 files changed, 910 insertions(+) create mode 100644 flatland_ros2_control_examples/CMakeLists.txt create mode 100644 flatland_ros2_control_examples/README.md create mode 100644 flatland_ros2_control_examples/config/2910_swerve.yaml create mode 100644 flatland_ros2_control_examples/config/a300_diff.yaml create mode 100644 flatland_ros2_control_examples/config/articulated_204g.yaml create mode 100644 flatland_ros2_control_examples/config/caster_diff.yaml create mode 100644 flatland_ros2_control_examples/config/front_drive_tricycle.yaml create mode 100644 flatland_ros2_control_examples/config/rear_drive_ackermann.yaml create mode 100644 flatland_ros2_control_examples/launch/example.launch.py create mode 100644 flatland_ros2_control_examples/models/2910_swerve.model.yaml create mode 100644 flatland_ros2_control_examples/models/a300_diff.model.yaml create mode 100644 flatland_ros2_control_examples/models/articulated_204g.model.yaml create mode 100644 flatland_ros2_control_examples/models/caster_diff.model.yaml create mode 100644 flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml create mode 100644 flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml create mode 100644 flatland_ros2_control_examples/package.xml create mode 100644 flatland_ros2_control_examples/test/controller_config_test.py create mode 100644 flatland_ros2_control_examples/test/worlds_test.cpp create mode 100644 flatland_ros2_control_examples/worlds/2910_swerve.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/a300_diff.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/articulated_204g.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/caster_diff.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/front_drive_tricycle.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/rear_drive_ackermann.world.yaml diff --git a/flatland_ros2_control_examples/CMakeLists.txt b/flatland_ros2_control_examples/CMakeLists.txt new file mode 100644 index 00000000..0bb3b7a4 --- /dev/null +++ b/flatland_ros2_control_examples/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.10) +project(flatland_ros2_control_examples) + +if(POLICY CMP0167) + cmake_policy(SET CMP0167 NEW) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY models worlds launch config + DESTINATION share/${PROJECT_NAME}) +install(FILES README.md DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(interactive_markers REQUIRED) + find_package(flatland_server REQUIRED) + find_package(flatland_plugins REQUIRED) + find_package(rclcpp REQUIRED) + find_package(OpenCV REQUIRED) + ament_add_gtest(worlds_test test/worlds_test.cpp) + target_include_directories(worlds_test PRIVATE ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(worlds_test flatland_server::flatland_lib + flatland_plugins::flatland_plugins_lib rclcpp::rclcpp) + + find_package(Python3 REQUIRED COMPONENTS Interpreter) + add_test(NAME controller_config_test + COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test/controller_config_test.py) +endif() + +ament_package() \ No newline at end of file diff --git a/flatland_ros2_control_examples/README.md b/flatland_ros2_control_examples/README.md new file mode 100644 index 00000000..499d4b90 --- /dev/null +++ b/flatland_ros2_control_examples/README.md @@ -0,0 +1,77 @@ +# Flatland ros2_control examples + +Six planar robot models demonstrate traction-limited `DriveWheel`, passive +`FreeWheel`, and revolute `SteeringMotor` plugins. Coordinates are in meters; +forward is +x and left is +y. These are illustrative layouts, not manufacturer +CAD reproductions or calibrated vehicle dynamics. + +| `robot` launch value | Geometry | ros2_control controller | +| --- | --- | --- | +| `caster_diff` | Two drive wheels and a rear trailing caster on a free revolute swivel | `diff_drive_controller` | +| `a300_diff` | Four fixed drive wheels, two per side, A300-style skid steering | `diff_drive_controller` | +| `2910_swerve` | Four independently steered and driven corner pods, inspired by [2910's Charged Up drivebase](https://frcdesign.org/mechanism-examples/drivebase/swerve/2910_2023_dt/) | Separate forward command controllers for wheel speeds and steering angles | +| `rear_drive_ackermann` | Two rear drive wheels and two front passive wheels on independent steered knuckles | `ackermann_steering_controller` | +| `front_drive_tricycle` | One driven and steered front wheel, two rear passive wheels, Neo 2.0-style | `tricycle_controller` with Ackermann command output | +| `articulated_204g` | Front and rear driven chassis joined by a steering hinge limited to +/-40 degrees, 204 G-Tier-style | Separate forward command controllers for wheel speeds and articulation | + +`tricycle_controller` is the ros2_controllers controller for a *single wheel +that both drives and steers*, and can publish an Ackermann command. The stock +`ackermann_steering_controller` needs two driven wheels and two separate steering +joints, so it is used on the rear-drive car instead. The swerve and articulated +profiles expose individual joint commands; they do not include body-twist to +wheel inverse kinematics. Swerve steering is limited to +/-2.98 radians, rather +than continuous swivel rotation. + +## Run + +Build after sourcing ROS and the Flatland dependencies: + +```sh +colcon build --packages-select flatland_ros2_control_examples +source install/setup.bash +ros2 launch flatland_ros2_control_examples example.launch.py robot:=caster_diff +``` + +Change `robot` to any value in the table. The default launch starts Flatland +only. To also start controller manager, the topic-based hardware interface, +robot state publisher, controllers and joystick teleop, install those ROS packages +and run: + +```sh +ros2 launch flatland_ros2_control_examples example.launch.py \ + robot:=rear_drive_ackermann use_ros2_control:=true +``` + +Joystick teleop is enabled by default with ros2_control; set `use_joystick:=false` +to skip it or `joy_device_id:=1` to select another device. Hold button 4 +(left bumper on a typical gamepad), use axis 1 for forward speed and axis 3 +for turning. `teleop_twist_joy` publishes TwistStamped on `/drive/cmd_vel` +for the differential, tricycle and Ackermann models. +The swerve and articulated profiles use forward joint controllers and do not +consume Twist commands; command their wheel and steering controllers directly. + +The six `models/*.model.yaml` files specify physical bodies, contact offsets, +wheel radii and plugin names; `worlds/*.world.yaml` each load one model. +`config/*.yaml` holds the corresponding controller configuration. The launch +file derives the ros2_control URDF joint interfaces from the model YAML, so +the plugin `name` is the joint name for controller commands and published +joint states. `joint` in a SteeringMotor identifies the underlying Flatland +revolute joint, which may have a different name. DriveWheel takes velocity +(rad/s) or effort (N m); these examples command velocity. SteeringMotor uses +position (rad). FreeWheel contributes a contact point but has no commanded +joint interface or JointState publisher. + +The topic hardware plugin exchanges `control_msgs/msg/JointCommand` on +`/robot_joint_commands/velocity` and `/robot_joint_commands/position`, and +receives `sensor_msgs/msg/JointState` on `/robot_joint_states`. Multiple plugins +publish single-joint states to that topic; ros2_control merges them. Flatland +physics and ros2_control are separate processes, so start Flatland before +sending commands. For example, `diff_drive_controller` accepts +`geometry_msgs/msg/TwistStamped` on `/drive/cmd_vel`; the Ackermann controller's +`~/reference` input is remapped there by launch. Forward controllers accept +`std_msgs/msg/Float64MultiArray` on their +`//commands` topics in the order given in their YAML configs. + +The ros2_control URDF generated by launch is for hardware registration, not a +visual or collision model; Flatland's YAML is authoritative for geometry. The +profile dimensions and controller parameters are starting points for tuning. \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/2910_swerve.yaml b/flatland_ros2_control_examples/config/2910_swerve.yaml new file mode 100644 index 00000000..5c9fb136 --- /dev/null +++ b/flatland_ros2_control_examples/config/2910_swerve.yaml @@ -0,0 +1,17 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + wheels: + type: forward_command_controller/ForwardCommandController + steering: + type: forward_command_controller/ForwardCommandController +wheels: + ros__parameters: + joints: [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel] + interface_name: velocity +steering: + ros__parameters: + joints: [front_left_steer, front_right_steer, rear_left_steer, rear_right_steer] + interface_name: position \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/a300_diff.yaml b/flatland_ros2_control_examples/config/a300_diff.yaml new file mode 100644 index 00000000..7b1c0c06 --- /dev/null +++ b/flatland_ros2_control_examples/config/a300_diff.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + drive: + type: diff_drive_controller/DiffDriveController +drive: + ros__parameters: + left_wheel_names: [front_left_wheel, rear_left_wheel] + right_wheel_names: [front_right_wheel, rear_right_wheel] + wheel_separation: 0.64 + wheel_radius: 0.15 + enable_odom_tf: false \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/articulated_204g.yaml b/flatland_ros2_control_examples/config/articulated_204g.yaml new file mode 100644 index 00000000..2019e409 --- /dev/null +++ b/flatland_ros2_control_examples/config/articulated_204g.yaml @@ -0,0 +1,17 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + wheels: + type: forward_command_controller/ForwardCommandController + steering: + type: forward_command_controller/ForwardCommandController +wheels: + ros__parameters: + joints: [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel] + interface_name: velocity +steering: + ros__parameters: + joints: [articulation] + interface_name: position \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/caster_diff.yaml b/flatland_ros2_control_examples/config/caster_diff.yaml new file mode 100644 index 00000000..0e527d94 --- /dev/null +++ b/flatland_ros2_control_examples/config/caster_diff.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + drive: + type: diff_drive_controller/DiffDriveController +drive: + ros__parameters: + left_wheel_names: [left_wheel] + right_wheel_names: [right_wheel] + wheel_separation: 0.32 + wheel_radius: 0.07 + enable_odom_tf: false \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/front_drive_tricycle.yaml b/flatland_ros2_control_examples/config/front_drive_tricycle.yaml new file mode 100644 index 00000000..bdcbdc79 --- /dev/null +++ b/flatland_ros2_control_examples/config/front_drive_tricycle.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + drive: + type: tricycle_controller/TricycleController +drive: + ros__parameters: + traction_joint_name: front_wheel + steering_joint_name: front_steer + wheelbase: 0.94 + wheel_radius: 0.17 + publish_ackermann_command: true + enable_odom_tf: false \ No newline at end of file diff --git a/flatland_ros2_control_examples/config/rear_drive_ackermann.yaml b/flatland_ros2_control_examples/config/rear_drive_ackermann.yaml new file mode 100644 index 00000000..d83ed566 --- /dev/null +++ b/flatland_ros2_control_examples/config/rear_drive_ackermann.yaml @@ -0,0 +1,16 @@ +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + drive: + type: ackermann_steering_controller/AckermannSteeringController +drive: + ros__parameters: + traction_joints_names: [rear_right_wheel, rear_left_wheel] + steering_joints_names: [front_right_steer, front_left_steer] + traction_track_width: 0.64 + steering_track_width: 0.64 + wheelbase: 1.04 + traction_wheels_radius: 0.19 + enable_odom_tf: false \ No newline at end of file diff --git a/flatland_ros2_control_examples/launch/example.launch.py b/flatland_ros2_control_examples/launch/example.launch.py new file mode 100644 index 00000000..906c16a1 --- /dev/null +++ b/flatland_ros2_control_examples/launch/example.launch.py @@ -0,0 +1,171 @@ +from pathlib import Path +from xml.etree import ElementTree + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import Action, LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +EXAMPLES = ( + "caster_diff", + "a300_diff", + "2910_swerve", + "rear_drive_ackermann", + "front_drive_tricycle", + "articulated_204g", +) + + +def robot_description(model): + robot = ElementTree.Element("robot", name="flatland_example") + ElementTree.SubElement(robot, "link", name="base_link") + control = ElementTree.SubElement(robot, "ros2_control", name="Flatland", type="system") + hardware = ElementTree.SubElement(control, "hardware") + ElementTree.SubElement(hardware, "plugin").text = ( + "joint_command_topic_hardware_interface/JointCommandTopicSystem" + ) + for parameter, value in ( + ("joint_commands_topic", "/robot_joint_commands"), + ("joint_states_topic", "/robot_joint_states"), + ): + ElementTree.SubElement(hardware, "param", name=parameter).text = value + + for plugin in model["plugins"]: + if plugin["type"] not in ("DriveWheel", "SteeringMotor"): + continue + name = plugin["name"] + is_steering = plugin["type"] == "SteeringMotor" + ElementTree.SubElement(robot, "link", name=f"{name}_link") + joint = ElementTree.SubElement( + robot, "joint", name=name, type="revolute" if is_steering else "continuous" + ) + ElementTree.SubElement(joint, "parent", link="base_link") + ElementTree.SubElement(joint, "child", link=f"{name}_link") + ElementTree.SubElement(joint, "axis", xyz="0 0 1" if is_steering else "0 1 0") + bounds = plugin.get("limit", {}) + limits = {"effort": "1000", "velocity": "100"} + if is_steering: + limits.update(lower=str(bounds["lower"]), upper=str(bounds["upper"])) + ElementTree.SubElement(joint, "limit", **limits) + + interface = ElementTree.SubElement(control, "joint", name=name) + ElementTree.SubElement(interface, "command_interface", name=plugin["mode"]) + position = ElementTree.SubElement(interface, "state_interface", name="position") + ElementTree.SubElement(position, "param", name="initial_value").text = "0.0" + ElementTree.SubElement(interface, "state_interface", name="velocity") + ElementTree.SubElement(interface, "state_interface", name="effort") + return ElementTree.tostring(robot, encoding="unicode") + + +def launch_example(context): + name = LaunchConfiguration("robot").perform(context) + if name not in EXAMPLES: + raise ValueError(f"Unknown robot {name!r}; choose one of {', '.join(EXAMPLES)}") + show_viz = LaunchConfiguration("show_viz").perform(context).lower() == "true" + share = Path(get_package_share_directory("flatland_ros2_control_examples")) + nodes: list[Action] = [ + Node( + package="flatland_server", + executable="flatland_server", + name="flatland_server", + output="screen", + parameters=[{ + "world_path": str(share / "worlds" / f"{name}.world.yaml"), + "update_rate": 100.0, + "step_size": 0.01, + "show_viz": show_viz, + "viz_pub_rate": 30.0, + "use_sim_time": True, + }], + ) + ] + viz_node = ( + Node( + package="flatland_viz", + executable="flatland_viz", + parameters=[{"use_sim_time": True}], + output="screen", + ) if show_viz else None + ) + if LaunchConfiguration("use_ros2_control").perform(context).lower() != "true": + if viz_node: + nodes.append(viz_node) + return nodes + + model = yaml.safe_load((share / "models" / f"{name}.model.yaml").read_text()) + config_path = share / "config" / f"{name}.yaml" + config = yaml.safe_load(config_path.read_text()) + controllers = [key for key in config if key != "controller_manager"] + nodes.append(Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_description(model), "use_sim_time": True}], + output="screen", + )) + nodes.append(Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[str(config_path), {"use_sim_time": True}], + output="screen", + )) + spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "--controller-manager", "/controller_manager", + "--param-file", str(config_path)] + ( + ["--controller-ros-args", "-r ~/reference:=~/cmd_vel"] + if name == "rear_drive_ackermann" and controller == "drive" else [] + ), + output="screen", + ) + for controller in ("joint_state_broadcaster", *controllers) + ] + nodes.extend(spawners) + if viz_node: + nodes.append(RegisterEventHandler(OnProcessExit( + target_action=spawners[-1], on_exit=[viz_node] + ))) + if LaunchConfiguration("use_joystick").perform(context).lower() == "true": + nodes.extend([ + Node( + package="joy", + executable="joy_node", + parameters=[{ + "device_id": int(LaunchConfiguration("joy_device_id").perform(context)), + "use_sim_time": True, + }], + output="screen", + ), + Node( + package="teleop_twist_joy", + executable="teleop_node", + parameters=[{ + "use_sim_time": True, + "publish_stamped_twist": True, + "axis_linear.x": 1, + "axis_angular.yaw": 3, + "scale_linear.x": 0.5, + "scale_angular.yaw": 0.8, + "enable_button": 4, + }], + remappings=[("cmd_vel", "/drive/cmd_vel")], + output="screen", + ), + ]) + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument("robot", default_value="caster_diff", choices=EXAMPLES), + DeclareLaunchArgument("show_viz", default_value="true", choices=["true", "false"]), + DeclareLaunchArgument("use_ros2_control", default_value="true", choices=["true", "false"]), + DeclareLaunchArgument("use_joystick", default_value="true", choices=["true", "false"]), + DeclareLaunchArgument("joy_device_id", default_value="0"), + OpaqueFunction(function=launch_example), + ]) \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/2910_swerve.model.yaml b/flatland_ros2_control_examples/models/2910_swerve.model.yaml new file mode 100644 index 00000000..dc5ac5b4 --- /dev/null +++ b/flatland_ros2_control_examples/models/2910_swerve.model.yaml @@ -0,0 +1,63 @@ +bodies: + - name: chassis + type: dynamic + color: [0.18, 0.29, 0.52, 1.0] + footprints: + - type: polygon + points: [[-0.37, -0.37], [0.37, -0.37], [0.37, 0.37], [-0.37, 0.37]] + density: 90 + layers: [robot] + - name: front_left_pod + type: dynamic + pose: [0.28, 0.28, 0.0] + footprints: + - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - name: front_right_pod + type: dynamic + pose: [0.28, -0.28, 0.0] + footprints: + - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - name: rear_left_pod + type: dynamic + pose: [-0.28, 0.28, 0.0] + footprints: + - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - name: rear_right_pod + type: dynamic + pose: [-0.28, -0.28, 0.0] + footprints: + - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} +joints: + - name: front_left_steer_pivot + type: revolute + bodies: + - {name: chassis, anchor: [0.28, 0.28]} + - {name: front_left_pod, anchor: [0.0, 0.0]} + - name: front_right_steer_pivot + type: revolute + bodies: + - {name: chassis, anchor: [0.28, -0.28]} + - {name: front_right_pod, anchor: [0.0, 0.0]} + - name: rear_left_steer_pivot + type: revolute + bodies: + - {name: chassis, anchor: [-0.28, 0.28]} + - {name: rear_left_pod, anchor: [0.0, 0.0]} + - name: rear_right_steer_pivot + type: revolute + bodies: + - {name: chassis, anchor: [-0.28, -0.28]} + - {name: rear_right_pod, anchor: [0.0, 0.0]} +plugins: + - {type: DriveWheel, name: front_left_wheel, body: front_left_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: front_right_wheel, body: front_right_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: rear_left_wheel, body: rear_left_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: rear_right_wheel, body: rear_right_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: SteeringMotor, name: front_left_steer, joint: front_left_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: SteeringMotor, name: front_right_steer, joint: front_right_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: SteeringMotor, name: rear_left_steer, joint: rear_left_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: SteeringMotor, name: rear_right_steer, joint: rear_right_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/a300_diff.model.yaml b/flatland_ros2_control_examples/models/a300_diff.model.yaml new file mode 100644 index 00000000..41776b95 --- /dev/null +++ b/flatland_ros2_control_examples/models/a300_diff.model.yaml @@ -0,0 +1,18 @@ +bodies: + - name: chassis + type: dynamic + color: [0.35, 0.39, 0.32, 1.0] + footprints: + - type: polygon + points: [[-0.43, -0.34], [0.43, -0.34], [0.43, 0.34], [-0.43, 0.34]] + density: 175 + layers: [robot] + - {type: polygon, points: [[0.15, 0.27], [0.45, 0.27], [0.45, 0.37], [0.15, 0.37]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[0.15, -0.37], [0.45, -0.37], [0.45, -0.27], [0.15, -0.27]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.45, 0.27], [-0.15, 0.27], [-0.15, 0.37], [-0.45, 0.37]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.45, -0.37], [-0.15, -0.37], [-0.15, -0.27], [-0.45, -0.27]], density: 50, sensor: true, layers: [robot]} +plugins: + - {type: DriveWheel, name: front_left_wheel, body: chassis, offset: [0.30, 0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: DriveWheel, name: front_right_wheel, body: chassis, offset: [0.30, -0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: DriveWheel, name: rear_left_wheel, body: chassis, offset: [-0.30, 0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: DriveWheel, name: rear_right_wheel, body: chassis, offset: [-0.30, -0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/articulated_204g.model.yaml b/flatland_ros2_control_examples/models/articulated_204g.model.yaml new file mode 100644 index 00000000..ba1623a8 --- /dev/null +++ b/flatland_ros2_control_examples/models/articulated_204g.model.yaml @@ -0,0 +1,35 @@ +bodies: + - name: rear_chassis + type: dynamic + pose: [-0.45, 0.0, 0.0] + color: [0.27, 0.48, 0.15, 1.0] + footprints: + - type: polygon + points: [[-0.43, -0.40], [0.43, -0.40], [0.43, 0.40], [-0.43, 0.40]] + density: 290 + layers: [robot] + - {type: polygon, points: [[-0.54, 0.23], [0.04, 0.23], [0.04, 0.41], [-0.54, 0.41]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.54, -0.41], [0.04, -0.41], [0.04, -0.23], [-0.54, -0.23]], density: 50, sensor: true, layers: [robot]} + - name: front_chassis + type: dynamic + pose: [0.45, 0.0, 0.0] + color: [0.3, 0.56, 0.17, 1.0] + footprints: + - type: polygon + points: [[-0.43, -0.40], [0.43, -0.40], [0.43, 0.40], [-0.43, 0.40]] + density: 290 + layers: [robot] + - {type: polygon, points: [[-0.04, 0.23], [0.54, 0.23], [0.54, 0.41], [-0.04, 0.41]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.04, -0.41], [0.54, -0.41], [0.54, -0.23], [-0.04, -0.23]], density: 50, sensor: true, layers: [robot]} +joints: + - name: articulation_pivot + type: revolute + bodies: + - {name: rear_chassis, anchor: [0.45, 0.0]} + - {name: front_chassis, anchor: [-0.45, 0.0]} +plugins: + - {type: DriveWheel, name: front_left_wheel, body: front_chassis, offset: [0.25, 0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: front_right_wheel, body: front_chassis, offset: [0.25, -0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: rear_left_wheel, body: rear_chassis, offset: [-0.25, 0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: rear_right_wheel, body: rear_chassis, offset: [-0.25, -0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: SteeringMotor, name: articulation, joint: articulation_pivot, mode: position, limit: {lower: -0.6981317, upper: 0.6981317}} \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/caster_diff.model.yaml b/flatland_ros2_control_examples/models/caster_diff.model.yaml new file mode 100644 index 00000000..3e14556b --- /dev/null +++ b/flatland_ros2_control_examples/models/caster_diff.model.yaml @@ -0,0 +1,32 @@ +bodies: + - name: chassis + type: dynamic + color: [0.12, 0.42, 0.55, 1.0] + footprints: + - type: polygon + points: [[-0.23, -0.16], [0.23, -0.16], [0.23, 0.16], [-0.23, 0.16]] + density: 21 + layers: [robot] + - {type: polygon, points: [[-0.05, 0.135], [0.09, 0.135], [0.09, 0.185], [-0.05, 0.185]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.05, -0.185], [0.09, -0.185], [0.09, -0.135], [-0.05, -0.135]], density: 50, sensor: true, layers: [robot]} + - name: caster_platform + type: dynamic + pose: [-0.19, 0.0, 0.0] + color: [0.3, 0.35, 0.37, 1.0] + footprints: + - type: circle + radius: 0.025 + density: 50 + sensor: true + layers: [robot] + - {type: polygon, points: [[-0.105, -0.025], [-0.035, -0.025], [-0.035, 0.025], [-0.105, 0.025]], density: 50, sensor: true, layers: [robot]} +joints: + - name: caster_swivel + type: revolute + bodies: + - {name: chassis, anchor: [-0.19, 0.0]} + - {name: caster_platform, anchor: [0.0, 0.0]} +plugins: + - {type: DriveWheel, name: left_wheel, body: chassis, offset: [0.02, 0.16, 0.0], radius: 0.07, mode: velocity, friction: 1.2} + - {type: DriveWheel, name: right_wheel, body: chassis, offset: [0.02, -0.16, 0.0], radius: 0.07, mode: velocity, friction: 1.2} + - {type: FreeWheel, name: rear_caster, body: caster_platform, offset: [-0.07, 0.0, 0.0], radius: 0.035, lateral_resistance: 0.2} \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml new file mode 100644 index 00000000..35a00132 --- /dev/null +++ b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml @@ -0,0 +1,28 @@ +bodies: + - name: chassis + type: dynamic + color: [0.11, 0.53, 0.48, 1.0] + footprints: + - type: polygon + points: [[-0.62, -0.35], [0.62, -0.35], [0.62, 0.35], [-0.62, 0.35]] + density: 220 + layers: [robot] + - {type: polygon, points: [[-0.64, 0.25], [-0.30, 0.25], [-0.30, 0.35], [-0.64, 0.35]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.64, -0.35], [-0.30, -0.35], [-0.30, -0.25], [-0.64, -0.25]], density: 50, sensor: true, layers: [robot]} + - name: front_drive_pod + type: dynamic + pose: [0.47, 0.0, 0.0] + footprints: + - {type: circle, radius: 0.045, density: 60, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.17, -0.05], [0.17, -0.05], [0.17, 0.05], [-0.17, 0.05]], density: 60, sensor: true, layers: [robot]} +joints: + - name: front_pivot + type: revolute + bodies: + - {name: chassis, anchor: [0.47, 0.0]} + - {name: front_drive_pod, anchor: [0.0, 0.0]} +plugins: + - {type: DriveWheel, name: front_wheel, body: front_drive_pod, radius: 0.17, mode: velocity, friction: 1.1} + - {type: FreeWheel, name: rear_left_wheel, body: chassis, offset: [-0.47, 0.30, 0.0], radius: 0.17} + - {type: FreeWheel, name: rear_right_wheel, body: chassis, offset: [-0.47, -0.30, 0.0], radius: 0.17} + - {type: SteeringMotor, name: front_steer, joint: front_pivot, mode: position, limit: {lower: -0.70, upper: 0.70}} \ No newline at end of file diff --git a/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml b/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml new file mode 100644 index 00000000..30f230b8 --- /dev/null +++ b/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml @@ -0,0 +1,41 @@ +bodies: + - name: chassis + type: dynamic + color: [0.64, 0.17, 0.13, 1.0] + footprints: + - type: polygon + points: [[-0.78, -0.37], [0.62, -0.37], [0.62, 0.37], [-0.78, 0.37]] + density: 180 + layers: [robot] + - {type: polygon, points: [[-0.71, 0.26], [-0.33, 0.26], [-0.33, 0.38], [-0.71, 0.38]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.71, -0.38], [-0.33, -0.38], [-0.33, -0.26], [-0.71, -0.26]], density: 50, sensor: true, layers: [robot]} + - name: front_left_knuckle + type: dynamic + pose: [0.52, 0.32, 0.0] + footprints: + - {type: circle, radius: 0.045, density: 150, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], density: 150, sensor: true, layers: [robot]} + - name: front_right_knuckle + type: dynamic + pose: [0.52, -0.32, 0.0] + footprints: + - {type: circle, radius: 0.045, density: 150, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], density: 150, sensor: true, layers: [robot]} +joints: + - name: front_left_pivot + type: revolute + bodies: + - {name: chassis, anchor: [0.52, 0.32]} + - {name: front_left_knuckle, anchor: [0.0, 0.0]} + - name: front_right_pivot + type: revolute + bodies: + - {name: chassis, anchor: [0.52, -0.32]} + - {name: front_right_knuckle, anchor: [0.0, 0.0]} +plugins: + - {type: DriveWheel, name: rear_left_wheel, body: chassis, offset: [-0.52, 0.32, 0.0], radius: 0.19, mode: velocity, friction: 1.1} + - {type: DriveWheel, name: rear_right_wheel, body: chassis, offset: [-0.52, -0.32, 0.0], radius: 0.19, mode: velocity, friction: 1.1} + - {type: FreeWheel, name: front_left_wheel, body: front_left_knuckle, radius: 0.19} + - {type: FreeWheel, name: front_right_wheel, body: front_right_knuckle, radius: 0.19} + - {type: SteeringMotor, name: front_left_steer, joint: front_left_pivot, mode: position, limit: {lower: -0.65, upper: 0.65}} + - {type: SteeringMotor, name: front_right_steer, joint: front_right_pivot, mode: position, limit: {lower: -0.65, upper: 0.65}} \ No newline at end of file diff --git a/flatland_ros2_control_examples/package.xml b/flatland_ros2_control_examples/package.xml new file mode 100644 index 00000000..65a9205f --- /dev/null +++ b/flatland_ros2_control_examples/package.xml @@ -0,0 +1,33 @@ + + + flatland_ros2_control_examples + 0.1.0 + Flatland wheeled vehicle models for topic-based ros2_control. + Joseph Duchesne + BSD-3-Clause + + ament_cmake + flatland_server + flatland_plugins + flatland_viz + python3-yaml + ament_index_python + launch + launch_ros + robot_state_publisher + controller_manager + joint_state_broadcaster + joint_command_topic_hardware_interface + diff_drive_controller + ackermann_steering_controller + tricycle_controller + forward_command_controller + joy + teleop_twist_joy + + ament_cmake_gtest + interactive_markers + rclcpp + + ament_cmake + \ No newline at end of file diff --git a/flatland_ros2_control_examples/test/controller_config_test.py b/flatland_ros2_control_examples/test/controller_config_test.py new file mode 100644 index 00000000..6ad9c099 --- /dev/null +++ b/flatland_ros2_control_examples/test/controller_config_test.py @@ -0,0 +1,146 @@ +import importlib.util +import unittest +from pathlib import Path +from xml.etree import ElementTree + +import yaml +from launch import LaunchContext +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.utilities import perform_substitutions +from launch_ros.actions import Node + + +ROOT = Path(__file__).resolve().parent.parent +SPEC = importlib.util.spec_from_file_location("example_launch", ROOT / "launch" / "example.launch.py") +assert SPEC is not None and SPEC.loader is not None +EXAMPLE_LAUNCH = importlib.util.module_from_spec(SPEC) +SPEC.loader.exec_module(EXAMPLE_LAUNCH) + + +class ControllerConfigTest(unittest.TestCase): + def test_launch_constructs_both_modes(self): + for example in EXAMPLE_LAUNCH.EXAMPLES: + with self.subTest(example=example): + config = yaml.safe_load((ROOT / "config" / f"{example}.yaml").read_text()) + context = LaunchContext() + context.launch_configurations.update({ + "robot": example, "use_ros2_control": "false", + "use_joystick": "true", "joy_device_id": "0", + "show_viz": "true", + }) + for show_viz in ("true", "false"): + context.launch_configurations["show_viz"] = show_viz + viz_count = int(show_viz == "true") + actions = EXAMPLE_LAUNCH.launch_example(context) + self.assertEqual([action.node_package for action in actions], + ["flatland_server"] + (["flatland_viz"] if viz_count else [])) + if viz_count: + self.assertEqual(actions[1].node_executable, "flatland_viz") + context.launch_configurations["use_ros2_control"] = "true" + control_actions = EXAMPLE_LAUNCH.launch_example(context) + self.assertEqual(len(control_actions), 5 + len(config) + viz_count) + spawners = [action for action in control_actions + if isinstance(action, Node) and action.node_executable == "spawner"] + for spawner in spawners: + arguments = [perform_substitutions(context, part) for part in spawner.cmd[1:]] + is_ackermann_drive = example == "rear_drive_ackermann" and arguments[0] == "drive" + self.assertEqual("--controller-ros-args" in arguments, is_ackermann_drive) + if is_ackermann_drive: + self.assertEqual(arguments[arguments.index("--controller-ros-args") + 1], + "-r ~/reference:=~/cmd_vel") + teleop = next(action for action in control_actions + if isinstance(action, Node) and action.node_package == "teleop_twist_joy") + teleop._perform_substitutions(context) + self.assertEqual(teleop.expanded_remapping_rules, [("cmd_vel", "/drive/cmd_vel")]) + handlers = [action for action in control_actions + if isinstance(action, RegisterEventHandler)] + self.assertEqual(len(handlers), viz_count) + for handler in handlers: + self.assertIsInstance(handler.event_handler, OnProcessExit) + context.launch_configurations["use_joystick"] = "false" + self.assertEqual(len(EXAMPLE_LAUNCH.launch_example(context)), 3 + len(config) + viz_count) + context.launch_configurations["use_joystick"] = "true" + context.launch_configurations["use_ros2_control"] = "false" + + def test_wheel_footprints_match_plugins(self): + for example in EXAMPLE_LAUNCH.EXAMPLES: + model = yaml.safe_load((ROOT / "models" / f"{example}.model.yaml").read_text()) + bodies = {body["name"]: body for body in model["bodies"]} + for wheel in model["plugins"]: + if wheel["type"] not in ("DriveWheel", "FreeWheel"): + continue + with self.subTest(example=example, wheel=wheel["name"]): + center_x, center_y, _ = wheel.get("offset", [0, 0, 0]) + rectangles = [] + for footprint in bodies[wheel["body"]]["footprints"]: + if footprint["type"] != "polygon" or not footprint.get("sensor"): + continue + points = footprint["points"] + min_x, max_x = min(point[0] for point in points), max(point[0] for point in points) + min_y, max_y = min(point[1] for point in points), max(point[1] for point in points) + if (abs((min_x + max_x) / 2 - center_x) < 1e-9 + and abs((min_y + max_y) / 2 - center_y) < 1e-9): + rectangles.append((points, min_x, max_x, min_y, max_y)) + self.assertEqual(len(rectangles), 1) + points, min_x, max_x, min_y, max_y = rectangles[0] + self.assertEqual({tuple(point) for point in points}, + {(min_x, min_y), (max_x, min_y), + (max_x, max_y), (min_x, max_y)}) + self.assertAlmostEqual(max_x - min_x, 2 * wheel["radius"]) + self.assertGreaterEqual(max_y - min_y, 0.05 - 1e-9) + + def test_joint_interfaces_match_plugins_and_controllers(self): + for example in EXAMPLE_LAUNCH.EXAMPLES: + with self.subTest(example=example): + model = yaml.safe_load((ROOT / "models" / f"{example}.model.yaml").read_text()) + config = yaml.safe_load((ROOT / "config" / f"{example}.yaml").read_text()) + plugins = { + plugin["name"]: plugin["mode"] + for plugin in model["plugins"] + if plugin["type"] in ("DriveWheel", "SteeringMotor") + } + description = ElementTree.fromstring(EXAMPLE_LAUNCH.robot_description(model)) + control = description.find("ros2_control") + assert control is not None + hardware_plugin = control.find("hardware/plugin") + assert hardware_plugin is not None + self.assertEqual( + hardware_plugin.text, + "joint_command_topic_hardware_interface/JointCommandTopicSystem", + ) + interfaces = {} + for joint in control.findall("joint"): + command_interface = joint.find("command_interface") + assert command_interface is not None + interfaces[joint.attrib["name"]] = command_interface.attrib["name"] + self.assertEqual(interfaces, plugins) + self.assertEqual( + {joint.attrib["name"] for joint in description.findall("joint")}, + set(plugins), + ) + controller_types = config["controller_manager"]["ros__parameters"] + for controller, definition in config.items(): + if controller == "controller_manager": + continue + self.assertIn("type", controller_types[controller]) + settings = definition["ros__parameters"] + for names_key in ( + "left_wheel_names", "right_wheel_names", "traction_joints_names", + "steering_joints_names", "joints", + ): + for name in settings.get(names_key, []): + self.assertEqual( + plugins[name], + settings.get("interface_name", "position" if "steering" in names_key else "velocity"), + ) + for name_key, mode in ( + ("traction_joint_name", "velocity"), + ("steering_joint_name", "position"), + ): + if name_key in settings: + self.assertEqual(plugins[settings[name_key]], mode) + + +if __name__ == "__main__": + unittest.main() \ No newline at end of file diff --git a/flatland_ros2_control_examples/test/worlds_test.cpp b/flatland_ros2_control_examples/test/worlds_test.cpp new file mode 100644 index 00000000..7a97c50b --- /dev/null +++ b/flatland_ros2_control_examples/test/worlds_test.cpp @@ -0,0 +1,106 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +TEST(ExampleWorlds, LoadAndStep) +{ + const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; + for (const auto & [name, plugin_count] : + std::array, 6>{{ + {"caster_diff", 3}, {"a300_diff", 4}, {"2910_swerve", 8}, + {"rear_drive_ackermann", 6}, {"front_drive_tricycle", 4}, + {"articulated_204g", 5}}}) { + SCOPED_TRACE(name); + auto node = rclcpp::Node::make_shared("example_" + name); + auto world = std::unique_ptr( + flatland_server::World::MakeWorld(node, (worlds / (name + ".world.yaml")).string())); + ASSERT_EQ(world->models_.size(), 1u); + EXPECT_EQ(world->plugin_manager_.model_plugins_.size(), plugin_count); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + world->Update(timekeeper); + } +} + +TEST(ExampleWorlds, WheelBearingBodiesHavePlausibleMass) +{ + const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; + auto node = rclcpp::Node::make_shared("example_mass_distribution"); + auto caster = std::unique_ptr(flatland_server::World::MakeWorld( + node, (worlds / "caster_diff.world.yaml").string())); + auto * caster_model = caster->models_[0]; + const double caster_mass = caster_model->GetBody("chassis")->physics_body_->GetMass(); + const double platform_mass = caster_model->GetBody("caster_platform")->physics_body_->GetMass(); + EXPECT_NEAR(caster_mass + platform_mass, 4.0, 0.5); + EXPECT_GT(platform_mass, 0.15); + EXPECT_LT(platform_mass, 0.5); + + auto car = std::unique_ptr(flatland_server::World::MakeWorld( + node, (worlds / "rear_drive_ackermann.world.yaml").string())); + auto * car_model = car->models_[0]; + const double chassis_mass = car_model->GetBody("chassis")->physics_body_->GetMass(); + EXPECT_GT(chassis_mass, 150.0); + EXPECT_LT(chassis_mass, 300.0); + double mass = 0.0; + double weighted_x = 0.0; + for (const auto * body : car_model->GetBodies()) { + const auto * physics = body->physics_body_; + mass += physics->GetMass(); + weighted_x += physics->GetMass() * physics->GetWorldCenter().x; + } + EXPECT_LT(weighted_x / mass, 0.0); + EXPECT_GT(weighted_x / mass, -0.15); + for (const auto * name : {"front_left_knuckle", "front_right_knuckle"}) { + const double wheel_mass = car_model->GetBody(name)->physics_body_->GetMass(); + EXPECT_GT(wheel_mass, 5.0); + EXPECT_LT(wheel_mass, 12.0); + } +} + +TEST(ExampleWorlds, AckermannAndCasterSteerUnderPower) +{ + const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; + for (const auto & name : {"rear_drive_ackermann", "caster_diff"}) { + SCOPED_TRACE(name); + auto node = rclcpp::Node::make_shared(std::string("drive_") + name); + auto world = std::unique_ptr(flatland_server::World::MakeWorld( + node, (worlds / (std::string(name) + ".world.yaml")).string())); + for (const auto & plugin : world->plugin_manager_.model_plugins_) { + if (auto wheel = std::dynamic_pointer_cast(plugin)) { + wheel->command_ = name == std::string("caster_diff") && wheel->GetName() == "right_wheel" ? + 5.0 : 3.0; + wheel->has_command_ = true; + } + if (auto steer = std::dynamic_pointer_cast(plugin)) { + steer->command_ = 0.25; + steer->has_command_ = true; + } + } + auto * body = world->models_[0]->GetBody("chassis")->physics_body_; + const auto start = body->GetWorldCenter(); + flatland_server::Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + for (int step = 0; step < 200; ++step) { + world->Update(timekeeper); + } + const auto end = body->GetWorldCenter(); + EXPECT_GT(end.x - start.x, 0.2); + EXPECT_GT(body->GetAngle(), 0.05); + EXPECT_GT(end.y - start.y, 0.01); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/2910_swerve.world.yaml b/flatland_ros2_control_examples/worlds/2910_swerve.world.yaml new file mode 100644 index 00000000..a147a276 --- /dev/null +++ b/flatland_ros2_control_examples/worlds/2910_swerve.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: swerve_2910 + model: ../models/2910_swerve.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/a300_diff.world.yaml b/flatland_ros2_control_examples/worlds/a300_diff.world.yaml new file mode 100644 index 00000000..222ef0e0 --- /dev/null +++ b/flatland_ros2_control_examples/worlds/a300_diff.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: a300_diff + model: ../models/a300_diff.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/articulated_204g.world.yaml b/flatland_ros2_control_examples/worlds/articulated_204g.world.yaml new file mode 100644 index 00000000..50a711dd --- /dev/null +++ b/flatland_ros2_control_examples/worlds/articulated_204g.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: articulated_204g + model: ../models/articulated_204g.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/caster_diff.world.yaml b/flatland_ros2_control_examples/worlds/caster_diff.world.yaml new file mode 100644 index 00000000..a20afcf6 --- /dev/null +++ b/flatland_ros2_control_examples/worlds/caster_diff.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: caster_diff + model: ../models/caster_diff.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/front_drive_tricycle.world.yaml b/flatland_ros2_control_examples/worlds/front_drive_tricycle.world.yaml new file mode 100644 index 00000000..8ff96544 --- /dev/null +++ b/flatland_ros2_control_examples/worlds/front_drive_tricycle.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: front_drive_tricycle + model: ../models/front_drive_tricycle.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/rear_drive_ackermann.world.yaml b/flatland_ros2_control_examples/worlds/rear_drive_ackermann.world.yaml new file mode 100644 index 00000000..48d5433e --- /dev/null +++ b/flatland_ros2_control_examples/worlds/rear_drive_ackermann.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: rear_drive_ackermann + model: ../models/rear_drive_ackermann.model.yaml \ No newline at end of file From ad555a00c134a4a606a3e343fca97978602b71dc Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Sun, 27 Sep 2026 22:58:37 +0000 Subject: [PATCH 03/13] renamed example --- flatland_ros2_control_examples/README.md | 4 ++-- .../config/{caster_diff.yaml => turtlebot_caster.yaml} | 0 flatland_ros2_control_examples/launch/example.launch.py | 4 ++-- ...caster_diff.model.yaml => turtlebot_caster.model.yaml} | 0 flatland_ros2_control_examples/test/worlds_test.cpp | 8 ++++---- .../worlds/caster_diff.world.yaml | 6 ------ .../worlds/turtlebot_caster.world.yaml | 6 ++++++ 7 files changed, 14 insertions(+), 14 deletions(-) rename flatland_ros2_control_examples/config/{caster_diff.yaml => turtlebot_caster.yaml} (100%) rename flatland_ros2_control_examples/models/{caster_diff.model.yaml => turtlebot_caster.model.yaml} (100%) delete mode 100644 flatland_ros2_control_examples/worlds/caster_diff.world.yaml create mode 100644 flatland_ros2_control_examples/worlds/turtlebot_caster.world.yaml diff --git a/flatland_ros2_control_examples/README.md b/flatland_ros2_control_examples/README.md index 499d4b90..cd9b6bb9 100644 --- a/flatland_ros2_control_examples/README.md +++ b/flatland_ros2_control_examples/README.md @@ -7,7 +7,7 @@ CAD reproductions or calibrated vehicle dynamics. | `robot` launch value | Geometry | ros2_control controller | | --- | --- | --- | -| `caster_diff` | Two drive wheels and a rear trailing caster on a free revolute swivel | `diff_drive_controller` | +| `turtlebot_caster` | Two drive wheels and a rear trailing caster on a free revolute swivel | `diff_drive_controller` | | `a300_diff` | Four fixed drive wheels, two per side, A300-style skid steering | `diff_drive_controller` | | `2910_swerve` | Four independently steered and driven corner pods, inspired by [2910's Charged Up drivebase](https://frcdesign.org/mechanism-examples/drivebase/swerve/2910_2023_dt/) | Separate forward command controllers for wheel speeds and steering angles | | `rear_drive_ackermann` | Two rear drive wheels and two front passive wheels on independent steered knuckles | `ackermann_steering_controller` | @@ -29,7 +29,7 @@ Build after sourcing ROS and the Flatland dependencies: ```sh colcon build --packages-select flatland_ros2_control_examples source install/setup.bash -ros2 launch flatland_ros2_control_examples example.launch.py robot:=caster_diff +ros2 launch flatland_ros2_control_examples example.launch.py robot:=turtlebot_caster ``` Change `robot` to any value in the table. The default launch starts Flatland diff --git a/flatland_ros2_control_examples/config/caster_diff.yaml b/flatland_ros2_control_examples/config/turtlebot_caster.yaml similarity index 100% rename from flatland_ros2_control_examples/config/caster_diff.yaml rename to flatland_ros2_control_examples/config/turtlebot_caster.yaml diff --git a/flatland_ros2_control_examples/launch/example.launch.py b/flatland_ros2_control_examples/launch/example.launch.py index 906c16a1..4c6732bf 100644 --- a/flatland_ros2_control_examples/launch/example.launch.py +++ b/flatland_ros2_control_examples/launch/example.launch.py @@ -11,7 +11,7 @@ EXAMPLES = ( - "caster_diff", + "turtlebot_caster", "a300_diff", "2910_swerve", "rear_drive_ackermann", @@ -162,7 +162,7 @@ def launch_example(context): def generate_launch_description(): return LaunchDescription([ - DeclareLaunchArgument("robot", default_value="caster_diff", choices=EXAMPLES), + DeclareLaunchArgument("robot", default_value="turtlebot_caster", choices=EXAMPLES), DeclareLaunchArgument("show_viz", default_value="true", choices=["true", "false"]), DeclareLaunchArgument("use_ros2_control", default_value="true", choices=["true", "false"]), DeclareLaunchArgument("use_joystick", default_value="true", choices=["true", "false"]), diff --git a/flatland_ros2_control_examples/models/caster_diff.model.yaml b/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml similarity index 100% rename from flatland_ros2_control_examples/models/caster_diff.model.yaml rename to flatland_ros2_control_examples/models/turtlebot_caster.model.yaml diff --git a/flatland_ros2_control_examples/test/worlds_test.cpp b/flatland_ros2_control_examples/test/worlds_test.cpp index 7a97c50b..28c266c1 100644 --- a/flatland_ros2_control_examples/test/worlds_test.cpp +++ b/flatland_ros2_control_examples/test/worlds_test.cpp @@ -13,7 +13,7 @@ TEST(ExampleWorlds, LoadAndStep) const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; for (const auto & [name, plugin_count] : std::array, 6>{{ - {"caster_diff", 3}, {"a300_diff", 4}, {"2910_swerve", 8}, + {"turtlebot_caster", 3}, {"a300_diff", 4}, {"2910_swerve", 8}, {"rear_drive_ackermann", 6}, {"front_drive_tricycle", 4}, {"articulated_204g", 5}}}) { SCOPED_TRACE(name); @@ -33,7 +33,7 @@ TEST(ExampleWorlds, WheelBearingBodiesHavePlausibleMass) const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; auto node = rclcpp::Node::make_shared("example_mass_distribution"); auto caster = std::unique_ptr(flatland_server::World::MakeWorld( - node, (worlds / "caster_diff.world.yaml").string())); + node, (worlds / "turtlebot_caster.world.yaml").string())); auto * caster_model = caster->models_[0]; const double caster_mass = caster_model->GetBody("chassis")->physics_body_->GetMass(); const double platform_mass = caster_model->GetBody("caster_platform")->physics_body_->GetMass(); @@ -66,14 +66,14 @@ TEST(ExampleWorlds, WheelBearingBodiesHavePlausibleMass) TEST(ExampleWorlds, AckermannAndCasterSteerUnderPower) { const auto worlds = std::filesystem::path(__FILE__).parent_path().parent_path() / "worlds"; - for (const auto & name : {"rear_drive_ackermann", "caster_diff"}) { + for (const auto & name : {"rear_drive_ackermann", "turtlebot_caster"}) { SCOPED_TRACE(name); auto node = rclcpp::Node::make_shared(std::string("drive_") + name); auto world = std::unique_ptr(flatland_server::World::MakeWorld( node, (worlds / (std::string(name) + ".world.yaml")).string())); for (const auto & plugin : world->plugin_manager_.model_plugins_) { if (auto wheel = std::dynamic_pointer_cast(plugin)) { - wheel->command_ = name == std::string("caster_diff") && wheel->GetName() == "right_wheel" ? + wheel->command_ = name == std::string("turtlebot_caster") && wheel->GetName() == "right_wheel" ? 5.0 : 3.0; wheel->has_command_ = true; } diff --git a/flatland_ros2_control_examples/worlds/caster_diff.world.yaml b/flatland_ros2_control_examples/worlds/caster_diff.world.yaml deleted file mode 100644 index a20afcf6..00000000 --- a/flatland_ros2_control_examples/worlds/caster_diff.world.yaml +++ /dev/null @@ -1,6 +0,0 @@ -properties: {} -layers: - - name: robot -models: - - name: caster_diff - model: ../models/caster_diff.model.yaml \ No newline at end of file diff --git a/flatland_ros2_control_examples/worlds/turtlebot_caster.world.yaml b/flatland_ros2_control_examples/worlds/turtlebot_caster.world.yaml new file mode 100644 index 00000000..b739e377 --- /dev/null +++ b/flatland_ros2_control_examples/worlds/turtlebot_caster.world.yaml @@ -0,0 +1,6 @@ +properties: {} +layers: + - name: robot +models: + - name: turtlebot_caster + model: ../models/turtlebot_caster.model.yaml \ No newline at end of file From 5f0c1329e5213f536b690c0f103f4d9d2a865ed4 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 00:48:27 +0000 Subject: [PATCH 04/13] Moved examples to use mass vrs. density --- .../models/2910_swerve.model.yaml | 18 +++++++++--------- .../models/a300_diff.model.yaml | 10 +++++----- .../models/articulated_204g.model.yaml | 12 ++++++------ .../models/front_drive_tricycle.model.yaml | 10 +++++----- .../models/rear_drive_ackermann.model.yaml | 14 +++++++------- .../models/turtlebot_caster.model.yaml | 10 +++++----- 6 files changed, 37 insertions(+), 37 deletions(-) diff --git a/flatland_ros2_control_examples/models/2910_swerve.model.yaml b/flatland_ros2_control_examples/models/2910_swerve.model.yaml index dc5ac5b4..999f45e9 100644 --- a/flatland_ros2_control_examples/models/2910_swerve.model.yaml +++ b/flatland_ros2_control_examples/models/2910_swerve.model.yaml @@ -5,32 +5,32 @@ bodies: footprints: - type: polygon points: [[-0.37, -0.37], [0.37, -0.37], [0.37, 0.37], [-0.37, 0.37]] - density: 90 + mass: 49.28 layers: [robot] - name: front_left_pod type: dynamic pose: [0.28, 0.28, 0.0] footprints: - - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - {type: circle, radius: 0.035, mass: 0.27, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], mass: 0.36, sensor: true, layers: [robot]} - name: front_right_pod type: dynamic pose: [0.28, -0.28, 0.0] footprints: - - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - {type: circle, radius: 0.035, mass: 0.27, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], mass: 0.36, sensor: true, layers: [robot]} - name: rear_left_pod type: dynamic pose: [-0.28, 0.28, 0.0] footprints: - - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - {type: circle, radius: 0.035, mass: 0.27, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], mass: 0.36, sensor: true, layers: [robot]} - name: rear_right_pod type: dynamic pose: [-0.28, -0.28, 0.0] footprints: - - {type: circle, radius: 0.035, density: 70, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], density: 70, sensor: true, layers: [robot]} + - {type: circle, radius: 0.035, mass: 0.27, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.051, -0.025], [0.051, -0.025], [0.051, 0.025], [-0.051, 0.025]], mass: 0.36, sensor: true, layers: [robot]} joints: - name: front_left_steer_pivot type: revolute diff --git a/flatland_ros2_control_examples/models/a300_diff.model.yaml b/flatland_ros2_control_examples/models/a300_diff.model.yaml index 41776b95..245e9cab 100644 --- a/flatland_ros2_control_examples/models/a300_diff.model.yaml +++ b/flatland_ros2_control_examples/models/a300_diff.model.yaml @@ -5,12 +5,12 @@ bodies: footprints: - type: polygon points: [[-0.43, -0.34], [0.43, -0.34], [0.43, 0.34], [-0.43, 0.34]] - density: 175 + mass: 102.34 layers: [robot] - - {type: polygon, points: [[0.15, 0.27], [0.45, 0.27], [0.45, 0.37], [0.15, 0.37]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[0.15, -0.37], [0.45, -0.37], [0.45, -0.27], [0.15, -0.27]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.45, 0.27], [-0.15, 0.27], [-0.15, 0.37], [-0.45, 0.37]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.45, -0.37], [-0.15, -0.37], [-0.15, -0.27], [-0.45, -0.27]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[0.15, 0.27], [0.45, 0.27], [0.45, 0.37], [0.15, 0.37]], mass: 1.50, sensor: true, layers: [robot]} + - {type: polygon, points: [[0.15, -0.37], [0.45, -0.37], [0.45, -0.27], [0.15, -0.27]], mass: 1.50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.45, 0.27], [-0.15, 0.27], [-0.15, 0.37], [-0.45, 0.37]], mass: 1.50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.45, -0.37], [-0.15, -0.37], [-0.15, -0.27], [-0.45, -0.27]], mass: 1.50, sensor: true, layers: [robot]} plugins: - {type: DriveWheel, name: front_left_wheel, body: chassis, offset: [0.30, 0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} - {type: DriveWheel, name: front_right_wheel, body: chassis, offset: [0.30, -0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} diff --git a/flatland_ros2_control_examples/models/articulated_204g.model.yaml b/flatland_ros2_control_examples/models/articulated_204g.model.yaml index ba1623a8..a1ab36d0 100644 --- a/flatland_ros2_control_examples/models/articulated_204g.model.yaml +++ b/flatland_ros2_control_examples/models/articulated_204g.model.yaml @@ -6,10 +6,10 @@ bodies: footprints: - type: polygon points: [[-0.43, -0.40], [0.43, -0.40], [0.43, 0.40], [-0.43, 0.40]] - density: 290 + mass: 199.52 layers: [robot] - - {type: polygon, points: [[-0.54, 0.23], [0.04, 0.23], [0.04, 0.41], [-0.54, 0.41]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.54, -0.41], [0.04, -0.41], [0.04, -0.23], [-0.54, -0.23]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.54, 0.23], [0.04, 0.23], [0.04, 0.41], [-0.54, 0.41]], mass: 5.22, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.54, -0.41], [0.04, -0.41], [0.04, -0.23], [-0.54, -0.23]], mass: 5.22, sensor: true, layers: [robot]} - name: front_chassis type: dynamic pose: [0.45, 0.0, 0.0] @@ -17,10 +17,10 @@ bodies: footprints: - type: polygon points: [[-0.43, -0.40], [0.43, -0.40], [0.43, 0.40], [-0.43, 0.40]] - density: 290 + mass: 199.52 layers: [robot] - - {type: polygon, points: [[-0.04, 0.23], [0.54, 0.23], [0.54, 0.41], [-0.04, 0.41]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.04, -0.41], [0.54, -0.41], [0.54, -0.23], [-0.04, -0.23]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.04, 0.23], [0.54, 0.23], [0.54, 0.41], [-0.04, 0.41]], mass: 5.22, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.04, -0.41], [0.54, -0.41], [0.54, -0.23], [-0.04, -0.23]], mass: 5.22, sensor: true, layers: [robot]} joints: - name: articulation_pivot type: revolute diff --git a/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml index 35a00132..2e3e409c 100644 --- a/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml +++ b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml @@ -5,16 +5,16 @@ bodies: footprints: - type: polygon points: [[-0.62, -0.35], [0.62, -0.35], [0.62, 0.35], [-0.62, 0.35]] - density: 220 + mass: 190.96 layers: [robot] - - {type: polygon, points: [[-0.64, 0.25], [-0.30, 0.25], [-0.30, 0.35], [-0.64, 0.35]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.64, -0.35], [-0.30, -0.35], [-0.30, -0.25], [-0.64, -0.25]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.64, 0.25], [-0.30, 0.25], [-0.30, 0.35], [-0.64, 0.35]], mass: 1.70, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.64, -0.35], [-0.30, -0.35], [-0.30, -0.25], [-0.64, -0.25]], mass: 1.70, sensor: true, layers: [robot]} - name: front_drive_pod type: dynamic pose: [0.47, 0.0, 0.0] footprints: - - {type: circle, radius: 0.045, density: 60, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.17, -0.05], [0.17, -0.05], [0.17, 0.05], [-0.17, 0.05]], density: 60, sensor: true, layers: [robot]} + - {type: circle, radius: 0.045, mass: 0.38, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.17, -0.05], [0.17, -0.05], [0.17, 0.05], [-0.17, 0.05]], mass: 2.04, sensor: true, layers: [robot]} joints: - name: front_pivot type: revolute diff --git a/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml b/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml index 30f230b8..32c60a70 100644 --- a/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml +++ b/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml @@ -5,22 +5,22 @@ bodies: footprints: - type: polygon points: [[-0.78, -0.37], [0.62, -0.37], [0.62, 0.37], [-0.78, 0.37]] - density: 180 + mass: 186.48 layers: [robot] - - {type: polygon, points: [[-0.71, 0.26], [-0.33, 0.26], [-0.33, 0.38], [-0.71, 0.38]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.71, -0.38], [-0.33, -0.38], [-0.33, -0.26], [-0.71, -0.26]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.71, 0.26], [-0.33, 0.26], [-0.33, 0.38], [-0.71, 0.38]], mass: 2.28, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.71, -0.38], [-0.33, -0.38], [-0.33, -0.26], [-0.71, -0.26]], mass: 2.28, sensor: true, layers: [robot]} - name: front_left_knuckle type: dynamic pose: [0.52, 0.32, 0.0] footprints: - - {type: circle, radius: 0.045, density: 150, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], density: 150, sensor: true, layers: [robot]} + - {type: circle, radius: 0.045, mass: 0.95, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], mass: 6.84, sensor: true, layers: [robot]} - name: front_right_knuckle type: dynamic pose: [0.52, -0.32, 0.0] footprints: - - {type: circle, radius: 0.045, density: 150, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], density: 150, sensor: true, layers: [robot]} + - {type: circle, radius: 0.045, mass: 0.95, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.19, -0.06], [0.19, -0.06], [0.19, 0.06], [-0.19, 0.06]], mass: 6.84, sensor: true, layers: [robot]} joints: - name: front_left_pivot type: revolute diff --git a/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml b/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml index 3e14556b..fdb24a23 100644 --- a/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml +++ b/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml @@ -5,10 +5,10 @@ bodies: footprints: - type: polygon points: [[-0.23, -0.16], [0.23, -0.16], [0.23, 0.16], [-0.23, 0.16]] - density: 21 + mass: 3.09 layers: [robot] - - {type: polygon, points: [[-0.05, 0.135], [0.09, 0.135], [0.09, 0.185], [-0.05, 0.185]], density: 50, sensor: true, layers: [robot]} - - {type: polygon, points: [[-0.05, -0.185], [0.09, -0.185], [0.09, -0.135], [-0.05, -0.135]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.05, 0.135], [0.09, 0.135], [0.09, 0.185], [-0.05, 0.185]], mass: 0.35, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.05, -0.185], [0.09, -0.185], [0.09, -0.135], [-0.05, -0.135]], mass: 0.35, sensor: true, layers: [robot]} - name: caster_platform type: dynamic pose: [-0.19, 0.0, 0.0] @@ -16,10 +16,10 @@ bodies: footprints: - type: circle radius: 0.025 - density: 50 + mass: 0.10 sensor: true layers: [robot] - - {type: polygon, points: [[-0.105, -0.025], [-0.035, -0.025], [-0.035, 0.025], [-0.105, 0.025]], density: 50, sensor: true, layers: [robot]} + - {type: polygon, points: [[-0.105, -0.025], [-0.035, -0.025], [-0.035, 0.025], [-0.105, 0.025]], mass: 0.18, sensor: true, layers: [robot]} joints: - name: caster_swivel type: revolute From e06e1713351868403a5d01c6e1bde299059f07ff Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 00:48:54 +0000 Subject: [PATCH 05/13] Added a new unit test to cover a gap that has caused issues in the past --- flatland_server/test/load_world_test.cpp | 20 +++++++++++++++++++ .../load_world_tests/no_models/world.yaml | 4 ++++ 2 files changed, 24 insertions(+) create mode 100644 flatland_server/test/load_world_tests/no_models/world.yaml diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp index 90b8143a..3d4e5949 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -520,6 +521,25 @@ class LoadWorldTest : public ::testing::Test } }; +TEST_F(LoadWorldTest, map_without_models) +{ + world_yaml = this_file_dir / "load_world_tests/no_models/world.yaml"; + auto node = rclcpp::Node::make_shared("map_without_models_node"); + w = World::MakeWorld(node, world_yaml.string()); + + ASSERT_TRUE(w->models_.empty()); + ASSERT_EQ(w->layers_.size(), 1u); + EXPECT_GT(w->layers_[0]->body_->GetFixturesCount(), 0); + + Timekeeper timekeeper(node); + timekeeper.SetMaxStepSize(0.01); + const auto start = timekeeper.GetSimTime(); + for (int step = 0; step < 3; ++step) { + w->Update(timekeeper); + } + EXPECT_GT((timekeeper.GetSimTime() - start).seconds(), 0.0); +} + /** * This test loads the world, layers, models from the given world * yaml file and checks that all configurations, data, and calculations are diff --git a/flatland_server/test/load_world_tests/no_models/world.yaml b/flatland_server/test/load_world_tests/no_models/world.yaml new file mode 100644 index 00000000..f0efac7d --- /dev/null +++ b/flatland_server/test/load_world_tests/no_models/world.yaml @@ -0,0 +1,4 @@ +properties: {} +layers: + - name: map + map: ../simple_test_A/map.yaml \ No newline at end of file From 75f4f58b2be850adf5adef5c54d8caab7404d40c Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 01:18:04 +0000 Subject: [PATCH 06/13] Added code coverage --- .github/workflows/industrial_ci_action.yml | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index c7ccabff..d0a6c6bd 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -25,4 +25,24 @@ jobs: steps: - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} \ No newline at end of file + 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 + CC: gcc + CXX: g++ + ADDITIONAL_DEBS: gcovr + 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" \ No newline at end of file From 9361de17ba352724a5ba035c5ba26be0c8f2afab Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 02:50:19 +0000 Subject: [PATCH 07/13] Added a temporary shim for two unrelated things: Twist->actuator controls for the final 2 ros2 control examples, and updates to the dockerfile and CI to allow building the ros2 control examples. --- .devcontainer/Dockerfile | 13 ++- .devcontainer/post-create-command.sh | 2 +- .github/workflows/industrial_ci_action.yml | 7 +- flatland-not-released.repos | 6 + flatland_ros2_control_examples/CMakeLists.txt | 1 + flatland_ros2_control_examples/README.md | 18 ++- .../launch/example.launch.py | 18 +++ flatland_ros2_control_examples/package.xml | 3 + .../scripts/twist_to_joint_commands.py | 104 ++++++++++++++++++ .../test/controller_config_test.py | 42 ++++++- 10 files changed, 200 insertions(+), 14 deletions(-) create mode 100644 flatland-not-released.repos create mode 100644 flatland_ros2_control_examples/scripts/twist_to_joint_commands.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 53e9cb6c..f1a180ff 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -12,13 +12,22 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ openssh-client \ python3-colcon-common-extensions \ python3-rosdep \ + python3-vcstool \ 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 diff --git a/.devcontainer/post-create-command.sh b/.devcontainer/post-create-command.sh index a99044c1..42eb93b8 100644 --- a/.devcontainer/post-create-command.sh +++ b/.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 diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index d0a6c6bd..d4adb100 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -16,11 +16,11 @@ 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: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} # - {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: 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', UPSTREAM_WORKSPACE: flatland-not-released.repos} + - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos} runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 @@ -37,6 +37,7 @@ jobs: ROS_DISTRO: lyrical ROS_REPO: main DOCKER_IMAGE: osrf/ros:lyrical-desktop-full + UPSTREAM_WORKSPACE: flatland-not-released.repos CC: gcc CXX: g++ ADDITIONAL_DEBS: gcovr diff --git a/flatland-not-released.repos b/flatland-not-released.repos new file mode 100644 index 00000000..ecd6ba95 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/flatland_ros2_control_examples/CMakeLists.txt b/flatland_ros2_control_examples/CMakeLists.txt index 0bb3b7a4..52b814e2 100644 --- a/flatland_ros2_control_examples/CMakeLists.txt +++ b/flatland_ros2_control_examples/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) install(DIRECTORY models worlds launch config DESTINATION share/${PROJECT_NAME}) install(FILES README.md DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS scripts/twist_to_joint_commands.py DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/flatland_ros2_control_examples/README.md b/flatland_ros2_control_examples/README.md index cd9b6bb9..617ceb7b 100644 --- a/flatland_ros2_control_examples/README.md +++ b/flatland_ros2_control_examples/README.md @@ -18,9 +18,12 @@ CAD reproductions or calibrated vehicle dynamics. that both drives and steers*, and can publish an Ackermann command. The stock `ackermann_steering_controller` needs two driven wheels and two separate steering joints, so it is used on the rear-drive car instead. The swerve and articulated -profiles expose individual joint commands; they do not include body-twist to -wheel inverse kinematics. Swerve steering is limited to +/-2.98 radians, rather -than continuous swivel rotation. +profiles use a small TwistStamped adapter to publish wheel speeds and steering +angles to their forward controllers. Swerve accepts forward, lateral and yaw +commands; its pods reverse drive direction to keep steering within +/-90 degrees +(the joints are limited to +/-2.98 radians). The articulated adapter approximates +yaw with a bounded hinge angle and left/right wheel speeds. It ignores lateral +velocity and cannot turn in place; zero forward speed stops all four wheels. ## Run @@ -46,9 +49,12 @@ Joystick teleop is enabled by default with ros2_control; set `use_joystick:=fals to skip it or `joy_device_id:=1` to select another device. Hold button 4 (left bumper on a typical gamepad), use axis 1 for forward speed and axis 3 for turning. `teleop_twist_joy` publishes TwistStamped on `/drive/cmd_vel` -for the differential, tricycle and Ackermann models. -The swerve and articulated profiles use forward joint controllers and do not -consume Twist commands; command their wheel and steering controllers directly. +for all six models. For swerve and articulated models, the adapter also runs +with `use_joystick:=false`, so an external publisher can send TwistStamped to +`/drive/cmd_vel`. It sends zero wheel speeds and centers steering if commands +stop arriving for 0.5 seconds. Their forward controllers can still be commanded +directly on `/wheels/commands` and `/steering/commands`, but avoid competing +publishers while the adapter is running. The six `models/*.model.yaml` files specify physical bodies, contact offsets, wheel radii and plugin names; `worlds/*.world.yaml` each load one model. diff --git a/flatland_ros2_control_examples/launch/example.launch.py b/flatland_ros2_control_examples/launch/example.launch.py index 4c6732bf..e215f633 100644 --- a/flatland_ros2_control_examples/launch/example.launch.py +++ b/flatland_ros2_control_examples/launch/example.launch.py @@ -126,6 +126,24 @@ def launch_example(context): for controller in ("joint_state_broadcaster", *controllers) ] nodes.extend(spawners) + if name in ("2910_swerve", "articulated_204g"): + wheels = {plugin["name"]: plugin for plugin in model["plugins"] + if plugin["type"] == "DriveWheel"} + geometry = {"robot": name, "wheel_radius": wheels["front_left_wheel"]["radius"]} + if name == "2910_swerve": + front_left = next(body for body in model["bodies"] if body["name"] == "front_left_pod") + geometry.update(module_x=front_left["pose"][0], module_y=front_left["pose"][1]) + else: + front = next(body for body in model["bodies"] if body["name"] == "front_chassis") + hinge = next(plugin for plugin in model["plugins"] if plugin["name"] == "articulation") + geometry.update(half_track=wheels["front_left_wheel"]["offset"][1], + half_wheelbase=front["pose"][0], max_angle=hinge["limit"]["upper"]) + nodes.append(Node( + package="flatland_ros2_control_examples", + executable="twist_to_joint_commands.py", + parameters=[geometry], + output="screen", + )) if viz_node: nodes.append(RegisterEventHandler(OnProcessExit( target_action=spawners[-1], on_exit=[viz_node] diff --git a/flatland_ros2_control_examples/package.xml b/flatland_ros2_control_examples/package.xml index 65a9205f..981d5a70 100644 --- a/flatland_ros2_control_examples/package.xml +++ b/flatland_ros2_control_examples/package.xml @@ -22,6 +22,9 @@ ackermann_steering_controller tricycle_controller forward_command_controller + rclpy + geometry_msgs + std_msgs joy teleop_twist_joy diff --git a/flatland_ros2_control_examples/scripts/twist_to_joint_commands.py b/flatland_ros2_control_examples/scripts/twist_to_joint_commands.py new file mode 100644 index 00000000..6bc0080e --- /dev/null +++ b/flatland_ros2_control_examples/scripts/twist_to_joint_commands.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +import math +import time + +import rclpy +from geometry_msgs.msg import TwistStamped +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray + + +def swerve_commands(linear_x, linear_y, angular_z, wheel_radius, module_x, module_y): + speeds = [] + angles = [] + for x, y in ((module_x, module_y), (module_x, -module_y), + (-module_x, module_y), (-module_x, -module_y)): + velocity_x = linear_x - angular_z * y + velocity_y = linear_y + angular_z * x + speed = math.hypot(velocity_x, velocity_y) + angle = math.atan2(velocity_y, velocity_x) if speed > 1e-9 else 0.0 + if abs(angle) > math.pi / 2: + angle = math.remainder(angle + math.pi, 2 * math.pi) + speed = -speed + speeds.append(speed / wheel_radius) + angles.append(angle) + return speeds, angles + + +def articulated_commands(linear_x, angular_z, wheel_radius, half_track, half_wheelbase, max_angle): + if abs(linear_x) < 1e-6: + return [0.0] * 4, [0.0] + angle = max(-max_angle, min(max_angle, 2 * math.atan(half_wheelbase * angular_z / linear_x))) + achievable_yaw = linear_x * math.tan(angle / 2) / half_wheelbase + left = (linear_x - achievable_yaw * half_track) / wheel_radius + right = (linear_x + achievable_yaw * half_track) / wheel_radius + return [left, right, left, right], [angle] + + +class TwistToJointCommands(Node): + def __init__(self): + super().__init__("twist_to_joint_commands") + self.declare_parameter("robot", "") + self.declare_parameter("wheel_radius", 0.0) + self.declare_parameter("module_x", 0.0) + self.declare_parameter("module_y", 0.0) + self.declare_parameter("half_track", 0.0) + self.declare_parameter("half_wheelbase", 0.0) + self.declare_parameter("max_angle", 0.0) + self.declare_parameter("cmd_timeout", 0.5) + self.robot = self.get_parameter("robot").value + self.wheel_radius = self.get_parameter("wheel_radius").value + self.module_x = self.get_parameter("module_x").value + self.module_y = self.get_parameter("module_y").value + self.half_track = self.get_parameter("half_track").value + self.half_wheelbase = self.get_parameter("half_wheelbase").value + self.max_angle = self.get_parameter("max_angle").value + self.timeout = self.get_parameter("cmd_timeout").value + if (self.robot not in ("2910_swerve", "articulated_204g") or self.wheel_radius <= 0 + or self.timeout <= 0 or (self.robot == "2910_swerve" and (self.module_x <= 0 or self.module_y <= 0)) + or (self.robot == "articulated_204g" and + (self.half_track <= 0 or self.half_wheelbase <= 0 or self.max_angle <= 0))): + raise ValueError("Invalid robot or geometry for twist_to_joint_commands") + + self.wheels = self.create_publisher(Float64MultiArray, "/wheels/commands", 10) + self.steering = self.create_publisher(Float64MultiArray, "/steering/commands", 10) + self.create_subscription(TwistStamped, "/drive/cmd_vel", self.on_twist, 10) + self.command = None + self.received_at = None + self.create_timer(0.05, self.publish_commands) + + def on_twist(self, message): + self.command = message.twist + self.received_at = time.monotonic() + + def publish_commands(self): + if (self.command is None or + time.monotonic() - self.received_at > self.timeout): + linear_x = linear_y = angular_z = 0.0 + else: + linear_x = self.command.linear.x + linear_y = self.command.linear.y + angular_z = self.command.angular.z + if self.robot == "2910_swerve": + speeds, angles = swerve_commands( + linear_x, linear_y, angular_z, self.wheel_radius, self.module_x, self.module_y) + else: + speeds, angles = articulated_commands( + linear_x, angular_z, self.wheel_radius, self.half_track, + self.half_wheelbase, self.max_angle) + self.wheels.publish(Float64MultiArray(data=speeds)) + self.steering.publish(Float64MultiArray(data=angles)) + + +def main(): + rclpy.init() + node = TwistToJointCommands() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/flatland_ros2_control_examples/test/controller_config_test.py b/flatland_ros2_control_examples/test/controller_config_test.py index 6ad9c099..ff1b2cd7 100644 --- a/flatland_ros2_control_examples/test/controller_config_test.py +++ b/flatland_ros2_control_examples/test/controller_config_test.py @@ -1,4 +1,5 @@ import importlib.util +import math import unittest from pathlib import Path from xml.etree import ElementTree @@ -16,6 +17,11 @@ assert SPEC is not None and SPEC.loader is not None EXAMPLE_LAUNCH = importlib.util.module_from_spec(SPEC) SPEC.loader.exec_module(EXAMPLE_LAUNCH) +ADAPTER_SPEC = importlib.util.spec_from_file_location( + "twist_to_joint_commands", ROOT / "scripts" / "twist_to_joint_commands.py") +assert ADAPTER_SPEC is not None and ADAPTER_SPEC.loader is not None +ADAPTER = importlib.util.module_from_spec(ADAPTER_SPEC) +ADAPTER_SPEC.loader.exec_module(ADAPTER) class ControllerConfigTest(unittest.TestCase): @@ -39,7 +45,14 @@ def test_launch_constructs_both_modes(self): self.assertEqual(actions[1].node_executable, "flatland_viz") context.launch_configurations["use_ros2_control"] = "true" control_actions = EXAMPLE_LAUNCH.launch_example(context) - self.assertEqual(len(control_actions), 5 + len(config) + viz_count) + has_adapter = example in ("2910_swerve", "articulated_204g") + self.assertEqual(len(control_actions), 5 + len(config) + viz_count + has_adapter) + self.assertEqual( + [action.node_executable for action in control_actions + if isinstance(action, Node) + and action.node_package == "flatland_ros2_control_examples"], + ["twist_to_joint_commands.py"] if has_adapter else [], + ) spawners = [action for action in control_actions if isinstance(action, Node) and action.node_executable == "spawner"] for spawner in spawners: @@ -59,10 +72,35 @@ def test_launch_constructs_both_modes(self): for handler in handlers: self.assertIsInstance(handler.event_handler, OnProcessExit) context.launch_configurations["use_joystick"] = "false" - self.assertEqual(len(EXAMPLE_LAUNCH.launch_example(context)), 3 + len(config) + viz_count) + no_joystick = EXAMPLE_LAUNCH.launch_example(context) + self.assertEqual(len(no_joystick), 3 + len(config) + viz_count + has_adapter) + self.assertEqual(sum(isinstance(action, Node) and + action.node_package == "flatland_ros2_control_examples" + for action in no_joystick), has_adapter) context.launch_configurations["use_joystick"] = "true" context.launch_configurations["use_ros2_control"] = "false" + def test_twist_to_joint_commands(self): + speeds, angles = ADAPTER.swerve_commands(1, 0.2, 0.5, 0.051, 0.28, 0.28) + for index, (x, y) in enumerate(((0.28, 0.28), (0.28, -0.28), + (-0.28, 0.28), (-0.28, -0.28))): + self.assertAlmostEqual(speeds[index] * 0.051 * math.cos(angles[index]), 1 - 0.5 * y) + self.assertAlmostEqual(speeds[index] * 0.051 * math.sin(angles[index]), 0.2 + 0.5 * x) + self.assertLessEqual(abs(angles[index]), math.pi / 2) + speeds, angles = ADAPTER.swerve_commands(0, 0, 0, 0.051, 0.28, 0.28) + self.assertEqual(speeds, [0.0] * 4) + self.assertEqual(angles, [0.0] * 4) + + speeds, angles = ADAPTER.articulated_commands(1, 0.3, 0.29, 0.32, 0.45, 0.6981317) + self.assertAlmostEqual(angles[0], 2 * math.atan(0.45 * 0.3)) + self.assertAlmostEqual(speeds[0], (1 - 0.3 * 0.32) / 0.29) + self.assertAlmostEqual(speeds[1], (1 + 0.3 * 0.32) / 0.29) + self.assertEqual(speeds, [speeds[0], speeds[1]] * 2) + self.assertEqual(ADAPTER.articulated_commands(0, 1, 0.29, 0.32, 0.45, 0.6981317), + ([0.0] * 4, [0.0])) + self.assertEqual(ADAPTER.articulated_commands(1, 100, 0.29, 0.32, 0.45, 0.6981317)[1], + [0.6981317]) + def test_wheel_footprints_match_plugins(self): for example in EXAMPLE_LAUNCH.EXAMPLES: model = yaml.safe_load((ROOT / "models" / f"{example}.model.yaml").read_text()) From 68ac4e60d4f90eef0ced1b8b9c3c124c53daa60e Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 03:15:11 +0000 Subject: [PATCH 08/13] Testing cyclonedds --- .github/workflows/industrial_ci_action.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index d4adb100..519aa24e 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -19,8 +19,8 @@ jobs: - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} # - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos} - - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos} + - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-lyrical-rmw-cyclonedds-cpp, RMW_IMPLEMENTATION: rmw_cyclonedds_cpp} + - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, RMW_IMPLEMENTATION: rmw_cyclonedds_cpp} runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 @@ -40,7 +40,8 @@ jobs: UPSTREAM_WORKSPACE: flatland-not-released.repos CC: gcc CXX: g++ - ADDITIONAL_DEBS: gcovr + ADDITIONAL_DEBS: gcovr ros-lyrical-rmw-cyclonedds-cpp + 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" From 969be14766516c3f4fbe89538a86746f6bc57db7 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 03:21:40 +0000 Subject: [PATCH 09/13] Updated CI action --- .devcontainer/Dockerfile | 3 +++ .github/workflows/industrial_ci_action.yml | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index f1a180ff..591919d7 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,6 +1,7 @@ ARG ROS_DISTRO=humble FROM ros:${ROS_DISTRO} +ARG ROS_DISTRO ARG DEBIAN_FRONTEND=noninteractive ARG USERNAME=flatland @@ -13,6 +14,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ python3-colcon-common-extensions \ python3-rosdep \ python3-vcstool \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ sudo \ && rm -rf /var/lib/apt/lists/* @@ -32,6 +34,7 @@ RUN mkdir -p /opt/flatland_upstream_ws/src && \ 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 \ diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 519aa24e..b1413a88 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -19,8 +19,8 @@ jobs: - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} # - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-lyrical-rmw-cyclonedds-cpp, RMW_IMPLEMENTATION: rmw_cyclonedds_cpp} - - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, RMW_IMPLEMENTATION: rmw_cyclonedds_cpp} + - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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: noble, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full-noble', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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 @@ -41,7 +41,7 @@ jobs: CC: gcc CXX: g++ ADDITIONAL_DEBS: gcovr ros-lyrical-rmw-cyclonedds-cpp - RMW_IMPLEMENTATION: 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" From 69c6a114c1b8c0cdd99bb6ea6ca7c27e48fc81f2 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 03:34:22 +0000 Subject: [PATCH 10/13] Fixed flakey test --- .github/workflows/industrial_ci_action.yml | 2 +- flatland_plugins/test/bumper_test.cpp | 24 ++++++++++++++++++---- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index b1413a88..a9818d3b 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -20,7 +20,7 @@ jobs: # - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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: noble, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full-noble', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-rolling-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', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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 diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 4fcbb37e..d6ad9eaf 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -136,6 +136,14 @@ class BumperPluginTest : public ::testing::Test return true; } + const Collision * CollisionForBody(const Collisions & collisions, const std::string & body) + { + for (const auto & collision : collisions.collisions) { + if (collision.body_a == body) return &collision; + } + return nullptr; + } + // check the received scan data is as expected bool CollisionEq( const Collision & collision, const std::string & entity_a, const std::string & body_A, @@ -248,10 +256,14 @@ TEST_F(BumperPluginTest, collision_test) } SpinRos(500, 10); ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); + const Collision * sensor_collision = CollisionForBody(msg1, "base_link_1"); + const Collision * wheel_collision = CollisionForBody(msg1, "base_link_2"); + ASSERT_NE(sensor_collision, nullptr); + ASSERT_NE(wheel_collision, nullptr); EXPECT_TRUE( - CollisionEq(msg1.collisions[0], "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); + CollisionEq(*sensor_collision, "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); EXPECT_TRUE( - CollisionEq(msg1.collisions[1], "robot1", "base_link_2", "layer_1", "layer_1", 1, {1, 0})); + CollisionEq(*wheel_collision, "robot1", "base_link_2", "layer_1", "layer_1", 1, {1, 0})); ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); EXPECT_TRUE( CollisionEq(msg2.collisions[0], "robot1", "base_link_2", "layer_1", "layer_1", 1, {1, 0})); @@ -280,10 +292,14 @@ TEST_F(BumperPluginTest, collision_test) SpinRos(500, 10); ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); + sensor_collision = CollisionForBody(msg1, "base_link_1"); + wheel_collision = CollisionForBody(msg1, "base_link_2"); + ASSERT_NE(sensor_collision, nullptr); + ASSERT_NE(wheel_collision, nullptr); EXPECT_TRUE( - CollisionEq(msg1.collisions[0], "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); + CollisionEq(*sensor_collision, "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); EXPECT_TRUE( - CollisionEq(msg1.collisions[1], "robot1", "base_link_2", "layer_1", "layer_1", 1, {-1, 0})); + CollisionEq(*wheel_collision, "robot1", "base_link_2", "layer_1", "layer_1", 1, {-1, 0})); ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); EXPECT_TRUE( CollisionEq(msg2.collisions[0], "robot1", "base_link_2", "layer_1", "layer_1", 1, {-1, 0})); From 817a948755893e5e1b0ddef2cdb39fae7197dac0 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 03:43:32 +0000 Subject: [PATCH 11/13] Excluded Humble from default CI, adding Jazzy --- .github/workflows/industrial_ci_action.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index a9818d3b..c2e7bc17 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -16,9 +16,9 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} - # - {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: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} + - {ROS_DISTRO: jazzy, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:jazzy-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} + # - {ROS_DISTRO: kilted, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:kilted-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'} runs-on: ubuntu-24.04 From 91700bce2288c683a6dd1d05b6aaf7c0416a7cfd Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 03:55:37 +0000 Subject: [PATCH 12/13] Updated CI again --- .github/workflows/industrial_ci_action.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index c2e7bc17..28fbd7ab 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -16,11 +16,11 @@ jobs: strategy: matrix: env: - # - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} - - {ROS_DISTRO: jazzy, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:jazzy-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} - # - {ROS_DISTRO: kilted, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:kilted-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, UPSTREAM_WORKSPACE: flatland-not-released.repos} - - {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', UPSTREAM_WORKSPACE: flatland-not-released.repos, 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', UPSTREAM_WORKSPACE: flatland-not-released.repos, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'} + # - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples'} + - {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'} + # - {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 @@ -37,7 +37,7 @@ jobs: ROS_DISTRO: lyrical ROS_REPO: main DOCKER_IMAGE: osrf/ros:lyrical-desktop-full - UPSTREAM_WORKSPACE: flatland-not-released.repos + ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface CC: gcc CXX: g++ ADDITIONAL_DEBS: gcovr ros-lyrical-rmw-cyclonedds-cpp From 7ded0888a73871aee0f96b6533f750ab48d7a68a Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Mon, 28 Sep 2026 04:05:11 +0000 Subject: [PATCH 13/13] Updated jazzy requirements --- .github/workflows/industrial_ci_action.yml | 3 +-- flatland-not-released.jazzy.repos | 5 +++++ 2 files changed, 6 insertions(+), 2 deletions(-) create mode 100644 flatland-not-released.jazzy.repos diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 28fbd7ab..b0d4df83 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -16,8 +16,7 @@ jobs: strategy: matrix: env: - # - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples'} - - {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'} + - {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'} diff --git a/flatland-not-released.jazzy.repos b/flatland-not-released.jazzy.repos new file mode 100644 index 00000000..bdf9bd62 --- /dev/null +++ b/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 \ No newline at end of file