robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

rbs_gym multi camera and multi object setup

Browse source
This commit is contained in:
Ilya Uraev 2024-09-12 14:44:58 +03:00
parent b4b452297d
commit 1a562b05ca
34 changed files with 1082 additions and 816 deletions
Download patch file Download diff file Expand all files Collapse all files

6
rbs_skill_servers/scripts/move_to_pose.py
View file

@ -48,11 +48,13 @@ class CartesianMoveToPose(Node):
self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
target_pose = goal_handle.request.target_pose
start_pose = self.current_pose.pose if self.current_pose else None
result = MoveitSendPose.Result()
if start_pose is None:
self.get_logger().error("Current pose is not available")
goal_handle.abort()
return MoveitSendPose.Result()
result.success = False
return result
trajectory = self.generate_trajectory(start_pose, target_pose)
for point in trajectory:
@ -64,8 +66,6 @@ class CartesianMoveToPose(Node):
rclpy.spin_once(self, timeout_sec=0.1)
goal_handle.succeed()
result = MoveitSendPose.Result()
result.success = True
return result