Running moveit servo demo - Component Demo - Teleoperation Demo

[yannischa]

Description

I'm completly new to ros moveit so I hope this is just an issue with my understanding. I installed moveit servo following the instructions from
https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/doc/running_the_demos.md and than tried the following demos.

If I start the Component Demo, rViz is opening with the robot in the default position but if I start the command publisher nothing is happing. The same problem is coming up using the Teleoperation Demo

The Cpp Interface Demo on the otherside is following the expected behaviour. The error from Possible error (below) is also coming up but seems to have no influence

Your environment

• ROS Distro: ROS Foxy
• OS Version: Ubuntu 20.04
• Source build
• branch: main, commit: 0bd801f3dad80486455e8e9f4073720a7e5b32f8, tag: 2.1.0

Steps to reproduce

Starting the demos right after installation

Expected behaviour

The Component Demo should listen to the command publisher

Actual behaviour

Nothing

Backtrace or Console output

Output servo_server_demo: https://gist.github.com/yannischa/da26a53c97e4f7581b31e4605a6b43f4

Output fake_command_publisher:

yannick@yannick-System-Product-Name:~$ source ~/ws_ros2/install/setup.bash
yannick@yannick-System-Product-Name:~$ ros2 run moveit_servo fake_command_publisher

Possible error:

[rviz2-1] [INFO] [1607863798.535862084] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?

Thank you really much for the help!

[kodie-artner]

@tylerjw are you able to run the demo launch files in moveit_servo? I am having these same issues even though all the tests are passing.

[AndyZe]

Basically what happened here is --

There used to be a ROS message-based executable called servo_server. See this CMakeLists snapshot from Aug 2020. For reasons I don't understand, the executable got turned into a library.

I'm trying to figure out how to go about reverting part of those changes or all of them.

[AndyZe]

was this part of trying to make the tests less flaky, @tylerjw and @Abishalini ?

[tylerjw]

Turning it into a library is making it a composable node. I don't see why we couldn't find some way to do both if it would be useful to users to be able to run it as a not-composable node. However, if the composable node interface works, we should use that by default for ros2.

[tylerjw]

Component (aka composable) nodes are shared libraries that are loaded using pluginlib. That is the reason for that change. The idea here (if I remember correctly) was to convert the interface into only loading as a composable node so there would be fewer options (and therefore less confusion) for users, and they would get the benefits of a component node. I think that was the reasoning behind removing the executable.

[AndyZe]

Hmmm. Well I would be cool with that if it worked :/ Looks like it's not super-easy to revert the change. It's more than a few lines in CMakeLists

[henningkayser]

Ok, the demo is working, the issue is just that the ServoServer is not started initially. All you need to do is trigger the start service using ros2 service call /servo_server/start_servo std_srvs/srv/Trigger {}. I'm not sure why this wasn't part of the demo, though and I don't think it has been at any point. I've definitely touched some of this when performing the last sync, but everything was working fine and the tests covering the start/stop service interfaces all passed and still do. Definitely nothing wrong with servo internally, both the server and cpp demos are functional. So, either the demo instructions are incomplete (looks like that to me) or the server is not started initially by default anymore (don't know where and when this should have happened). @AdamPettinger any thoughts?

[henningkayser]

Got it, when adapting the namespace/service logic to ROS 2 in d9d1976a3edda09abffdfd3136b3cd78add5ec89, I simply forgot to update the service topic in the fake teleop command publisher https://github.com/ros-planning/moveit2/blob/518fede1ff1515eee75cef61bba16106be35c8b3/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp#L169 should use /servo_server/start_servo

[henningkayser]

Possible error:

[rviz2-1] [INFO] [1607863798.535862084] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?

@yannischa Btw, this the PSM in Rviz complaining that there is no planning scene service available which is totally fine for the demo and not an issue. Usually, you would use MoveGroup or MoveItCpp for maintaining the main planning scene so that RViz can call this service for syncing initially. Servo is not intended to maintain the system's planning scene so it doesn't provide the service, but it would instead subscribe/use MoveGroup's/MoveItCpp's planning scene. The scene in RViz gets updated regardless since it's also monitoring joint states and TF.