diff --git a/rbs_mill_assist/CMakeLists.txt b/rbs_mill_assist/CMakeLists.txt index 43e6bc2..84deb0d 100644 --- a/rbs_mill_assist/CMakeLists.txt +++ b/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 diff --git a/rbs_mill_assist/config/grasping.yaml b/rbs_mill_assist/config/grasping.yaml new file mode 100644 index 0000000..02a27ae --- /dev/null +++ b/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 diff --git a/rbs_mill_assist/launch/bt_executor.launch.py b/rbs_mill_assist/launch/bt_executor.launch.py new file mode 100644 index 0000000..96dacea --- /dev/null +++ b/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) diff --git a/rbs_mill_assist/launch/simulation.launch.py b/rbs_mill_assist/launch/simulation.launch.py index 2d8446c..f125236 100644 --- a/rbs_mill_assist/launch/simulation.launch.py +++ b/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", ), ) diff --git a/rbs_mill_assist/package.xml b/rbs_mill_assist/package.xml index 1e8add8..3900d05 100644 --- a/rbs_mill_assist/package.xml +++ b/rbs_mill_assist/package.xml @@ -19,6 +19,7 @@ cartesian_force_controller cartesian_motion_controller cartesian_twist_controller + rbs_bt_executor ament_lint_auto diff --git a/rbs_mill_assist/scripts/CMakeLists.txt b/rbs_mill_assist/scripts/CMakeLists.txt new file mode 100644 index 0000000..fa24557 --- /dev/null +++ b/rbs_mill_assist/scripts/CMakeLists.txt @@ -0,0 +1,4 @@ +install(PROGRAMS + grasping_service.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/rbs_mill_assist/scripts/grasping_service.py b/rbs_mill_assist/scripts/grasping_service.py new file mode 100755 index 0000000..bbec6a1 --- /dev/null +++ b/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() diff --git a/rbs_mill_assist/world/default.sdf b/rbs_mill_assist/world/default.sdf index 8aecf35..6837e9c 100644 --- a/rbs_mill_assist/world/default.sdf +++ b/rbs_mill_assist/world/default.sdf @@ -10,7 +10,7 @@ 0.001 1.0 - bullet + @@ -55,9 +55,42 @@ - -0.35 0.0 0 0 0 0 + shildik_0 + 0.0 -0.28 0.0 0 0 3.14159 model://shildik + + shildik_1 + 0.0 -0.28 0.01 0 0 3.14159 + model://shildik + + + shildik_2 + 0.0 -0.28 0.02 0 0 3.14159 + model://shildik + + + shildik_3 + 0.0 -0.28 0.03 0 0 3.14159 + model://shildik + + + shildik_4 + 0.0 -0.28 0.04 0 0 3.14159 + model://shildik + + + + true + 0.30 0.0 0 0 0 -1.57 + model://laser + + + + + + +