diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml new file mode 100644 index 00000000..21bf42ab --- /dev/null +++ b/.github/workflows/ci-humble.yaml @@ -0,0 +1,72 @@ +name: ign_ros2_control CI - Humble + +on: + workflow_dispatch: + pull_request: + branches: [ humble ] + push: + branches: [ humble ] + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 5 * * *' + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - version: fortress + ros-repo-packages: "" + - version: fortress + ros-repo-packages: "-testing" + env: + IGNITION_VERSION: ${{ matrix.version }} + ROS_REPO_PACKAGES: ${{ matrix.ros-repo-packages }} + container: + image: ubuntu:22.04 + steps: + - name: Checkout code + if: github.event_name != 'schedule' + uses: actions/checkout@v4 + - name: Checkout code for scheduled workflow + if: github.event_name == 'schedule' + uses: actions/checkout@v4 + with: + ref: humble + - name: Setup colcon workspace + id: configure + run: | + export DEBIAN_FRONTEND=noninteractive + apt update -qq + apt install -qq -y lsb-release wget curl gnupg2 + cd .. + mkdir -p /home/ros2_ws/src + cp -r gz_ros2_control /home/ros2_ws/src/ + sh -c 'echo "deb http://packages.ros.org/ros2$ROS_REPO_PACKAGES/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-testing.list' + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -qq -y \ + dirmngr \ + python3-colcon-ros \ + python3-colcon-common-extensions \ + python3-rosdep \ + build-essential + cd /home/ros2_ws/src/ + rosdep init + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro humble --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon build --packages-up-to ign_ros2_control_demos gz_ros2_control_tests + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select ign_ros2_control ign_ros2_control_demos gz_ros2_control_tests + colcon test-result diff --git a/.github/workflows/ci-iron.yaml b/.github/workflows/ci-iron.yaml new file mode 100644 index 00000000..81837e0e --- /dev/null +++ b/.github/workflows/ci-iron.yaml @@ -0,0 +1,108 @@ +name: gz_ros2_control CI - Iron + +on: + workflow_dispatch: + pull_request: + branches: [ iron ] + push: + branches: [ iron ] + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 4 * * *' + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - docker-image: "ubuntu:22.04" + gz-version: "fortress" + ros-distro: "iron" + ros-repo-packages: "" + - docker-image: "ubuntu:22.04" + gz-version: "garden" + ros-distro: "iron" + ros-repo-packages: "" + - docker-image: "ubuntu:22.04" + gz-version: "fortress" + ros-distro: "iron" + ros-repo-packages: "-testing" + - docker-image: "ubuntu:22.04" + gz-version: "garden" + ros-distro: "iron" + ros-repo-packages: "-testing" + + env: + DOCKER_IMAGE: ${{ matrix.docker-image }} + GZ_VERSION: ${{ matrix.gz-version }} + ROS_DISTRO: ${{ matrix.ros-distro }} + ROS_REPO_PACKAGES: ${{ matrix.ros-repo-packages }} + container: + image: ${{ matrix.docker-image }} + steps: + - name: Checkout code + if: github.event_name != 'schedule' + uses: actions/checkout@v4 + - name: Checkout code for scheduled workflow + if: github.event_name == 'schedule' + uses: actions/checkout@v4 + with: + ref: iron + - name: Setup colcon workspace + id: configure + shell: bash + run: | + export DEBIAN_FRONTEND=noninteractive + apt update -qq + apt install -qq -y lsb-release wget curl gnupg2 git + cd .. + mkdir -p /home/ros2_ws/src + if [ "$ROS_DISTRO" == "rolling" ]; then + git clone https://github.com/gazebosim/ros_gz/ + fi + cp -r gz_ros2_control /home/ros2_ws/src/ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2$ROS_REPO_PACKAGES/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + if [ "$GZ_VERSION" == "garden" ]; then + export GZ_DEPS="libgz-sim7-dev libgz-plugin2-dev gz-sim7-cli" + fi + + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -qq -y \ + dirmngr \ + python3-colcon-ros \ + python3-colcon-common-extensions \ + python3-rosdep \ + build-essential \ + ${GZ_DEPS} + + if [ "$GZ_VERSION" == "garden" ]; then + export ROSDEP_ARGS="--skip-keys ros_gz_sim --skip-keys gz-plugin2 --skip-keys gz-sim7 --skip-keys gz-transport12 --skip-keys gz-math7 --skip-keys gz-msgs9" + fi + cd /home/ros2_ws/src/ + if [ "$ROS_DISTRO" == "rolling" ]; then + git clone https://github.com/gazebosim/ros_gz/ + fi + if [ "$ROS_DISTRO" == "iron" ]; then + git clone https://github.com/gazebosim/ros_gz/ -b iron + fi + rosdep init + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} --ignore-src ${ROSDEP_ARGS} + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests + colcon test-result diff --git a/.github/workflows/ci-jazzy.yaml b/.github/workflows/ci-jazzy.yaml new file mode 100644 index 00000000..c4463466 --- /dev/null +++ b/.github/workflows/ci-jazzy.yaml @@ -0,0 +1,76 @@ +name: gz_ros2_control CI - Jazzy + +on: + workflow_dispatch: + pull_request: + branches: [ jazzy ] + push: + branches: [ jazzy ] + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 4 * * *' + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - docker-image: "ubuntu:24.04" + ros-distro: "jazzy" + ros-repo-packages: "-testing" + + env: + DOCKER_IMAGE: ${{ matrix.docker-image }} + ROS_DISTRO: ${{ matrix.ros-distro }} + ROS_REPO_PACKAGES: ${{ matrix.ros-repo-packages }} + container: + image: ${{ matrix.docker-image }} + steps: + - name: Checkout code + if: github.event_name != 'schedule' + uses: actions/checkout@v4 + - name: Checkout code for scheduled workflow + if: github.event_name == 'schedule' + uses: actions/checkout@v4 + with: + ref: jazzy + - name: Setup colcon workspace + id: configure + shell: bash + run: | + export DEBIAN_FRONTEND=noninteractive + apt update -qq + apt install -qq -y lsb-release wget curl gnupg2 git + cd .. + mkdir -p /home/ros2_ws/src + cp -r gz_ros2_control /home/ros2_ws/src/ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2$ROS_REPO_PACKAGES/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -qq -y \ + dirmngr \ + python3-colcon-ros \ + python3-colcon-common-extensions \ + python3-rosdep \ + build-essential + + cd /home/ros2_ws/src/ + rosdep init + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests + colcon test-result diff --git a/README.md b/README.md index 95fbfb71..f99d7f68 100644 --- a/README.md +++ b/README.md @@ -16,16 +16,18 @@ Iron | Edifice | [iron](https://github.com/ros-controls/gz_ros2_control/tree/iro Iron | Fortress | [iron](https://github.com/ros-controls/gz_ros2_control/tree/iron) | https://packages.ros.org | `ros-iron-gz-ros2-control` Iron | Garden | [iron](https://github.com/ros-controls/gz_ros2_control/tree/iron) | only from source | Iron | Harmonic | [iron](https://github.com/ros-controls/gz_ros2_control/tree/iron) | only from source | -Jazzy | Harmonic | [master](https://github.com/ros-controls/gz_ros2_control/tree/master) | https://packages.ros.org | `ros-jazzy-gz-ros2-control` +Jazzy | Harmonic | [jazzy](https://github.com/ros-controls/gz_ros2_control/tree/jazzy) | https://packages.ros.org | `ros-jazzy-gz-ros2-control` Rolling | Harmonic | [master](https://github.com/ros-controls/gz_ros2_control/tree/master) | https://packages.ros.org | `ros-rolling-gz-ros2-control` ## Build status ROS 2 Distro | Branch | Build status | Documentation :----------: | :----: | :----------: | :-----------: -**Rolling** | [`master`](https://github.com/ros-controls/gz_ros2_control/tree/master) | [![gazebo_ros2_control CI - Rolling](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) +**Rolling** | [`master`](https://github.com/ros-controls/gz_ros2_control/tree/master) | [![gazebo_ros2_control CI - Rolling](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml) | [Documentation](https://control.ros.org/rolling/index.html)
[API Reference](https://control.ros.org/rolling/doc/api/index.html) +**Jazzy** | [`jazzy`](https://github.com/ros-controls/gz_ros2_control/tree/jazzy) | [![gazebo_ros2_control CI - Jazzy](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-jazzy.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-jazzy.yaml) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) **Iron** | [`iron`](https://github.com/ros-controls/gz_ros2_control/tree/iron) | [![gazebo_ros2_control CI - Iron](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-iron.yaml/badge.svg?branch=iron)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-iron.yaml) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) **Humble** | [`humble`](https://github.com/ros-controls/gz_ros2_control/tree/humble) | [![ign_ros2_control CI - Humble](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-humble.yaml/badge.svg?branch=humble)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-humble.yaml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) + ## Documentation See the [documentation file](doc/index.rst) or [control.ros.org](https://control.ros.org/master/doc/gz_ros2_control/doc/index.html) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 3767bd56..14e594ca 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -436,12 +436,14 @@ void GazeboSimROS2ControlPlugin::Configure( // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); + rclcpp::NodeOptions options = controller_manager::get_cm_node_options(); + options.arguments(arguments); this->dataPtr->controller_manager_.reset( new controller_manager::ControllerManager( std::move(resource_manager_), this->dataPtr->executor_, controllerManagerNodeName, - this->dataPtr->node_->get_namespace())); + this->dataPtr->node_->get_namespace(), options)); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); this->dataPtr->update_rate = this->dataPtr->controller_manager_->get_update_rate(); diff --git a/gz_ros2_control_demos/config/ackermann_drive_controller.yaml b/gz_ros2_control_demos/config/ackermann_drive_controller.yaml index e834c438..a3651db8 100644 --- a/gz_ros2_control_demos/config/ackermann_drive_controller.yaml +++ b/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 diff --git a/gz_ros2_control_demos/config/cart_controller_effort.yaml b/gz_ros2_control_demos/config/cart_controller_effort.yaml index 920e4651..9f72d0c1 100644 --- a/gz_ros2_control_demos/config/cart_controller_effort.yaml +++ b/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 diff --git a/gz_ros2_control_demos/config/cart_controller_position.yaml b/gz_ros2_control_demos/config/cart_controller_position.yaml index 835a8360..543a99c6 100644 --- a/gz_ros2_control_demos/config/cart_controller_position.yaml +++ b/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: diff --git a/gz_ros2_control_demos/config/cart_controller_velocity.yaml b/gz_ros2_control_demos/config/cart_controller_velocity.yaml index 7215d760..f215830f 100644 --- a/gz_ros2_control_demos/config/cart_controller_velocity.yaml +++ b/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 diff --git a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml index a5c426d2..6335341f 100644 --- a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml +++ b/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"] diff --git a/gz_ros2_control_demos/config/gripper_controller_effort.yaml b/gz_ros2_control_demos/config/gripper_controller_effort.yaml index ac5855eb..2440bca8 100644 --- a/gz_ros2_control_demos/config/gripper_controller_effort.yaml +++ b/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 diff --git a/gz_ros2_control_demos/config/gripper_controller_position.yaml b/gz_ros2_control_demos/config/gripper_controller_position.yaml index d72296e1..e8bedb36 100644 --- a/gz_ros2_control_demos/config/gripper_controller_position.yaml +++ b/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 diff --git a/gz_ros2_control_demos/config/tricycle_drive_controller.yaml b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml index c8e0c5b7..ff90ad57 100644 --- a/gz_ros2_control_demos/config/tricycle_drive_controller.yaml +++ b/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 diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp index 7e3b4414..35469e25 100644 --- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp +++ b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp @@ -17,7 +17,7 @@ #include -#include +#include using namespace std::chrono_literals; @@ -28,22 +28,26 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("ackermann_drive_test_node"); - auto publisher = node->create_publisher( - "/ackermann_steering_controller/reference_unstamped", 10); + auto publisher = node->create_publisher( + "/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); diff --git a/gz_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp index 365ecb59..6858564c 100644 --- a/gz_ros2_control_demos/examples/example_diff_drive.cpp +++ b/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); diff --git a/gz_ros2_control_demos/examples/example_gripper.cpp b/gz_ros2_control_demos/examples/example_gripper.cpp index 449cf6ab..c534fcb5 100644 --- a/gz_ros2_control_demos/examples/example_gripper.cpp +++ b/gz_ros2_control_demos/examples/example_gripper.cpp @@ -23,13 +23,11 @@ #include "std_msgs/msg/float64_multi_array.hpp" -std::shared_ptr node; - int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - node = std::make_shared("gripper_test_node"); + std::shared_ptr node = std::make_shared("gripper_test_node"); auto publisher = node->create_publisher( "/gripper_controller/commands", 10); diff --git a/gz_ros2_control_demos/examples/example_position.cpp b/gz_ros2_control_demos/examples/example_position.cpp index 1b294db9..b0a6820f 100644 --- a/gz_ros2_control_demos/examples/example_position.cpp +++ b/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::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); diff --git a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp index 1783ce1e..ff49808b 100644 --- a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp +++ b/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); diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index cb2bbf99..fb6f065b 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/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, diff --git a/gz_ros2_control_demos/launch/cart_example_effort.launch.py b/gz_ros2_control_demos/launch/cart_example_effort.launch.py index 8d6b4fe9..211e1c95 100644 --- a/gz_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/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, diff --git a/gz_ros2_control_demos/launch/cart_example_position.launch.py b/gz_ros2_control_demos/launch/cart_example_position.launch.py index 14391bb4..17658998 100644 --- a/gz_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_position.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_position.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_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_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_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/cart_example_velocity.launch.py b/gz_ros2_control_demos/launch/cart_example_velocity.launch.py index 3406e35d..ee4786ff 100644 --- a/gz_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_velocity.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_velocity.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,28 @@ 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_velocity_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', - '--set-state', 'active', 'velocity_controller'], - output='screen' + velocity_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'velocity_controller', + '--param-file', + robot_controllers, + ], + ) + imu_sensor_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'imu_sensor_broadcaster', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +97,14 @@ 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_velocity_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[velocity_controller_spawner, + imu_sensor_broadcaster_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index f91560d3..44b23a78 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_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', + 'diff_drive_controller_velocity.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): 'diff_drive', '-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_diff_drive_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'diff_drive_base_controller'], - output='screen' + diff_drive_base_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'diff_drive_base_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_diff_drive_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[diff_drive_base_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py index dbeae8db..e4f39eb6 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py @@ -16,7 +16,7 @@ # 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 @@ -42,6 +42,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'gripper_controller_effort.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -58,16 +65,19 @@ def generate_launch_description(): 'gripper', '-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_gripper_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'gripper_controller'], - output='screen' + gripper_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'gripper_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -81,13 +91,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_gripper_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[gripper_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml index a1127a48..a83c4ac2 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml @@ -26,6 +26,7 @@ Author: Dr. Denis + @@ -50,8 +51,8 @@ Author: Dr. Denis - - + + diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py index 6e932da5..fe936755 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -16,7 +16,7 @@ # 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 @@ -42,6 +42,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'gripper_controller_position.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -58,16 +65,19 @@ def generate_launch_description(): 'gripper', '-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_gripper_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'gripper_controller'], - output='screen' + gripper_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'gripper_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -81,13 +91,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_gripper_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[gripper_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml index 76dd181d..4cdc2be5 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml @@ -26,6 +26,7 @@ Author: Dr. Denis + @@ -50,8 +51,8 @@ Author: Dr. Denis - - + + diff --git a/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py b/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py index d09ae2f9..e8b25df8 100644 --- a/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/pendulum_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, diff --git a/gz_ros2_control_demos/launch/pendulum_example_position.launch.py b/gz_ros2_control_demos/launch/pendulum_example_position.launch.py index 24045420..2a920394 100644 --- a/gz_ros2_control_demos/launch/pendulum_example_position.launch.py +++ b/gz_ros2_control_demos/launch/pendulum_example_position.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_position.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_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_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_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py b/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py index 9b2d31b6..a1ab9c20 100644 --- a/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/tricycle_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', + 'tricycle_drive_controller.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,23 @@ def generate_launch_description(): 'tricyle', '-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', + '--param-file', + robot_controllers, + ], ) - - load_tricycle_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'tricycle_controller'], - output='screen' + tricycle_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'tricycle_controller', + '--param-file', + robot_controllers, + ], ) # Bridge @@ -87,13 +101,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_tricycle_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[tricycle_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index c9ac1ee8..f63ce717 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -23,28 +23,30 @@ std_msgs ament_index_python - control_msgs - effort_controllers geometry_msgs - hardware_interface - gz_ros2_control - ros_gz_bridge - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - launch launch_ros - ros2launch + launch rclcpp robot_state_publisher + ros_gz_bridge ros_gz_sim - ros2controlcli + ros2launch std_msgs - velocity_controllers + xacro + + + ackermann_steering_controller + control_msgs diff_drive_controller + effort_controllers + gz_ros2_control + hardware_interface + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + ros2controlcli tricycle_controller - ackermann_steering_controller - xacro + velocity_controllers ament_cmake_gtest ament_lint_auto diff --git a/gz_ros2_control_tests/config/cart_controller_position.yaml b/gz_ros2_control_tests/config/cart_controller_position.yaml index 835a8360..543a99c6 100644 --- a/gz_ros2_control_tests/config/cart_controller_position.yaml +++ b/gz_ros2_control_tests/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: diff --git a/gz_ros2_control_tests/tests/position_test.py b/gz_ros2_control_tests/tests/position_test.py index 7080c719..ddb775cc 100755 --- a/gz_ros2_control_tests/tests/position_test.py +++ b/gz_ros2_control_tests/tests/position_test.py @@ -19,7 +19,7 @@ from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -59,6 +59,11 @@ def generate_test_description(): xacro_file = os.path.join(gz_ros2_control_tests_path, 'urdf', 'test_cart_position.xacro.urdf') + robot_controllers = os.path.join( + gz_ros2_control_tests_path, + 'config', + 'cart_controller_position.yaml' + ) print('xacro_file ', xacro_file) doc = xacro.parse(open(xacro_file)) @@ -81,16 +86,19 @@ def generate_test_description(): '-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_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_controller', + '--param-file', + robot_controllers, + ], ) ld = launch.LaunchDescription([ @@ -100,13 +108,13 @@ def generate_test_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_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), KeepAliveProc(), @@ -120,9 +128,10 @@ def generate_test_description(): class TestFixture(unittest.TestCase): def test_arm(self, launch_service, proc_info, proc_output): - proc_output.assertWaitFor('Successfully loaded controller joint_trajectory_controller ' - 'into state active', - timeout=100, stream='stdout') + proc_output.assertWaitFor('Configured and activated', + process='spawner', + cmd_args=['joint_trajectory_controller'], + timeout=100) proc_action = Node( package='gz_ros2_control_tests', @@ -137,7 +146,10 @@ def test_arm(self, launch_service, proc_info, proc_output): launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, allowable_exit_codes=[0]) + def tearDown(self): for proc in psutil.process_iter(): # check whether the process name matches if proc.name() == 'ruby': proc.kill() + if 'gz sim' in proc.name(): + proc.kill()