def interactive_gripper_marker(pose_stamped, finger_distance):
# gripper marker
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000]
gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(CLOSE_FINGER_POS,
min(finger_distance, OPEN_FINGER_POS))
# left finger marker
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = -finger_distance / 2.0
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
# right finger marker
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = finger_distance / 2.0
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.3
return interactive_marker
def gripper_interactive_marker(pose_stamped, finger_distance):
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166
gripper_marker.pose.orientation.w = 1
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(0, min(finger_distance, 0.1))
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = finger_distance / 2.0 - 0.1
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = -finger_distance / 2.0 + 0.1
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.25
return interactive_marker
def add_marker(x_distance, y_distance, ps, camera_name, self):
controls = InteractiveMarkerControl()
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.05
box_marker.scale.y = 0.05
box_marker.scale.z = 0.05
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header = ps.header
int_marker.name = "click_location"
if camera_name == 'camera1':
int_marker.pose.position.z = ps.pose.position.z
int_marker.pose.position.x = transform_broadcaster_mapping['camera1'][0][0] + x_distance
int_marker.pose.position.y = transform_broadcaster_mapping['camera1'][0][1] - y_distance
elif camera_name == 'camera2':
int_marker.pose.position.y = ps.pose.position.y
int_marker.pose.position.x = transform_broadcaster_mapping['camera2'][0][0] + x_distance
int_marker.pose.position.z = transform_broadcaster_mapping['camera2'][0][2] - y_distance
elif camera_name == 'camera3':
int_marker.pose.position.x = ps.pose.position.x
int_marker.pose.position.y = transform_broadcaster_mapping['camera3'][0][1] + x_distance
int_marker.pose.position.z = transform_broadcaster_mapping['camera3'][0][2] - y_distance
else:
raise ValueError('Did not pass in a valid camera_name')
int_marker.pose.orientation.w = 1
controls.markers.append(box_marker)
int_marker.controls.append(controls)
self._im_server.insert(int_marker)
self._im_server.applyChanges()
print("Marker changes should now be applied")
