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 __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_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 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 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 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 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)
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)
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)
