def remove_from_acm(acm, object_name):
# in python3 compiled version moveit, acm.default_entry_values
# and acm.entry_values return map object
new_acm = AllowedCollisionMatrix()
new_acm.entry_names = acm.entry_names
entry_values = list(acm.entry_values)
for entry in entry_values:
new_entry = AllowedCollisionEntry()
new_entry.enabled = list(entry.enabled)
new_acm.entry_values.append(new_entry)
if object_name not in new_acm.entry_names:
new_acm.entry_names.append(object_name)
object_entry = AllowedCollisionEntry()
for i in range(len(new_acm.entry_values) + 1):
object_entry.enabled.append(False)
new_acm.entry_values.append(object_entry)
for entry in new_acm.entry_values:
entry.enabled.append(False)
else:
index = new_acm.entry_names.index(object_name)
for i in range(len(new_acm.entry_names)):
new_acm.entry_values[index].enabled[i] = False
for entry in new_acm.entry_values:
entry.enabled[index] = False
new_acm.default_entry_names = acm.default_entry_names
new_acm.default_entry_values = list(acm.default_entry_values)
return new_acm
def __init__(self, name, robot_state):
self.drone_name = name
self.current_state = robot_state
self.scene_publisher = rospy.Publisher(
'/{}/planning_scene'.format(name), PlanningScene, queue_size=10)
self.scene_msg = PlanningScene()
self.scene_msg.name = "Drone_scene"
self.scene_msg.world.octomap.header.frame_id = '{}/map'.format(name)
self.scene_msg.world.octomap.origin = Pose()
self.scene_msg.allowed_collision_matrix.entry_names = [
"base_link", "camera_link", "sonar_link"
]
self.scene_msg.allowed_collision_matrix.entry_values.append(
AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[0].enabled = [
False, False, True
]
self.scene_msg.allowed_collision_matrix.entry_values.append(
AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[1].enabled = [
False, False, False
]
self.scene_msg.allowed_collision_matrix.entry_values.append(
AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[2].enabled = [
True, False, False
]
self.odometry_me = Condition()
self.octomap_me = Condition()
# Subscribe to octomap topic
self.octomap = None
self.octo_sub = rospy.Subscriber("/{}/octomap_binary".format(
self.drone_name),
Octomap,
self.octomap_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
self.odometry = None
rospy.Subscriber("/{}/ground_truth/state".format(self.drone_name),
Odometry, self.poseCallback)
def __init__(self, name):
self.drone_name = name
self.scene_publisher = rospy.Publisher('/{}/planning_scene'.format(name), PlanningScene, queue_size=10)
self.scene_msg = PlanningScene()
self.scene_msg.name = "Drone_scene"
self.scene_msg.world.octomap.header.frame_id = '{}/map'.format(name)
self.scene_msg.world.octomap.origin = Pose()
self.scene_msg.allowed_collision_matrix.entry_names = ["base_link", "camera_link", "sonar_link"]
self.scene_msg.allowed_collision_matrix.entry_values.append(AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[0].enabled = [False,False,True]
self.scene_msg.allowed_collision_matrix.entry_values.append(AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[1].enabled = [False,False,False]
self.scene_msg.allowed_collision_matrix.entry_values.append(AllowedCollisionEntry())
self.scene_msg.allowed_collision_matrix.entry_values[2].enabled = [True,False,False]
def allow_collision(self):
# self.pubPlanningScene = rospy.Publisher("planning_scene", PlanningScene)
ps = PlanningScene()
psc = PlanningSceneComponents()
acm = AllowedCollisionMatrix()
ace = AllowedCollisionEntry()
is_allow = Bool()
is_allow.data = False
ace.enabled = [False]
# get scene components
# psc.ALLOWED_COLLISION_MATRIX
getScene = self.gps(psc) # make it empty for now
# ps = getScene
ps.allowed_collision_matrix.entry_names = ["cube"]
ps.allowed_collision_matrix.entry_values = [ace]
ps.allowed_collision_matrix.default_entry_names = ["cube"]
ps.allowed_collision_matrix.default_entry_values = [1]
ps.is_diff = 1
# print "gripper name", self.gripper
applyScene = self.aps(ps)
print ps