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 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 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 __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 __init__(self):
rospy.init_node('moveit_octomap_handler')
rospy.Subscriber('/octomap_full_static',
Octomap,
self.sub_octomap,
queue_size=10)
rospy.Subscriber('/planning_scene/update',
Bool,
self.sub_sw,
queue_size=1)
pub_planning_scene = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
r = rospy.Rate(10)
# Setup about planning scene
self.ps = PlanningScene()
self.ps.world.octomap.header.frame_id = '/map'
self.ps.is_diff = True
self.sw = True
while not rospy.is_shutdown():
if (self.mapMsg is not None):
pub_planning_scene.publish(self.mapMsg)
# print("Publish OctomapMessage to PlanningScene\n")
else:
print("pass")
pass
r.sleep()
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(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(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 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 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 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(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):
# 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 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):
# 初始化规划场景对象
p = PlanningScene()
# 需要设置规划场景是否有差异
p.is_diff = True
# 从颜色字典中取出颜色设置
for color in self.colors.values():
p.object_colors.append(color)
# 发布场景物体颜色设置
self.scene_pub.publish(p)
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo(
"waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
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 reset_box_scene(self):
"""
Sets up the scene for BoxTest.
"""
self.diff = PlanningScene()
self.diff.is_diff = True
rospy.loginfo("BoxAdder: reset_box_scene: starting")
self._add_box_scene()
rospy.loginfo("BoxAdder: reset_box_scene: added box scene")
self._set_planning_scene()
rospy.loginfo("BoxAdder: reset_box_scene: set 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 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 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 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 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 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 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 set_table_scene(self, new_positions):
"""
Sets up the scene for TableTest.
"""
height = SMALL_BOX_SIZE
side = SMALL_BOX_SIZE
self.diff = PlanningScene()
self.diff.is_diff = True
self._add_static_table_boxes()
for i in xrange(len(new_positions)):
self._add_box("box" + str(i), new_positions[i][0],
new_positions[i][1], new_positions[i][2], side, side,
height)
self._set_planning_scene()
