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:
parent
96ef9cb69c
commit
e268a556b5
8 changed files with 238 additions and 6 deletions
|
@ -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
|
||||
|
|
60
rbs_mill_assist/config/grasping.yaml
Normal file
60
rbs_mill_assist/config/grasping.yaml
Normal 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
|
23
rbs_mill_assist/launch/bt_executor.launch.py
Normal file
23
rbs_mill_assist/launch/bt_executor.launch.py
Normal 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)
|
|
@ -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",
|
||||
),
|
||||
)
|
||||
|
|
|
@ -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>
|
||||
|
|
4
rbs_mill_assist/scripts/CMakeLists.txt
Normal file
4
rbs_mill_assist/scripts/CMakeLists.txt
Normal file
|
@ -0,0 +1,4 @@
|
|||
install(PROGRAMS
|
||||
grasping_service.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
101
rbs_mill_assist/scripts/grasping_service.py
Executable file
101
rbs_mill_assist/scripts/grasping_service.py
Executable 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()
|
|
@ -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> -->
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue