Reproduced current status of gazebo_ros2_control

[ahcorde]

I have created two PRs where I have added:

• gazebo_ros2_control - preliminary version Added initial version of gazebo_ros2_control #1
• gazebo_ros2_control_demos - preliminary demo package with a cart in a 30 meter rail. Added initial demos in gazebo_ros2_control_demos #2

The ROS 2 distro used for this tutorial is Foxy

Run the demos:

mkdir -p ~/ros2_control_ws/src 
cd ~/ros2_control_ws/src
git clone https://github.com/ros-simulation/gazebo_ros2_control/ -b ahcorde/test
git clone https://github.com/ros/xacro -b dashing-devel
git clone https://github.com/ros-simulation/gazebo_ros_pkgs -b ros2
git clone https://github.com/ros-controls/realtime_tools -b ros2_devel
git clone https://github.com/ros-controls/ros2_control
git clone https://github.com/ros-controls/ros2_controllers
git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster
touch ros2_control_ddengster/COLCON_IGNORE
cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface  ros2_control
cd ~/ros2_control_ws
colcon build --merge-install

Running the examples

I created two examples: hardware_interface/PositionJointInterface and hardware_interface/VelocityJointInterface. I only made it work the PositionJointInterface example

Launch the Gazebo classic world with the gazebo_ros2_plugin

ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py

Call the action follow_joint_trajectories:

ros2 run gazebo_ros2_control_demos example_position

Check /joint_states or /state

ros2 topic echo /joint_states
ros2 topic echo /state

Dependencies

• xacro
• gazebo_ros_pkgs
• ros2_control
• ros2_controllers
• realtime_tools

@chapulina @Karsten1987 @ddengster

[oktkhr0916]

Thank you for your great project!

I have one question.
I tried the demo of example_position in my environment which is "ros2 eloquent".
To run the demo, I changed some codes like:

https://github.com/ros-simulation/gazebo_ros2_control/blob/5942107c360a7f07c546565f59cc51f2c5c6b380/gazebo_ros2_control_demos/examples/example_position.cpp#L126-L127
-> rclcpp::executor::FutureReturnCode::SUCCESS
https://github.com/ros-simulation/gazebo_ros2_control/blob/83254fa148bd7053ab8d085d508b32352093e98c/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L419-L421
-> auto controller = controller_manager_->load_controller("ros_controllers", controller_type, controller_name);
https://github.com/ros-simulation/gazebo_ros2_control/blob/5942107c360a7f07c546565f59cc51f2c5c6b380/gazebo_ros2_control_demos/config/cartpole_controller.yaml#L1-L3

->
cart_pole_controller:
  ros__parameters:
    type: ros_controllers::JointStateController

As the result, I succeed to launch cart_example_position.launch.py
like :

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_context import LaunchContext
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro

def generate_launch_description():
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
             )

    gazebo_ros2_control_demos_path = os.path.join(
        get_package_share_directory('gazebo_ros2_control_demos'))

    xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_position.xacro.urdf')
    config_file = os.path.join(gazebo_ros2_control_demos_path, 'config', 'cartpole_controller.yaml')

    urdf_path = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_position.urdf')
    doc = xacro.process_file(xacro_file)
    robot_desc = doc.toprettyxml(indent='  ')
    f = open(urdf_path, 'w')
    f.write(robot_desc)
    f.close()

    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        node_executable='robot_state_publisher',
        output='screen',          #screen
        arguments=[urdf_path]#params
    )

    spawn_entity = Node(package='gazebo_ros', node_executable='spawn_entity.py',
                        arguments=['-file', urdf_path, '-entity', 'cartpole'],
                        output='screen')

    return LaunchDescription([
        gazebo,
        node_robot_state_publisher,
        spawn_entity
    ])

And, I executed ros2 run gazebo_ros2_control_demos example_position.
Then, I got ERROR like: "could not get action server" in
https://github.com/ros-simulation/gazebo_ros2_control/blob/5942107c360a7f07c546565f59cc51f2c5c6b380/gazebo_ros2_control_demos/examples/example_position.cpp#L79-L83

To detect Action Server, they need rclcpp_action::create_server however I couldn't find it in this project.
Where is it declared?

Thank you.

[ddengster]

To detect Action Server, they need rclcpp_action::create_server however I couldn't find it in this project.
Where is it declared?

Revert the JointTrajectoryController change and checkout https://github.com/ros-controls/ros2_controllers for the action server commit. It's not in eloquent yet.

