Problems with ros2 port of rrbot to Gazebo + Moveit2 + ROS 2

[farbod-new]

Hello,

I built Moveit2 from source. Its demos (moveit_cpp demo and MoveGroup Demo) work fine.

Then, I downloaded the following package and built it.
https://github.com/ros-simulation/gazebo_ros_demos/tree/ahcorde/port/ros2

Then, I ran:
ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py

The output of that is in the following file:
Command_Output.txt

Screen shots of RViZ and Gazebo are as follow:

It seems to me that the packages do not do what they are supposed to.

The culprit might be that no controller is loaded? See below:

[run_moveit_cpp-6] [WARN] [1614898855.549109564] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Waiting for joint_trajectory_controller/follow_joint_trajectory to come up
[run_moveit_cpp-6] [ERROR] [1614898860.549334240] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected: joint_trajectory_controller/follow_joint_trajectory
[run_moveit_cpp-6] [INFO] [1614898860.553521909] [moveit.plugins.moveit_simple_controller_manager]: Returned 0 controllers in list
[run_moveit_cpp-6] [INFO] [1614898860.554275052] [moveit_ros.trajectory_execution_manager]: Trajectory execution is not managing controllers

and later:

[run_moveit_cpp-6] [ERROR] [1614898863.582072095] [moveit.ros_planning_interface.moveit_cpp]: Execution failed! No active controllers configured for group 'rrbot_arm'

I am wondering how to fix this.

Thanks,
Farbod

[ahcorde]

Hi @farbod-new,

can you try the last commit in the PR ?

we made some changes in gazebo_ros2_control, controllers are not launched in gazebo anymore, we should launch the controllers externally ( in this case the launch file ).

[farbod-new]

Hi Alejandro,

Thanks for the fix.

I tried it. If I understood it correctly, I needed to replace the file rrbot_world.launch.py with the new one posted in the last commit. The new one has code that attempts to start the joint_state_controller and the joint_trajectory_controller.

However, I think the controller did not start correctly. Here is the full output:
Command_Output_02.txt

The lines that make me think that the controllers did not start are:

[ros2-8] Could not contact service /controller_manager/load_and_start_controller
[ERROR] [ros2-8]: process has died [pid 3229, exit code 1, cmd 'ros2 control load_start_controller joint_state_controller'].
[INFO] [ros2-9]: process started with pid [3318]
[ros2-9] Could not contact service /controller_manager/load_and_start_controller
[ERROR] [ros2-9]: process has died [pid 3318, exit code 1, cmd 'ros2 control load_start_controller joint_trajectory_controller'].

I am not an expert in ros2_control. But from what I saw in the ros2_control_demos here: https://github.com/ros-controls/ros2_control_demos several steps are involved in starting a controller.

1. [start control_manager] In the rrbot_system_position_only.launch of the above package, ros2_control_node (from control_manager package) is started based on the robot description and a control yaml file.

2. [load and configure] Then, in the README.md file it instructs to do this:
   ros2 control load_start_controller joint_state_controller
   ros2 control load_configure_controller forward_command_controller_position

3. [starting controller] Then, in the README.md file it instructs to do this:
   ros2 control switch_controllers --start-controllers forward_command_controller_position

4. [check if everything worked] Finally, in the README.md file it instructs to do this:
   ros2 control list_controllers
   which should result in the following output:
   joint_state_controller[joint_state_controller/JointStateController] active
   forward_command_controller_position[forward_command_controller/ForwardCommandController] active

I understand the we need to replace forward_command_controller_position in the mentioned package with joint_trajectory_controller in our case.

I hope this helps. I am not too confident in editing the launch file(s). I would have tried myself to put all these in there, in hopes that they may work.

Thanks,
Farbod

[shonigmann]

Hi @ahcorde ,

I am also having trouble running the rrbot_demo. I get a number of strange warnings and errors when running the demo.

Running:
ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py

Returns a number of errors and warnings, pulled out below for brevity:

