diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 53e9cb6c..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 @@ -12,17 +13,28 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ openssh-client \ python3-colcon-common-extensions \ python3-rosdep \ + python3-vcstool \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ sudo \ && rm -rf /var/lib/apt/lists/* COPY . /tmp/flatland -RUN rosdep update && \ +RUN mkdir -p /opt/flatland_upstream_ws/src && \ + vcs import /opt/flatland_upstream_ws/src < /tmp/flatland/flatland-not-released.repos && \ + rosdep update && \ apt-get update && \ - rosdep install --from-paths /tmp/flatland --ignore-src --rosdistro "$ROS_DISTRO" -y && \ + rosdep install --from-paths /tmp/flatland /opt/flatland_upstream_ws/src \ + --ignore-src --rosdistro "$ROS_DISTRO" -y && \ + . "/opt/ros/$ROS_DISTRO/setup.sh" && \ + colcon --log-base /opt/flatland_upstream_ws/log build \ + --base-paths /opt/flatland_upstream_ws/src \ + --build-base /opt/flatland_upstream_ws/build \ + --install-base /opt/flatland_upstream_ws/install && \ rm -rf /tmp/flatland /var/lib/apt/lists/* ENV CCACHE_DIR=/tmp/.ccache ENV QT_QPA_PLATFORM=xcb +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp WORKDIR /opt/overlay_ws RUN if id -u ubuntu >/dev/null 2>&1; then \ if [ "$USERNAME" != ubuntu ]; then \ 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 c7ccabff..b0d4df83 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -16,13 +16,34 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: humble, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:humble-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true} - # - {ROS_DISTRO: jazzy, ROS_REPO: main, AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true} - # - {ROS_DISTRO: kilted, ROS_REPO: main, AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'} - - {ROS_DISTRO: lyrical, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:lyrical-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'} - - {ROS_DISTRO: rolling, ROS_REPO: main, OS_CODE_NAME: noble, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full-noble', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON'} + - {ROS_DISTRO: jazzy, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:jazzy-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples', UPSTREAM_WORKSPACE: flatland-not-released.jazzy.repos} + # - {ROS_DISTRO: kilted, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:kilted-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_WORKSPACE: '. -flatland_ros2_control_examples'} + - {ROS_DISTRO: lyrical, ROS_REPO: main, DOCKER_IMAGE: 'osrf/ros:lyrical-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface, ADDITIONAL_DEBS: ros-lyrical-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'} + - {ROS_DISTRO: rolling, ROS_REPO: main, OS_CODE_NAME: resolute, DOCKER_IMAGE: 'osrf/ros:rolling-desktop-full', AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true, TARGET_CMAKE_ARGS: '-DCMAKE_COMPILE_WARNING_AS_ERROR=ON', ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface, ADDITIONAL_DEBS: ros-rolling-rmw-cyclonedds-cpp, DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'} runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} \ 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 + ROSDEP_SKIP_KEYS: joint_command_topic_hardware_interface + CC: gcc + CXX: g++ + ADDITIONAL_DEBS: gcovr ros-lyrical-rmw-cyclonedds-cpp + DOCKER_RUN_OPTS: '-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp' + TARGET_CMAKE_ARGS: '-DCMAKE_C_FLAGS=--coverage -DCMAKE_CXX_FLAGS=--coverage -DCMAKE_EXE_LINKER_FLAGS=--coverage -DCMAKE_SHARED_LINKER_FLAGS=--coverage' + AFTER_SCRIPT: >- + gcovr --root "$BASEDIR/${PREFIX}target_ws/src" + --filter "$BASEDIR/${PREFIX}target_ws/src" + --exclude '.*/test/.*' + --print-summary "$BASEDIR/${PREFIX}target_ws/build" \ No newline at end of file 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-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 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_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/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})); 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_ros2_control_examples/CMakeLists.txt b/flatland_ros2_control_examples/CMakeLists.txt new file mode 100644 index 00000000..52b814e2 --- /dev/null +++ b/flatland_ros2_control_examples/CMakeLists.txt @@ -0,0 +1,32 @@ +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}) +install(PROGRAMS scripts/twist_to_joint_commands.py DESTINATION lib/${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..617ceb7b --- /dev/null +++ b/flatland_ros2_control_examples/README.md @@ -0,0 +1,83 @@ +# 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 | +| --- | --- | --- | +| `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` | +| `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 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 + +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:=turtlebot_caster +``` + +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 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. +`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/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/config/turtlebot_caster.yaml b/flatland_ros2_control_examples/config/turtlebot_caster.yaml new file mode 100644 index 00000000..0e527d94 --- /dev/null +++ b/flatland_ros2_control_examples/config/turtlebot_caster.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/launch/example.launch.py b/flatland_ros2_control_examples/launch/example.launch.py new file mode 100644 index 00000000..e215f633 --- /dev/null +++ b/flatland_ros2_control_examples/launch/example.launch.py @@ -0,0 +1,189 @@ +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 = ( + "turtlebot_caster", + "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 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] + ))) + 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="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"]), + 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..999f45e9 --- /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]] + mass: 49.28 + layers: [robot] + - name: front_left_pod + type: dynamic + pose: [0.28, 0.28, 0.0] + footprints: + - {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, 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, 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, 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 + 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..245e9cab --- /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]] + mass: 102.34 + 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} + - {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..a1ab36d0 --- /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]] + mass: 199.52 + 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] + 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]] + mass: 199.52 + 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 + 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/front_drive_tricycle.model.yaml b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml new file mode 100644 index 00000000..2e3e409c --- /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]] + mass: 190.96 + 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, 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 + 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..32c60a70 --- /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]] + mass: 186.48 + 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, 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, 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 + 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/models/turtlebot_caster.model.yaml b/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml new file mode 100644 index 00000000..fdb24a23 --- /dev/null +++ b/flatland_ros2_control_examples/models/turtlebot_caster.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]] + mass: 3.09 + 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] + color: [0.3, 0.35, 0.37, 1.0] + footprints: + - type: circle + radius: 0.025 + 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]], mass: 0.18, 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/package.xml b/flatland_ros2_control_examples/package.xml new file mode 100644 index 00000000..981d5a70 --- /dev/null +++ b/flatland_ros2_control_examples/package.xml @@ -0,0 +1,36 @@ + + + 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 + rclpy + geometry_msgs + std_msgs + 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/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 new file mode 100644 index 00000000..ff1b2cd7 --- /dev/null +++ b/flatland_ros2_control_examples/test/controller_config_test.py @@ -0,0 +1,184 @@ +import importlib.util +import math +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) +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): + 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) + 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: + 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" + 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()) + 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..28c266c1 --- /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>{{ + {"turtlebot_caster", 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 / "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(); + 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", "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("turtlebot_caster") && 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/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 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 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..3d4e5949 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -49,8 +49,11 @@ #include #include #include +#include +#include #include #include +#include #include #include @@ -518,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 @@ -759,6 +781,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/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 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()