class WorldManagerServer: def __init__(self): rospy.init_node('world_manager_node') moveit_commander.roscpp_initialize(sys.argv) # wait for moveit to come up self.scene = PlanningSceneInterface() self.tf_manager = TFManager() self.clear_planning_scene_service = rospy.Service( "/world_manager/clear_objects", std_srvs.srv.Empty, self.clear_objects_cb) self.add_box = rospy.Service( "/world_manager/add_box", world_manager_msgs.srv.AddBox, self.add_box_cb) self.add_mesh = rospy.Service( "/world_manager/add_mesh", world_manager_msgs.srv.AddMesh, self.add_mesh_cb) self.add_tf_service = rospy.Service( "/world_manager/add_tf", world_manager_msgs.srv.AddTF, self.add_tf_cb) self.add_walls_service = rospy.Service( "/world_manager/add_walls", std_srvs.srv.Empty, self.add_walls_cb) rospy.sleep(1.0) self.clear_objects_cb(request=None) self.add_walls_cb(request=None) rospy.loginfo("World Manager Node is Up and Running") def add_mesh_cb(self, request): so = request.scene_object # add the tf self.tf_manager.add_tf(so.object_name, so.pose_stamped) # remove the old completion if it is there self.scene.remove_world_object(so.object_name) # add the new object to the planning scene self.scene.add_mesh(so.object_name, so.pose_stamped, so.mesh_filepath) return [] def add_box_cb(self, request): # type: (world_manager_msgs.srv.AddBoxRequest) -> [] box = request.scene_box # type: box -> world_manager_msgs.msg.SceneBox # add the tf self.tf_manager.add_tf(box.object_name, box.pose_stamped) # remove the old box if it is there self.scene.remove_world_object(box.object_name) # add the new box to the planning scene self.scene.add_box(name=box.object_name, pose=box.pose_stamped, size=(box.edge_length_x, box.edge_length_y, box.edge_length_z)) rospy.loginfo("Added box {}".format(box.object_name)) return [] def add_tf_cb(self, request): self.tf_manager.add_tf(request.frame_name, request.pose_stamped) return [] def clear_objects_cb(self, request): body_names = self.scene.get_known_object_names() try: walls = rospy.get_param('walls') except: rospy.loginfo("No wall in this scene") rospy.loginfo("Clearing objects: {}".format(body_names)) for body_name in body_names: if not body_name in walls: rospy.loginfo("Removing object: {}".format(body_name)) self.scene.remove_world_object(body_name) self.tf_manager.clear_tfs() return [] def add_walls_cb(self, request): try: walls = rospy.get_param('walls') except: rospy.loginfo("No wall in this scene") walls = [] for wall_params in walls: rospy.loginfo("Adding wall " + str(wall_params)) self._add_wall(wall_params) return [] def _add_wall(self, wall_params): name = wall_params["name"] x_thickness = wall_params["x_thickness"] y_thickness = wall_params["y_thickness"] z_thickness = wall_params["z_thickness"] x = wall_params["x"] y = wall_params["y"] z = wall_params["z"] qx = wall_params["qx"] qy = wall_params["qy"] qz = wall_params["qz"] qw = wall_params["qw"] frame_id = wall_params["frame_id"] back_wall_pose = geometry_msgs.msg.PoseStamped() back_wall_pose.header.frame_id = '/' + frame_id wall_dimensions = [x_thickness, y_thickness, z_thickness] back_wall_pose.pose.position = geometry_msgs.msg.Point( **{'x': x, 'y': y, 'z': z}) back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion( **{'x': qx, 'y': qy, 'z': qz, 'w': qw}) self.scene.add_box(name, back_wall_pose, wall_dimensions)
if __name__ == '__main__': init_node() group = MoveGroupCommander("right_arm") # Initialize the Planning Scene rospy.loginfo("Setting the Planning Scene...") scene = PlanningSceneInterface() rospy.sleep(2) scene.remove_world_object() # Remove all objects first rospy.sleep(2) rospy.loginfo("All objects Removed : {}".format( scene.get_known_object_names())) # Pose Target 1 rospy.loginfo("Start Pose Target 1") pose_target_1 = Pose() pose_target_1.position.x = 0.3 pose_target_1.position.y = -0.1 pose_target_1.position.z = 0.15 pose_target_1.orientation.x = 0.0 pose_target_1.orientation.y = -0.707 pose_target_1.orientation.z = 0.0 pose_target_1.orientation.w = 0.707 rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1))
def main(): rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) # centre table p1 = PoseStamped() p1.header.frame_id = robot.get_planning_frame() p1.pose.position.x = 1. # position of the table in the world frame p1.pose.position.y = 0. p1.pose.position.z = 0. # left table (from baxter's perspective) p1_l = PoseStamped() p1_l.header.frame_id = robot.get_planning_frame() p1_l.pose.position.x = 0.5 # position of the table in the world frame p1_l.pose.position.y = 1. p1_l.pose.position.z = 0. p1_l.pose.orientation.x = 0.707 p1_l.pose.orientation.y = 0.707 # right table (from baxter's perspective) p1_r = PoseStamped() p1_r.header.frame_id = robot.get_planning_frame() p1_r.pose.position.x = 0.5 # position of the table in the world frame p1_r.pose.position.y = -1. p1_r.pose.position.z = 0. p1_r.pose.orientation.x = 0.707 p1_r.pose.orientation.y = 0.707 # open back shelf p2 = PoseStamped() p2.header.frame_id = robot.get_planning_frame() p2.pose.position.x = 1.2 # position of the table in the world frame p2.pose.position.y = 0.0 p2.pose.position.z = 0.75 p2.pose.orientation.x = 0.5 p2.pose.orientation.y = -0.5 p2.pose.orientation.z = -0.5 p2.pose.orientation.w = 0.5 pw = PoseStamped() pw.header.frame_id = robot.get_planning_frame() # add an object to be grasped p_ob1 = PoseStamped() p_ob1.header.frame_id = robot.get_planning_frame() p_ob1.pose.position.x = .92 p_ob1.pose.position.y = 0.3 p_ob1.pose.position.z = 0.89 # the ole duck p_ob2 = PoseStamped() p_ob2.header.frame_id = robot.get_planning_frame() p_ob2.pose.position.x = 0.87 p_ob2.pose.position.y = -0.4 p_ob2.pose.position.z = 0.24 # clean environment scene.remove_world_object("table_center") scene.remove_world_object("table_side_left") scene.remove_world_object("table_side_right") scene.remove_world_object("shelf") scene.remove_world_object("wall") scene.remove_world_object("part") scene.remove_world_object("duck") rospy.sleep(1) scene.add_box("table_center", p1, size=(.5, 1.5, 0.4)) # dimensions of the table scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4)) scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4)) scene.add_mesh( "shelf", p2, "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/bookshelf_light.stl", size=(.025, .01, .01)) scene.add_plane("wall", pw, normal=(0, 1, 0)) part_size = (0.07, 0.05, 0.12) scene.add_box("part", p_ob1, size=part_size) scene.add_mesh( "duck", p_ob2, "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/duck.stl", size=(.001, .001, .001)) rospy.sleep(1) print(scene.get_known_object_names()) ## ---> SET COLOURS table_color = color_norm([105, 105, 105]) # normalize colors to range [0, 1] shelf_color = color_norm([139, 69, 19]) duck_color = color_norm([255, 255, 0]) _colors = {} setColor('table_center', _colors, table_color, 1) setColor('table_side_left', _colors, table_color, 1) setColor('table_side_right', _colors, table_color, 1) setColor('shelf', _colors, shelf_color, 1) setColor('duck', _colors, duck_color, 1) sendColors(_colors, scene)
def main(): # moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('get_position_client') scene = PlanningSceneInterface() objects = scene.get_known_object_names() # print("objects", objects) # print("scene", scene) left_arm = MoveGroupCommander("left_arm") # left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper") right_arm = MoveGroupCommander("right_arm") # right_gripper = moveit_commander.MoveGroupCommander("right_gripper") dual_arm_group = MoveGroupCommander("dual_arm") grippers = MoveGroupCommander("grippers") get_position = rospy.ServiceProxy('get_position', target_position) rate = rospy.Rate(2.0) done_l = False done_r = False # dual_joints = dual_arm_group.get_joints() # print("dual_joints" , dual_joints) # dual_joint_values = dual_arm_group.get_current_joint_values() # print("dual_joint_values", dual_joint_values) # left_arm_group.set_named_target("left_home") # plan1 = left_arm_group.plan() # rospy.sleep(2) # left_arm_group.go() # print "...Left Done..." # rospy.sleep(1) dual_arm_group.set_named_target("both_home") plan1 = dual_arm_group.plan() rospy.sleep(2) # print("dual_arm plan1", plan1) dual_arm_group.go() print "...Both Done..." rospy.sleep(1) # print("scene", scene) while not rospy.is_shutdown(): try: rospy.wait_for_service('get_position', timeout=5) res = get_position() left_handle = res.points[0] right_handle = res.points[1] # scene.remove_world_object() # print "right_handle" , right_handle # pose_target_l = [] # pose_target_l.append(( left_handle.x - 0.03)) # pose_target_l.append(left_handle.y) # pose_target_l.append(left_handle.z) # pose_target_r = [] # pose_target_r.append(right_handle.x) # pose_target_r.append(right_handle.y) # pose_target_r.append(right_handle.z) # print(pose_target_l) print('left_handle', left_handle, 'right_handle', right_handle) # left_arm_group.set_position_target(pose_target_l) pose_target_l = Pose() pose_target_r = Pose() # pose_target_l.position = left_handle; # pose_target_l.position.x -= 0.06 # pose_target_l.position.y -= 0.005 # pose_target_l.position.z -= 0.005 # q = quaternion_from_euler(-1.57, 0, -1.57) # Horizental Gripper : paraller to x-axis # # q = quaternion_from_euler(-2.432, -1.57, -0.709) # Vertical Gripper: Vertical to x-axis # pose_target_l.orientation.x = q[0] # pose_target_l.orientation.y = q[1] # pose_target_l.orientation.z = q[2] # pose_target_l.orientation.w = q[3] # print ("pose_target_l",pose_target_l ) # left_arm_group.set_pose_target(pose_target_l) # left_arm_group.set_planning_time(10) # plan1 = left_arm_group.plan() # rospy.sleep(2) # left_arm_group.go() # done_l = True # pose_target_r.position.x = 0.5232 # pose_target_r.position.y = -0.2743 # pose_target_r.position.z = 0.6846 # right_gripper.set_named_target("right_open") # right_gripper.plan() # right_gripper.go() pose_target_r.position = right_handle translation = get_translations(right_handle) # pose_target_r.position.x -= 0.065 pose_target_r.position.x -= translation.x print("translation.x", translation.x) pose_target_r.position.y -= translation.y pose_target_r.position.z -= translation.z # pose_target_r.position.z = 0.7235 # q = quaternion_from_euler(-1.57, 0, -1.57) # This works from 2.35 : Gripper paraller to x-axis q = quaternion_from_euler( -1.57, 1.57, -1.57 ) # This work from 2.35 : Gripper Vertical to x-axis (-2.36, 1.5708, -2.36) pose_target_r.orientation.x = q[0] pose_target_r.orientation.y = q[1] pose_target_r.orientation.z = q[2] pose_target_r.orientation.w = q[3] # print ("pose_target_r",pose_target_r ) right_arm.set_pose_target(pose_target_r) right_arm.set_planning_time(10) plan1 = right_arm.plan() # rospy.sleep(1) # right_arm.go() if done_r == False: # print ("target pose",pose_target_r) if (plan1.joint_trajectory.points ): # True if trajectory contains points # last = (len(plan1.joint_trajectory.points) - 1) # print("joint_trajectory", (plan1.joint_trajectory.points[-1].positions) ) # getting the last position of trajectory r_joints = plan1.joint_trajectory.points[-1].positions d_joints = get_dual_trajectory(r_joints) # print("l_joints", l_joints) # d_joints = l_joints + list(r_joints) # print ("d_joints", d_joints) dual_arm_group.set_joint_value_target(d_joints) plan2 = dual_arm_group.plan() if (plan2.joint_trajectory.points): move_success = dual_arm_group.execute(plan2, wait=True) # eef_pose = right_arm.get_current_pose() # print("eef_pose", eef_pose) # move_success = right_arm.execute(plan1, wait = True) if move_success == True: rospy.sleep(2) right_arm.set_start_state_to_current_state() left_arm.set_start_state_to_current_state() waypoints_l = [] waypoints = [] wpose = right_arm.get_current_pose().pose wpose_l = left_arm.get_current_pose().pose print("wpose", wpose) # Open the gripper to the full position grippers.set_named_target("both_open") grippers.plan() grippers.go() # Create Cartesian Path to move forward mainting the end-effector pose # waypoints = [] # rospy.sleep(5) # wpose = right_arm.get_current_pose().pose if (translation.x >= 0.0505): wpose.position.x += (translation.x + 0.00 ) # move forward in (x) wpose_l.position.x += (translation.x + 0.002) wpose_l.position.z += 0.001 else: wpose.position.x += 0.051 # move forward in (x) wpose_l.position.x += 0.053 wpose_l.position.z += 0.001 waypoints.append(deepcopy(wpose)) (plan1, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) waypoints_l.append(deepcopy(wpose_l)) (plan_l, fraction) = left_arm.compute_cartesian_path( waypoints_l, # waypoints to follow 0.01, # eef_step 0.0) # print("plan1 len", len(plan1.joint_trajectory.points)) # waypoints.append(copy.deepcopy(wpose)) # (plan2, fraction) = dual_arm_group.compute_cartesian_path( # waypoints, # waypoints to follow # 0.005, # eef_step # 0.0) rospy.sleep(1) if plan1.joint_trajectory.points: move_success = right_arm.execute(plan1, wait=True) if move_success == True: rospy.loginfo( "Right Move forward successful") # done_r = True # break; if plan_l.joint_trajectory.points: move_success_l = left_arm.execute(plan_l, wait=True) if move_success_l == True: rospy.loginfo( "Left Move forward successful") # done_r = True if move_success == True and move_success_l == True: rospy.sleep(1) grippers.set_named_target("both_close") grippers.plan() grippers.go() # rospy.sleep(1) waypoints = [] rospy.sleep(1) wpose = right_arm.get_current_pose().pose # q = quaternion_from_euler(-1.57, 1.57, -1.57) # wrist up 5 degrees = 1.66 10deg = 1.75 # wpose.orientation.x = q[0] # wpose.orientation.y = q[1] # wpose.orientation.z = q[2] # wpose.orientation.w = q[3] wpose.position.z += 0.07 # move up in (z) waypoints.append(deepcopy(wpose)) (plan1, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) if plan1.joint_trajectory.points: # move_success = right_arm.execute(plan1, wait=True) # for testing right arm trajectory # done_r = True r_joints = plan1.joint_trajectory.points[ -1].positions # print ("r_joints", r_joints) d_joints = get_dual_trajectory(r_joints) # print ("d_joints", d_joints) dual_arm_group.set_joint_value_target( d_joints) # plan2 = 0 plan2 = dual_arm_group.plan() # print("planed plan len", len(plan2.joint_trajectory.points)) # print("dual arm trajectory", (plan2.joint_trajectory.points)) # done_r = True if (plan2.joint_trajectory.points): move_success = dual_arm_group.go() print("move_success", move_success) done_r = True # traj_points = plan2.joint_trajectory.points[0:-3] # plan2.joint_trajectory.points = traj_points # print("eddited plan len", len(plan2.joint_trajectory.points)) # move_success = dual_arm_group.execute(plan2, wait = True) # # done_r = True # move_success = right_arm.execute(plan1, wait=True) # if move_success == True: # rospy.loginfo ("Lift Successful") # done_r = True # r_joints = plan1.joint_trajectory.points[-1].positions # d_joints = get_dual_trajectory(r_joints) # # print("l_joints", l_joints) # # d_joints = l_joints + list(r_joints) # # print ("d_joints", d_joints) # dual_arm_group.set_joint_value_target(d_joints) # plan2 = dual_arm_group.plan() # if (plan2.joint_trajectory.points) : # move_success = dual_arm_group.execute(plan2, wait = True) # if move_success == True: # rospy.loginfo("Move forward successful") # rospy.sleep(1) # grippers.set_named_target("both_close") # grippers.plan() # grippers.go() # rospy.sleep(1) # waypoints = [] # rospy.sleep(2) # wpose = right_arm.get_current_pose().pose # wpose.position.z += 0.1 # move up in (z) # waypoints.append(copy.deepcopy(wpose)) # (plan1, fraction) = right_arm.compute_cartesian_path( # waypoints, # waypoints to follow # 0.01, # eef_step # 0.0) # if plan1.joint_trajectory.points: # r_joints = plan1.joint_trajectory.points[-1].positions # d_joints = get_dual_trajectory(r_joints) # # print ("d_joints", d_joints) # dual_arm_group.set_joint_value_target(d_joints) # plan2 = dual_arm_group.plan() # if (plan2.joint_trajectory.points) : # move_success = dual_arm_group.execute(plan2, wait = True) # done_r = True # move_success = right_arm.execute(plan1, wait=True) # if move_success == True: # rospy.loginfo ("Lift Successful") # done_r = True # else: # rospy.logwarn("Cartesian Paht Planning Failed for forward movement") else: print("Execution Completed") except rospy.ROSException: rospy.logerr('get_position_server did not respond in 5 sec') return rate.sleep()
class CollisionEnv(object): NS = "/art/collision_env/" def __init__(self, setup, world_frame): assert setup != "" self.ready = False self.setup = setup self.world_frame = world_frame self.tf_listener = TransformListener() self.api = ArtApiHelper() rospy.loginfo("Waiting for DB API") self.api.wait_for_db_api() self.ignored_prefixes = array_from_param("~ignored_prefixes") rospy.loginfo("Will ignore following prefixes: " + str(self.ignored_prefixes)) self.lock = RLock() self.ps = PlanningSceneInterface() self._paused = False self.oh = ObjectHelper(self.object_cb) self.artificial_objects = {} self.collision_objects_pub = rospy.Publisher(self.NS + "artificial", CollisionObjects, latch=True, queue_size=1) self.pub_artificial() self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb) self.paused_pub = rospy.Publisher(self.NS + "paused", Bool, latch=True, queue_size=1) self.paused = False def start(self): self.ready = True rospy.loginfo("Ready") def load_from_db(self): for prim in self.api.get_collision_primitives(self.setup): rospy.loginfo("Loading object: " + prim.name) self.add_primitive(prim) def save_primitive(self, name): with self.lock: if name not in self.artificial_objects: raise CollisionEnvException("Unknown object name") p = self.artificial_objects[name] self.api.add_collision_primitive(p) def save_primitives(self): with self.lock: for name in self.artificial_objects.keys(): self.save_primitive(name) def set_primitive_pose(self, name, ps): with self.lock: p = self.artificial_objects[name] assert p.pose.header.frame_id == ps.header.frame_id p.pose.pose = ps.pose self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def add_primitive(self, p): with self.lock: self.artificial_objects[p.name] = p self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def pub_artificial(self): msg = CollisionObjects() for v in self.artificial_objects.values(): # transform all collision primitives into world frame (= marker) if v.pose.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(v.pose.header.frame_id, self.world_frame, rospy.Time(0), rospy.Duration(1.0)) v.pose = self.tf_listener.transformPose( self.world_frame, v.pose) except tf.Exception as e: rospy.logwarn("Failed to transform artificial object: " + str(e)) continue msg.primitives.append(v) self.collision_objects_pub.publish(msg) @property def paused(self): return self._paused @paused.setter def paused(self, val): self._paused = val self.paused_pub.publish(val) def remove_name(self, name): with self.lock: if name not in self.artificial_objects: rospy.logwarn("Unknown artificial object: " + name) return False self.ps.remove_world_object(name) del self.artificial_objects[name] self.pub_artificial() if not self.api.clear_collision_primitives(self.setup, names=[name]): rospy.logwarn("Failed to remove from permanent storage") rospy.loginfo("Removed object: " + name) return True def clear_all(self, permanent=True): with self.lock: rospy.loginfo("Clearing " + str(len(self.artificial_objects)) + " artificial objects...") for k in self.artificial_objects.keys(): if self.is_ignored(k): continue self.ps.remove_world_object(k) self.artificial_objects = {} self.pub_artificial() try: if permanent and not self.api.clear_collision_primitives( self.setup): rospy.logwarn("Failed to remove from permanent storage") except ArtApiException as e: rospy.logerr(str(e)) def reload(self): self.clear_all(permanent=False) self.load_from_db() self.pub_artificial() def _generate_name(self): # as we use only part of uuid, there might be collisions... while True: name = str(uuid.uuid4())[:8] if name not in self.artificial_objects.keys(): break return name def set_det_pose(self, name, ps): object_type = self.api.get_object_type( self.oh.objects[name].object_type) if object_type is not None: self.add_detected(name, ps, object_type) def clear_det_on_table(self, inv=False, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): if v.object_id in ignore: continue if not inv and not v.on_table: continue if inv and v.on_table: continue ret.append(v.object_id) self.clear_detected(v.object_id) return ret def is_ignored(self, name): for ip in self.ignored_prefixes: if name.startswith(ip): return True return False def timer_cb(self, evt): if self.paused: return with self.lock: known_objects = self.ps.get_known_object_names() for name in known_objects: if name not in self.artificial_objects and name not in self.oh.objects and not self.is_ignored( name): rospy.loginfo("Removing outdated object: " + name) self.clear_detected(name) # restore artificial objects if they are lost somehow (e.g. by restart of MoveIt!) for k, v in self.artificial_objects.iteritems(): if k in known_objects: continue rospy.loginfo("Restoring artificial object: " + v.name) if v.bbox.type == SolidPrimitive.BOX: self.ps.add_box(v.name, v.pose, v.bbox.dimensions) else: # TODO other types pass def object_cb(self, evt, h, inst): if self.paused or not self.ready: return with self.lock: attached_objects = self.ps.get_attached_objects() if evt in (ObjectHelper.OBJECT_ADDED, ObjectHelper.OBJECT_UPDATED): if inst.object_id in attached_objects: return ps = PoseStamped() ps.header = h ps.pose = inst.pose object_type = self.api.get_object_type(inst.object_type) if object_type is not None: self.add_detected(inst.object_id, ps, object_type) elif evt == ObjectHelper.OBJECT_LOST: if inst.object_id not in attached_objects: self.clear_detected(inst.object_id) def add_detected(self, name, ps, object_type): with self.lock: self.ps.add_box(name, ps, object_type.bbox.dimensions) def clear_detected(self, name): with self.lock: self.ps.remove_world_object(name) def clear_all_det(self, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): name = v.object_id if name in ignore: continue self.clear_detected(name) ret.append(name) rospy.loginfo("Removed " + str(len(ret)) + " detected objects.") return ret def get_attached(self, transform_to_world=True): """ keep in mind - attached objects might not be detected """ ret = [] with self.lock: ao = self.ps.get_attached_objects() for k, v in ao.iteritems(): if len(v.object.primitives) != 1 or len( v.object.primitive_poses) != 1: rospy.logwarn("Unsupported type of attached object: " + k) continue if v.object.primitives[0].type != SolidPrimitive.BOX: rospy.logwarn( "Only box-like attached objects are supported so far.") continue ps = PoseStamped() ps.header = v.object.header ps.pose = v.object.primitive_poses[0] if transform_to_world and ps.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(ps.header.frame_id, self.world_frame, ps.header.stamp, rospy.Duration(1.0)) ps = self.tf_listener.transformPose(self.world_frame, ps) except tf.Exception as e: rospy.logwarn("Failed to transform attached object: " + str(e)) continue ret.append((k, ps, v.object.primitives[0])) return ret
class PickAndPlace: def __init__(self, robot_name="panda_arm", frame="panda_link0"): try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node(name="pick_place_test") self.scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # region Robot initial self.robot = MoveGroupCommander(robot_name) self.robot.set_goal_joint_tolerance(0.00001) self.robot.set_goal_position_tolerance(0.00001) self.robot.set_goal_orientation_tolerance(0.01) self.robot.set_goal_tolerance(0.00001) self.robot.allow_replanning(True) self.robot.set_pose_reference_frame(frame) self.robot.set_planning_time(3) # endregion self.gripper = MoveGroupCommander("hand") self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_OPEN) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() # Robot go home self.robot.set_named_target("home") self.robot.go() # clear all object in world self.clear_all_object() table_pose = un.Pose(0, 0, -10, 0, 0, 0) table_color = un.Color(255, 255, 0, 100) self.add_object_box("table", table_pose, table_color, frame, (2000, 2000, 10)) bearing_pose = un.Pose(250, 250, 500, -90, 45, -90) bearing_color = un.Color(255, 0, 255, 255) bearing_file_name = "../stl/bearing.stl" self.add_object_mesh("bearing", bearing_pose, bearing_color, frame, bearing_file_name) obpose = self.scene.get_object_poses(["bearing"]) # self.robot.set_support_surface_name("table") g = Grasp() # Create gripper position open or close g.pre_grasp_posture = self.open_gripper() g.grasp_posture = self.close_gripper() g.pre_grasp_approach = self.make_gripper_translation( 0.01, 0.1, [0, 1.0, 0]) g.post_grasp_retreat = self.make_gripper_translation( 0.01, 0.9, [0, 1.0, 0]) p = PoseStamped() p.header.frame_id = "panda_link0" p.pose.orientation = obpose["bearing"].orientation p.pose.position = obpose["bearing"].position g.grasp_pose = p g.allowed_touch_objects = ["bearing"] a = [] a.append(g) result = self.robot.pick(object_name="bearing", grasp=a) print(result) except Exception as ex: print(ex) moveit_commander.roscpp_shutdown() moveit_commander.roscpp_shutdown() def make_gripper_translation(self, min_dist, desired, vector): g = GripperTranslation() g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] g.direction.header.frame_id = "panda_link0" g.min_distance = min_dist g.desired_distance = desired return g def open_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.04, 0.04] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def close_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.0, 0.0] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def clear_all_object(self): for world_object in (self.scene.get_known_object_names()): self.scene.remove_world_object(world_object) for attached_object in (self.scene.get_attached_objects()): self.scene.remove_attached_object(attached_object) def add_object_box(self, object_name, pose, color, frame, size): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param size: size of object(mm) :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_box(name=object_name, pose=object_pose, size=(size[0] / 1000.0, size[1] / 1000.0, size[2] / 1000.0)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p) def add_object_mesh(self, object_name, pose, color, frame, file_name): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param file_name: mesh file name :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_mesh(name=object_name, pose=object_pose, filename=file_name, size=(0.001, 0.001, 0.001)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p)
class Scene: interface = None # type: PlanningSceneInterface publisher = None # type: rospy.Publisher ref_frame = '' # type: str colors = dict() # type: dict default_color = (0.75, 0.75, 0.75, 1) # type: tuple def __init__(self, ref_frame='base_link'): self.interface = PlanningSceneInterface() self.publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) self.ref_frame = ref_frame rospy.sleep(1) def destroy(self): pass # ---- add & remove ---- # def add_box(self, name, size, transform, rgba=None): if rgba is None: rgba = self.default_color self.interface.remove_world_object(name) pose = PoseStamped() pose.header.frame_id = self.ref_frame pose.pose = frame_to_pose(transform) self.interface.add_box(name, pose, size) self.wait_state_update(name) self.set_color(name, rgba) self.send_colors() def remove_object(self, name): self.interface.remove_world_object(name) self.wait_state_update(name, is_known=False) # ---- utils ---- # def set_color(self, name, rgba): color = ObjectColor() color.id = name color.color.r = rgba[0] color.color.g = rgba[1] color.color.b = rgba[2] color.color.a = rgba[3] self.colors[name] = color def send_colors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.publisher.publish(p) def wait_state_update(self, name, is_known=True, is_attached=False, timeout=4): start = rospy.get_time() current = rospy.get_time() while (current - start < timeout) and not rospy.is_shutdown(): attached_objects = self.interface.get_attached_objects([name]) cur_is_attached = len(attached_objects.keys()) > 0 cur_is_known = name in self.interface.get_known_object_names() if (cur_is_attached == is_attached) and (cur_is_known == is_known): rospy.loginfo('Scene update succeeded.') return True rospy.sleep(0.1) current = rospy.get_time() # rospy.loginfo("Wait scene update [" + str(current - start) + "s]...") rospy.loginfo('Scene update failed') return False
class WorldManager: def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.model_pose_broadcaster = ModelPoseBroadcaster() # self.clear_planning_scene_service = rospy.Service("/world_manager/clear_planning_scene", graspit_msgs.srv.ClearModels, self.remove_all_objects_from_planner) self.add_mesh_to_planning_scene_service = rospy.Service( "/world_manager/add_object", graspit_msgs.srv.AddObject, self.add_object_to_planning_scene) rospy.sleep(1.0) # self.add_walls() rospy.loginfo("World Manager Node is Up and Running") def add_object_to_planning_scene(self, request): self.remove_all_objects_from_planner(None) #this makes sure tf is continually broadcast self.model_pose_broadcaster.add_model(request.objectname, request.pose_stamped) #remove the old completion if it is there self.scene.remove_world_object(request.objectname) #add the new object to the planning scene self.scene.add_mesh(request.objectname, request.pose_stamped, request.mesh_filepath) return [] def remove_all_objects_from_planner(self, request): body_names = self.scene.get_known_object_names() while len(body_names) > 0: print( "removing bodies from the planner, this can potentially take several tries" ) for body_name in body_names: self.scene.remove_world_object(body_name) body_names = self.scene.get_known_object_names() self.model_pose_broadcaster.clear_models() # self.add_walls() return [] def add_walls(self): walls = rospy.get_param('/walls') for wall_params in walls: rospy.loginfo("Adding wall " + str(wall_params)) self.add_wall(wall_params) def add_wall(self, wall_params): name = wall_params["name"] x_thickness = wall_params["x_thickness"] y_thickness = wall_params["y_thickness"] z_thickness = wall_params["z_thickness"] x = wall_params["x"] y = wall_params["y"] z = wall_params["z"] qx = wall_params["qx"] qy = wall_params["qy"] qz = wall_params["qz"] qw = wall_params["qw"] frame_id = wall_params["frame_id"] back_wall_pose = geometry_msgs.msg.PoseStamped() back_wall_pose.header.frame_id = '/' + frame_id wall_dimensions = [x_thickness, y_thickness, z_thickness] back_wall_pose.pose.position = geometry_msgs.msg.Point(**{ 'x': x, 'y': y, 'z': z }) back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{ 'x': qx, 'y': qy, 'z': qz, 'w': qw }) self.scene.add_box(name, back_wall_pose, wall_dimensions)