[rviz2-7] [WARN] [1615249418.847080638] [moveit_robot_model.robot_model]: Skipping virtual joint 'virtual_joint' because its child frame 'link1' does not match the URDF frame 'world'
[rviz2-7] [INFO] [1615249418.847085975] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-7] [WARN] [1615249418.847481022] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'end_effector'
[rviz2-7] [ERROR] [1615249418.849011616] [rviz2]: Could not load resource [/home/robo/demo_ws/install/rrbot_description/share/rrbot_description/meshes/hokuyo.dae]: Unable to open file "/home/robo/demo_ws/install/rrbot_description/share/rrbot_description/meshes/hokuyo.dae".
...
[rviz2-7] [WARN] [1615249418.950111410] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'end_effector'
[rviz2-7] [WARN] [1615249418.951640212] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
...
[ros2-8] usage: ros2 [-h] Call `ros2 <command> -h` for more detailed usage. ...
[ros2-8] ros2: error: argument Call `ros2 <command> -h` for more detailed usage.: invalid choice: 'control' (choose from 'action', 'bag', 'component', 'daemon', 'doctor', 'extension_points', 'extensions', 'interface', 'launch', 'lifecycle', 'multicast', 'node', 'param', 'pkg', 'run', 'security', 'service', 'test', 'topic', 'wtf')
[ERROR] [ros2-8]: process has died [pid 146057, exit code 2, cmd 'ros2 control load_start_controller joint_state_controller'].
[INFO] [ros2-9]: process started with pid [146098]
[ros2-9] usage: ros2 [-h] Call `ros2 <command> -h` for more detailed usage. ...
[ros2-9] ros2: error: argument Call `ros2 <command> -h` for more detailed usage.: invalid choice: 'control' (choose from 'action', 'bag', 'component', 'daemon', 'doctor', 'extension_points', 'extensions', 'interface', 'launch', 'lifecycle', 'multicast', 'node', 'param', 'pkg', 'run', 'security', 'service', 'test', 'topic', 'wtf')
[ERROR] [ros2-9]: process has died [pid 146098, exit code 2, cmd 'ros2 control load_start_controller joint_trajectory_controller'].
...
[run_moveit_cpp-6] [WARN] [1615249423.614810583] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-7] [INFO] [1615249423.982409184] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
[run_moveit_cpp-6] [WARN] [1615249428.665141999] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Waiting for joint_trajectory_controller/follow_joint_trajectory to come up
[run_moveit_cpp-6] [ERROR] [1615249438.665600354] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected: joint_trajectory_controller/follow_joint_trajectory
...
[run_moveit_cpp-6] [ERROR] [1615249441.680820867] [moveit.ros_planning_interface.moveit_cpp]: Execution failed! No active controllers configured for group 'rrbot_arm'
[run_moveit_cpp-6] [INFO] [1615249441.680826935] [moveit.ros_planning_interface.planning_component]: Deleting PlanningComponent 'rrbot_arm'

A few details:
My system is running Ubuntu 20.04, ROS2 Foxy. The dependencies were installed from source using the instructions specified in the gazebo_ros2_control README.md. I also installed the latest PR commit (105333d9df14f8ddb7b5f6a5862b19d2746fff36), per your comment above.

The rviz2 error is quite strange to me as the file exists at the specified location. ~~But what I think is the critical issue is the use of cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'] in rrbot_world.launch.py, which evaluates to cmd 'ros2 control load_start_controller joint_trajectory_controller'

Am I missing a dependency that would give ros2 a control command, am I using obsolete packages, or is this a mistake in the launch file? ~~

Thanks in advance for any help you can offer!

Full console output for reference:
rrbot_moveit_demo_output.txt

[quick update]:
I also tried with the 2nd to last commit (65fc070), which has the previous iteration of launch file; this commit at least avoids the ros2 error, but still fails with:

[run_moveit_cpp-6] [ERROR] [1615252226.204822535] [moveit.ros_planning_interface.moveit_cpp]: Execution failed! No active controllers configured for group 'rrbot_arm'
[run_moveit_cpp-6] [INFO] [1615252226.204833753] [moveit.ros_planning_interface.planning_component]: Deleting PlanningComponent 'rrbot_arm'

[Edit 2]:
installing ros2_control and ros2_controllers debians (in addition to the same packages being installed by source through the MoveIt demo instructions) resolved the cmd 'ros2 control load_start_controller joint_trajectory_controller'] issue.

I now get the same errors and warnings as mentioned in @farbod-new's initial issue comment.

[Edit 3]:
The rviz error can be fixed by changing the model path in rrbot.xacro to an absolute path, from:

<mesh filename="$(find rrbot_description)/meshes/hokuyo.dae"/>

to

<mesh filename="file:///$(find rrbot_description)/meshes/hokuyo.dae"/>

[farbod-new]

[Edit 2]:
installing ros2_control and ros2_controllers debians (in addition to the same packages being installed by source through the MoveIt demo instructions) resolved the cmd 'ros2 control load_start_controller joint_trajectory_controller'] issue.

I now get the same errors and warnings as mentioned in @farbod-new's initial issue comment.

Hi shonigmann ,

I wanted to reply to this with fix for ros2 control commands not being found, but you beat me to it!

[Edit 3]:
The rviz error can be fixed by changing the model path in rrbot.xacro to an absolute path, from:

<mesh filename="$(find rrbot_description)/meshes/hokuyo.dae"/>

to

<mesh filename="file:///$(find rrbot_description)/meshes/hokuyo.dae"/>

I will try this fix!

Thanks so much,
Farbod

[farbod-new]

Hi all,

I found out what my problem was!!

