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
This commit is contained in:
Ilya Uraev 2025-03-02 14:43:09 +03:00
parent 96ef9cb69c
commit e268a556b5
8 changed files with 238 additions and 6 deletions

View file

@ -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

View file

@ -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

View file

@ -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)

View file

@ -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",
),
)

View file

@ -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>

View file

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

View file

@ -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()

View file

@ -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> -->