Use spawner with --params-file argument instead of cli verbs (#399) (…

…#409)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
(cherry picked from commit 30e6705)

Co-authored-by: Christoph Fröhlich <[email protected]>

gz_ros2_control_demos/config/ackermann_drive_controller.yaml

@@ -5,12 +5,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-
-    ackermann_steering_controller:
-      type: 'ackermann_steering_controller/AckermannSteeringController'
-
 ackermann_steering_controller:
   ros__parameters:
+    type: 'ackermann_steering_controller/AckermannSteeringController'
     wheelbase: 1.7
     front_wheel_track: 1.0
     rear_wheel_track: 1.0

gz_ros2_control_demos/config/cart_controller_effort.yaml

@@ -2,13 +2,11 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    effort_controller:
-      type: effort_controllers/JointGroupEffortController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 effort_controller:
   ros__parameters:
+    type: effort_controllers/JointGroupEffortController
     joints:
       - slider_to_cart

gz_ros2_control_demos/config/cart_controller_position.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 joint_trajectory_controller:
   ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
     joints:
       - slider_to_cart
     command_interfaces:

gz_ros2_control_demos/config/cart_controller_velocity.yaml

@@ -2,21 +2,17 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    velocity_controller:
-      type: velocity_controllers/JointGroupVelocityController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    imu_sensor_broadcaster:
-      type: imu_sensor_broadcaster/IMUSensorBroadcaster
-
 velocity_controller:
   ros__parameters:
+    type: velocity_controllers/JointGroupVelocityController
     joints:
       - slider_to_cart
 
 imu_sensor_broadcaster:
   ros__parameters:
+    type: imu_sensor_broadcaster/IMUSensorBroadcaster
     sensor_name: cart_imu_sensor
     frame_id: imu

gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml

@@ -5,11 +5,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    diff_drive_base_controller:
-      type: diff_drive_controller/DiffDriveController
-
 diff_drive_base_controller:
   ros__parameters:
+    type: diff_drive_controller/DiffDriveController
     left_wheel_names: ["left_wheel_joint"]
     right_wheel_names: ["right_wheel_joint"]
 

gz_ros2_control_demos/config/gripper_controller_effort.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    gripper_controller:
-      type: forward_command_controller/ForwardCommandController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 gripper_controller:
   ros__parameters:
+    type: forward_command_controller/ForwardCommandController
     joints:
       - right_finger_joint
     interface_name: effort

gz_ros2_control_demos/config/gripper_controller_position.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    gripper_controller:
-      type: forward_command_controller/ForwardCommandController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 gripper_controller:
   ros__parameters:
+    type: forward_command_controller/ForwardCommandController
     joints:
       - right_finger_joint
     interface_name: position

gz_ros2_control_demos/config/tricycle_drive_controller.yaml

@@ -2,18 +2,15 @@ controller_manager:
   ros__parameters:
     update_rate: 50 # Hz
 
-    tricycle_controller:
-      type: tricycle_controller/TricycleController
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints: ["right_wheel_joint", "left_wheel_joint"]
 
 tricycle_controller:
   ros__parameters:
+    type: tricycle_controller/TricycleController
+
     # Model
     traction_joint_name: traction_joint # Name of traction joint in URDF
     steering_joint_name: steering_joint # Name of steering joint in URDF

gz_ros2_control_demos/examples/example_ackermann_drive.cpp

@@ -17,7 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -28,22 +28,26 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("ackermann_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
-    "/ackermann_steering_controller/reference_unstamped", 10);
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+    "/ackermann_steering_controller/reference", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::Twist tw;
 
-  command.linear.x = 0.5;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  tw.linear.x = 0.5;
+  tw.linear.y = 0.0;
+  tw.linear.z = 0.0;
 
-  command.angular.x = 0.0;
-  command.angular.y = 0.0;
-  command.angular.z = 0.3;
+  tw.angular.x = 0.0;
+  tw.angular.y = 0.0;
+  tw.angular.z = 0.3;
+
+  geometry_msgs::msg::TwistStamped command;
+  command.twist = tw;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);

gz_ros2_control_demos/examples/example_diff_drive.cpp

@@ -34,8 +34,6 @@ int main(int argc, char * argv[])
 
   geometry_msgs::msg::TwistStamped command;
 