Moveit2 package that I used for building includes ros2_control packages. Those ros2_control packages are not compatible with gazebo_ros2_control. When I sourced the Moveit2 workspace, where the rrbot_moveit_demo_nodes is, of course the ros2_control package that came within Moveit2 package was being used, leading to gazebo_ros2_control not loading!!

I have installed all the foxy ros2_control packages from ros2 foxy repository. So, I could delete ros2_control package that came with the Moveit2 source. This is what I did:

1. The working directory where I built Moveit2 is named: gazebo_moveit2_ros2
2. I went in this directory: gazebo_moveit2_ros2/install
3. Then, I got ride of ros2_control that came with Moveit2 by deleting the following directories:

controller_interface
controller_manager
controller_manager_msgs
hardware_interface
joint_limits_interface
ros2_control
ros2_control_test_assets
ros2controlcli
transmission_interface

4. Then I went to gazebo_moveit2_ros2 directory and sourced it.
5. Then, the following command worked:

ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py

You have to see the following in the command output, which indicates gazebo_ros2_control is loaded correctly (something that did not happen previously):

[gzserver-1] [INFO] [1616183243.859860995] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1616183243.866641729] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-1] [INFO] [1616183243.866722675] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [INFO] [1616183243.872574453] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [INFO] [1616183243.874025542] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] [INFO] [1616183243.881959214] [gazebo_ros2_control]: Loading joint: joint1
[gzserver-1] [INFO] [1616183243.882012572] [gazebo_ros2_control]: 	Command:
[gzserver-1] [INFO] [1616183243.882025074] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1616183243.882035969] [gazebo_ros2_control]: 	State:
[gzserver-1] [INFO] [1616183243.882046934] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1616183243.882059925] [gazebo_ros2_control]: 		 velocity
[gzserver-1] [INFO] [1616183243.882071728] [gazebo_ros2_control]: 		 effort
[gzserver-1] [INFO] [1616183243.882084648] [gazebo_ros2_control]: Loading joint: joint2
[gzserver-1] [INFO] [1616183243.882096591] [gazebo_ros2_control]: 	Command:
[gzserver-1] [INFO] [1616183243.882107556] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1616183243.882118032] [gazebo_ros2_control]: 	State:
[gzserver-1] [INFO] [1616183243.882128578] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1616183243.882138426] [gazebo_ros2_control]: 		 velocity
[gzserver-1] [INFO] [1616183243.882149042] [gazebo_ros2_control]: 		 effort
[gzserver-1] [INFO] [1616183243.882369391] [gazebo_ros2_control]: Loading controller_manager
[gzserver-1] [INFO] [1616183243.904599679] [controller_manager]: Loading controller 'joint_trajectory_controller'
[gzserver-1] [INFO] [1616183243.925174594] [joint_trajectory_controller]: Controller state will be published at 50Hz.
[gzserver-1] [INFO] [1616183243.927576360] [joint_trajectory_controller]: Action status changes will be monitored at 20Hz.
[gzserver-1] [INFO] [1616183243.932380382] [controller_manager]: Loading controller 'joint_state_controller'
[gzserver-1] [INFO] [1616183244.045907960] [gazebo_ros2_control]: Loaded gazebo_ros2_control.

So, in general, if you are building Moveit2 from scratch (which you should be), discard ros2_control that comes with it, and install ros2_control from foxy repository, before building Moveit2. Then, go to:

https://github.com/ros-simulation/gazebo_ros_demos/tree/ahcorde/port/ros2

and DOWNLOAD THE ZIP FILE (the git link is not correct!!). Unzip it in the working directory of your Moveit2 package. Then, go to the working directory of your Moveit2 package and build each package separately as follows:

colcon build --packages-select rrbot_description
colcon build --packages-select rrbot_gazebo
colcon build --packages-select rrbot_moveit_config
colcon build --packages-select rrbot_moveit_demo_nodes

Thanks,
Farbod

PS1: I am using the gazebo_ros2_control from the foxy repository!
PS2: Deleting the ros2_control package that comes with Moveit2 breaks the "panda arm" demos that also come with Moveit2!!

[farbod-new]

Hi all,

Also, the following lines in rrbot_world.launch.py are not needed. I commented them without any problems:

#    load_joint_state_controller = ExecuteProcess(
#        cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'],
#        output='screen'
#    )

#    load_joint_trajectory_controller = ExecuteProcess(
#        cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'],
#        output='screen'
#    )

and some lines below those, I commented:

 #     RegisterEventHandler(
 #         event_handler=OnProcessExit(
 #             target_action=spawn_entity,
 #             on_exit=[load_joint_state_controller],
 #         )
 #     ),
 #     RegisterEventHandler(
 #         event_handler=OnProcessExit(
 #             target_action=load_joint_state_controller,
 #             on_exit=[load_joint_trajectory_controller],
 #         )
 #     ),

Thanks,
Farbod

[farbod-new]

Hi Shonigmann,

Did you have a chance to try my proposed solution? Have you found another way?

Thanks,
Farbod