Improve perception tutorial

[v4hn]

I tried to address #631
and improved various aspects of this tutorial. It's still quite ugly in general
and actually segfaults for me locally, but that's hopefully just because of an issue in my custom PCL version?

Please review @simonschmeisser

EDIT: For the sake of completeness here is my gdb backtrace:

#0  0x00007ffff3946c20 in free () from /lib/libc.so.6
#1  0x00007ffff2d1816c in ?? () from /usr/lib/libpcl_sample_consensus.so.1.10
#2  0x00007ffff351eea1 in GOMP_parallel (fn=0x7ffff2d17fb0, data=0x7fffffffc8b0, num_threads=1, flags=0) at ../../../libgomp/parallel.c:171
#3  0x00007ffff2cfc423 in pcl::RandomSampleConsensus<pcl::PointXYZ>::computeModel(int) () from /usr/lib/libpcl_sample_consensus.so.1.10
#4  0x00007ffff46b219f in pcl::SACSegmentation<pcl::PointXYZ>::segment(pcl::PointIndices&, pcl::ModelCoefficients&) () from /usr/lib/libpcl_segmentation.so.1.10
#5  0x00000000004630a7 in CylinderSegment::removePlaneSurface (this=0x7fffffffd778, cloud=..., inliers_plane=...) at moveit_tutorials/doc/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp:223
#6  0x00000000004580a9 in CylinderSegment::cloudCB (this=0x7fffffffd778, input=...) at moveit_tutorials/doc/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp:290

[simonschmeisser]

What happened to the launchfile? 😆

Also I still get some warnings with the obstacle avoidance launchfile:

roslaunch moveit_tutorials obstacle_avoidance_demo.launch

[ INFO] [1619641423.033922611]: Loading robot model 'panda'...
[ WARN] [1619641423.034247609]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1619641423.034282222]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ERROR] [1619641423.432320913]: Transform error: Lookup would require extrapolation 0.093739482s into the past.  Requested time 1619641422.912770033 but the earliest data is at time 1619641423.006509542, when looking up transform from frame [panda_link1] to frame [camera_rgb_optical_frame]                                                                                                                                                                                    
[ERROR] [1619641423.432396877]: Transform cache was not updated. Self-filtering may fail.
[ INFO] [1619641423.685372414]: Loading robot model 'panda'...
[ WARN] [1619641423.685528053]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1619641423.685561210]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ INFO] [1619641423.812810514]: Starting planning scene monitor
[ INFO] [1619641423.815368518]: Listening to '/move_group/monitored_planning_scene'

[v4hn]

I pointed out the "no collision geometry" errors to the panda team before when they changed their collision geometry and they ignored me.
Personally, I reverted the description locally to release 0.6.0-1 which still had sane models.

[ERROR] [1619641423.432320913]: Transform error: Lookup would require extrapolation 0.093739482s into the past. > Requested time 1619641422.912770033 but the earliest data is at time 1619641423.006509542, when looking up transform from frame [panda_link1] to frame [camera_rgb_optical_frame]
[ERROR] [1619641423.432396877]: Transform cache was not updated. Self-filtering may fail.

This one's a bit harder to fix because the transforms world->camera and world->robot are published independently and the logic will only wait for the former one before attempting the transforms for all individual robot transforms.
To fix this, the point data updaters would need to wait for valid robot transforms before starting up.
Everyone is invited to implement that feature in MoveIt. ;)

[simonschmeisser]

You wrote something somewhere about not being able to reference a launchfile from another and this being the reason for the removal of said launchfile? Could you elaborate a bit?

[v4hn]

You wrote something somewhere about not being able to reference a launchfile from another and this being the reason for the removal of said launchfile?

