Skip to content

Commit 13c4fc1

Browse files
AbishaliniAbishalini
authored
Mesh Filter tutorial with UR5 and Kinect simulated in Gazebo (#558)
1 parent e1c74b7 commit 13c4fc1

15 files changed

+824
-0
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,3 +73,4 @@ add_subdirectory(doc/moveit_cpp)
7373
add_subdirectory(doc/collision_environments)
7474
add_subdirectory(doc/visualizing_collisions)
7575
add_subdirectory(doc/bullet_collision_checker)
76+
add_subdirectory(doc/mesh_filter)

‎doc/mesh_filter/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
2+
3+
# Need to use non-standard install location to find plugin description both in devel and install space
4+
install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/mesh_filter)
5+
install(DIRECTORY rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/mesh_filter)
6+
install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/mesh_filter)

‎doc/mesh_filter/Filtered_Depth.png

202 KB
Loading

‎doc/mesh_filter/MeshFilter.gif

2.64 MB
Loading

‎doc/mesh_filter/Model_Depth.png

183 KB
Loading

‎doc/mesh_filter/launch/mesh_filter.launch

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
4+
<include file="$(dirname)/ur5_gazebo.launch"/>
5+
6+
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
7+
<node pkg="nodelet" type="nodelet" name="mesh_filter" args="load mesh_filter/DepthSelfFiltering standalone_nodelet" output="screen">
8+
<remap to="/camera/depth/image_raw" from="/depth"/>
9+
<remap to="/camera/color/camera_info" from="/camera_info"/>
10+
</node>
11+
12+
</launch>

‎doc/mesh_filter/launch/ur5_gazebo.launch

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
4+
<arg name="paused" default="false" doc="Starts gazebo in paused mode" />
5+
<arg name="gui" default="true" doc="Starts gazebo gui" />
6+
7+
<!-- startup simulated world -->
8+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
9+
<arg name="gui" value="$(arg gui)"/>
10+
</include>
11+
12+
<param name="robot_description" command="$(find xacro)/xacro '$(find moveit_tutorials)/doc/mesh_filter/urdf/ur5_sensor.urdf.xacro' "/>>
13+
14+
<!-- push robot_description to factory and spawn robot in gazebo -->
15+
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
16+
17+
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
18+
19+
<!-- start this controller -->
20+
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
21+
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>
22+
23+
<!-- load other controllers -->
24+
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />
25+
26+
<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
27+
args="-d $(find moveit_tutorials)/doc/mesh_filter/rviz/mesh_filter.rviz" output="screen">
28+
</node>
29+
</launch>

‎doc/mesh_filter/mesh_filter_tutorial.rst

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
Mesh Filter with UR5 and Kinect
2+
===============================
3+
4+
MoveIt's mesh filter functionality removes your robot's geometry from a point cloud! If your robot's arm is in your depth sensor's view, the points associated with the arm are subtracted from the point cloud.
5+
6+
This is accomplished by giving the original point cloud, the robot's transforms (``\tf``) and the robot's URDF as inputs.
7+
The filter then publishes a modified point cloud which does not contain the points that overlaps with the current robot state.
8+
9+
.. image:: MeshFilter.gif
10+
11+
Getting Started
12+
---------------
13+
14+
* Follow the instructions for :moveit_website:`installing MoveIt<install>`
15+
first if you have not already done that.
16+
17+
* Clone the `Universal Robot package <https://github.com/ros-industrial/universal_robot>`_ to your workspace for Melodic. Remember to rebuild your workspace after cloning.
18+
19+
* Install `Gazebo <http://gazebosim.org/tutorials?tut=install_ubuntu&cat=install>`_. You need to install ros-${ROS_DISTRO}-gazebo-plugins too.
20+
21+
22+
Running the Demo
23+
-----------------
24+
25+
Roslaunch the launch file to run the code directly from moveit_tutorials: ::
26+
27+
roslaunch moveit_tutorials mesh_filter.launch
28+
29+
The above command opens a UR5 arm with Kinect sensor on Gazebo and Rviz
30+
31+
Topic ``/filtered/depth`` produces the modified point cloud with points subtracted that overlaps with the robot state.
32+
33+
.. image:: Filtered_Depth.png
34+
35+
Topic ``/model/depth`` gives the points that overlap with the current robot state
36+
37+
.. image:: Model_Depth.png
38+
39+
Check out the mesh filter code `here <https://github.com/ros-planning/moveit/blob/master/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp>`_
40+
41+
42+
How to add sensor to arm in simulation
43+
--------------------------------------
44+
45+
Include sensor plugin in a ``.gazebo`` file. In this tutorial, a kinect sensor plugin is added to ``kinect_camera.gazebo`` ::
46+
47+
<gazebo reference="camera_depth_frame">
48+
<sensor name="kinect_camera" type="depth">
49+
<update_rate>20</update_rate>
50+
<camera>
51+
<horizontal_fov>1.047198</horizontal_fov>
52+
<image>
53+
<width>640</width>
54+
<height>480</height>
55+
<format>B8G8R8</format>
56+
</image>
57+
<clip>
58+
<near>0.05</near>
59+
<far>3</far>
60+
</clip>
61+
</camera>
62+
<plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
63+
<baseline>0.1</baseline>
64+
<alwaysOn>true</alwaysOn>
65+
<updateRate>10</updateRate>
66+
<cameraName>camera_ir</cameraName>
67+
<imageTopicName>/camera/color/image_raw</imageTopicName>
68+
<cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
69+
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
70+
<depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
71+
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
72+
<frameName>camera_depth_optical_frame</frameName>
73+
<distortion_k1>0.0</distortion_k1>
74+
<distortion_k2>0.0</distortion_k2>
75+
<distortion_k3>0.0</distortion_k3>
76+
<distortion_t1>0.0</distortion_t1>
77+
<distortion_t2>0.0</distortion_t2>
78+
<pointCloudCutoff>0.16</pointCloudCutoff>
79+
<pointCloudCutoffMax>10.0</pointCloudCutoffMax>
80+
</plugin>
81+
</sensor>
82+
</gazebo>
83+
84+
85+
Attach sensor to `base urdf <https://github.com/ros-industrial/universal_robot/blob/melodic-devel/ur_description/urdf/ur5.urdf.xacro>`_ of UR5 using links and joints as shown in ``ur5_sensor.urdf.xacro`` ::
86+
87+
<!-- ur5 -->
88+
<xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
89+
90+
<!-- Attach UR5 to table -->
91+
<joint name="table_joint" type="fixed">
92+
<parent link="table"/>
93+
<child link="base_link"/>
94+
</joint>
95+
96+
<!-- Attach Kinect to table -->
97+
<joint type="fixed" name="table_camera_joint">
98+
<origin xyz="0.15 -1 0.1" rpy="0 0 1.57"/>
99+
<child link="camera_rgb_frame"/>
100+
<parent link="table"/>
101+
<axis xyz="0 0 0" rpy="0 0 0"/>
102+
<limit effort="10000" velocity="1000"/>
103+
<dynamics damping="1.0" friction="1.0"/>
104+
</joint>
105+
106+
107+
108+
109+
References
110+
----------
111+
`Understanding ROS Nodelets <https://medium.com/@waleedmansoor/understanding-ros-nodelets-c43a11c8169e>`_

‎doc/mesh_filter/meshes/kinect.dae

Lines changed: 174 additions & 0 deletions
Large diffs are not rendered by default.

‎doc/mesh_filter/meshes/materials/textures/kinect.png

3.26 KB
Loading

0 commit comments

Comments
 (0)