Fix Gazebo Scan and Plan Config (#317)

src/picknik_ur_gazebo_scan_and_plan_config/config/config.yaml

@@ -5,10 +5,11 @@
 # Name of the package to specialize
 based_on_package: "picknik_ur_gazebo_config"
 
-# Optional parameters that can be read in your launch files for specific functionality
-optional_feature_params:
-  gazebo_world_package_name: "picknik_ur_gazebo_scan_and_plan_config"
-  gazebo_world_path: "description/simulation_worlds/scan_and_plan_world.sdf"
+hardware:
+  # The following launch file is started when hardware.simulated is True
+  simulated_hardware_launch_file:
+    package: "picknik_ur_gazebo_scan_and_plan_config"
+    path: "launch/sim/hardware_sim.launch.py"
 
 objectives:
   # Override with a new set of waypoints based on the Gazebo world.

src/picknik_ur_gazebo_scan_and_plan_config/launch/sim/hardware_sim.launch.py

@@ -0,0 +1,261 @@
+# Copyright 2023 PickNik Inc.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the PickNik Inc. nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+import os
+import re
+import shlex
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
+from launch_ros.actions import Node
+
+from moveit_studio_utils_py.launch_common import get_launch_file, get_ros_path
+from moveit_studio_utils_py.system_config import get_config_folder, SystemConfigParser
+
+
+def path_pattern_change_for_gazebo(urdf_string):
+    """
+    Replaces strings in a URDF file such as
+        package://package_name/path/to/file
+    to the actual full path of the file.
+    """
+    data = urdf_string
+    package_expressions = re.findall("(package://([^//]*))", data)
+    for expr in set(package_expressions):
+        data = data.replace(expr[0], get_ros_path(expr[1]))
+    return data
+
+
+def generate_simulation_description(context, *args, **settings):
+    world_path = "description/simulation_worlds/scan_and_plan_world.sdf"
+    use_gui = False
+    is_verbose = False
+
+    # Create a Gazebo world file that swaps out package:// paths with absolute path.
+    original_world_file = get_ros_path(
+        "picknik_ur_gazebo_scan_and_plan_config",
+        world_path,
+    )
+    modified_world_file = os.path.join(
+        get_config_folder(), "auto_created", "gazebo_world.sdf"
+    )
+    with open(original_world_file, "r") as file:
+        world_sdf = path_pattern_change_for_gazebo(file.read())
+    with open(modified_world_file, "w") as file:
+        file.write(world_sdf)
+
+    # Launch Gazebo.
+    print(f"Starting Gazebo with world at {world_path}")
+    print(f"GUI: {use_gui}, Verbose: {is_verbose}")
+
+    sim_args = "-r --render-engine ogre"
+    if is_verbose:
+        sim_args += " -v 4"
+    if not use_gui:
+        sim_args += " -s --headless-rendering"
+
+    gazebo = IncludeLaunchDescription(
+        get_launch_file("ros_gz_sim", "launch/gz_sim.launch.py"),
+        launch_arguments=[("gz_args", [f"{sim_args} {modified_world_file}"])],
+    )
+    return [gazebo]
+
+
+def generate_launch_description():
+    system_config_parser = SystemConfigParser()
+
+    # The path to the auto_created urdf files
+    robot_urdf = system_config_parser.get_processed_urdf()
+    robot_urdf_ignition = path_pattern_change_for_gazebo(robot_urdf)
+
+    # Launch Gazebo
+    gazebo = OpaqueFunction(function=generate_simulation_description)
+
+    init_pose_args = shlex.split("-x 0.0 -y 0.0 -z 1.03 -R 0.0 -P 0.0 -Y 0.0")
+    spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        output="both",
+        arguments=[
+            "-string",
+            robot_urdf_ignition,
+            "-name",
+            "robot",
+            "-allow_renaming",
+            "true",
+        ]
+        + init_pose_args,
+    )
+
+    ########################
+    # Camera Topic Bridges #
+    ########################
+    # For the scene camera, enable RGB image topics only.
+    scene_image_rgb_ignition_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        name="scene_image_rgb_ignition_bridge",
+        arguments=[
+            "/scene_camera/image",
+        ],
+        remappings=[
+            ("/scene_camera/image", "/scene_camera/color/image_raw"),
+        ],
+        output="both",
+    )
+    scene_image_depth_ignition_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        name="scene_image_depth_ignition_bridge",
+        arguments=[
+            "/scene_camera/depth_image",
+        ],
+        remappings=[
+            (
+                "/scene_camera/depth_image",
+                "/scene_camera/depth/image_rect_raw",
+            ),
+        ],
+        output="both",
+    )
+
+    scene_camera_info_ignition_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="scene_camera_info_ignition_bridge",
+        arguments=[
+            "/scene_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo",
+        ],
+        remappings=[
+            ("/scene_camera/camera_info", "/scene_camera/color/camera_info"),
+        ],
+        output="both",
+    )
+
+    # For the wrist mounted camera, enable RGB and depth topics.
+    wrist_image_rgb_ignition_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        name="wrist_image_rgb_ignition_bridge",
+        arguments=[
+            "/wrist_mounted_camera/image",
+        ],
+        remappings=[
+            ("/wrist_mounted_camera/image", "/wrist_mounted_camera/color/image_raw"),
+        ],
+        output="both",
+    )
+    wrist_image_depth_ignition_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        name="wrist_image_depth_ignition_bridge",
+        arguments=[
+            "/wrist_mounted_camera/depth_image",
+        ],
+        remappings=[
+            (
+                "/wrist_mounted_camera/depth_image",
+                "/wrist_mounted_camera/depth/image_rect_raw",
+            ),
+        ],
+        output="both",
+    )
+    wrist_camera_pointcloud_ignition_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="wrist_camera_pointcloud_ignition_bridge",
+        arguments=[
+            "/wrist_mounted_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked",
+        ],
+        remappings=[
+            (
+                "/wrist_mounted_camera/points",
+                "/wrist_mounted_camera/depth/color/points",
+            ),
+        ],
+        output="both",
+    )
+    wrist_camera_info_ignition_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="wrist_camera_info_ignition_bridge",
+        arguments=[
+            "/wrist_mounted_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo",
+        ],
+        remappings=[
+            (
+                "/wrist_mounted_camera/camera_info",
+                "/wrist_mounted_camera/color/camera_info",
+            ),
+        ],
+        output="both",
+    )
+
+    #######################
+    # Force Torque Sensor #
+    #######################
+    fts_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="fts_bridge",
+        arguments=[
+            "/tcp_fts_sensor/ft_data@geometry_msgs/msg/WrenchStamped[ignition.msgs.Wrench",
+        ],
+        remappings=[
+            (
+                "/tcp_fts_sensor/ft_data",
+                "/force_torque_sensor_broadcaster/wrench",
+            ),
+        ],
+        output="both",
+    )
+
+    clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="clock_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
+        output="both",
+    )
+
+    return LaunchDescription(
+        [
+            scene_image_rgb_ignition_bridge,
+            scene_image_depth_ignition_bridge,
+            scene_camera_info_ignition_bridge,
+            wrist_image_rgb_ignition_bridge,
+            wrist_camera_info_ignition_bridge,
+            wrist_image_depth_ignition_bridge,
+            wrist_camera_pointcloud_ignition_bridge,
+            clock_bridge,
+            fts_bridge,
+            gazebo,
+            spawn_robot,
+        ]
+    )

