class AnnotatorServer(object):
def __init__(self):
self._annotator = Annotator()
self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names",
PoseNames, queue_size=10, latch=True)
# self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses",
# InteractiveMarker, queue_size=10, latch=True)
self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses")
self.INITIAL_POSE = Pose()
self.INITIAL_POSE.orientation.w = 1
print("Initializing saved markers: " +
str(self._annotator.get_position_names()))
for name, pose in self._annotator.get_position_items():
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
print("Initialization finished...")
def __pub_pose_info__(self):
pose_names = PoseNames()
pose_names.names = self._annotator.get_position_names()
pose_names.names.sort()
self._pose_names_pub.publish(pose_names)
def __update_marker_pose__(self, input):
if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP):
name = input.marker_name
new_pose = self._int_marker_server.get(name).pose
# Overwrite the previous pose with the new pose
self._annotator.save_position(name, new_pose)
self._int_marker_server.setPose(name, new_pose)
self._int_marker_server.applyChanges()
print("updated pose for: " + name)
def __create_int_marker__(self, name, pose):
print("creating int marker: " + name)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
# Move it 0.25 meters up to make it easier to click
int_marker.pose.position.z = 0.25
text_marker = Marker()
text_marker.text = name
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.position.z = 2
text_marker.scale.x = 0.4
text_marker.scale.y = 0.4
text_marker.scale.z = 0.4
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1.0
text_control = InteractiveMarkerControl()
text_control.name = "text_control"
text_control.markers.append(text_marker)
text_control.always_visible = True
text_control.interaction_mode = InteractiveMarkerControl.NONE
int_marker.controls.append(text_control)
rotation_ring_control = InteractiveMarkerControl()
rotation_ring_control.name = "position_control"
rotation_ring_control.always_visible = True
rotation_ring_control.orientation.x = 0
rotation_ring_control.orientation.w = 1
rotation_ring_control.orientation.y = 1
rotation_ring_control.orientation.z = 0
rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(rotation_ring_control)
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.pose.position.z = 0.15
arrow_marker.pose.position.x = -0.5
arrow_marker.scale.x = 1
arrow_marker.scale.y = 0.25
arrow_marker.scale.z = 0.25
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
position_control = InteractiveMarkerControl()
position_control.name = "rotation_control"
position_control.always_visible = True
position_control.markers.append(arrow_marker)
position_control.orientation.w = 1
position_control.orientation.x = 0
position_control.orientation.y = 1
position_control.orientation.z = 0
position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(position_control)
self._int_marker_server.insert(int_marker, self.__update_marker_pose__)
self._int_marker_server.applyChanges()
def create(self, name, pose=None):
print("creating new pose: " + name)
if pose is None:
pose = self.INITIAL_POSE
self._annotator.save_position(name, pose)
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
def delete(self, name):
if self._annotator.exists(name):
print("deleting pose: " + name)
self._annotator.delete_position(name)
self._int_marker_server.erase(name)
self._int_marker_server.applyChanges()
self.__pub_pose_info__()
def handle_callback(self, user_action_msg):
cmd = user_action_msg.command
name = user_action_msg.name
if cmd == UserAction.CREATE:
self.create(name)
elif cmd == UserAction.DELETE:
self.delete(name)
elif cmd == UserAction.GOTO:
print("going to pose: " + name)
self._annotator.goto_position(name)
class MeasurementVisServer(object):
def __init__(self):
self.marker_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=50)
self.positions_pub = rospy.Publisher('object_positions', ObjectMeasurement, queue_size=50)
self.init_positions_pub = rospy.Publisher('object_initialization_positions', ObjectMeasurement, queue_size=50)
self.target_pub = rospy.Publisher('get_target_poses', PoseArray, queue_size=50)
#self.object_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=10)'
self.nbr_targets = rospy.get_param('~number_targets', 2)
self.nbr_noise = rospy.get_param('~number_noise', 4)
self.markers = MarkerArray()
self.object_counters = np.zeros((self.nbr_targets,), dtype=int)
self.initialized = np.zeros((self.nbr_targets,), dtype=bool)
self.marker_server = InteractiveMarkerServer("object_interactive_markers")
#self.room_server = InteractiveMarkerServer("room_interactive_markers")
self.marker_poses = [Pose() for j in range(0, self.nbr_targets)]
self.previous_poses = [Pose() for j in range(0, self.nbr_targets)]
self.did_move = np.zeros((self.nbr_targets,), dtype=bool)
self.marker_times = np.zeros((self.nbr_targets,), dtype=np.int64)
self.marker_clicked_times = np.zeros((self.nbr_targets,), dtype=np.int64)
self.regions, self.centers = get_regions()
self.room_time = 0
self.room_id = 0
for i, region in enumerate(self.regions):
print self.centers[i]
#self.clear_markers()
self.timestep = 0
self.measurement_counter = 0
self.clear_markers()
#rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_poses)
rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_rooms)
rospy.Subscriber("filter_measurements", ObjectMeasurement, self.callback)
rospy.Subscriber("sim_filter_measurements", ObjectMeasurement, self.callback)
rospy.Subscriber("set_target_poses", PoseArray, self.set_target_poses)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.clicked_callback)
self.initialize_room_markers()
def set_target_poses(self, poses):
for j, p in enumerate(poses.poses):
if p.position.x == 0 and p.position.x == 0:
continue
if not self.initialized[j]:
self.initialized[j] = True
self.initialize_object_marker(j, p)
else:
name = "object_marker_" + str(j)
p.position.z = 0.15
self.marker_poses[j] = p
self.marker_server.setPose(name, p)
self.marker_server.applyChanges()
def publish_target_poses(self):
poses = PoseArray()
for j in range(0, self.nbr_targets):
poses.poses.append(self.marker_poses[j])
self.target_pub.publish(poses)
def maybe_publish_rooms(self, event):
self.publish_target_poses()
seconds = rospy.Time.now().to_sec()
for j in range(0, self.nbr_targets):
mtime = self.marker_clicked_times[j]
if mtime != 0 and seconds - mtime > 1:
print "Publishing initial position for object ", j
self.marker_clicked_times[j] = 0
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose = self.marker_poses[j]
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = j
object_pos.timestep = 0
object_pos.observation_id = 0
object_pos.negative_observation = False
self.init_positions_pub.publish(object_pos)
seconds = rospy.Time.now().to_sec()
if self.room_time == 0 or seconds - self.room_time < 1:
return
room = self.regions[self.room_id]
# ok, time to see if we have any objects within this region:
vertices = np.zeros((len(room.posearray.poses), 2), dtype=float)
for i, pose in enumerate(room.posearray.poses):
vertices[i] = np.array([pose.position.x, pose.position.y])
#hull = ConvexHull(vertices)
#print vertices
#if not isinstance(hull, Delaunay):
# hull = Delaunay(hull)
hull = Delaunay(vertices)
#hull = Delaunay(ConvexHull(vertices))
published = False
shuffled = np.arange(self.nbr_targets)
np.random.shuffle(shuffled)
for j in shuffled:#range(0, self.nbr_targets):
if not self.initialized[j]:
continue
pose = [self.marker_poses[j].position.x, self.marker_poses[j].position.y]
if hull.find_simplex(pose) >= 0:
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose = self.marker_poses[j]
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = j
object_pos.timestep = self.timestep
object_pos.observation_id = self.measurement_counter
object_pos.location_id = self.room_id
object_pos.negative_observation = False
self.measurement_counter += 1
self.positions_pub.publish(object_pos)
self.room_time = 0
published = True
# if self.did_move[j]:
# pose = [self.previous_poses[j].position.x, self.previous_poses[j].position.y]
# if hull.find_simplex(pose) >= 0:
# print "Previous pose was inside, PUBLISHING!"
# #self.did_move[j] = False # need to fix a history?
# #self.previous_poses[j] = self.marker_poses[j]
# pose = PoseStamped()
# pose.header.frame_id = "map"
# pose.header.stamp = rospy.Time.now()
# pose.pose = self.previous_poses[j]
# object_pos = ObjectMeasurement()
# object_pos.pose = pose
# object_pos.initialization_id = j
# object_pos.timestep = self.timestep
# object_pos.observation_id = self.measurement_counter
# object_pos.location_id = self.room_id
# object_pos.negative_observation = True
# self.positions_pub.publish(object_pos)
# published = True
# compute min an max vertices in x and y to be able to sample uniform
# then use rejection sampling to get points within the polygon
maxs = np.amax(vertices, axis=0)
mins = np.amin(vertices, axis=0)
for i in range(0, self.nbr_noise):
while True:
x = random.uniform(mins[0], maxs[0])
y = random.uniform(mins[1], maxs[1])
pose = [x, y]
if hull.find_simplex(pose) >= 0:
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.w = 1.
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = -1
object_pos.timestep = self.timestep
object_pos.observation_id = self.measurement_counter
object_pos.location_id = self.room_id
object_pos.negative_observation = False
self.measurement_counter += 1
self.positions_pub.publish(object_pos)
self.room_time = 0
published = True
break
if published:
self.timestep = self.timestep + 1
def maybe_publish_poses(self, event):
seconds = rospy.Time.now().to_sec()
for j in range(0, self.nbr_targets):
mtime = self.marker_times[j]
#if mtime != 0:
# print "Time diff for ", j , " is: ", mtime - seconds
if mtime != 0 and seconds - mtime > 1:
self.did_move[j] = True
self.marker_times[j] = 0
#pose = PoseStamped()
#pose.header.frame_id = "map"
#pose.header.stamp = rospy.Time.now()
#pose.pose = self.marker_poses[j]
#object_pos = ObjectMeasurement()
#object_pos.pose = pose
#object_pos.initialization_id = j
#object_pos.observation_id = self.measurement_counter
#self.measurement_counter += 1
#self.positions_pub.publish(object_pos)
#self.marker_times[j] = 0
def object_id_color(self, object_id):
colors = {"vivid_yellow": (255, 179, 0),
"strong_purple": (128, 62, 117),
"vivid_orange": (255, 104, 0),
"very_light_blue": (166, 189, 215),
"vivid_red": (193, 0, 32),
"grayish_yellow": (206, 162, 98),
"medium_gray": (129, 112, 102),
# these aren't good for people with defective color vision:
"vivid_green": (0, 125, 52),
"strong_purplish_pink": (246, 118, 142),
"strong_blue": (0, 83, 138),
"strong_yellowish_pink": (255, 122, 92),
"strong_violet": (83, 55, 122),
"vivid_orange_yellow": (255, 142, 0),
"strong_purplish_red": (179, 40, 81),
"vivid_greenish_yellow": (244, 200, 0),
"strong_reddish_brown": (127, 24, 13),
"vivid_yellowish_green": (147, 170, 0),
"deep_yellowish_brown": (89, 51, 21),
"vivid_reddish_orange": (241, 58, 19),
"dark_olive_green": (35, 44, 22)}
color = np.array(colors[colors.keys()[object_id]], dtype=float) / 255.0
return color
def clear_markers(self):
clear_markers = MarkerArray()
pose = Pose()
pose.position.x = 0.
pose.position.y = 0.
pose.position.z = 0.
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
for i in range(0, 1000):
clear_marker = Marker()
clear_marker.header.frame_id = "map"
clear_marker.header.stamp = rospy.Time.now()
clear_marker.type = Marker.SPHERE
clear_marker.action = Marker.DELETE
clear_marker.ns = "my_namespace"
clear_marker.id = i
clear_marker.pose = pose
#clear_marker.lifetime = rospy.Time(0)
clear_markers.markers.append(clear_marker)
self.marker_pub.publish(clear_markers)
def initialize_room_markers(self):
for room_id in range(0, len(self.regions)):
pose = Pose()
pose.position.x = self.centers[room_id, 0]
pose.position.y = self.centers[room_id, 1]
pose.position.z = 0.2
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "room_marker_" + str(room_id)
marker.description = "Room " + str(room_id)
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.35
box_marker.scale.y = 0.35
box_marker.scale.z = 0.35
box_marker.color.r = 0.
box_marker.color.g = 0.
box_marker.color.b = 1.
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
#marker.controls.append(box_control)
# move x
#control = InteractiveMarkerControl()
#control.orientation.w = 1
#control.orientation.x = 0
#control.orientation.y = 1
#control.orientation.z = 0
#control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.marker_server.insert(marker, self.room_feedback)
self.marker_server.applyChanges()
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
def room_feedback(self, feedback):
room_id = int(feedback.marker_name.rsplit('_', 1)[-1])
print "Room id: ", room_id
self.room_id = room_id
self.room_time = rospy.Time.now().to_sec()
def initialize_object_marker(self, object_id, pose):
print "Adding interactive marker for object: ", object_id
color = self.object_id_color(object_id)
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "object_marker_" + str(object_id)
marker.description = "Object " + str(object_id)
click_marker = InteractiveMarker()
click_marker.header.frame_id = "map"
click_marker.name = "button_object_marker_" + str(object_id)
click_marker.description = ""
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.25
box_marker.scale.y = 0.25
box_marker.scale.z = 0.25
box_marker.color.r = color[0]
box_marker.color.g = color[1]
box_marker.color.b = color[2]
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
# move x
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = "move_plane"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
marker.controls.append(control)
self.marker_poses[object_id] = pose
self.previous_poses[object_id] = pose
self.marker_server.insert(marker, self.marker_feedback)
self.marker_server.applyChanges()
pose.position.z = 0.15
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
def marker_feedback(self, feedback):
if feedback.control_name == "button":
object_id = int(feedback.marker_name.rsplit('_', 1)[-1])
self.marker_clicked_times[object_id] = rospy.Time.now().to_sec()
print "Marker id:", feedback.marker_name
print "Object id: ", object_id
elif feedback.control_name == "move_plane":
#self.in_feedback=True
#vertex_name = feedback.marker_name.rsplit('-', 1)
object_id = int(feedback.marker_name.rsplit('_', 1)[-1])
print "Marker id: ", object_id
#self.topo_map.update_node_vertex(node_name, vertex_index, feedback.pose)
#self.update_needed=True
# just do something if there has been no updates for the
# last x seconds
feedback.pose.position.z = 0.15
self.marker_poses[object_id] = feedback.pose
self.marker_times[object_id] = rospy.Time.now().to_sec()
def clicked_callback(self, clicked_pose):
non_initialized = np.nonzero(self.initialized == False)[0]
if len(non_initialized) == 0:
return
ind = non_initialized[0]
self.initialized[ind] = True
self.initialize_object_marker(ind, clicked_pose.pose.pose)
def callback(self, clicked_pose):
if clicked_pose.initialization_id != -1 and not self.initialized[clicked_pose.initialization_id]:
self.initialized[clicked_pose.initialization_id] = True
self.initialize_object_marker(clicked_pose.initialization_id, clicked_pose.pose.pose)
print "Got measurement, adding to GMMPoses ", clicked_pose.initialization_id
color = self.object_id_color(clicked_pose.initialization_id)
sphere_marker = Marker()
sphere_marker.header.frame_id = "map"
sphere_marker.header.stamp = rospy.Time.now()
sphere_marker.ns = "my_namespace"
sphere_marker.id = len(self.markers.markers)
sphere_marker.type = Marker.SPHERE
sphere_marker.action = Marker.ADD
sphere_marker.pose.position.x = clicked_pose.pose.pose.position.x
sphere_marker.pose.position.y = clicked_pose.pose.pose.position.y
sphere_marker.pose.position.z = 0.2
sphere_marker.pose.orientation.x = 0.
sphere_marker.pose.orientation.y = 0.
sphere_marker.pose.orientation.z = 0.
sphere_marker.pose.orientation.w = 1.
sphere_marker.scale.x = 0.2
sphere_marker.scale.y = 0.2
sphere_marker.scale.z = 0.2
sphere_marker.color.a = 1. # Don't forget to set the alpha!
sphere_marker.color.r = color[0]
sphere_marker.color.g = color[1]
sphere_marker.color.b = color[2]
#sphere_marker.lifetime = rospy.Time(secs=1000)
text_marker = Marker()
text_marker.header.frame_id = "map"
text_marker.header.stamp = rospy.Time.now()
text_marker.ns = "my_namespace"
text_marker.id = len(self.markers.markers)+1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = clicked_pose.pose.pose.position.x
text_marker.pose.position.y = clicked_pose.pose.pose.position.y
text_marker.pose.position.z = 0.4
text_marker.pose.orientation.x = 0.
text_marker.pose.orientation.y = 0.
text_marker.pose.orientation.z = 0.
text_marker.pose.orientation.w = 1.
text_marker.scale.z = 0.2
text_marker.color.a = 1. # Don't forget to set the alpha!
text_marker.color.r = 0.
text_marker.color.g = 0.
text_marker.color.b = 0.
if clicked_pose.negative_observation:
text_marker.text = "Negative " + str(self.object_counters[clicked_pose.initialization_id])
else:
text_marker.text = str(clicked_pose.timestep) #str(self.object_counters[clicked_pose.initialization_id])
#text_marker.lifetime = rospy.Time(secs=1000)
if clicked_pose.initialization_id != -1: # noise!
self.object_counters[clicked_pose.initialization_id] += 1
self.markers.markers.append(sphere_marker)
self.markers.markers.append(text_marker)
self.marker_pub.publish(self.markers)
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 1
control.orientation.z = 1
control.name = "move_3D"
control.always_visible = True
control.markers.append(make_sphere())
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
server.insert(int_marker, process_feedback)
server.applyChanges()
# main loop
while not rospy.is_shutdown():
publish_target_pose()
if has_error:
reset_marker_pose_blocking()
publish_target_pose()
server.setPose("centering_frame_pose", marker_pose.pose)
server.applyChanges()
rospy.sleep(0.5)
rospy.sleep(0.1)
class CircleMarker(object):
def __init__(self):
self._server = InteractiveMarkerServer("simple_marker")
def callback_marker(self, input):
pass
def align_marker(self, feedback):
pose = feedback.pose
self._server.setPose(feedback.marker_name, pose)
print("Arrow Marker's name is ", feedback.marker_name)
self._server.setPose("orientation_ring", pose)
self._server.applyChanges()
def orientation_ring_callback(self, feedback):
if (feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE):
print("Updating arrow marker's position ")
self._server.setPose("int_arrow_marker", feedback.pose)
self._server.applyChanges()
def create_draggable_marker(self, position):
int_arrow_marker = InteractiveMarker()
int_arrow_marker.header.frame_id = "map"
int_arrow_marker.name = "int_arrow_marker"
int_arrow_marker.description = "right"
int_arrow_marker.pose.position.y = position.y
int_arrow_marker.pose.position.x = position.x
int_arrow_marker.pose.position.z = position.z + 0.1
int_arrow_marker.scale = 1
arrow_marker = Marker()
arrow_marker.ns = "arrow_marker"
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 0
arrow_marker.scale.x = 0.5
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
button_control.always_visible = True
button_control.orientation.w = 1
button_control.orientation.x = 0
button_control.orientation.y = 1
button_control.orientation.z = 0
button_control.markers.append(arrow_marker)
int_arrow_marker.controls.append(button_control)
orientation_ring_marker = InteractiveMarker()
orientation_ring_marker.header.frame_id = "map"
orientation_ring_marker.scale = 1
orientation_ring_marker.pose.position.y = position.y
orientation_ring_marker.pose.position.z = position.z + 0.1
orientation_ring_marker.pose.position.x = position.x
orientation_ring_marker.name = "orientation_ring"
orientation_ring_marker.description = "Orientation Ring"
orientation_ring_marker_control = InteractiveMarkerControl()
orientation_ring_marker_control.always_visible = True
orientation_ring_marker_control.orientation.w = 1
orientation_ring_marker_control.orientation.x = 0
orientation_ring_marker_control.orientation.y = 1
orientation_ring_marker_control.orientation.z = 0
orientation_ring_marker_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
orientation_ring_marker.controls.append(orientation_ring_marker_control)
self._server.insert(orientation_ring_marker, self.orientation_ring_callback)
self._server.insert(int_arrow_marker, self.callback_marker)
print"inserted"
self._server.setCallback("int_arrow_marker", self.align_marker, InteractiveMarkerFeedback.POSE_UPDATE)
self._server.applyChanges()
class PR2TrajectoryMarkers(object):
"""
A class to create and store a trajectory for one PR2 arm. The created
trajectory can be published as a PoseArray message.
This class published on the following topics:
~trajectory_markers are the main interactive markers.
~trajectory_poses a markerarray to display the trajectory.
~trajectory_poses a posesarray with the resulting pose
The class subscribes to the topic ~overwrite_trajectory_
to change the stored trajectory. This is useful to resume working on a
trajectory after re-starting the node. The message type is PoseArray.
A std_srvs/Empty service named ~execute_trajectory is provided to
externally trigger the execution of the trajectory.
Constructor:
TrajectoryMarkers(whicharm = "left")
or
TrajectoryMarkers(whicharm = "right")
"""
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses",
PoseArray)
self.gripper_pose_pub = rospy.Publisher("~gripper_pose",
PoseStamped)
rospy.Subscriber("~overwrite_trajectory",
PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker);
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
self.planning_waiting_time = 10.0
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head",
callback = self.move_head)
menu_handler.insert("Add position to trajectory",
callback = self.add_point)
menu_handler.insert("Place marker over gripper",
callback = self.place_gripper)
menu_handler.insert("Execute trjectory",
callback = self.execute_trajectory)
menu_handler.insert("Clear trajectory",
callback = self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback = self.publish_trajectory)
menu_handler.insert("Move the arm (planning)",
callback = self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback = self.collision_free_arm)
menu_handler.insert("Move the arm to trajectory start (collision-free)",
callback = self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback = self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def main_callback(self, feedback):
"""
If the mouse button is released change the gripper color according to
the IK result. Quite awkward, trying to get a nicer way to do it.
"""
#publish the gripper pose
gripper_pos = PoseStamped()
gripper_pos.header.frame_id = feedback.header.frame_id
gripper_pos.pose = feedback.pose
self.gripper_pose_pub.publish(gripper_pos)
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.last_gripper_pose = feedback.pose
if (self.last_gripper_pose and
feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP):
self.last_gripper_pose = None
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
color = None
else:
color = (1,0,0,1)
int_marker = self.server.get(self.int_marker.name)
int_marker.controls.remove(self.gripper_marker)
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#rospy.loginfo("Control: %s", int_marker.controls)
self.server.insert(int_marker, self.main_callback)
self.server.setPose(int_marker.name, self.last_gripper_pose)
self.server.applyChanges()
def overwrite_trajectory(self, msg):
self.trajectory = msg
def add_point(self, feedback):
"""
Add a point to self.trajectory if it is allowed by IK.
"""
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
rospy.loginfo("Pose is reachable")
self.trajectory.poses.append(feedback.pose)
else:
rospy.logerr("Pose is not reachable!")
def place_gripper(self, feedback):
"""
Move the marker where the gripper is
"""
if self.whicharm == "right":
gripper_pos = self.planner.get_right_gripper_pose()
else:
gripper_pos = self.planner.get_left_gripper_pose()
self.server.setPose(self.int_marker.name, gripper_pos.pose,
gripper_pos.header)
self.server.applyChanges()
def execute_trajectory(self, feedback):
"""
Executes the tracjectory memorized so far. It interpolates between
the poses and creates suitable times and velocities.
"""
traj = self.interpolate_poses()
if len(traj) == 0:
rospy.logerr("Something went wrong when interpolating")
return
times, vels = self.ik_utils.trajectory_times_and_vels(traj)
if len(vels) == 0 or len(times) == 0:
rospy.logerr("Something went wrong when finding the times")
return
self.joint_controller.execute_trajectory(traj, times, vels,
self.whicharm,
wait = True)
return
def execute_trajectory_srv(self, _):
"""Same as execute_trajectory, but as a service.
"""
self.execute_trajectory(None)
return EmptyResponse()
def arm_trajectory_start(self, feedback):
"""
Move the gripper to the first pose in the trajectory.
"""
if len(self.trajectory.poses) == 0:
rospy.logwarn("Empty trajectory!")
return
pose = self.trajectory.poses[0]
if self.whicharm == "right":
moveit = self.planner.move_right_arm_non_collision
else:
moveit = self.planner.move_left_arm_non_collision
pos, quat = create_tuples_from_pose(pose)
res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0)
if not res:
rospy.logerr("Something went wrong when moving")
return
def clear_trajectory(self, feedback):
"""
Removes all the points stored so far
"""
self.trajectory.poses = []
def move_head(self, feedback):
"""
Moves the head to face the marker
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
print "Moving the head"
self.joint_controller.time_to_reach = 1.0
self.joint_controller.point_head_to(pos, frame)
def plan_arm(self, feedback):
"""
Moves the arm on the marker using motion collision-aware motion
planning.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm")
self.planner.move_right_arm(pos, orientation, frame, self.planning_waiting_time)
else:
rospy.loginfo("Moving the left arm")
self.planner.move_left_arm(pos, orientation, frame, self.planning_waiting_time)
def collision_free_arm(self, feedback):
"""
Moves the rm on the marker using motion NON-collision-aware inverse
kinematiks.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm (non collision)")
self.planner.move_right_arm_non_collision(pos, orientation,
frame, 2.0)
else:
rospy.loginfo("Moving the left arm (non collision)")
self.planner.move_left_arm_non_collision(pos, orientation,
frame, 2.0)
def update_planning(self, feedback):
"""
Updates the planning scene.
"""
self.planner.take_static_map()
self.planner.update_planning_scene()
def publish_trajectory(self, feedback):
"""
Publishes the trajectory as a PoseArray message
"""
self.trajectory_pub.publish(self.trajectory)
def interpolate_poses(self):
"""
Refines the trajectory by interpolating between the joints.
"""
if len(self.trajectory.poses) < 2:
rospy.logerr("At least two points in the trajectory are needed")
return []
if self.whicharm == "right":
starting_angles = self.robot_state.right_arm_positions
else:
starting_angles = self.robot_state.left_arm_positions
all_trajectory = [starting_angles]
for i in xrange(len(self.trajectory.poses) - 1):
start = PoseStamped()
start.header = self.trajectory.header
start.pose = self.trajectory.poses[i]
end = PoseStamped()
end.header = self.trajectory.header
end.pose = self.trajectory.poses[i+1]
traj, errs = self.ik_utils.check_cartesian_path(start, end,
all_trajectory[-1],
#starting_angles,
#pos_spacing = 0.05,
collision_aware = 0,
num_steps = 5,
)
if any(e == 3 for e in errs):
rospy.logerr("Error while running IK, codes are: %s", errs)
return []
filtered_traj = [t for (t,e) in zip(traj,errs) if e == 0]
all_trajectory.extend(filtered_traj)
all_trajectory = normalize_trajectory(all_trajectory, starting_angles)
rospy.loginfo("New interpolated trajectory with %d elements",
len(all_trajectory))
return all_trajectory
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
class POIServer(object):
'''
Node to act as a server to hold a list of points of interest which
can be modified by services or interactive markers
'''
def __init__(self):
'''
Create a POIServer
'''
# TF bootstrap
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# Radius of interactive marker for POIs
self.marker_scale = rospy.get_param("~marker_scale", 0.5)
# Create intial empty POI array
self.pois = POIArray()
# Get the global frame to be used
self.global_frame = rospy.get_param("~global_frame", "map")
self.pois.header.frame_id = self.global_frame
# Create publisher to notify clients of updates and interactive marker server
self.pub = rospy.Publisher("points_of_interest",
POIArray,
queue_size=1,
latch=True)
self.interactive_marker_server = InteractiveMarkerServer(
"points_of_interest")
# Load initial POIs from params
if rospy.has_param('~initial_pois'):
pois = rospy.get_param('~initial_pois')
assert isinstance(pois, dict)
for key, value in pois.iteritems():
assert type(key) == str
assert type(value) == list
assert len(value) == 3
name = key
position = numpy_to_point(value)
self._add_poi(name, position)
# Update clients / markers of changes from param
self.update()
# Create services to add / delete / move a POI
self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb)
self.delete_poi_server = rospy.Service('~delete', DeletePOI,
self.delete_poi_cb)
self.move_poi_service = rospy.Service('~move', MovePOI,
self.move_poi_cb)
self.save_to_param = rospy.Service("~save_to_param", Trigger,
self.save_to_param_cb)
def transform_position(self, ps):
'''
Attempty to transform a PointStamped message into the global frame, returning
the position of the transformed point or None if transform failed.
'''
# If no frame, assume user wanted it in the global frame
if ps.header.frame_id == "":
return ps.point
try:
ps_tf = self.tf_buffer.transform(ps,
self.global_frame,
timeout=rospy.Duration(5))
return ps_tf.point
except tf2_ros.TransformException as e:
rospy.logwarn('Error transforming "{}" to "{}": {}'.format(
ps.header.frame_id, self.global_frame, e))
return None
def process_feedback(self, feedback):
'''
Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ
'''
# Only look at changes when mouse button is unclicked
if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
return
# Transform new position into global frame
ps = PointStamped()
ps.header = feedback.header
ps.point = feedback.pose.position
position = self.transform_position(ps)
if position is None:
return
# Update position of marker
self.update_position(feedback.marker_name, feedback.pose.position)
@thread_lock(lock)
def save_to_param_cb(self, req):
rospy.set_param('~global_frame', self.global_frame)
d = {}
for poi in self.pois.pois:
d[poi.name] = [
float(poi.position.x),
float(poi.position.y),
float(poi.position.z)
]
rospy.set_param('~initial_pois', d)
return {'success': True}
def add_poi_cb(self, req):
'''
Callback for AddPOI service
'''
name = req.name
# If frame is empty, just initialize it to zero
if len(req.position.header.frame_id) == 0:
position = numpy_to_point([0., 0., 0.])
# Otherwise transform position into global frame
else:
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
if not self.add_poi(name, position):
return {'success': False, 'message': 'alread exists (bad poi)'}
return {'success': True, 'message': 'good poi'}
def delete_poi_cb(self, req):
'''
Callback for DeletePOI service
'''
# Return error if marker did not exist
if not self.remove_poi(req.name):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
def move_poi_cb(self, req):
'''
Callback for MovePOI service
'''
name = req.name
# Transform position into global frame
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
# Return error if marker did not exist
if not self.update_position(name, position):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
@thread_lock(lock)
def add_poi(self, name, position):
'''
Add a POI, updating clients / rviz when done
@return False if POI already exists
'''
if not self._add_poi(name, position):
return False
self.update()
return True
@thread_lock(lock)
def remove_poi(self, name):
'''
Remove a POI, updating clients / rviz when done
@return False if POI with name does not exist
'''
if not self._remove_poi(name):
return False
self.update()
return True
@thread_lock(lock)
def update_position(self, name, position):
'''
Update the position of a POI, updating clients / rviz when done
@param position: a Point message of the new position in global frame
@return False if POI with name does not exist
'''
if not self._update_position(name, position):
return False
self.update()
return True
def update(self):
'''
Update interactive markers server and POI publish of changes
'''
self.pois.header.stamp = rospy.Time.now()
self.pub.publish(self.pois)
self.interactive_marker_server.applyChanges()
def _update_position(self, name, position):
'''
Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change
'''
# Find poi with specified name
for poi in self.pois.pois:
if poi.name == name:
pose = Pose()
pose.orientation.w = 1.0
pose.position = position
if not self.interactive_marker_server.setPose(name, pose):
return False
# Set pose in message
poi.position = position
return True
return False
def _remove_poi(self, name):
'''
Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change
'''
# Return false if marker with that name not added to interactive marker server
if not self.interactive_marker_server.erase(name):
return False
# Find POI with specifed name and delete it from list
for i, poi in enumerate(self.pois.pois):
if poi.name == name:
del self.pois.pois[i]
return True
return False
def _add_poi(self, name, position):
'''
Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change
'''
if self.interactive_marker_server.get(name) is not None:
return False
poi = POI()
poi.name = name
poi.position = position
self.pois.pois.append(poi)
point_marker = Marker()
point_marker.type = Marker.SPHERE
point_marker.scale.x = self.marker_scale
point_marker.scale.y = self.marker_scale
point_marker.scale.z = self.marker_scale
point_marker.color.r = 1.0
point_marker.color.g = 1.0
point_marker.color.b = 1.0
point_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1.0
text_marker.pose.position.x = 1.5
text_marker.text = poi.name
text_marker.scale.z = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame
int_marker.pose.orientation.w = 1.0
int_marker.pose.position = poi.position
int_marker.scale = 1
int_marker.name = poi.name
# insert a box
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.always_visible = True
control.markers.append(point_marker)
control.markers.append(text_marker)
int_marker.controls.append(control)
self.interactive_marker_server.insert(int_marker,
self.process_feedback)
return True
class PR2TrajectoryMarkers(object):
"""
A class to create and store a trajectory for one PR2 arm. The created
trajectory can be published as a PoseArray message.
This class published on the following topics:
~trajectory_markers are the main interactive markers.
~trajectory_poses a markerarray to display the trajectory.
~trajectory_poses a posesarray with the resulting pose
The class subscribes to the topic ~overwrite_trajectory_
to change the stored trajectory. This is useful to resume working on a
trajectory after re-starting the node. The message type is PoseArray.
A std_srvs/Empty service named ~execute_trajectory is provided to
externally trigger the execution of the trajectory.
Constructor:
TrajectoryMarkers(whicharm = "left")
or
TrajectoryMarkers(whicharm = "right")
"""
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(
self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray)
rospy.Subscriber("~overwrite_trajectory", PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head", callback=self.move_head)
menu_handler.insert("Add position to trajectory",
callback=self.add_point)
menu_handler.insert("Place marker over gripper",
callback=self.place_gripper)
menu_handler.insert("Execute trjectory",
callback=self.execute_trajectory)
menu_handler.insert("Clear trajectory", callback=self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback=self.publish_trajectory)
menu_handler.insert("Move the arm (planning)", callback=self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback=self.collision_free_arm)
menu_handler.insert(
"Move the arm to trajectory start (collision-free)",
callback=self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback=self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def main_callback(self, feedback):
"""
If the mouse button is released change the gripper color according to
the IK result. Quite awkward, trying to get a nicer way to do it.
"""
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.last_gripper_pose = feedback.pose
#rospy.loginfo("Updating Marker: %d", feedback.event_type)
#rospy.loginfo("POS: %s", feedback.pose.position)
if (self.last_gripper_pose
and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP):
self.last_gripper_pose = None
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
color = None
else:
color = (1, 0, 0, 1)
int_marker = self.server.get(self.int_marker.name)
int_marker.controls.remove(self.gripper_marker)
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#rospy.loginfo("Control: %s", int_marker.controls)
self.server.insert(int_marker, self.main_callback)
self.server.setPose(int_marker.name, self.last_gripper_pose)
self.server.applyChanges()
def overwrite_trajectory(self, msg):
self.trajectory = msg
def add_point(self, feedback):
"""
Add a point to self.trajectory if it is allowed by IK.
"""
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
rospy.loginfo("Pose is reachable")
self.trajectory.poses.append(feedback.pose)
else:
rospy.logerr("Pose is not reachable!")
def place_gripper(self, feedback):
"""
Move the marker where the gripper is
"""
if self.whicharm == "right":
gripper_pos = self.planner.get_right_gripper_pose()
else:
gripper_pos = self.planner.get_left_gripper_pose()
self.server.setPose(self.int_marker.name, gripper_pos.pose,
gripper_pos.header)
self.server.applyChanges()
def execute_trajectory(self, feedback):
"""
Executes the tracjectory memorized so far. It interpolates between
the poses and creates suitable times and velocities.
"""
traj = self.interpolate_poses()
if len(traj) == 0:
rospy.logerr("Something went wrong when interpolating")
return
times, vels = self.ik_utils.trajectory_times_and_vels(traj)
if len(vels) == 0 or len(times) == 0:
rospy.logerr("Something went wrong when finding the times")
return
self.joint_controller.execute_trajectory(traj,
times,
vels,
self.whicharm,
wait=True)
return
def execute_trajectory_srv(self, _):
"""Same as execute_trajectory, but as a service.
"""
self.execute_trajectory(None)
return EmptyResponse()
def arm_trajectory_start(self, feedback):
"""
Move the gripper to the first pose in the trajectory.
"""
if len(self.trajectory.poses) == 0:
rospy.logwarn("Empty trajectory!")
return
pose = self.trajectory.poses[0]
if self.whicharm == "right":
moveit = self.planner.move_right_arm_non_collision
else:
moveit = self.planner.move_left_arm_non_collision
pos, quat = create_tuples_from_pose(pose)
res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0)
if not res:
rospy.logerr("Something went wrong when moving")
return
def clear_trajectory(self, feedback):
"""
Removes all the points stored so far
"""
self.trajectory.poses = []
def move_head(self, feedback):
"""
Moves the head to face the marker
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
print "Moving the head"
self.joint_controller.time_to_reach = 1.0
self.joint_controller.point_head_to(pos, frame)
def plan_arm(self, feedback):
"""
Moves the arm on the marker using motion collision-aware motion
planning.
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (
feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm")
self.planner.move_right_arm(pos, orientation, frame, 2.0)
else:
rospy.loginfo("Moving the left arm")
self.planner.move_left_arm(pos, orientation, frame, 2.0)
def collision_free_arm(self, feedback):
"""
Moves the rm on the marker using motion NON-collision-aware inverse
kinematiks.
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (
feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm (non collision)")
self.planner.move_right_arm_non_collision(pos, orientation, frame,
2.0)
else:
rospy.loginfo("Moving the left arm (non collision)")
self.planner.move_left_arm_non_collision(pos, orientation, frame,
2.0)
def update_planning(self, feedback):
"""
Updates the planning scene.
"""
self.planner.take_static_map()
self.planner.update_planning_scene()
def publish_trajectory(self, feedback):
"""
Publishes the trajectory as a PoseArray message
"""
self.trajectory_pub.publish(self.trajectory)
def interpolate_poses(self):
"""
Refines the trajectory by interpolating between the joints.
"""
if len(self.trajectory.poses) < 2:
rospy.logerr("At least two points in the trajectory are needed")
return []
if self.whicharm == "right":
starting_angles = self.robot_state.right_arm_positions
else:
starting_angles = self.robot_state.left_arm_positions
all_trajectory = [starting_angles]
for i in xrange(len(self.trajectory.poses) - 1):
start = PoseStamped()
start.header = self.trajectory.header
start.pose = self.trajectory.poses[i]
end = PoseStamped()
end.header = self.trajectory.header
end.pose = self.trajectory.poses[i + 1]
traj, errs = self.ik_utils.check_cartesian_path(
start,
end,
all_trajectory[-1],
#starting_angles,
#pos_spacing = 0.05,
collision_aware=0,
num_steps=5,
)
if any(e == 3 for e in errs):
rospy.logerr("Error while running IK, codes are: %s", errs)
return []
filtered_traj = [t for (t, e) in zip(traj, errs) if e == 0]
all_trajectory.extend(filtered_traj)
all_trajectory = normalize_trajectory(all_trajectory, starting_angles)
rospy.loginfo("New interpolated a trajectory with %d elements",
len(all_trajectory))
return all_trajectory
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