-  command.header.stamp = node->now();
-
   command.twist.linear.x = 0.1;
   command.twist.linear.y = 0.0;
   command.twist.linear.z = 0.0;
@@ -45,6 +43,7 @@ int main(int argc, char * argv[])
   command.twist.angular.z = 0.1;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);

gz_ros2_control_demos/examples/example_gripper.cpp

@@ -23,13 +23,11 @@
 
 #include "std_msgs/msg/float64_multi_array.hpp"
 
-std::shared_ptr<rclcpp::Node> node;
-
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  node = std::make_shared<rclcpp::Node>("gripper_test_node");
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("gripper_test_node");
 
   auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
     "/gripper_controller/commands", 10);

gz_ros2_control_demos/examples/example_position.cpp

@@ -154,7 +154,7 @@ int main(int argc, char * argv[])
     node.reset();
     return 1;
   }
-  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
+  RCLCPP_INFO(node->get_logger(), "send goal call ok :)");
 
   rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
     goal_handle = goal_handle_future.get();
@@ -164,7 +164,7 @@ int main(int argc, char * argv[])
     node.reset();
     return 1;
   }
-  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
+  RCLCPP_INFO(node->get_logger(), "Goal was accepted by server");
 
   // Wait for the server to be done with the goal
   auto result_future = action_client->async_get_result(goal_handle);

gz_ros2_control_demos/examples/example_tricycle_drive.cpp

@@ -34,8 +34,6 @@ int main(int argc, char * argv[])
 
   geometry_msgs::msg::TwistStamped command;
 
-  command.header.stamp = node->now();
-
   command.twist.linear.x = 0.2;
   command.twist.linear.y = 0.0;
   command.twist.linear.z = 0.0;
@@ -45,6 +43,7 @@ int main(int argc, char * argv[])
   command.twist.angular.z = 0.1;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);

gz_ros2_control_demos/launch/ackermann_drive_example.launch.py

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -39,6 +39,13 @@ def generate_launch_description():
         ]
     )
     robot_description = {'robot_description': robot_description_content}
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare('gz_ros2_control_demos'),
+            'config',
+            'ackermann_drive_controller.yaml',
+        ]
+    )
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
@@ -55,16 +62,18 @@ def generate_launch_description():
                    'ackermann', '-allow_renaming', 'true'],
     )
 
-    load_joint_state_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_state_broadcaster'],
-        output='screen'
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster'],
     )
-
-    load_ackermann_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'ackermann_steering_controller'],
-        output='screen'
+    ackermann_steering_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['ackermann_steering_controller',
+                   '--param-file',
+                   robot_controllers,
+                   ],
     )
 
     # Bridge
@@ -87,13 +96,13 @@ def generate_launch_description():
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,
-                on_exit=[load_joint_state_broadcaster],
+                on_exit=[joint_state_broadcaster_spawner],
             )
         ),
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=load_joint_state_broadcaster,
-                on_exit=[load_ackermann_controller],
+                target_action=joint_state_broadcaster_spawner,
+                on_exit=[ackermann_steering_controller_spawner],
             )
         ),
         node_robot_state_publisher,

gz_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -39,6 +39,13 @@ def generate_launch_description():
         ]
     )
     robot_description = {'robot_description': robot_description_content}
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare('gz_ros2_control_demos'),
+            'config',
+            'cart_controller_effort.yaml',
+        ]
+    )
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
@@ -55,16 +62,19 @@ def generate_launch_description():
                    '-name', 'cart', '-allow_renaming', 'true'],
     )
 
-    load_joint_state_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_state_broadcaster'],
-        output='screen'
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster'],
     )
-
-    load_joint_effort_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller',
-             '--set-state', 'active', 'effort_controller'],
-        output='screen'
+    effort_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=[
+            'effort_controller',
+            '--param-file',
+            robot_controllers,
+            ],
     )
 
     return LaunchDescription([
@@ -78,13 +88,13 @@ def generate_launch_description():
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,
-                on_exit=[load_joint_state_broadcaster],
+                on_exit=[joint_state_broadcaster_spawner],
             )
         ),
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=load_joint_state_broadcaster,
-                on_exit=[load_joint_effort_controller],
+                target_action=joint_state_broadcaster_spawner,
+                on_exit=[effort_controller_spawner],
             )
         ),
         node_robot_state_publisher,