src/picknik_ur_gazebo_scan_and_plan_config/objectives/estimate_object_pose.xml

@@ -10,7 +10,7 @@
       <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{object_stamped_pose_estimate}" output_cloud="{model_point_cloud}"/>
       <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_iterations="30" max_correspondence_distance="{icp_max_correspondence_distance}" target_pose_in_base_frame="{model_to_real_pose}"/>
       <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/>
-      <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
       <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_to_real_pose}"/>
     </Control>
   </BehaviorTree>

src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml

@@ -1,6 +1,6 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Object Inspection">
-  <!-- ////////// -->
+  <!--//////////-->
   <BehaviorTree ID="Object Inspection" _description="Estimate the pose of an object using ICP; then, perform a 3D reconstruction of the object from multiple views." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot"/>
@@ -10,16 +10,25 @@
       <Decorator ID="ForEachPoseStamped" vector_in="{pose_stamped_msgs}" out="{trajectory_pose}">
         <Control ID="Sequence">
           <Action ID="TransformPoseWithPose" input_pose="{trajectory_pose}" transform_pose="{model_to_real_pose}" output_pose="{target_pose}"/>
-          <SubTree ID="Move To Pose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
+          <SubTree ID="Move To Pose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" _collapsed="false"/>
           <Action ID="GetSyncedImageAndPointCloud" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
           <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud_world}"/>
           <Action ID="AddPointCloudToVector" point_cloud="{point_cloud_world}" point_cloud_vector="{point_cloud_vector}"/>
         </Control>
       </Decorator>
       <Action ID="ClearSnapshot"/>
       <Action ID="MergePointClouds" point_clouds="{point_cloud_vector}" grid_resolution_meters="0.001" merged_cloud="{merged_cloud}"/>
-      <Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
       <Action ID="SavePointCloudToFile" point_cloud="{merged_cloud}" file_path="~/.config/moveit_pro/saved_behavior_data" file_prefix="pointcloud"/>
     </Control>
   </BehaviorTree>
+  <BehaviorTree ID="Move To Pose">
+        <Control ID="Sequence">
+            <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
+            <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+            <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+        </Control>
+    </BehaviorTree>
 </root>