Refactored perception pipeline tutorial from ros1 to ros 2 (#906)

CMakeLists.txt

@@ -44,14 +44,14 @@ add_subdirectory(doc/examples/collision_environments)
 # add_subdirectory(doc/examples/interactivity)
 # add_subdirectory(doc/examples/kinematics)
 # add_subdirectory(doc/examples/move_group_python_interface)
-# add_subdirectory(doc/examples/perception_pipeline)
 # add_subdirectory(doc/examples/pick_place)
 # add_subdirectory(doc/examples/planning)
 # add_subdirectory(doc/examples/state_display)
 # add_subdirectory(doc/examples/subframes)
 # add_subdirectory(doc/examples/tests)
 # add_subdirectory(doc/examples/trajopt_planner)
 # add_subdirectory(doc/examples/visualizing_collisions)
+add_subdirectory(doc/examples/perception_pipeline)
 add_subdirectory(doc/examples/jupyter_notebook_prototyping)
 add_subdirectory(doc/examples/motion_planning_api)
 add_subdirectory(doc/examples/motion_planning_pipeline)

doc/examples/perception_pipeline/CMakeLists.txt

@@ -1,13 +1,2 @@
-add_executable(cylinder_segment src/cylinder_segment.cpp)
-target_link_libraries(cylinder_segment ${catkin_LIBRARIES})
-
-add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp)
-target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-install(
-TARGETS
-    bag_publisher_maintain_time
-    cylinder_segment
-DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY urdf launch meshes config worlds rviz2
+  DESTINATION share/moveit2_tutorials)

doc/examples/perception_pipeline/config/sensors_3d.yaml

@@ -0,0 +1,22 @@
+sensors:
+  - camera_1_pointcloud
+  - camera_2_depth_image
+camera_1_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera_1/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_1/filtered_points
+camera_2_depth_image:
+    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+    image_topic: /camera_2/depth/image_raw
+    queue_size: 5
+    near_clipping_plane_distance: 0.3
+    far_clipping_plane_distance: 5.0
+    shadow_threshold: 0.2
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_2/filtered_points

doc/examples/perception_pipeline/launch/depth_camera_environment.launch.py

