def __make_planning_scene_diff_req(collision_object): scene = PlanningScene() scene.is_diff = True scene.world.collision_objects = [collision_object] planning_scene_diff_req = ApplyPlanningSceneRequest() planning_scene_diff_req.scene = scene return planning_scene_diff_req
def add_board_object(): # Some publisher scene_diff_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=1) rospy.sleep(10.0) # Create board object board = CollisionObject() board.header.frame_id = 'base' board.id = 'board' board_box = SolidPrimitive() board_box.type = 1 # board_box.dimensions = [3.0, 4.0, 0.185] board_box.dimensions = [DEPTH_BIAS*2, 4.0, 3.0] board.primitives.append(board_box) board_pose = Pose() board_pose.position.x = transformed_message.pose.position.x board_pose.position.y = transformed_message.pose.position.y board_pose.position.z = transformed_message.pose.position.z # board_pose.orientation.x = transformed_message.pose.orientation.x board_pose.orientation.x = 0 # board_pose.orientation.y = transformed_message.pose.orientation.y board_pose.orientation.y = 0 # board_pose.orientation.z = transformed_message.pose.orientation.z board_pose.orientation.z = 0 # board_pose.orientation.w = transformed_message.pose.orientation.w board_pose.orientation.w = 0 board.primitive_poses.append(board_pose) scene = PlanningScene() scene.world.collision_objects.append(board) scene.is_diff = True scene_diff_publisher.publish(scene)
def add_invisible_collision_object(self): co_box = CollisionObject() co_box.header.frame_id = 'base_footprint' co_box.id = 'invisible_box' box = SolidPrimitive() box.type = SolidPrimitive.BOX box_height = 0.76 box.dimensions.append(0.80) box.dimensions.append(1.60) box.dimensions.append(box_height) co_box.primitives.append(box) box_pose = Pose() box_pose.position.x = 0.80 box_pose.position.y = 0.0 box_pose.position.z = box_height / 2.0 box_pose.orientation.w = 1.0 co_box.primitive_poses.append(box_pose) co_box.operation = CollisionObject.ADD color = ObjectColor() color.id = 'invisible_box' color.color.g = 1.0 color.color.a = 0.15 ps = PlanningScene() ps.world.collision_objects.append(co_box) ps.object_colors.append(color) ps.is_diff = True try: self.planning_scene_service(ps) except rospy.ServiceException, e: print("Service call failed: %s" % e)
def update_collision_object_with_pose(self, model, modelinstance_name, pose): if model: model.for_all_links(self.update_collision_object, name=modelinstance_name, pose=pose) else: return response = self.get_planning_scene(self.request) current_scene_objects = [ object.id for object in response.scene.world.collision_objects ] planning_scene_msg = PlanningScene() planning_scene_msg.is_diff = True for (collision_object_root, collision_object) in self.collision_objects_updated.iteritems(): if collision_object_root in current_scene_objects: # Object is present in the planning scene if collision_object_root in self.ignored_submodels: pass else: planning_scene_msg.world.collision_objects.append( collision_object) planning_scene_msg.world.collision_objects[ -1].header.frame_id = 'world' self.planning_scene_pub.publish(planning_scene_msg)
def sendColors(color_dict, scene): # Need to send a planning scene diff p = PlanningScene() p.is_diff = True for color in color_dict.values(): p.object_colors.append(color) scene._scene_pub.publish(p)
def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p)
def sendUpdate(self, collision_object, attached_collision_object, use_service=True): ''' Send new update to planning scene :param collision_object: A collision object to add to scene, or None :param attached_collision_object: Attached collision object to add to scene, or None :param use_service: If true, update will be sent via apply service, otherwise by topic ''' ps = PlanningScene() ps.is_diff = True if collision_object: ps.world.collision_objects.append(collision_object) if attached_collision_object: ps.robot_state.attached_collision_objects.append( attached_collision_object) if use_service: resp = self._apply_service.call(ps) if not resp.success: rospy.logerr("Could not apply planning scene diff.") else: self._scene_pub.publish(ps)
def execute_take(self, goal): feedback = HandOverFeedback() self._result = HandOverResult() self._result.success = False # Approach feedback.phase = HandOverFeedback.PHASE_APPROACH self._as_take.publish_feedback(feedback) approach_result = tiago_play_motion( self._approach_motion) # TODO check return value # if approach_result == False or approach_result.error_code != approach_result.SUCCEEDED: # Open gripper feedback.phase = HandOverFeedback.PHASE_EXECUTING self._as_take.publish_feedback(feedback) open_gripper() feedback.phase = HandOverFeedback.PHASE_WAITING_FOR_CONTACT self._as_take.publish_feedback(feedback) if self.wait_for_force_handover(self._as_take, self._threshold): self._result.success = True # Close gripper feedback.phase = HandOverFeedback.PHASE_EXECUTING self._as_take.publish_feedback(feedback) close_gripper() if len(goal.object.object.primitives) != 0 or len( goal.object.object.meshes) != 0 or len( goal.object.object.planes) != 0: rospy.loginfo("AttachedCollisionObject given, attaching now") scene = PlanningScene() scene.is_diff = True scene.robot_state.is_diff = True scene.robot_state.attached_collision_objects = [goal.object] planning_scene_diff_req = ApplyPlanningSceneRequest() planning_scene_diff_req.scene = scene rospy.loginfo( self._apply_planning_scene_diff.call( planning_scene_diff_req)) else: rospy.loginfo( "No AttachedCollisionObject given, so not attaching") rospy.sleep(self._retreat_delay) # Retreat feedback.phase = HandOverFeedback.PHASE_RETREAT self._as_take.publish_feedback(feedback) retreat_result = tiago_play_motion(self._retreat_motion, wait_duration=15.0) if retreat_result == False or retreat_result.error_code != retreat_result.SUCCEEDED: self._result.success = False # Callback Finished if self._result.success: rospy.loginfo('Succeeded') self._as_take.set_succeeded(self._result) else: self._as_take.set_aborted(self._result)
def main(): parser = argparse.ArgumentParser() #parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)') #parser.add_argument('-p', '--prefix', default='', help='Publish with prefix') parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)') args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('sdf2moveit_collision') global ignored_submodels ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';') rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels) planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) sdf = pysdf.SDF(model=args.sdf) world = sdf.world world.for_all_links(convert_to_collision_object) planning_scene_msg = PlanningScene() planning_scene_msg.is_diff = True for (collision_object_root, collision_object) in collision_objects.iteritems(): if collision_object_root in ignored_submodels: print('TODO2') # attached object instead of collision object else: planning_scene_msg.world.collision_objects.append(collision_object) planning_scene_msg.world.collision_objects[-1].header.frame_id = 'world' while planning_scene_pub.get_num_connections() < 1: rospy.sleep(0.1) planning_scene_pub.publish(planning_scene_msg)
def add_new_collision_object(self, model_name, modelinstance_name): sdf = pysdf.SDF(model=model_name) num_collision_objects = len(self.collision_objects) model = sdf.world.models[0] if len(sdf.world.models) >= 1 else None if model: model.for_all_links(self.convert_to_collision_object, name=modelinstance_name) if len(self.collision_objects) == num_collision_objects: rospy.logerr('Unable to load model: %s' % model_name) return None planning_scene_msg = PlanningScene() planning_scene_msg.is_diff = True for (collision_object_root, collision_object) in self.collision_objects.iteritems(): if collision_object_root in self.ignored_submodels: pass else: planning_scene_msg.world.collision_objects.append( collision_object) planning_scene_msg.world.collision_objects[ -1].header.frame_id = 'world' self.planning_scene_pub.publish(planning_scene_msg) rospy.loginfo('Loaded model: %s' % modelinstance_name) return model else: rospy.logerr('Unable to load model: %s' % model_name) return None
def main(): parser = argparse.ArgumentParser() #parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)') #parser.add_argument('-p', '--prefix', default='', help='Publish with prefix') parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)') args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('sdf2moveit_collision') global ignored_submodels ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';') rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels) planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) sdf = pysdf.SDF(model=args.sdf) world = sdf.world world.for_all_links(convert_to_collision_object) planning_scene_msg = PlanningScene() planning_scene_msg.is_diff = True for (collision_object_root, collision_object) in collision_objects.iteritems(): if collision_object_root in ignored_submodels: print('TODO2') # attached object instead of collision object else: planning_scene_msg.world.collision_objects.append(collision_object) planning_scene_msg.world.collision_objects[ -1].header.frame_id = 'world' while planning_scene_pub.get_num_connections() < 1: rospy.sleep(0.1) planning_scene_pub.publish(planning_scene_msg)
def sendColors(self): # Need to send a planning scene diff p = PlanningScene() p.is_diff = True for color in self._colors.values(): p.object_colors.append(color) self._scene_pub.publish(p)
def execute(self, userdata): """ Get the modified ACM and publish it in order to update the allowed collisions when moveit plans @param userdata: Input and output data that can be communicated to other states @return: - outcomes[-1] ("fail" by default) if an error occurs when modifying the ACM - outcomes[0] otherwise """ # Sanity check during execution if self.modification_type is None: rospy.logerr( "The parameter collision_type can only be {'', 'self-collision', 'object-collision'}" ) return self.outcomes[-1] # Call the service allowing to modify an updated ACM with all the interactively added objects response = self.get_modified_acm(self.modification_type, self.objects, self.allow_collision) # If anything went wrong on the service side display an error message and fail the state if not response.success: rospy.logerr( "Could not allow collisions with the provided parameters") return self.outcomes[-1] # If everything works fine, we just set the output ACM in a PlanningScene message and send it to Moveit planning_scene_diff = PlanningScene() planning_scene_diff.is_diff = True try: planning_scene_diff.allowed_collision_matrix = response.acm self.planning_scene_publisher.publish(planning_scene_diff) return self.outcomes[0] except rospy.ROSException as exception: rospy.logerr( "Can not change collision state of the scene: {}".format( exception)) return self.outcomes[-1]
def sendColors(scene): # Need to send a planning scene diff # print(_colors['table_center']) p = PlanningScene() p.is_diff = True for color in _colors.values(): p.object_colors.append(color) # print(p) scene._scene_pub.publish(p)
def send_color(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self._scene_pub.publish(p) rospy.sleep(0.1)
def sendColors(self): # 初始化规划场景对象 p = PlanningScene() # 需要设置规划场景是否有差异 p.is_diff = True # 从颜色字典中取出颜色设置 for color in self.colors.values(): p.object_colors.append(color) # 发布场景物体颜色设置 self.scene_pub.publish(p)
def callback(self, msg): rospy.loginfo("PlanningSceneUpdater received map update") scene_update = PlanningScene() scene_update.is_diff = True scene_update.world.octomap.octomap = msg scene_update.world.octomap.header.stamp = rospy.Time.now() scene_update.world.octomap.header.frame_id = "world" self.scene_pub.publish(scene_update) rospy.loginfo("PlanningSceneUpdater published map update")
def sendColors(self): # Need to send a planning scene diff p = PlanningScene() p.is_diff = True for color in self._colors.values(): p.object_colors.append(color) resp = self._apply_service.call(p) if not resp.success: rospy.logerr("Could not update colors through service, using topic instead.") self._scene_pub.publish(p)
def broadcast_diff(self, objects): planning_scene = PlanningScene() planning_scene.is_diff = True if isinstance(objects, collections.Iterable): for ob in objects: co = self._create_collision_object(ob) planning_scene.world.collision_objects.append(co) else: co = self._create_collision_object(objects) planning_scene.world.collision_objects.append(co) self._diff_publisher.publish(planning_scene)
def __make_planning_scene_diff_req(collision_object, attach=False): scene = PlanningScene() scene.is_diff = True scene.robot_state.is_diff = True if attach: scene.robot_state.attached_collision_objects = [collision_object] else: scene.world.collision_objects = [collision_object] planning_scene_diff_req = ApplyPlanningSceneRequest() planning_scene_diff_req.scene = scene return planning_scene_diff_req
def remove_invisible_collision_object(self): co_box = CollisionObject() co_box.id = 'invisible_box' co_box.operation = CollisionObject.REMOVE ps = PlanningScene() ps.world.collision_objects.append(co_box) ps.is_diff = True try: self.planning_scene_service(ps) except rospy.ServiceException, e: print("Service call failed: %s" % e)
def spawnObjects(objects): scene_diff = PlanningScene() scene_diff.world.collision_objects = objects scene_diff.is_diff = True rospy.wait_for_service('/apply_planning_scene') try: apply = rospy.ServiceProxy('/apply_planning_scene', ApplyPlanningScene) resp = apply(scene_diff) return resp except rospy.ServiceException, e: print "Service call failed: %s" % e
def send_meshes_cb(self, *args): planning_scene_msg = PlanningScene() planning_scene_msg.is_diff = self.moveit_planning_scene_msg.is_diff planning_scene_msg.allowed_collision_matrix = self.moveit_planning_scene_msg.allowed_collision_matrix planning_scene_msg.name = self.moveit_planning_scene_msg.name for collision_object in self.moveit_planning_scene_msg.world.collision_objects: mesh_id = collision_object.id if self.mesh_list_vars[mesh_id].get() == '1': planning_scene_msg.world.collision_objects.append(collision_object) self.refined_planning_scene_publisher.publish(planning_scene_msg)
def remove_object_from_acm(self, object_name): req = PlanningSceneComponents( components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX | PlanningSceneComponents.SCENE_SETTINGS)) res = self.get_planning_scene(req) acm = res.scene.allowed_collision_matrix planning_scene_diff = PlanningScene() planning_scene_diff.is_diff = True acm = remove_from_acm(acm, object_name) planning_scene_diff.allowed_collision_matrix = acm self.planning_scene_pub.publish(planning_scene_diff)
def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
def sendColors(self): # Initialize scene object p = PlanningScene() # Declare if scene is different p.is_diff = True # Obtain color value from dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish scene object color setting self.scene_pub.publish(p)
def cb(self, msg): psw = PlanningSceneWorld() psw.octomap.header.stamp = rospy.Time.now() psw.octomap.header.frame_id = 'world' psw.octomap.octomap = msg psw.octomap.origin.position.x = 0 psw.octomap.origin.orientation.w = 1 ps = PlanningScene() ps.world = psw ps.is_diff = True self.mapMsg = ps
def add_object_to_acm(self, object_name): req = PlanningSceneComponents( components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX | PlanningSceneComponents.SCENE_SETTINGS)) res = self.get_planning_scene(req) acm = res.scene.allowed_collision_matrix planning_scene_diff = PlanningScene(is_diff=True) acm = add_to_acm(acm, object_name) planning_scene_diff.allowed_collision_matrix = acm self.planning_scene_pub.publish(planning_scene_diff) rospy.sleep(2)
def add_planning_scene(card_pose): # Use the planning scene object to add or remove objects //Interface REFERENCE_FRAME = '/world' p = PlanningScene() p.is_diff = True # Create a scene publisher to push changes to the scene //PlanningScene scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene, queue_size=50) # Give each of the scene objects a unique name Ground_id = 'ground' Card_id = 'card' # Object2_id = 'box2' # Remove leftover objects from a previous run # scene.remove_world_object(Ground_id) # scene.remove_world_object(Card_id) # scene.remove_world_object(Object2_id) # scene.remove_world_object(target_id) # add ground into the scene pose_Ground = geometry_msgs.msg.PoseStamped() pose_Ground.header.frame_id = REFERENCE_FRAME pose_Ground.pose.position.x = -2.5 pose_Ground.pose.position.y = -2.5 pose_Ground.pose.position.z = -0.01 scene.add_mesh(Ground_id, pose_Ground, '/home/sslrayray/table.stl') # add card into the scene pose_card = geometry_msgs.msg.PoseStamped() pose_card.header.frame_id = REFERENCE_FRAME pose_card.pose.position.x = card_pose[0] pose_card.pose.position.y = card_pose[1] pose_card.pose.position.z = card_pose[2] pose_card.pose.orientation = geometry_msgs.msg.Quaternion( *tf_conversions.transformations.quaternion_from_euler( card_pose[3], card_pose[4], card_pose[5])) scene.add_mesh(Card_id, pose_card, '/home/sslrayray/card.stl') scene.add_mesh(Ground_id, pose_Ground, '/home/sslrayray/table.stl') scene_pub.publish(p) rospy.sleep(1) table_pub.publish('on')
def sendColors(self): ''' Send the colors set using 'setColor' to moveit. ''' p = PlanningScene() p.is_diff = True for color in self._colors.values(): p.object_colors.append(color) resp = self._apply_service.call(p) if not resp.success: rospy.logerr( "Could not update colors through service, using topic instead." ) self._scene_pub.publish(p)
def _apply_scene_diff(self, items): scene = PlanningScene() scene.is_diff = True scene.robot_state.is_diff = True for item in items: if isinstance(item, CollisionObject): scene.world.collision_objects.append(item) elif isinstance(item, AttachedCollisionObject): scene.robot_state.attached_collision_objects.append(item) elif isinstance(item, ObjectColor): scene.object_colors.append(item) resp = self._apply_planning_scene(scene) if not resp.success: raise RuntimeError("Could not apply planning scene diff.")
def sendUpdate(self, collision_object, attached_collision_object, use_service=True): ps = PlanningScene() ps.is_diff = True if collision_object: ps.world.collision_objects.append(collision_object) if attached_collision_object: ps.robot_state.attached_collision_objects.append(attached_collision_object) if use_service: resp = self._apply_service.call(ps) if not resp.success: rospy.logerr("Could not apply planning scene diff.") else: self._scene_pub.publish(ps)