Skip to content

Commit

Permalink
Perception only: Cherry-pick from moveit/moveit_tutorials#633
Browse files Browse the repository at this point in the history
  • Loading branch information
130s committed Jun 22, 2023
1 parent b22437d commit f796302
Show file tree
Hide file tree
Showing 4 changed files with 101 additions and 103 deletions.
6 changes: 4 additions & 2 deletions doc/examples/perception_pipeline/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,12 @@ endforeach()

include_directories(include ${PCL_INCLUDE_DIRS})

add_executable(cylinder_segment src/cylinder_segment.cpp)
ament_target_dependencies(cylinder_segment
set(APP_CYLINDER detect_and_add_cylinder_collision_object_demo)
add_executable(${APP_CYLINDER} src/detect_and_add_cylinder_collision_object_demo.cpp)
ament_target_dependencies(${APP_CYLINDER}
${SUBTUTORIAL_DEPENDS}
${THIS_PACKAGE_INCLUDE_DEPENDS})
install(TARGETS ${APP_CYLINDER} RUNTIME DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY bags launch
DESTINATION share/${PROJECT_NAME}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,19 @@
<arg name="launchfile_panda"
default="$(find-pkg-share moveit_resources_panda_moveit_config)/launch/demo.launch.py" />

<!--
In this tutorial we wait much longer for the robot transforms than we usually would,
because the user's machine might be quite slow.
-->
<param name="robot_description_planning/shape_transform_cache_lookup_wait_time" value="0.5" />

<!-- Play the rosbag that contains the pointcloud data -->
<executable cmd="ros2 bag play -l $(var bagfile)" output="screen" />

<include file="$(var launchfile_panda)" />

<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf2_ros" exec="static_transform_publisher" name="to_panda" args="0 0 0 0 0 0 world panda_link0" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="to_camera" args="0.115 0.427 0.570 0 0.2 1.92 camera_rgb_optical_frame world" />
<node pkg="tf2_ros" type="static_transform_publisher" name="to_camera" args="0.00 -.40 0.60 0.19 0.07 -1.91 world camera_rgb_optical_frame" />

</launch>
18 changes: 6 additions & 12 deletions doc/examples/perception_pipeline/perception_pipeline_tutorial.rst
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,8 @@ This tutorial uses ``moveit2_tutorials`` that depends on ``moveit_task_construct

Running the Interface
+++++++++++++++++++++
Roslaunch the launch file to run the code directly from moveit_tutorials: ::

Launch the prepared launch file in moveit_tutorials to see the planning scene integrating sample point cloud data into an octomap: ::

ros2 launch moveit_tutorials obstacle_avoidance_demo.launch

Expand All @@ -157,7 +158,7 @@ If not, you may have run into a `known OpenGL rendering issue <http://wiki.ros.o

export LIBGL_ALWAYS_SOFTWARE=1

You can test obstacle avoidance for yourself by setting the goal state manually and then planning and executing. To learn how to do that look at :doc:`MoveIt Quickstart in RViz </doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial>`
You can test obstacle avoidance with the generated octomap for yourself by setting the goal state manually and then planning and executing. To learn how to do that look at `MoveIt Quickstart in RViz </doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial>`_

Detecting and Adding Object as Collision Object
-----------------------------------------------
Expand All @@ -172,17 +173,10 @@ After running the code, you should be able to see something like this in RViz:

Running the Code
++++++++++++++++
Roslaunch the launch file to run the code directly from moveit_tutorials: ::

roslaunch moveit_tutorials detect_and_add_cylinder_collision_object_demo.launch

KNOWN ISSUE - You may see the following error when running the demo ::

ros.moveit_ros_planning.planning_scene_monitor: Transform error: Lookup would require extrapolation into the future. Requested time 1527473962.793050157 but the latest data is at time 1527473962.776993978, when looking up transform from frame [panda_link2] to frame [camera_rgb_optical_frame]
ros.moveit_ros_perception: Transform cache was not updated. Self-filtering may fail.
Keep the launch file from above running and run the code directly from moveit_tutorials: ::

We are working on fixing it, it should not break the working of the demo.
You can follow its status in the `issue tracker <https://github.com/ros-planning/moveit_tutorials/issues/192>`_
ros2 run moveit2_tutorials detect_and_add_cylinder_collision_object_demo

Relevant Code
+++++++++++++
Expand All @@ -202,4 +196,4 @@ The details regarding the implementation of each of the perception pipeline func

</code>

.. tutorial-formatter:: ./src/cylinder_segment.cpp
.. tutorial-formatter:: ./src/detect_and_add_cylinder_collision_object_demo.cpp

0 comments on commit f796302

Please sign in to comment.