Add pickable object service and update world setup

- Create a service node to provide information about available objects for picking
- Update the Gazebo world with multiple shildik models arranged vertically
- Add a laser sensor to the environment
- Disable the bunker model inclusion

rbs_mill_assist/CMakeLists.txt

@@ -48,9 +48,13 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
-install(DIRECTORY launch urdf config world assets meshes
+
+install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config
 DESTINATION share/${PROJECT_NAME})
 
+add_subdirectory(scripts)
+add_subdirectory(bt)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

rbs_mill_assist/config/grasping.yaml

@@ -0,0 +1,60 @@
+pregrasp_pose:
+  position:
+    x: 0.10395
+    y: -0.28
+    z: 0.1
+  orientation:
+    x: 0.0
+    y: 1.0
+    z: 0.0
+    w: 0.0
+grasp_pose:
+  position:
+    x: 0.10395
+    y: -0.28
+    z: 0.004
+  orientation:
+    x: 0.0
+    y: 1.0
+    z: 0.0
+    w: 0.04
+postgrasp_pose:
+  position:
+    x: 0.10395
+    y: -0.28
+    z: 0.1
+  orientation:
+    x: 0.0
+    y: 1.0
+    z: 0.0
+    w: 0.0
+preplace_pose:
+  position:
+    x: 0.360
+    y: -0.06
+    z: 0.1
+  orientation:
+    x: -0.707
+    y: 0.707
+    z: 0.0
+    w: 0.0
+place_pose:
+  position:
+    x: 0.360
+    y: -0.06
+    z: 0.05
+  orientation:
+    x: -0.707
+    y: 0.707
+    z: 0.0
+    w: 0.0
+postplace_pose:
+  position:
+    x: 0.360
+    y: -0.06
+    z: 0.1
+  orientation:
+    x: -0.707
+    y: 0.707
+    z: 0.0
+    w: 0.0

rbs_mill_assist/launch/bt_executor.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+    executor_params = PathJoinSubstitution([FindPackageShare("rbs_mill_assist"), "config", "bt_executor.yaml"])
+
+    nodes_to_start = [
+        Node(
+            package='rbs_bt_executor',
+            executable='rbs_bt_executor',
+            # prefix=['gdbserver localhost:1234'],
+            parameters=[
+                executor_params,
+                {'use_sim_time': True}
+            ],
+        )
+    ]
+
+    return LaunchDescription(nodes_to_start)

rbs_mill_assist/launch/simulation.launch.py

@@ -165,12 +165,18 @@ def launch_setup(context, *args, **kwargs):
         config_file=os.path.join(description_package_abs_path, "config", "gz_bridge.yaml")
     )
 
+    grasping_service = Node(
+        package="rbs_mill_assist",
+        executable="grasping_service.py"
+    )
+
     nodes_to_start = [
         rbs_robot_setup,
         rviz_node,
         gazebo,
         gazebo_spawn_robot,
-        gz_bridge
+        gz_bridge,
+        grasping_service
     ]
     return nodes_to_start
 
@@ -272,7 +278,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "ee_link_name",
-            default_value="Link6",
+            default_value="grasp_point",
             description="End effector name of robot arm",
         )
     )
@@ -302,7 +308,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "interactive",
-            default_value="true",
+            default_value="false",
             description="Wheter to run the motion_control_handle controller",
         ),
     )

rbs_mill_assist/package.xml

@@ -19,6 +19,7 @@
   <depend>cartesian_force_controller</depend>
   <depend>cartesian_motion_controller</depend>
   <depend>cartesian_twist_controller</depend>
+  <depend>rbs_bt_executor</depend>
 
 
   <test_depend>ament_lint_auto</test_depend>

rbs_mill_assist/scripts/CMakeLists.txt

@@ -0,0 +1,4 @@
+install(PROGRAMS
+  grasping_service.py
+  DESTINATION lib/${PROJECT_NAME}
+)