[bmagyar]

@ahcorde as @oktkhr0916 's comment highlights, you never mentioned which distro you used, please add this to the opening post.

[ahcorde]

I'm working with Foxy. Updated the opening post.

[oktkhr0916]

Revert the JointTrajectoryController change and checkout https://github.com/ros-controls/ros2_controllers for the action server commit. It's not in eloquent yet.

@bmagyar Thank you! It's work with eloquent.
After I reverted the branch, I changed code like

rclpp::Executor -> rclcpp::executor::Executor
rclcpp::FutureReturnCode::SUCCESS -> rclcpp::executor::FutureReturnCode::SUCCESS

[guru-florida]

@ahcorde I am trying to reproduce what you did but having trouble. First I tried exactly your instructions but got SEGV in gzserver. So I updated the joint limits and transmission modules to sync with master and #ros2_control/134. I also updated your gazebo_ros2_control and gazebo plugin code to work with recent master changes. It almost works but I believe I am getting some initialization happening out of order due to recent ControllerManager changes, take a look at my snipped of verbose output:

Output shows gazebo_ros2_control plugin is initialized. It registers the slider_to_cart joint, but when the cart_pole_controller is initialized it is looking for a "joints" parameter that is not declared yet. We can see the declaration happens after the controller initialization (and the "no joint names specified" error). I believe this is a change in the ControllerManager behavior. CM no longer has the configure() and activate() methods...did CM used to be a Lifecycle node? It seems like previous behavior was controllers were constructed in load_controller() call but then later initialized in configure() but in the new behavior they are initialized in the load_controller() method and therefor they expect the joints parameter before your yaml processing explicitly declares them. ControllerManager::load_controller() does in fact call init() on the created controller instance (controller_manager.cpp:445). I am not sure how to proceed from here. thoughts?

  [gzserver-1] DefaultRobotHWSim::InitSim 
  [gzserver-1] Loading joint 'slider_to_cart' of type 'hardware_interface/PositionJointInterface'
  [gzserver-1] [ERROR] [1602302832.948902796] [ros2_control_plugin]: Loading controller_manager
  [gzserver-1] [INFO] [1602302832.977279934] [gazebo_controller_manager]: Loading controller 'cart_pole_controller'
  [gzserver-1] [WARN] [1602302833.006905324] [cart_pole_controller]: no joint names specified
       ^^^ the controller was created and initialized here, params not loaded yet!
  [gzserver-1] [WARN] [1602302833.007236933] []: Error occurred while doing error handling.

[gzserver-1] [ERROR] [1602302833.008763604] [ros_control_plugin]: nodename: cart_pole_controller
[gzserver-1] [INFO] [1602302833.008958976] [ros2_control_plugin]: reading param cart_pole_controller from cart_pole_controller from prefix cart_pole_controller
[gzserver-1] [INFO] [1602302833.008988258] [ros2_control_plugin]: reading param  from cart_pole_controller from prefix cart_pole_controller
[gzserver-1] [INFO] [1602302833.009007506] [ros2_control_plugin]: reading param type from cart_pole_controller from prefix cart_pole_controller
[gzserver-1] [INFO] [1602302833.009508284] [ros2_control_plugin]: reading param joints from cart_pole_controller from prefix cart_pole_controller
      ^^^ I added this INFO output, we are loading "joints" param from YAML here...but it is after the initialization of the controller!
      [snip]  more param output
[gzserver-1] [ERROR] [1602302833.011254262] [ros_control_plugin]: nodename: joint_state_controller
[gzserver-1] [INFO] [1602302833.011368954] [gazebo_controller_manager]: Loading controller 'joint_state_controller'
[gzserver-1] [ERROR] [1602302833.039356358] [ros_control_plugin]: nodename: cart_pole_controller
[gzserver-1] [ERROR] [1602302833.039386470] [ros_control_plugin]: nodename: joint_state_controller
[gzserver-1] [ERROR] [1602302833.041011645] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-1] Segmentation fault (core dumped)
[ERROR] [gzserver-1]: process has died [pid 3733671, exit code 139, cmd 'gzserver       --verbose                                                                -s libgazebo_ros_init.so   -s libgazebo_ros_factory.so   -s libgazebo_ros_force_system.so       '].

[ahcorde]

Hello @guru-florida,

I had updated the code with the recent changes in ros2_control and ros2_controllers in this PR #34

Let me know if you have more issues.

[ahcorde]

Package is released