moveit_tutorials installs all its launch files to ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch (and in my opinion that makes sense for the installed package. But launch files in the source package lie with their respective tutorials in ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/<tutorial>/. So $(find <pkg>)/... includes in launch files can't work in both modes. In the end that reason was resolved by John in an independent discussion, because launch was extended in ROS lunar by a $(dirname) keyword that solves this exact situation and allows relative includes. (Horray)

I left this patch as-is though, because it actually makes sense to look at the full pointcloud/octomap and then start the node that recognizes and filters the cylinder.

[felixvd]

I was just asked to run this blindly and it doesn't segfault for me on Ubuntu 18.04 (in the master-source container).

I didn't read this in detail, but my superficial impression is that the copy-pastable, executable lines should come first, and the explanation about the configuration and how to do it with a custom robot second.

[jspricke]

works for me on Debian unstable as well.

[davetcoleman]

Would be best if more of a perception expert also reviewed this, like @JStech

[davetcoleman]

Does this PR address this issue, enough to remove this disclaimer? Below the fold it says "We are working on fixing it" which is not true, no one has commented on the linked issue in 3 years

[JStech]

I'm still seeing the errors.

[v4hn]

The source of the error is that the shape updater used for self-filtering has to wait for transforms from the whole robot to the planning_frame.
The waiting time can be configured by a magic ROS parameter and I increased the value to 0.5 seconds.
That works well on my intel i5 laptop and I expected it would be enough time for the robot_state_publisher to publish the robot transforms even on a weak user machine.
@JStech Could you please try different values and see how much time is actually needed on your system to get rid of these errors?
Alternatively (or in addition) we could throttle the publish frequency of the point cloud and reduce the maximum iterations of the RANSAC runs to reduce some computational load.

[JStech]

This was sufficient to stop the errors for me:

diff --git a/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp b/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp
index fa40247..1ad1b96 100644
--- a/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp
+++ b/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp
@@ -71,7 +71,7 @@ int main(int argc, char** argv)
   ros::Rate loop_rate(0.2);
   while (ros::ok())
   {
-    point_cloud_ptr->header.stamp = ros::Time::now();
+    point_cloud_ptr->header.stamp = ros::Time::now() - ros::Duration(0.1);
     point_cloud_publisher.publish(*point_cloud_ptr);
     loop_rate.sleep();
   }

[v4hn]

And increasing shape_transform_cache_lookup_wait_time to 0.6 seconds instead does not work?
I'm not happy with introducing a delay to the "camera driver".
If the driver is that perfect and can provide current data on the topic, MoveIt should be able to handle that, right?

[felixvd]

Does that fix it reliably?

[v4hn]

Sorry for leaving this hanging out to dry for so long, I'm overwhelmed by all the open things to do in MoveIt and my strongly constraint personal availability.

What did fix it was adding a half second sleep before the publish loop.

I'm somewhat unsure what you mean here. Do you actually mean before the loop or before the publish call.
I just made the publisher latched (makes sense to me anyway with a 0.2s publish rate) and increased the queue size.
If you meant the first location, this should solve it as well.

I still wouldn't be happy to add a sleep to the second location because it artificially delays messages and if MoveIt can't handle perfect times we should fix that in MoveIt.
However, by now I'm more interested in finally getting this merged, so if you say the problem only disappears with a sleep before publishing, I'd add it for now with a note.

Please confirm @JStech .

[v4hn]

ping @JStech

[JStech]

Sorry, been busy here too. I mean before the loop:

  ros::Rate loop_rate(0.2);
  ros::Duration(0.5).sleep();
  while (ros::ok())
  ...

[JStech]

I also just pulled, built, and ran the latest version (cab19fd), and I still get the error. (And the delay still fixes it.)

[JStech]

Looks good to me--just a couple minor word choice things. I built and ran the code and it all worked.

Something that occurred to me while reading through it is it would be good to have a style guide for the tutorials, to specify how to format code and file names and package names etc.

[JStech]

I'm still seeing the errors.

[v4hn]

Something that occurred to me while reading through it is it would be good to have a style guide for the tutorials, to specify how to format code and file names and package names etc.

👍 Please feel free to sketch out something and we can talk about it at a maintainer meeting.

[felixvd]

It was posted in #641, for the record.

I haven't tested this but unless it introduces new errors, I would merge it anyway. It's definitely an improvement.

[v4hn]

@JStech wrote:

I also just pulled, built, and ran the latest version (cab19fd), and I still get the error. (And the delay still fixes it.)

Ok. I just pushed such a sleep (above the Rate instantiation and longer though).
Please test this one last time and merge by squashing if it works for you now.

[JStech]

Yup, all good now.

[JStech]

merge by squashing if it works for you now.

I'm not a maintainer. (Just a pseudo-maintainer of MoveIt Calibration.)