From c9c20acb18189501cd08aaddd3fa1dde1c092ba5 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Sun, 4 Oct 2026 18:22:20 +0000 Subject: [PATCH] Split out the ros2 control dependant plugins, fixing a bunch of namespace issues for plugins --- docs/included_plugins/drive_wheel.rst | 3 +- docs/included_plugins/steering_motor.rst | 3 +- docs/overview.rst | 5 +- flatland_controls_plugins/CMakeLists.txt | 71 ++++++++++++++++++ .../flatland_controls_plugins}/drive_wheel.h | 10 +-- .../steering_motor.h | 10 +-- flatland_controls_plugins/package.xml | 27 +++++++ .../plugin_description.xml | 8 ++ .../src/drive_wheel.cpp | 8 +- .../src/steering_motor.cpp | 8 +- .../test/joint_actuator_test.cpp | 52 ++++++------- .../test/update_timer_test/map_1.png | Bin 0 -> 290 bytes .../test/update_timer_test/map_1.yaml | 7 ++ .../test/update_timer_test/robot.model.yaml | 11 +++ .../test/update_timer_test/world.yaml | 9 +++ flatland_plugins/CMakeLists.txt | 9 +-- flatland_plugins/package.xml | 1 - flatland_plugins/plugin_description.xml | 6 -- flatland_ros2_control_examples/CMakeLists.txt | 3 +- .../launch/example.launch.py | 6 +- .../models/2910_swerve.model.yaml | 16 ++-- .../models/a300_diff.model.yaml | 8 +- .../models/articulated_204g.model.yaml | 10 +-- .../models/front_drive_tricycle.model.yaml | 4 +- .../models/rear_drive_ackermann.model.yaml | 8 +- .../models/turtlebot_caster.model.yaml | 4 +- flatland_ros2_control_examples/package.xml | 1 + .../test/controller_config_test.py | 4 +- .../test/worlds_test.cpp | 8 +- 29 files changed, 222 insertions(+), 98 deletions(-) create mode 100644 flatland_controls_plugins/CMakeLists.txt rename {flatland_plugins/include/flatland_plugins => flatland_controls_plugins/include/flatland_controls_plugins}/drive_wheel.h (83%) rename {flatland_plugins/include/flatland_plugins => flatland_controls_plugins/include/flatland_controls_plugins}/steering_motor.h (77%) create mode 100644 flatland_controls_plugins/package.xml create mode 100644 flatland_controls_plugins/plugin_description.xml rename {flatland_plugins => flatland_controls_plugins}/src/drive_wheel.cpp (95%) rename {flatland_plugins => flatland_controls_plugins}/src/steering_motor.cpp (94%) rename {flatland_plugins => flatland_controls_plugins}/test/joint_actuator_test.cpp (83%) create mode 100644 flatland_controls_plugins/test/update_timer_test/map_1.png create mode 100644 flatland_controls_plugins/test/update_timer_test/map_1.yaml create mode 100644 flatland_controls_plugins/test/update_timer_test/robot.model.yaml create mode 100644 flatland_controls_plugins/test/update_timer_test/world.yaml diff --git a/docs/included_plugins/drive_wheel.rst b/docs/included_plugins/drive_wheel.rst index ba8c1265..54f79e47 100644 --- a/docs/included_plugins/drive_wheel.rst +++ b/docs/included_plugins/drive_wheel.rst @@ -5,11 +5,12 @@ 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. +This plugin is provided by ``flatland_controls_plugins``. .. code-block:: yaml plugins: - - type: DriveWheel + - type: flatland_controls_plugins::DriveWheel name: left_drive body: base offset: [0.0, 0.3, 0.0] diff --git a/docs/included_plugins/steering_motor.rst b/docs/included_plugins/steering_motor.rst index f45fe079..82dbd70b 100644 --- a/docs/included_plugins/steering_motor.rst +++ b/docs/included_plugins/steering_motor.rst @@ -3,11 +3,12 @@ SteeringMotor SteeringMotor actuates a model's revolute Box2D joint and publishes its angle, angular speed and applied torque as joint state. +This plugin is provided by ``flatland_controls_plugins``. .. code-block:: yaml plugins: - - type: SteeringMotor + - type: flatland_controls_plugins::SteeringMotor name: front_steering joint: steering_joint mode: position diff --git a/docs/overview.rst b/docs/overview.rst index 5d70063e..2a6b9f34 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -12,8 +12,9 @@ Flatland Simulator is a light weight, performance centric 2D robot simulator. It is built to integrate directly with ROS and uses Box2D for physics simulation. The build fetches Box2D 3.1.1 (MIT license). -This project is divided into four ROS packages: flatland_server, flatland_plugins, -and flatland_viz, and flatland messages. +Core simulator functionality is provided by ``flatland_server``, +``flatland_plugins``, and ``flatland_controls_plugins``, with visualization in +``flatland_viz`` and message definitions in ``flatland_msgs``. Flatland Server --------------- diff --git a/flatland_controls_plugins/CMakeLists.txt b/flatland_controls_plugins/CMakeLists.txt new file mode 100644 index 00000000..f304a768 --- /dev/null +++ b/flatland_controls_plugins/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.10) + +project(flatland_controls_plugins) + +if(POLICY CMP0167) + cmake_policy(SET CMP0167 NEW) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 23) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(flatland_server REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_library(flatland_controls_plugins_lib SHARED + src/drive_wheel.cpp + src/steering_motor.cpp +) + +target_include_directories(flatland_controls_plugins_lib PUBLIC + $ + $ +) + +target_link_libraries(flatland_controls_plugins_lib PUBLIC + flatland_server::flatland_lib + pluginlib::pluginlib + rclcpp::rclcpp + ${control_msgs_TARGETS} + ${sensor_msgs_TARGETS} +) + +ament_export_targets(export_flatland_controls_plugins_lib HAS_LIBRARY_TARGET) +ament_export_dependencies(control_msgs flatland_server interactive_markers pluginlib rclcpp sensor_msgs) + +pluginlib_export_plugin_description_file(flatland_server plugin_description.xml) + +install( + TARGETS flatland_controls_plugins_lib + EXPORT export_flatland_controls_plugins_lib + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(OpenCV REQUIRED) + ament_add_gtest(joint_actuator_test test/joint_actuator_test.cpp) + target_include_directories(joint_actuator_test PRIVATE ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(joint_actuator_test flatland_controls_plugins_lib) +endif() + +ament_package() \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/drive_wheel.h b/flatland_controls_plugins/include/flatland_controls_plugins/drive_wheel.h similarity index 83% rename from flatland_plugins/include/flatland_plugins/drive_wheel.h rename to flatland_controls_plugins/include/flatland_controls_plugins/drive_wheel.h index 0f50f157..65db6ed0 100644 --- a/flatland_plugins/include/flatland_plugins/drive_wheel.h +++ b/flatland_controls_plugins/include/flatland_controls_plugins/drive_wheel.h @@ -1,11 +1,11 @@ -#ifndef FLATLAND_PLUGINS_DRIVE_WHEEL_H -#define FLATLAND_PLUGINS_DRIVE_WHEEL_H +#ifndef FLATLAND_CONTROLS_PLUGINS_DRIVE_WHEEL_H +#define FLATLAND_CONTROLS_PLUGINS_DRIVE_WHEEL_H #include #include #include -namespace flatland_plugins +namespace flatland_controls_plugins { class DriveWheel : public flatland_server::ModelPlugin @@ -35,6 +35,6 @@ class DriveWheel : public flatland_server::ModelPlugin rclcpp::Publisher::SharedPtr state_pub_; }; -} // namespace flatland_plugins +} // namespace flatland_controls_plugins -#endif // FLATLAND_PLUGINS_DRIVE_WHEEL_H \ No newline at end of file +#endif // FLATLAND_CONTROLS_PLUGINS_DRIVE_WHEEL_H \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/steering_motor.h b/flatland_controls_plugins/include/flatland_controls_plugins/steering_motor.h similarity index 77% rename from flatland_plugins/include/flatland_plugins/steering_motor.h rename to flatland_controls_plugins/include/flatland_controls_plugins/steering_motor.h index 76e8bead..5054a747 100644 --- a/flatland_plugins/include/flatland_plugins/steering_motor.h +++ b/flatland_controls_plugins/include/flatland_controls_plugins/steering_motor.h @@ -1,11 +1,11 @@ -#ifndef FLATLAND_PLUGINS_STEERING_MOTOR_H -#define FLATLAND_PLUGINS_STEERING_MOTOR_H +#ifndef FLATLAND_CONTROLS_PLUGINS_STEERING_MOTOR_H +#define FLATLAND_CONTROLS_PLUGINS_STEERING_MOTOR_H #include #include #include -namespace flatland_plugins +namespace flatland_controls_plugins { class SteeringMotor : public flatland_server::ModelPlugin @@ -26,6 +26,6 @@ class SteeringMotor : public flatland_server::ModelPlugin rclcpp::Publisher::SharedPtr state_pub_; }; -} // namespace flatland_plugins +} // namespace flatland_controls_plugins -#endif // FLATLAND_PLUGINS_STEERING_MOTOR_H \ No newline at end of file +#endif // FLATLAND_CONTROLS_PLUGINS_STEERING_MOTOR_H \ No newline at end of file diff --git a/flatland_controls_plugins/package.xml b/flatland_controls_plugins/package.xml new file mode 100644 index 00000000..1f3f2655 --- /dev/null +++ b/flatland_controls_plugins/package.xml @@ -0,0 +1,27 @@ + + + + flatland_controls_plugins + 2.0.0 + Control command plugins for Flatland + BSD + + Joseph Duchesne + Joseph Duchesne + + ament_cmake + + control_msgs + flatland_server + interactive_markers + pluginlib + rclcpp + sensor_msgs + + ament_cmake_gtest + libopencv-dev + + + ament_cmake + + \ No newline at end of file diff --git a/flatland_controls_plugins/plugin_description.xml b/flatland_controls_plugins/plugin_description.xml new file mode 100644 index 00000000..7636628e --- /dev/null +++ b/flatland_controls_plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + Traction-limited wheel controlled by joint commands + + + Revolute steering joint controlled by joint commands + + \ No newline at end of file diff --git a/flatland_plugins/src/drive_wheel.cpp b/flatland_controls_plugins/src/drive_wheel.cpp similarity index 95% rename from flatland_plugins/src/drive_wheel.cpp rename to flatland_controls_plugins/src/drive_wheel.cpp index 48fd9a3d..90659daa 100644 --- a/flatland_plugins/src/drive_wheel.cpp +++ b/flatland_controls_plugins/src/drive_wheel.cpp @@ -1,11 +1,11 @@ -#include +#include #include #include #include #include -namespace flatland_plugins +namespace flatland_controls_plugins { void DriveWheel::OnInitialize(const YAML::Node & config) @@ -109,6 +109,6 @@ void DriveWheel::AfterPhysicsStep(const flatland_server::Timekeeper & timekeeper state_pub_->publish(state); } -} // namespace flatland_plugins +} // namespace flatland_controls_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DriveWheel, flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_controls_plugins::DriveWheel, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/steering_motor.cpp b/flatland_controls_plugins/src/steering_motor.cpp similarity index 94% rename from flatland_plugins/src/steering_motor.cpp rename to flatland_controls_plugins/src/steering_motor.cpp index 72327b79..927aa805 100644 --- a/flatland_plugins/src/steering_motor.cpp +++ b/flatland_controls_plugins/src/steering_motor.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -7,7 +7,7 @@ #include #include -namespace flatland_plugins +namespace flatland_controls_plugins { void SteeringMotor::OnInitialize(const YAML::Node & config) @@ -101,6 +101,6 @@ void SteeringMotor::AfterPhysicsStep(const flatland_server::Timekeeper & timekee state_pub_->publish(state); } -} // namespace flatland_plugins +} // namespace flatland_controls_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::SteeringMotor, flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_controls_plugins::SteeringMotor, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/test/joint_actuator_test.cpp b/flatland_controls_plugins/test/joint_actuator_test.cpp similarity index 83% rename from flatland_plugins/test/joint_actuator_test.cpp rename to flatland_controls_plugins/test/joint_actuator_test.cpp index 28a86871..85b57c66 100644 --- a/flatland_plugins/test/joint_actuator_test.cpp +++ b/flatland_controls_plugins/test/joint_actuator_test.cpp @@ -1,5 +1,5 @@ -#include -#include +#include +#include #include #include #include @@ -44,8 +44,8 @@ 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); + EXPECT_NE(loader.createSharedInstance("flatland_controls_plugins::DriveWheel"), nullptr); + EXPECT_NE(loader.createSharedInstance("flatland_controls_plugins::SteeringMotor"), nullptr); } TEST(JointActuatorTest, DistributesLoadAndLimitsWheelEffort) @@ -53,7 +53,7 @@ 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; + std::vector> wheels; for (double x : {-1.0, 1.0}) { YAML::Node config; config["body"] = "base"; @@ -61,8 +61,8 @@ TEST(JointActuatorTest, DistributesLoadAndLimitsWheelEffort) 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); + auto wheel = std::make_shared(); + wheel->Initialize(node, "flatland_controls_plugins::DriveWheel", x < 0 ? "left" : "right", world->models_[0], config); world->plugin_manager_.model_plugins_.push_back(wheel); wheels.push_back(wheel); } @@ -107,8 +107,8 @@ TEST(JointActuatorTest, VelocityWheelAcceleratesTowardCommand) 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); + auto wheel = std::make_shared(); + wheel->Initialize(node, "flatland_controls_plugins::DriveWheel", "velocity_wheel", world->models_[0], config); world->plugin_manager_.model_plugins_.push_back(wheel); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -131,8 +131,8 @@ TEST(JointActuatorTest, LateralGripIsLoadLimited) 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); + auto wheel = std::make_shared(); + wheel->Initialize(node, "flatland_controls_plugins::DriveWheel", "grip_test", world->models_[0], config); wheel->UpdateGroundContactForces({body->GetMass() * 9.81}); wheel->command_ = 1000.0; wheel->has_command_ = true; @@ -160,9 +160,9 @@ TEST(JointActuatorTest, DriveWheelRejectsPositionMode) config["body"] = "base"; config["radius"] = 0.1; config["mode"] = "position"; - auto wheel = std::make_shared(); + auto wheel = std::make_shared(); EXPECT_THROW( - wheel->Initialize(node, "DriveWheel", "unsupported_wheel", world->models_[0], config), + wheel->Initialize(node, "flatland_controls_plugins::DriveWheel", "unsupported_wheel", world->models_[0], config), flatland_server::YAMLException); } @@ -172,15 +172,15 @@ TEST(JointActuatorTest, FourContactLoadsBalanceCenterOfMass) auto world = MakeWorld(node, false); auto * body = world->models_[0]->GetBody("base")->physics_body_; const auto center = body->GetWorldCenter(); - std::vector> wheels; + 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()), + auto wheel = std::make_shared(); + wheel->Initialize(node, "flatland_controls_plugins::DriveWheel", "wheel_" + std::to_string(wheels.size()), world->models_[0], config); world->plugin_manager_.model_plugins_.push_back(wheel); wheels.push_back(wheel); @@ -215,8 +215,8 @@ TEST(JointActuatorTest, SteeringSupportsAllModes) YAML::Node config; config["joint"] = "tail_revolute"; config["mode"] = mode; - auto motor = std::make_shared(); - motor->Initialize(node, "SteeringMotor", "steering", world->models_[0], config); + auto motor = std::make_shared(); + motor->Initialize(node, "flatland_controls_plugins::SteeringMotor", "steering", world->models_[0], config); world->plugin_manager_.model_plugins_.push_back(motor); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -249,8 +249,8 @@ TEST(JointActuatorTest, SteeringRequiresPluginNameInCommand) 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); + auto motor = std::make_shared(); + motor->Initialize(node, "flatland_controls_plugins::SteeringMotor", "steering_plugin", world->models_[0], config); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); auto publisher = node->create_publisher( @@ -288,8 +288,8 @@ TEST(JointActuatorTest, SteeringLimitsRotation) 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); + auto motor = std::make_shared(); + motor->Initialize(node, "flatland_controls_plugins::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); @@ -321,9 +321,9 @@ TEST(JointActuatorTest, SteeringRejectsInvalidLimits) config["joint"] = "tail_revolute"; config["limit"]["lower"] = lower; config["limit"]["upper"] = upper; - auto motor = std::make_shared(); + auto motor = std::make_shared(); EXPECT_THROW( - motor->Initialize(node, "SteeringMotor", "invalid_steering", world->models_[0], config), + motor->Initialize(node, "flatland_controls_plugins::SteeringMotor", "invalid_steering", world->models_[0], config), flatland_server::YAMLException); } } @@ -339,8 +339,8 @@ TEST(JointActuatorTest, SteeringAcceptsNoLimits) 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); + auto motor = std::make_shared(); + motor->Initialize(node, "flatland_controls_plugins::SteeringMotor", "unlimited_steering", world->models_[0], config); EXPECT_FALSE(motor->joint_->IsLimitEnabled()); } } diff --git a/flatland_controls_plugins/test/update_timer_test/map_1.png b/flatland_controls_plugins/test/update_timer_test/map_1.png new file mode 100644 index 0000000000000000000000000000000000000000..dc20f776d139f02c798b16c5bf5e0afb66e3e73d GIT binary patch literal 290 zcmeAS@N?(olHy`uVBq!ia0vp^DImFdh=kX=|D_uY#$r9Iy66gHf+|;}h2Ir#G#FEq$h4Rdj3Rg-iCJ|`S(V&xXo zfshekcEko2ph(1q1Wqt{Pzc06*rW<3S-nAQR_wxZL@O1Ta JS?83{1OQXmT7v)p literal 0 HcmV?d00001 diff --git a/flatland_controls_plugins/test/update_timer_test/map_1.yaml b/flatland_controls_plugins/test/update_timer_test/map_1.yaml new file mode 100644 index 00000000..87a89361 --- /dev/null +++ b/flatland_controls_plugins/test/update_timer_test/map_1.yaml @@ -0,0 +1,7 @@ +image: map_1.png +resolution: 0.25 +origin: [0, 0, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/flatland_controls_plugins/test/update_timer_test/robot.model.yaml b/flatland_controls_plugins/test/update_timer_test/robot.model.yaml new file mode 100644 index 00000000..5a7ff1a4 --- /dev/null +++ b/flatland_controls_plugins/test/update_timer_test/robot.model.yaml @@ -0,0 +1,11 @@ +# Turtlebot + +bodies: # List of named bodies + - name: base + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 0, 1] + footprints: + - type: polygon + density: 1 + points: [[1.5, 0], [0.9, 0.7], [-0.9, 0.7], [-0.9, -0.7], [0.9, -0.7]] \ No newline at end of file diff --git a/flatland_controls_plugins/test/update_timer_test/world.yaml b/flatland_controls_plugins/test/update_timer_test/world.yaml new file mode 100644 index 00000000..ae449cba --- /dev/null +++ b/flatland_controls_plugins/test/update_timer_test/world.yaml @@ -0,0 +1,9 @@ +properties: {} +layers: + - name: "layer_1" + map: "map_1.yaml" + color: [0, 1, 0, 1] +models: + - name: robot1 + pose: [0, 0, 0] + model: robot.model.yaml diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 66c81987..3d56d10b 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -25,7 +25,6 @@ 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) @@ -73,8 +72,6 @@ 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 @@ -107,7 +104,6 @@ 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} @@ -120,7 +116,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 control_msgs sensor_msgs nav_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 sensor_msgs nav_msgs geometry_msgs tf2 tf2_ros tf2_msgs tf2_geometry_msgs yaml_cpp_vendor) ############# ## Install ## @@ -165,9 +161,6 @@ 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/package.xml b/flatland_plugins/package.xml index bbd94d29..de7d88fa 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -18,7 +18,6 @@ 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 45e6048d..a7d0868e 100644 --- a/flatland_plugins/plugin_description.xml +++ b/flatland_plugins/plugin_description.xml @@ -11,12 +11,6 @@ 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_ros2_control_examples/CMakeLists.txt b/flatland_ros2_control_examples/CMakeLists.txt index 52b814e2..89fda229 100644 --- a/flatland_ros2_control_examples/CMakeLists.txt +++ b/flatland_ros2_control_examples/CMakeLists.txt @@ -15,6 +15,7 @@ install(PROGRAMS scripts/twist_to_joint_commands.py DESTINATION lib/${PROJECT_NA if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(interactive_markers REQUIRED) + find_package(flatland_controls_plugins REQUIRED) find_package(flatland_server REQUIRED) find_package(flatland_plugins REQUIRED) find_package(rclcpp REQUIRED) @@ -22,7 +23,7 @@ if(BUILD_TESTING) 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) + flatland_controls_plugins::flatland_controls_plugins_lib rclcpp::rclcpp) find_package(Python3 REQUIRED COMPONENTS Interpreter) add_test(NAME controller_config_test diff --git a/flatland_ros2_control_examples/launch/example.launch.py b/flatland_ros2_control_examples/launch/example.launch.py index e215f633..4186e0a9 100644 --- a/flatland_ros2_control_examples/launch/example.launch.py +++ b/flatland_ros2_control_examples/launch/example.launch.py @@ -35,10 +35,10 @@ def robot_description(model): ElementTree.SubElement(hardware, "param", name=parameter).text = value for plugin in model["plugins"]: - if plugin["type"] not in ("DriveWheel", "SteeringMotor"): + if plugin["type"] not in ("flatland_controls_plugins::DriveWheel", "flatland_controls_plugins::SteeringMotor"): continue name = plugin["name"] - is_steering = plugin["type"] == "SteeringMotor" + is_steering = plugin["type"] == "flatland_controls_plugins::SteeringMotor" ElementTree.SubElement(robot, "link", name=f"{name}_link") joint = ElementTree.SubElement( robot, "joint", name=name, type="revolute" if is_steering else "continuous" @@ -128,7 +128,7 @@ def launch_example(context): nodes.extend(spawners) if name in ("2910_swerve", "articulated_204g"): wheels = {plugin["name"]: plugin for plugin in model["plugins"] - if plugin["type"] == "DriveWheel"} + if plugin["type"] == "flatland_controls_plugins::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") diff --git a/flatland_ros2_control_examples/models/2910_swerve.model.yaml b/flatland_ros2_control_examples/models/2910_swerve.model.yaml index 999f45e9..8879c18a 100644 --- a/flatland_ros2_control_examples/models/2910_swerve.model.yaml +++ b/flatland_ros2_control_examples/models/2910_swerve.model.yaml @@ -53,11 +53,11 @@ joints: - {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 + - {type: flatland_controls_plugins::DriveWheel, name: front_left_wheel, body: front_left_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: front_right_wheel, body: front_right_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: rear_left_wheel, body: rear_left_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: rear_right_wheel, body: rear_right_pod, radius: 0.051, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::SteeringMotor, name: front_left_steer, joint: front_left_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: flatland_controls_plugins::SteeringMotor, name: front_right_steer, joint: front_right_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: flatland_controls_plugins::SteeringMotor, name: rear_left_steer, joint: rear_left_steer_pivot, mode: position, limit: {lower: -2.98, upper: 2.98}} + - {type: flatland_controls_plugins::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 index 245e9cab..e5d3942a 100644 --- a/flatland_ros2_control_examples/models/a300_diff.model.yaml +++ b/flatland_ros2_control_examples/models/a300_diff.model.yaml @@ -12,7 +12,7 @@ bodies: - {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 + - {type: flatland_controls_plugins::DriveWheel, name: front_left_wheel, body: chassis, offset: [0.30, 0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: flatland_controls_plugins::DriveWheel, name: front_right_wheel, body: chassis, offset: [0.30, -0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: flatland_controls_plugins::DriveWheel, name: rear_left_wheel, body: chassis, offset: [-0.30, 0.32, 0.0], radius: 0.15, mode: velocity, friction: 1.1} + - {type: flatland_controls_plugins::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 index a1ab36d0..953b2050 100644 --- a/flatland_ros2_control_examples/models/articulated_204g.model.yaml +++ b/flatland_ros2_control_examples/models/articulated_204g.model.yaml @@ -28,8 +28,8 @@ joints: - {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 + - {type: flatland_controls_plugins::DriveWheel, name: front_left_wheel, body: front_chassis, offset: [0.25, 0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: front_right_wheel, body: front_chassis, offset: [0.25, -0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: rear_left_wheel, body: rear_chassis, offset: [-0.25, 0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::DriveWheel, name: rear_right_wheel, body: rear_chassis, offset: [-0.25, -0.32, 0.0], radius: 0.29, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::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 index 2e3e409c..6602416d 100644 --- a/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml +++ b/flatland_ros2_control_examples/models/front_drive_tricycle.model.yaml @@ -22,7 +22,7 @@ joints: - {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: flatland_controls_plugins::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 + - {type: flatland_controls_plugins::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 index 32c60a70..3dfe310c 100644 --- a/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml +++ b/flatland_ros2_control_examples/models/rear_drive_ackermann.model.yaml @@ -33,9 +33,9 @@ joints: - {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: flatland_controls_plugins::DriveWheel, name: rear_left_wheel, body: chassis, offset: [-0.52, 0.32, 0.0], radius: 0.19, mode: velocity, friction: 1.1} + - {type: flatland_controls_plugins::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 + - {type: flatland_controls_plugins::SteeringMotor, name: front_left_steer, joint: front_left_pivot, mode: position, limit: {lower: -0.65, upper: 0.65}} + - {type: flatland_controls_plugins::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 index fdb24a23..97c5084e 100644 --- a/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml +++ b/flatland_ros2_control_examples/models/turtlebot_caster.model.yaml @@ -27,6 +27,6 @@ joints: - {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: flatland_controls_plugins::DriveWheel, name: left_wheel, body: chassis, offset: [0.02, 0.16, 0.0], radius: 0.07, mode: velocity, friction: 1.2} + - {type: flatland_controls_plugins::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 index 981d5a70..2d6eed1a 100644 --- a/flatland_ros2_control_examples/package.xml +++ b/flatland_ros2_control_examples/package.xml @@ -9,6 +9,7 @@ ament_cmake flatland_server flatland_plugins + flatland_controls_plugins flatland_viz python3-yaml ament_index_python diff --git a/flatland_ros2_control_examples/test/controller_config_test.py b/flatland_ros2_control_examples/test/controller_config_test.py index ff1b2cd7..1d96b385 100644 --- a/flatland_ros2_control_examples/test/controller_config_test.py +++ b/flatland_ros2_control_examples/test/controller_config_test.py @@ -106,7 +106,7 @@ def test_wheel_footprints_match_plugins(self): 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"): + if wheel["type"] not in ("flatland_controls_plugins::DriveWheel", "FreeWheel"): continue with self.subTest(example=example, wheel=wheel["name"]): center_x, center_y, _ = wheel.get("offset", [0, 0, 0]) @@ -136,7 +136,7 @@ def test_joint_interfaces_match_plugins_and_controllers(self): plugins = { plugin["name"]: plugin["mode"] for plugin in model["plugins"] - if plugin["type"] in ("DriveWheel", "SteeringMotor") + if plugin["type"] in ("flatland_controls_plugins::DriveWheel", "flatland_controls_plugins::SteeringMotor") } description = ElementTree.fromstring(EXAMPLE_LAUNCH.robot_description(model)) control = description.find("ros2_control") diff --git a/flatland_ros2_control_examples/test/worlds_test.cpp b/flatland_ros2_control_examples/test/worlds_test.cpp index 28c266c1..cda5e595 100644 --- a/flatland_ros2_control_examples/test/worlds_test.cpp +++ b/flatland_ros2_control_examples/test/worlds_test.cpp @@ -1,5 +1,5 @@ -#include -#include +#include +#include #include #include @@ -72,12 +72,12 @@ TEST(ExampleWorlds, AckermannAndCasterSteerUnderPower) 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)) { + 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)) { + if (auto steer = std::dynamic_pointer_cast(plugin)) { steer->command_ = 0.25; steer->has_command_ = true; }