@@ -0,0 +1,153 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import Command
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import PathJoinSubstitution
+import math
+
+
+def generate_launch_description():
+    urdf_file = os.path.join(
+        get_package_share_directory("moveit2_tutorials"),
+        "urdf/realsense_d435/camera.urdf.xacro",
+    )
+
+    # The list presents the pose in euler coordinates ordered x, y, z, yaw, pitch, roll respectively.
+    camera_1_pose = ["-1", "-1", "0", f"{math.pi/2}", "0.0", "0.0"]
+    camera_2_pose = ["-1", "1", "0", f"-{math.pi/2}", "0.0", "0.0"]
+
+    # It is necessary for gazebo spawner to use the description of camera_1 in gazebo environment
+    camera_1_robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {
+                "use_sim_time": True,
+                "robot_description": Command(
+                    [f"xacro {urdf_file}", " camera_name:='camera_1'"]
+                ),
+            }
+        ],
+        remappings=[
+            ("robot_description", "/camera_1/robot_description"),
+        ],
+    )
+
+    # It is necessary for spawning camera_1 in gazebo environment
+    camera_1_gazebo_spawner_node = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=[
+            "-entity",
+            "mr_camera",
+            "-topic",
+            "/camera_1/robot_description",
+            "-x",
+            camera_1_pose[0],
+            "-y",
+            camera_1_pose[1],
+            "-z",
+            camera_1_pose[2],
+            "-Y",
+            camera_1_pose[3],
+            "-P",
+            camera_1_pose[4],
+            "-R",
+            camera_1_pose[5],
+        ],
+        output="screen",
+    )
+
+    # It is necessary to make transformation between world frame and camera frames enable later.
+    camera_1_tf_from_world_publisher_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=[*camera_1_pose, "world", "camera_1_base_link"],
+    )
+
+    # It is necessary for gazebo spawner to use the description of camera_2 in gazebo environment
+    camera_2_gazebo_spawner_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher2",
+        output="screen",
+        parameters=[
+            {
+                "use_sim_time": True,
+                "robot_description": Command(
+                    [f"xacro {urdf_file}", " camera_name:='camera_2'"]
+                ),
+            }
+        ],
+        remappings=[
+            ("robot_description", "/camera_2/robot_description"),
+        ],
+    )
+
+    # It is necessary for spawning camera_2 in gazebo environment
+    camera_2_robot_state_publisher_node = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        name="spawn2",
+        arguments=[
+            "-entity",
+            "mr_camera2",
+            "-topic",
+            "/camera_2/robot_description",
+            "-x",
+            camera_2_pose[0],
+            "-y",
+            camera_2_pose[1],
+            "-z",
+            camera_2_pose[2],
+            "-Y",
+            camera_2_pose[3],
+            "-P",
+            camera_2_pose[4],
+            "-R",
+            camera_2_pose[5],
+        ],
+        output="screen",
+    )
+
+    # It is necessary to make transformation between world frame and camera frames enable later.
+    camera_2_tf_from_world_publisher_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=[*camera_2_pose, "world", "camera_2_base_link"],
+    )
+
+    # It is necessary to open previously created gazebo world for perception pipeline demo.
+    gazebo_launch = IncludeLaunchDescription(
+        PathJoinSubstitution(
+            [FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"]
+        ),
+        launch_arguments={
+            "world": os.path.join(
+                get_package_share_directory("moveit2_tutorials"),
+                "worlds",
+                "perception_pipeline_demo.world",
+            ),
+        }.items(),
+    )
+
+    return LaunchDescription(
+        [
+            camera_1_robot_state_publisher_node,
+            camera_1_gazebo_spawner_node,
+            camera_1_tf_from_world_publisher_node,
+            camera_2_robot_state_publisher_node,
+            camera_2_gazebo_spawner_node,
+            camera_2_tf_from_world_publisher_node,
+            gazebo_launch,
+        ]
+    )

doc/examples/perception_pipeline/launch/perception_pipeline_demo.launch.py

@@ -0,0 +1,154 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+from launch_param_builder import ParameterBuilder
+
+
+def generate_launch_description():
+
+    # Command-line arguments
+    rviz_config_arg = DeclareLaunchArgument(
+        "rviz_config",
+        default_value="moveit.rviz",
+        description="RViz configuration file",
+    )
+
+    ros2_control_hardware_type = DeclareLaunchArgument(
+        "ros2_control_hardware_type",
+        default_value="mock_components",
+        description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
+    )
+
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(
+            file_path="config/panda.urdf.xacro",
+            mappings={
+                "ros2_control_hardware_type": LaunchConfiguration(
+                    "ros2_control_hardware_type"
+                )
+            },
+        )
+        .robot_description_semantic(file_path="config/panda.srdf")
+        .robot_description_kinematics(file_path="config/kinematics.yaml")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
+        )
+        .sensors_3d(
+            file_path=os.path.join(
+                get_package_share_directory("moveit2_tutorials"),
+                "config/sensors_3d.yaml",
+            )
+        )
+        .to_moveit_configs()
+    )
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict()],
+        arguments=["--ros-args", "--log-level", "info"],
+    )
+
+    # RViz
+    rviz_base = LaunchConfiguration("rviz_config")
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare("moveit_resources_panda_moveit_config"), "launch", rviz_base]
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.planning_pipelines,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    # Static TF
+    static_tf_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
+        output="screen",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    panda_arm_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    panda_hand_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    )
+
+    return LaunchDescription(
+        [
+            rviz_config_arg,
+            ros2_control_hardware_type,
+            rviz_node,
+            static_tf_node,
+            robot_state_publisher,
+            move_group_node,
+            ros2_control_node,
+            joint_state_broadcaster_spawner,
+            panda_arm_controller_spawner,
+            panda_hand_controller_spawner,
+        ]
+    )