rbs_mill_assist/scripts/grasping_service.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+
+import os
+from typing import Dict
+
+import rclpy
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from geometry_msgs.msg import Pose
+from rbs_skill_interfaces.srv import GetPickPlacePoses
+from rclpy.node import Node
+from rclpy.service import Service
+
+
+class GetGraspPickPoses(Node):
+    def __init__(self) -> None:
+        super().__init__("get_grasp_pick_poses")
+        self.srv: Service = self.create_service(
+            GetPickPlacePoses, "get_pick_place_poses", self.get_grasp_pick_poses
+        )
+        yaml_file_path: str = os.path.join(
+            get_package_share_directory("rbs_mill_assist"), "config", "grasping.yaml"
+        )
+        with open(yaml_file_path, "r", encoding="utf-8") as file:
+            self.grasping: Dict = yaml.safe_load(file)
+
+    def create_pose(self, pose_data: Dict) -> Pose:
+        """
+        Helper function to create a Pose from the given pose data.
+        """
+        pose = Pose()
+        if pose_data:
+            pose.position.x = pose_data.get("position", {}).get("x", 0.0)
+            pose.position.y = pose_data.get("position", {}).get("y", 0.0)
+            pose.position.z = pose_data.get("position", {}).get("z", 0.0)
+            pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
+            pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
+            pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
+            pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
+        return pose
+
+    def get_grasp_pick_poses(
+        self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
+    ) -> GetPickPlacePoses.Response:
+        if request.action_name == "pick":
+            pregrasp_pose = self.grasping.get("pregrasp_pose", None)
+            grasp_pose = self.grasping.get("grasp_pose", None)
+            postgrasp_pose = self.grasping.get("postgrasp_pose", None)
+
+            if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
+                response.ok = False
+                self.get_logger().error(
+                    "Missing one or more of the pregrasp, grasp, or postgrasp poses."
+                )
+                return response
+
+            # Create Pose messages
+            response.grasp = [
+                self.create_pose(pregrasp_pose),
+                self.create_pose(grasp_pose),
+                self.create_pose(postgrasp_pose),
+            ]
+
+        elif request.action_name == "place":
+            preplace_pose = self.grasping.get("preplace_pose", None)
+            place_pose = self.grasping.get("place_pose", None)
+            postplace_pose = self.grasping.get("postplace_pose", None)
+
+            if None in [preplace_pose, place_pose, postplace_pose]:
+                response.ok = False
+                self.get_logger().error(
+                    "Missing one or more of the preplace, place, or postplace poses."
+                )
+                return response
+
+            # Create Pose messages for placing
+            response.place = [
+                self.create_pose(preplace_pose),
+                self.create_pose(place_pose),
+                self.create_pose(postplace_pose),
+            ]
+
+        response.ok = True
+        self.get_logger().info(f"Handled action: {request.action_name}")
+        return response
+
+
+def main():
+    rclpy.init()
+    executor = rclpy.executors.SingleThreadedExecutor()
+    # executor = rclpy.executors.MultiThreadedExecutor() # can't be used
+    i_node = GetGraspPickPoses()
+    executor.add_node(i_node)
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+        i_node.destroy_node()
+
+
+if __name__ == "__main__":
+    main()

rbs_mill_assist/world/default.sdf

@@ -10,7 +10,7 @@
       <max_step_size>0.001</max_step_size>
       <real_time_factor>1.0</real_time_factor>
       <dart>
-        <collision_detector>bullet</collision_detector>
+        <!-- <collision_detector>bullet</collision_detector> -->
         <!-- <solver> -->
         <!--   <solver_type>pgs</solver_type> -->
         <!-- </solver> -->
@@ -55,9 +55,42 @@
     </light>
     
     <include>
-      <pose>-0.35 0.0 0 0 0 0</pose>
+      <name>shildik_0</name>
+      <pose>0.0 -0.28 0.0 0 0 3.14159</pose>
       <uri>model://shildik</uri>
     </include>
+    <include>
+      <name>shildik_1</name>
+      <pose>0.0 -0.28 0.01 0 0 3.14159</pose>
+      <uri>model://shildik</uri>
+    </include>
+    <include>
+      <name>shildik_2</name>
+      <pose>0.0 -0.28 0.02 0 0 3.14159</pose>
+      <uri>model://shildik</uri>
+    </include>
+    <include>
+      <name>shildik_3</name>
+      <pose>0.0 -0.28 0.03 0 0 3.14159</pose>
+      <uri>model://shildik</uri>
+    </include>
+    <include>
+      <name>shildik_4</name>
+      <pose>0.0 -0.28 0.04 0 0 3.14159</pose>
+      <uri>model://shildik</uri>
+    </include>
+
+    <include>
+      <static>true</static>
+      <pose>0.30 0.0 0 0 0 -1.57</pose>
+      <uri>model://laser</uri>
+    </include>
+
+    <!-- <include> -->
+    <!--   <static>true</static> -->
+    <!--   <pose>0.0 -0.3 0.0752 1.57 0 3.14159</pose> -->
+    <!--   <uri>model://bunker</uri> -->
+    <!-- </include> -->
 
     <!-- <model name="ground_plane"> -->
     <!--   <static>true</static> -->