class ParkingSpotServer(Node):
def __init__(self):
super(ParkingSpotServer, self).__init__('parking_spot_server')
self.declare_parameter('map_yaml')
self.markers = {}
self.poses = {}
self.marker_server = InteractiveMarkerServer(self, 'markers')
map_param = self.get_parameter('map_yaml').value
self.map_path = Path(map_param)
self.load_map()
self.name_counter = 0
# Create a timeout to throttle saving data on feedback
# We reset it on marker feedback so that once the user stops editing, save is triggered
self.save_timer = self.create_timer(0.1, self.save_map)
self.save_timer.cancel()
self.add_srv = self.create_service(AddParkingSpot, 'add_parking_spot',
self.add_spot)
self.del_srv = self.create_service(DeleteParkingSpot,
'delete_parking_spot',
self.delete_spot)
self.get_srv = self.create_service(GetParkingSpot, 'get_parking_spot',
self.get_spot)
self.rename_srv = self.create_service(RenameParkingSpot,
'rename_parking_spot',
self.rename_spot)
def save_map(self):
self.save_timer.cancel()
dump = {}
for name, marker in self.markers.items():
pose = marker.pose
euler = euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
yaw = euler[2]
dump[name] = [
pose.position.x,
pose.position.y,
yaw,
]
self.map_data['parking'] = dump
with self.map_path.open('w') as map_yaml:
yaml.dump(self.map_data, map_yaml)
def load_map(self):
with self.map_path.open('r') as map_yaml:
self.map_data = yaml.safe_load(map_yaml)
self.poses = {
name: Pose2D(x=pose[0], y=pose[1], theta=pose[2])
for name, pose in self.map_data.get('parking', {}).items()
}
self.markers = {
name: self.add_marker(name,
Pose2D(x=pose[0], y=pose[1], theta=pose[2]))
for name, pose in self.map_data.get('parking', {}).items()
}
def marker_feedback(self, event: InteractiveMarkerFeedback) -> None:
self.save_timer.reset()
def add_marker(self, name: str, pose: Pose2D) -> InteractiveMarker:
marker = InteractiveMarker(name=name,
pose=Pose(
position=Point(x=pose.x, y=pose.y,
z=0.),
orientation=quaternion_from_euler(
pose.theta, 0., 0.),
))
marker.header.frame_id = 'map'
sc = 0.2
z = 0.1
name_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
scale=Vector3(x=sc, y=sc, z=sc),
color=ColorRGBA(r=1., g=1., b=1., a=1.),
text=name,
)
name_marker.pose.position.x = sc * -0.1
name_marker.pose.position.z = z + sc * -0.1
marker.controls = [
InteractiveMarkerControl(
name='name',
orientation_mode=InteractiveMarkerControl.VIEW_FACING,
interaction_mode=InteractiveMarkerControl.NONE,
independent_marker_orientation=False,
always_visible=True,
markers=[name_marker],
),
InteractiveMarkerControl(
name='xaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
orientation=Quaternion(x=0., y=0., z=0.7068252, w=0.7068252),
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='yaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='turn',
interaction_mode=InteractiveMarkerControl.ROTATE_AXIS,
orientation=Quaternion(x=0., y=0.7068252, z=0., w=0.7068252),
)
]
self.marker_server.insert(marker,
feedback_callback=self.marker_feedback)
self.marker_server.applyChanges()
return marker
def add_spot(self, request, response):
print(request)
name = 'park{:02}'.format(self.name_counter)
while name in self.poses:
self.name_counter += 1
name = 'park{:02}'.format(self.name_counter)
marker = self.add_marker(name, request.pose)
self.poses[name] = request.pose
self.markers[name] = marker
response.success = True
self.save_map()
return response
def delete_spot(self, request, response):
try:
del self.poses[request.name]
self.marker_server.erase(request.name)
self.marker_server.applyChanges()
del self.markers[request.name]
response.success = True
self.save_map()
except KeyError:
response.success = False
return response
def get_spot(self, request, response):
try:
response.pose = self.poses[request.name]
response.success = True
except KeyError:
response.success = False
return response
def rename_spot(self, request, response):
if request.name not in self.poses:
response.success = False
response.msg = 'Spot does not exist'
elif request.new_name in self.poses:
response.success = False
response.msg = 'New name already taken'
else:
response.success = True
name = request.name
pose = self.poses[name]
del self.markers[name]
del self.poses[name]
self.poses[request.new_name] = pose
self.marker_server.erase(name)
marker = self.add_marker(request.new_name, pose)
self.markers[request.new_name] = marker
self.save_map()
return response
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 TrajectoryAnalyzer():
def __init__(self, marker_name):
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self._client = pymongo.MongoClient(host, port)
self._traj = dict()
self._retrieve_logs()
self._server = InteractiveMarkerServer(marker_name)
def get_poses_persecond(self):
average_poses = 0
for uuid in self._traj:
traj = self._traj[uuid]
inner_counter = 1
outer_counter = 1
prev_sec = traj.secs[0]
for i, sec in enumerate(traj.secs[1:]):
if prev_sec == sec:
inner_counter += 1
else:
prev_sec = sec
outer_counter += 1
average_poses += round(inner_counter/outer_counter)
return round(average_poses/len(self._traj))
def _retrieve_logs(self):
logs = self._client.message_store.people_perception.find()
for log in logs:
for i, uuid in enumerate(log['uuids']):
if uuid not in self._traj:
t = Trajectory(uuid)
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
self._traj[uuid] = t
else:
t = self._traj[uuid]
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
def visualize_trajectories(self, mode="all", average_length=0,
longest_length=0):
counter = 0
for uuid in self._traj:
if len(self._traj[uuid].pose) > 1:
if mode == "average":
if abs(self._traj[uuid].length - average_length) \
< (average_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "longest":
if abs(self._traj[uuid].length - longest_length) \
< (longest_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "shortest":
if self._traj[uuid].length < 1:
self.visualize_trajectory(self._traj[uuid])
counter += 1
else:
self.visualize_trajectory(self._traj[uuid])
counter += 1
rospy.loginfo("Total Trajectories: " + str(len(self._traj)))
rospy.loginfo("Printed trajectories: " + str(counter))
self.delete_trajectory(self._traj[uuid])
def _update_cb(self, feedback):
return
def visualize_trajectory(self, traj):
int_marker = self.create_trajectory_marker(traj)
self._server.insert(int_marker, self._update_cb)
self._server.applyChanges()
def delete_trajectory(self, traj):
self._server.erase(traj.uuid)
self._server.applyChanges()
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
# int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 3
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
if traj.max_vel == 0:
val = vel / 0.01
else:
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def trajectory_visualization(self, mode):
average_length = 0
longest_length = -1
short_trajectories = 0
average_max_vel = 0
highest_max_vel = -1
minimal_frame = self.get_poses_persecond() * 5
for k, v in self._traj.items():
v.sort_pose()
v.calc_stats()
# Delete non-moving objects
if (v.max_vel < 0.1 or v.length < 0.1) and k in self._traj:
del self._traj[k]
# Delete trajectories that appear less than 5 seconds
if len(v.pose) < minimal_frame and k in self._traj:
del self._traj[k]
for k, v in self._traj.iteritems():
average_length += v.length
average_max_vel += v.max_vel
if v.length < 1:
short_trajectories += 1
if longest_length < v.length:
longest_length = v.length
if highest_max_vel < v.max_vel:
highest_max_vel = v.max_vel
average_length /= len(self._traj)
average_max_vel /= len(self._traj)
rospy.loginfo("Average length of tracks is " + str(average_length))
rospy.loginfo("Longest length of tracks is " + str(longest_length))
rospy.loginfo("Short trajectories are " + str(short_trajectories))
rospy.loginfo("Average maximum velocity of tracks is " +
str(average_max_vel))
rospy.loginfo("Highest maximum velocity of tracks is " +
str(highest_max_vel))
self.visualize_trajectories(mode, average_length, longest_length)
class ActuatorServer(object):
def __init__(self):
self.goto_pub = rospy.Publisher('/move_base_simple/goal',
geometry_msgs.msg.PoseStamped)
self.pose_pub = rospy.Publisher('/pose_names',
order_system.msg.PoseNames,
latch=True)
rospy.Subscriber('/amcl_pose',
geometry_msgs.msg.PoseWithCovarianceStamped,
self._handle_pose_callback)
self.names = {}
try:
with open(
"/home/team3/catkin_ws/src/cse481c/order_service/LOL.pickle",
"r") as f:
self.names = pickle.load(f)
except EOFError:
self.names = {}
self.pose = None
self.server = InteractiveMarkerServer('simple_marker')
self.name_markers = {}
self.name_controls = {}
def _create_marker(self, name):
self.name_markers[name] = InteractiveMarker()
self.name_markers[name].header.frame_id = "map"
self.name_markers[name].name = name
self.name_markers[name].description = name
self.name_markers[
name].pose.position.x = self.pose.pose.pose.position.x
self.name_markers[
name].pose.position.y = self.pose.pose.pose.position.y
# TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL
self.name_markers[name].pose.position.z = 1
box_marker = create_box_marker()
self.name_controls[name] = InteractiveMarkerControl()
self.name_controls[
name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.name_controls[name].always_visible = True
self.name_controls[name].orientation.w = 1
self.name_controls[name].orientation.x = 0
self.name_controls[name].orientation.y = 1
self.name_controls[name].orientation.z = 0
new_controller_lol = InteractiveMarkerControl()
new_controller_lol.orientation.w = 1
new_controller_lol.orientation.x = 0
new_controller_lol.orientation.y = 1
new_controller_lol.orientation.z = 0
new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED
self.name_markers[name].controls.append(new_controller_lol)
# self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w
# self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x
# self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y
# self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z
self.name_controls[name].markers.append(box_marker)
self.name_markers[name].controls.append(self.name_controls[name])
self.server.insert(self.name_markers[name], self._callback)
self.server.applyChanges()
def _callback(self, dog):
name = dog.marker_name
pose = dog.pose
rospy.logerr(str(self.names[name]))
self.names[name].pose.pose.position.x = pose.position.x
self.names[name].pose.pose.position.y = pose.position.y
self.names[name].pose.pose.orientation = pose.orientation
rospy.logerr(str(self.names[name]))
def _delete_marker(self, name):
self.server.erase(name)
self.server.applyChanges()
def handle_send_fetch(self, request):
rospy.logerr("yo we got fam" + str(request))
if request.command == request.CREATE:
self._handle_create(request.name)
self._create_marker(request.name)
elif request.command == request.DELETE:
self._handle_delete(request.name)
self._delete_marker(request.name)
elif request.command == request.GOTO:
self._handle_goto(request.name)
elif request.command == request.LIST:
rospy.loginfo('hey Karen')
else:
rospy.logerr('none of these work')
pose_message = order_system.msg.PoseNames()
pose_message.poses = list(self.names.keys())
self.pose_pub.publish(pose_message)
fetch_response = SendFetchResponse()
fetch_response.names = list(self.names.keys())
return fetch_response
def _handle_create(self, name):
rospy.loginfo(str(self.pose))
self.names[name] = self.pose
def _handle_delete(self, name):
if name not in self.names:
rospy.loginfo('wtf bro')
else:
del self.names[name]
def _handle_goto(self, name):
new_goto = geometry_msgs.msg.PoseStamped()
rospy.loginfo(str(new_goto))
new_goto.header = self.names[name].header
new_goto.pose = self.names[name].pose.pose
rospy.logerr("wtf" + str(self.names[name].pose.pose))
rospy.logerr("bro" + str(new_goto))
self.goto_pub.publish(new_goto)
def _handle_pose_callback(self, data):
self.pose = copy.deepcopy(data)
def pickle_it_up_bro(self):
with open("/home/team3/catkin_ws/src/cse481c/map_annotator/LOL.pickle",
"w") as f:
pickle.dump(self.names, f)
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 World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
bb_service_name = 'find_cluster_bounding_box'
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name,
FindClusterBoundingBox)
rospy.Subscriber('interactive_object_recognition_result',
GraspableObjectList, self.receive_object_info)
self._object_action_client = actionlib.SimpleActionClient(
'object_detection_user_command', UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo('Interactive object detection action ' +
'server has responded.')
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber('tabletop_segmentation_markers',
Marker, self.receive_table_marker)
rospy.Subscriber("ar_pose_marker",
AlvarMarkers, self.receive_ar_markers)
self.marker_dims = Vector3(0.04, 0.04, 0.01)
def receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, marker.id)
self._lock.release()
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def get_tool_id(self):
if (len(self.objects) == 0):
rospy.warning('There are no fiducials, cannot get tool ID.')
return None
elif (len(self.objects) > 1):
rospy.warning('There are more than one fiducials, returning the first as tool ID.')
return World.objects[0].marker_id
def get_surface(self):
if (len(self.objects) < 4):
rospy.warning('There are not enough fiducials to detect surface.')
return None
elif (len(self.objects) > 4):
rospy.warning('There are more than four fiducials for surface, will use first four.')
return
points = [World.objects[0].position, World.objects[1].position,
World.objects[2].position, World.objects[3].position]
s = Surface(points)
return s
def receive_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), i,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims, i)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose'''
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, id=None, mesh=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects,
dimensions, id))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
'''Function that returns the list of ref. frames'''
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
rospy.logwarn('One of the frame objects might not exist: ' +
from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING or
Response.gaze_client.get_state() == GoalStatus.ACTIVE):
time.sleep(0.1)
if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Looking at table now.')
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects()
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Table segmentation is complete.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not segment.')
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Objects on the table have been recognized.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
# Record the result
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
wait_time = 0
total_wait_time = 5
while (not World.has_objects() and wait_time < total_wait_time):
time.sleep(0.1)
wait_time += 0.1
if (not World.has_objects()):
rospy.logerr('Timeout waiting for a recognition result.')
return False
else:
rospy.loginfo('Got the object list.')
return True
else:
rospy.logerr('Could not recognize.')
return False
def clear_all_objects(self):
'''Removes all objects from the world'''
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('Successfully reset object localization pipeline.')
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
'''Gives a pointed to the nearest object'''
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Distance between two world poses'''
if pose1 == [] or pose2 == []:
return 0.0
else:
if (is_on_table):
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x,
pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x,
pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Clicked on object ' + str(feedback.marker_name))
rospy.loginfo('Number of objects ' + str(len(World.objects)))
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def update(self):
'''Update function called in a loop'''
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if (World.has_objects()):
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose,
World.objects[i].get_name(), 'base_link')
if (World.objects[i].is_removed):
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
class World:
"""Object recognition and localization related stuff"""
tf_listener = None
objects = []
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer("world_objects")
bb_service_name = "find_cluster_bounding_box"
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox)
rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info)
self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo("Interactive object detection action " + "server has responded.")
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)
def _reset_objects(self):
"""Function that removes all objects"""
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
"""Callback function for markers to determine table"""
if marker.type == Marker.LINE_STRIP:
if len(marker.points) == 6:
rospy.loginfo("Received a TABLE marker.")
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface, self.marker_feedback_cb)
self._im_server.applyChanges()
def receieve_object_info(self, object_list):
"""Callback function to receive object info"""
self._lock.acquire()
rospy.loginfo("Received recognized object list.")
if len(object_list.graspable_objects) > 0:
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if len(models) > 0:
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if best_confidence < models[j].confidence:
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if object_pose != None:
rospy.logwarn("Adding the recognized object " + "with most confident model.")
self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i])
else:
rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.")
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if cluster_pose != None:
rospy.loginfo(
"Adding unrecognized object with pose:"
+ World.pose_to_string(cluster_pose)
+ "\n"
+ "In ref frame"
+ str(bbox.pose.header.frame_id)
)
self._add_new_object(cluster_pose, bbox.box_dims, False)
else:
rospy.logwarn("... but the list was empty.")
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
"""Returns pose for transformation matrix"""
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
"""Returns the transformation matrix for given pose"""
rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
"""Returns absolute pose of an end effector state"""
if arm_state.refFrame == ArmState.OBJECT:
arm_state_copy = ArmState(
arm_state.refFrame,
Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject,
)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list):
"""Finds the most similar object in the world"""
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if dist < best_dist:
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn("Did not find a similar object..")
return None
else:
print "Object dissimilarity is --- ", best_dist
if best_dist > 0.075:
rospy.logwarn("Found some objects, but not similar enough.")
return None
else:
rospy.loginfo("Most similar to new object " + str(chosen_obj_index))
return ref_frame_list[chosen_obj_index]
@staticmethod
def _get_mesh_marker(marker, mesh):
"""Function that generated a marker from a mesh"""
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while index + 2 < len(mesh.triangles):
if (
(mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))
):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr("Mesh contains invalid triangle!")
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
"""Function to add new objects"""
dist_threshold = 0.02
to_remove = None
if is_recognized:
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose, pose)
if distance < dist_threshold:
if World.objects[i].is_recognized:
rospy.loginfo(
"Previously recognized object at " + "the same location, will not add this object."
)
return False
else:
rospy.loginfo(
"Previously unrecognized object at "
+ "the same location, will replace it with the "
+ "recognized object."
)
to_remove = i
break
if to_remove != None:
self._remove_object(to_remove)
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold:
rospy.loginfo("Previously detected object at the same" + "location, will not add this object.")
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
"""Function to remove object by index"""
obj = World.objects.pop(to_remove)
rospy.loginfo("Removing object " + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
"""Function to request removing surface"""
rospy.loginfo("Removing surface")
self._im_server.erase("surface")
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
"""Generate a marker for world objects"""
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = "base_link"
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose,
)
if mesh != None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
""" Function that generates a surface marker"""
int_marker = InteractiveMarker()
int_marker.name = "surface"
int_marker.header.frame_id = "base_link"
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose,
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
"""Function that returns the list of ref. frames"""
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
"""Function that checks if there are any objects"""
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
"""Distance between two objects"""
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
"""Ref. frame type from ref. frame name"""
if ref_name == "base_link":
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
"""Transforms an arm frame to a new ref. frame"""
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, "base_link")
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
elif ref_frame == ArmState.OBJECT:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
rel_ee_pose = World.transform(arm_frame.ee_pose, "base_link", ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
if arm_frame.refFrameObject.name == ref_frame_obj.name:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
return arm_frame
@staticmethod
def has_object(object_name):
"""Checks if the world contains the object"""
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
"""Checks if the frame is valid for transforms"""
return (object_name == "base_link") or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
""" Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms"""
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr("TF exception during transform.")
return pose
except rospy.ServiceException:
rospy.logerr("Exception during transform.")
return pose
else:
rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
"""For printing a pose to stdout"""
return (
"Position: "
+ str(pose.position.x)
+ ", "
+ str(pose.position.y)
+ ", "
+ str(pose.position.z)
+ "\n"
+ "Orientation: "
+ str(pose.orientation.x)
+ ", "
+ str(pose.orientation.y)
+ ", "
+ str(pose.orientation.z)
+ ", "
+ str(pose.orientation.w)
+ "\n"
)
def _publish_tf_pose(self, pose, name, parent):
""" Publishes a TF for object pose"""
if pose != None:
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent)
def update_object_pose(self):
""" Function to externally update an object pose"""
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (
Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE
):
time.sleep(0.1)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not look down to take table snapshot")
return False
rospy.loginfo("Looking at table now.")
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
self._reset_objects()
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not reset recognition.")
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Table segmentation is complete.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not segment.")
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Objects on the table have been recognized.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
# Record the result
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
wait_time = 0
total_wait_time = 5
while not World.has_objects() and wait_time < total_wait_time:
time.sleep(0.1)
wait_time += 0.1
if not World.has_objects():
rospy.logerr("Timeout waiting for a recognition result.")
return False
else:
rospy.loginfo("Got the object list.")
return True
else:
rospy.logerr("Could not recognize.")
return False
def clear_all_objects(self):
"""Removes all objects from the world"""
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Successfully reset object localization pipeline.")
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
"""Gives a pointed to the nearest object"""
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
"""Distance between two world poses"""
if pose1 == [] or pose2 == []:
return 0.0
else:
if is_on_table:
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
"""Callback for when feedback from a marker is received"""
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo("Clicked on object " + str(feedback.marker_name))
rospy.loginfo("Number of objects " + str(len(World.objects)))
else:
rospy.loginfo("Unknown event" + str(feedback.event_type))
def update(self):
"""Update function called in a loop"""
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if World.has_objects():
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), "base_link")
if World.objects[i].is_removed:
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
class InteractiveMarkerManager(object):
def __init__(self, parent_instance_handle, server_name, ik_solver):
self.parent_instance_handle = parent_instance_handle
self.server = InteractiveMarkerServer(server_name)
self.ik_solver = ik_solver
self.ik_position_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK
self._menu_handlers = {}
self._menu_cmds = {}
self.menu_handler = MenuHandler()
del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
def _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_waypoint_position(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['del_wp']:
self._delete_waypoint(feedback)
elif menu_entry_id == self._menu_cmds['ins_wp_before']:
self._insert_waypoint(feedback, before=True)
elif menu_entry_id == self._menu_cmds['ins_wp_after']:
self._insert_waypoint(feedback, before=False)
elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
self._process_menu_time_cmds(feedback)
else:
pass
def _delete_waypoint(self, feedback):
marker = self.name_to_marker_dict[feedback.marker_name]
waypoint = self.name_to_waypoint_dict[marker.name]
self.parent_instance_handle.delete_waypoint(waypoint)
# Update dictionaries
del self.name_to_marker_dict[marker.name]
del self.waypoint_to_marker_dict[waypoint]
del self.name_to_waypoint_dict[marker.name]
del self._menu_handlers[marker.name]
del self._menu_cmds[marker.name]
# Erase marker from server
self.server.erase(feedback.marker_name)
self.server.applyChanges()
self.marker_cnt -=1
self._int_marker_name_list.remove(marker.name)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _insert_waypoint(self, feedback, before=True):
ref_marker = self.name_to_marker_dict[feedback.marker_name]
ref_waypoint = self.name_to_waypoint_dict[ref_marker.name]
self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _process_menu_time_cmds(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
# Uncheck all other menu entries
for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
if menu_entry_id != menu_entry_handle:
menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED)
# Update waypoints
waypoint = self.name_to_waypoint_dict[feedback.marker_name]
waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle]
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def _update_waypoint_pose(self):
# TODO maybe handle pose changes differently?
pass
def _update_waypoint_position(self, marker_name, marker_pose):
waypoint = self.name_to_waypoint_dict[marker_name]
eff_pos = marker_pose.position
eff_orient = marker_pose.orientation
target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions,
eff_pos.x, eff_pos.y, eff_pos.z,
eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z,
self.ik_position_xyz_bounds[0],
self.ik_position_xyz_bounds[1],
self.ik_position_xyz_bounds[2],
self.ik_position_rpy_bounds[0],
self.ik_position_rpy_bounds[1],
self.ik_position_rpy_bounds[2])
if target_jt_angles is not None:
waypoint.end_effector_pose = copy.deepcopy(marker_pose)
waypoint.joint_state.positions = target_jt_angles
self._change_wp_marker_color(marker_name, "white", 0.9)
else:
self._change_wp_marker_color(marker_name, "red", 0.9)
def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(wp_marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update wp marker
if changed:
self._replace_wp_marker(int_marker, replacement_int_marker)
def _update_wp_marker_descriptions(self):
for index, marker_name in enumerate(self._int_marker_name_list):
marker = self.name_to_marker_dict[marker_name]
marker_description = int(marker.description)
if marker_description != index+1:
marker.description = str(index+1)
replacement_marker = copy.deepcopy(self.server.get(marker.name))
replacement_marker.description = marker.description
self._replace_wp_marker(marker, replacement_marker)
def _replace_wp_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
def clear_waypoint_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
world = None
segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service")
def __init__(self):
if World.tf_listener is None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
self.clear_all_objects()
self.relative_frame_threshold = 0.4
# rospy.Subscriber("ar_pose_marker",
# AlvarMarkers, self.receive_ar_markers)
self.is_looking_for_markers = False
self.marker_dims = Vector3(0.04, 0.04, 0.01)
World.world = self
@staticmethod
def get_world():
if World.world is None:
World.world = World()
return World.world
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
#self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if self.is_looking_for_markers:
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, False, id=marker.id)
self._lock.release()
def receieve_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), True,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims,
False)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose'''
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list, threshold=0.075):
'''Finds the most similar object in the world'''
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if (dist < best_dist):
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn('Did not find a similar object..')
return None
else:
print 'Object dissimilarity is --- ', best_dist
if best_dist > threshold:
rospy.logwarn('Found some objects, but not similar enough.')
return None
else:
rospy.loginfo('Most similar to object '
+ ref_frame_list[chosen_obj_index].name)
return ref_frame_list[chosen_obj_index]
@staticmethod
def get_map_of_most_similar_obj(object_list, ref_frame_list, threshold=0.075):
if None in object_list:
object_list.remove(None)
if len(object_list) == 0 or len(ref_frame_list) == 0:
return None
markers_dict = {}
marker_names = []
for obj in object_list:
object_name = obj.name
if object_name.startswith("marker"):
marker_names.append(object_name)
found_match = False
for ref_frame in ref_frame_list:
if ref_frame.name == object_name:
markers_dict[object_name] = ref_frame
found_match = True
break
if not found_match:
return None
ref_frame_list = [x for x in ref_frame_list if x.name not in marker_names]
object_list = [x for x in object_list if x.name not in marker_names]
if len(object_list) == 0 or len(ref_frame_list) == 0:
return markers_dict if len(markers_dict) > 0 else None
orderings = []
World.permute(object_list, orderings)
costs = []
assignments = []
for ordering in orderings:
cur_cost = 0
cur_ref_frame_list = ref_frame_list[:]
cur_assignment = []
for object in ordering:
most_similar_object = World.get_most_similar_obj(object, cur_ref_frame_list, threshold)
if most_similar_object is None:
return None
cur_ref_frame_list.remove(most_similar_object)
cur_assignment.append(most_similar_object)
cur_cost += World.object_dissimilarity(object, most_similar_object)
costs.append(cur_cost)
assignments.append(cur_assignment)
min_cost, min_idx = min((val, idx) for (idx, val) in enumerate(costs))
names = [x.name for x in orderings[min_idx]]
object_dict = dict(zip(names, assignments[min_idx]))
object_dict.update(markers_dict)
return object_dict
@staticmethod
def permute(a, results):
if len(a) == 1:
results.insert(len(results), a)
else:
for i in range(0, len(a)):
element = a[i]
a_copy = [a[j] for j in range(0, len(a)) if j != i]
subresults = []
World.permute(a_copy, subresults)
for subresult in subresults:
result = [element] + subresult
results.insert(len(results), result)
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None, id=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
if (is_recognized):
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose,
pose)
if (distance < dist_threshold):
if (World.objects[i].is_recognized):
rospy.loginfo('Previously recognized object at ' +
'the same location, will not add this object.')
return False
else:
rospy.loginfo('Previously unrecognized object at ' +
'the same location, will replace it with the ' +
'recognized object.')
to_remove = i
break
if (to_remove != None):
self._remove_object(to_remove)
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
if id is not None and World.objects[i].get_name() == "marker" + str(id):
rospy.loginfo('Previously added marker with the same id, will not add this object.')
return False
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _add_new_marker(self, id, pose):
'''Function to add new markers'''
#dist_threshold = 0.01
#to_remove = None
#for i in range(len(World.markers)):
# if (World.pose_distance(World.markers[i].pose, pose)
# < dist_threshold):
# rospy.loginfo('Previously detected marker at the same' +
# 'location, will not add this marker.')
# return False
#n_markers = len(World.markers)
#World.markers.append(WorldMarker(id, pose))
#int_marker = self._get_object_marker(len(World.objects) - 1)
#World.markers[-1].int_marker = int_marker
#self._im_server.insert(int_marker, self.marker_feedback_cb)
#self._im_server.applyChanges()
#World.markers[-1].menu_handler.apply(self._im_server,
# int_marker.name)
#self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_reachability_marker(self, world_object):
radius = self.relative_frame_threshold
pointsList = []
nx = 8
ny = 8
pointsList.append(Point(0, 0, radius))
pointsList.append(Point(0, 0, -radius))
for x in range(nx):
theta = 2 * pi * (x * 1.0 / nx)
for y in range(1, ny):
phi = pi * (y * 1.0 / ny)
destx = radius * cos(theta) * sin(phi)
desty = radius * sin(theta) * sin(phi)
destz = radius * cos(phi)
pointsList.append(Point(destx, desty, destz))
id = abs(hash(world_object.get_name() + "_reachability")) % (10 ** 8)
marker = Marker(type=Marker.SPHERE_LIST, id=id,
lifetime=rospy.Duration(nsecs=10 ** 8),
scale=Vector3(0.01, 0.01, 0.01),
points=set(pointsList),
header=Header(frame_id='base_link'),
color=ColorRGBA(1, 1, 1, 0.5),
pose=world_object.object.pose)
return marker
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
color = WorldObject.default_color if not World.objects[index].is_fake else WorldObject.fake_color
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=color,
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = 0
text_pos.y = 0
text_pos.z = World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0.05, 0.05, 0.05),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
button_control.markers.append(object_marker)
text_pos = Point()
dimensions = dimensions
text_pos.x = dimensions.x / 2 - 0.06
text_pos.y = - dimensions.y / 2 + 0.06
text_pos.z = dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0.05, 0.05, 0.05), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def get_frame_list(self):
'''Function that returns the list of ref. frames'''
self._lock.acquire()
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
self._lock.release()
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def has_marker_objects():
'''Function that checks if there are any markers'''
for o in World.objects:
if o.is_marker:
return True
return False
@staticmethod
def has_non_marker_objects():
'''Function that checks if there are any objects'''
for o in World.objects:
if not o.is_marker:
return True
return False
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Distance between two objects'''
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) -
array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rospy.logwarn('No reference frame transformations ' +
'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
rospy.logwarn('No reference frame transformations ' +
'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
# rospy.logwarn('One of the frame objects might not exist: ' +
# from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
rospy.loginfo("waiting for segmentation service")
rospy.wait_for_service(self.segmentation_service)
try:
get_segmentation = rospy.ServiceProxy(self.segmentation_service, TabletopSegmentation)
resp = get_segmentation()
rospy.loginfo("Adding landmarks")
self._reset_objects()
# add the table
xmin = resp.table.x_min
ymin = resp.table.y_min
xmax = resp.table.x_max
ymax = resp.table.y_max
depth = xmax - xmin
width = ymax - ymin
pose = resp.table.pose.pose
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
for cluster in resp.clusters:
points = cluster.points
if (len(points) == 0):
return Point(0, 0, 0)
[minX, maxX, minY, maxY, minZ, maxZ] = [
points[0].x, points[0].x, points[0].y, points[0].y,
points[0].z, points[0].z]
for pt in points:
minX = min(minX, pt.x)
minY = min(minY, pt.y)
minZ = min(minZ, pt.z)
maxX = max(maxX, pt.x)
maxY = max(maxY, pt.y)
maxZ = max(maxZ, pt.z)
self._add_new_object(Pose(Point((minX + maxX) / 2, (minY + maxY) / 2,
(minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
Point(maxX - minX, maxY - minY, maxZ - minZ), False)
return True
except rospy.ServiceException, e:
print "Call to segmentation service failed: %s" % e
return False
class InteractiveMarkerManager(object):
def __init__(self, server_ns='interactive_markers'):
self.server = InteractiveMarkerServer(server_ns)
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
def _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_marker_pose(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['position_only']:
self._set_position_only_checkmark(feedback)
else:
pass
def _set_position_only_checkmark(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
self.name_to_position_only_flag[feedback.marker_name] = True
else:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.UNCHECKED)
self.name_to_position_only_flag[feedback.marker_name] = False
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def change_marker_color(self, marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update marker
if changed:
self._replace_marker(int_marker, replacement_int_marker)
def _replace_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
def add_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
def _update_marker_pose(self, marker_name, new_pose):
self.name_to_marker_dict[marker_name].pose = new_pose
def get_marker_pose(self, marker_name):
return self.name_to_marker_dict[marker_name].pose
def clear_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
class TrajectoryAnalyzer():
def __init__(self, marker_name):
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self._client = pymongo.MongoClient(host, port)
self._traj = dict()
self._retrieve_logs(marker_name)
self._server = InteractiveMarkerServer(marker_name)
def _retrieve_logs(self, marker_name):
#logs = self._client.message_store.people_perception_marathon_uob.find()
logs = self._client.message_store.people_perception.find()
for log in logs:
#print "logs: " + repr(log)
#print "log keys: " + repr(log.keys())
for i, uuid in enumerate(log['uuids']):
#if uuid not in ['21c75fa0-2ed9-5359-b4db-250142fe0f5d', '89c29b5f-e568-56ea-bca2-f3e59ddff3f7', '0824a8d9-cf9c-5aca-89fc-03e08c14275f']:
# continue
if uuid not in self._traj:
t = Trajectory(uuid)
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
self._traj[uuid] = t
else:
t = self._traj[uuid]
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
#print "pose x,y: " + repr(t.uuid) + repr(t.pose[0]['position'][u'x']) + ", " + repr( t.pose[0]['position']['y'])
#print ""
#sys.exit(1)
def visualize_trajectories(self,
mode="all",
average_length=0,
longest_length=0):
counter = 0
for uuid in self._traj:
if len(self._traj[uuid].pose) > 1:
if mode == "average":
if abs(self._traj[uuid].length - average_length) \
< (average_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "longest":
if abs(self._traj[uuid].length - longest_length) \
< (longest_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "shortest":
if self._traj[uuid].length < 1:
self.visualize_trajectory(self._traj[uuid])
counter += 1
else:
self.visualize_trajectory(self._traj[uuid])
#print "uuid: " + repr(uuid)
#raw_input("Press 'Enter' for the next trajectory.")
#self.delete_trajectory(self._traj[uuid])
counter += 1
rospy.loginfo("Total Trajectories: " + str(len(self._traj)))
rospy.loginfo("Printed trajectories: " + str(counter))
self.delete_trajectory(self._traj[uuid])
def _update_cb(self, feedback):
return
def visualize_trajectory(self, traj):
int_marker = self.create_trajectory_marker(traj)
self._server.insert(int_marker, self._update_cb)
self._server.applyChanges()
def delete_trajectory(self, traj):
self._server.erase(traj.uuid)
self._server.applyChanges()
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
# int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
# for i in range(len(traj.pose)):
# print "Velocity: ", traj.vel[i]
# print "X,Y: ", traj.pose[i]['position']['x'],\
# traj.pose[i]['position']['y']
# print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i])
# print traj.max_vel, traj.length
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
if traj.max_vel == 0:
val = vel / 0.01
else:
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
class World:
'''Handles object recognition, localization, and coordinate space
transformations.'''
tf_listener = None
# Type: [WorldObject]
objects = []
def __init__(self):
# Public attributes
if World.tf_listener is None:
World.tf_listener = TransformListener()
self.surface = None
# Private attributes
self._lock = threading.Lock()
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
rospy.wait_for_service(SERVICE_BB)
self._bb_service = rospy.ServiceProxy(
SERVICE_BB, FindClusterBoundingBox)
self._object_action_client = actionlib.SimpleActionClient(
ACTION_OBJ_DETECTION, UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo(
'Interactive object detection action server has responded.')
# Setup other ROS machinery
rospy.Subscriber(
TOPIC_OBJ_RECOGNITION, GraspableObjectList,
self.receive_object_info)
rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker)
# Init
self.clear_all_objects()
# ##################################################################
# Static methods: Public (API)
# ##################################################################
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3): (I think this is the correct type.
See ActionStepMarker as a reference for how to use.)
Returns:
Pose
'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(
Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3])
)
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose.
Args:
pose (Pose)
Returns:
Matrix3x3: (I think this is the correct type. See
ActionStepMarker as a reference for how to use.)
'''
pp, po = pose.position, pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state (trasnforming
if relative).
Args:
arm_state (ArmState)
Returns:
Pose
'''
if arm_state.refFrame == ArmState.OBJECT:
arm_state_copy = ArmState(
arm_state.refFrame, Pose(
arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list):
'''Finds the most similar object in the world.
Args:
ref_object (?)
ref_frame_list ([Object]): List of objects (as defined by
Object.msg).
Returns:
Object|None: As in one of Object.msg, or None if no object
was found close enough.
'''
best_dist = 10000 # Not a constant; an absurdly high number.
chosen_obj = None
for ref_frame in ref_frame_list:
dist = World.object_dissimilarity(ref_frame, ref_object)
if dist < best_dist:
best_dist = dist
chosen_obj = ref_frame
if chosen_obj is None:
rospy.loginfo('Did not find a similar object.')
else:
rospy.loginfo('Object dissimilarity is --- ' + str(best_dist))
if best_dist > OBJ_SIMILAR_DIST_THRESHHOLD:
rospy.loginfo('Found some objects, but not similar enough.')
chosen_obj = None
else:
rospy.loginfo(
'Most similar to new object: ' + str(chosen_obj.name))
# Regardless, return the "closest object," which may be None.
return chosen_obj
@staticmethod
def get_frame_list():
'''Function that returns the list of reference frames (Objects).
Returns:
[Object]: List of Object (as defined by Object.msg), the
current reference frames.
'''
return [w_obj.object for w_obj in World.objects]
@staticmethod
def has_objects():
'''Returns whetehr there are any objects (reference frames).
Returns:
bool
'''
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Returns distance between two objects.
Returns:
float
'''
d1 = obj1.dimensions
d2 = obj2.dimensions
return norm(array([d1.x, d1.y, d1.z]) - array([d2.x, d2.y, d2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Returns the reference frame type from the reference frame
name specified by ref_name.
Args:
ref_name (str): Name of a referene frame.
Returns:
int: One of ArmState.*, the number code of the reference
frame specified by ref_name.
'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame.
Args:
arm_frame (ArmState)
ref_frame (int): One of ArmState.*
ref_frame_obj (Object): As in Object.msg
Returns:
ArmState: arm_frame (passed in), but modified.
'''
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to itself (nothing to do).
rospy.logdebug(
'No reference frame transformations needed (both ' +
'absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform from object to robot base.
abs_ee_pose = World.transform(
arm_frame.ee_pose,
arm_frame.refFrameObject.name,
'base_link'
)
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr(
'Unhandled reference frame conversion: ' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to object.
rel_ee_pose = World.transform(
arm_frame.ee_pose, 'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform between the same object (nothign to do).
if arm_frame.refFrameObject.name == ref_frame_obj.name:
rospy.logdebug(
'No reference frame transformations needed (same ' +
'object).')
else:
# Transform between two different objects.
rel_ee_pose = World.transform(
arm_frame.ee_pose,
arm_frame.refFrameObject.name,
ref_frame_obj.name
)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr(
'Unhandled reference frame conversion: ' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Returns whether the world contains an Object with object_name.
Args:
object_name (str)
Returns:
bool
'''
return object_name in [wobj.object.name for wobj in World.objects]
@staticmethod
def is_frame_valid(object_name):
'''Returns whether the frame (object) name is valid for
transforms.
Args:
object_name (str)
Returns:
bool
'''
return object_name == 'base_link' or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
'''Transforms a pose between two reference frames. If there is a
TF exception or object does not exist, it will return the pose
back without any transforms.
Args:
pose (Pose)
from_frame (str)
to_frame (str)
Returns:
Pose
'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(
from_frame, to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(
to_frame, pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('ServiceException during transform.')
return pose
else:
rospy.logdebug(
'One of the frame objects might not exist: ' + from_frame +
' or ' + to_frame)
return pose
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Returns distance between two world poses.
Args:
pose1 (Pose)
pose2 (Pose)
is_on_table (bool, optional): Whether the objects are on the
table (if so, disregards z-values in computations).
Returns:
float
'''
if pose1 == [] or pose2 == []:
return 0.0
else:
p1p = pose1.position
p2p = pose2.position
if is_on_table:
arr1 = array([p1p.x, p1p.y])
arr2 = array([p2p.x, p2p.y])
else:
arr1 = array([p1p.x, p1p.y, p1p.z])
arr2 = array([p2p.x, p2p.y, p2p.z])
dist = norm(arr1 - arr2)
if dist < OBJ_DIST_ZERO_CLAMP:
dist = 0
return dist
@staticmethod
def log_pose(log_fn, pose):
'''For printing a pose to rosout. We don't do it on one line
becuase that messes up the indentation with the rest of the log.
Args:
log_fn (function(str)): A logging function that takes a
string as an argument. For example, rospy.loginfo.
pose (Pose): The pose to log
'''
p, o = pose.position, pose.orientation
log_fn(' - position: (%f, %f, %f)' % (p.x, p.y, p.z))
log_fn(' - orientation: (%f, %f, %f, %f)' % (o.x, o.y, o.z, o.w))
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Generates and returns a marker from a mesh.
Args:
marker (Marker)
mesh (Mesh)
Returns:
Marker
'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while index + 2 < len(mesh.triangles):
if (mesh.triangles[index] < len(mesh.vertices)
and mesh.triangles[index + 1] < len(mesh.vertices)
and mesh.triangles[index + 2] < len(mesh.vertices)):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
@staticmethod
def _get_surface_marker(pose, dimensions):
'''Returns a surface marker with provided pose and dimensions.
Args:
pose (Pose)
dimensions (Vector3)
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = BASE_LINK
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=MARKER_DURATION,
scale=dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_SURFACE,
pose=pose
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=SCALE_TEXT, text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def receive_table_marker(self, marker):
'''Callback function for markers to determine table'''
if marker.type == Marker.LINE_STRIP:
if len(marker.points) == 6:
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, SURFACE_HEIGHT)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(
self.surface, self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if len(object_list.graspable_objects) > 0:
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if len(models) > 0:
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if best_confidence < models[j].confidence:
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if object_pose is not None:
rospy.logwarn(
'Adding the recognized object with most ' +
'confident model.')
self._add_new_object(
object_pose,
DIMENSIONS_OBJ,
True,
object_list.meshes[i]
)
else:
rospy.logwarn(
'... this is not a recognition result, it is ' +
'probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if cluster_pose is not None:
rospy.loginfo('Adding unrecognized object with pose:')
World.log_pose(rospy.loginfo, cluster_pose)
rospy.loginfo(
'...in ref frame ' +
str(bbox.pose.header.frame_id))
self._add_new_object(
cluster_pose, bbox.box_dims, False)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
def update_object_pose(self):
''' Function to externally update an object pose.'''
# Look down at the table.
rospy.loginfo('Head attempting to look at table.')
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING or
Response.gaze_client.get_state() == GoalStatus.ACTIVE):
rospy.sleep(PAUSE_SECONDS)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Head is now (successfully) stairing at table.')
# Reset object recognition.
rospy.loginfo('About to attempt to reset object recognition.')
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
rospy.sleep(PAUSE_SECONDS)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects() # Also do this internally.
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
rospy.loginfo('About to attempt table segmentation.')
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
rospy.sleep(PAUSE_SECONDS)
rospy.loginfo('Table segmentation is complete.')
rospy.loginfo(
'STATUS: ' + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logwarn('Could not segment.')
return False
# Do recognition
rospy.loginfo('About to attempt object recognition.')
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
rospy.sleep(PAUSE_SECONDS)
rospy.loginfo('Objects on the table have been recognized.')
rospy.loginfo(
'STATUS: ' + self._object_action_client.get_goal_status_text())
# Record the result
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
wait_time = rospy.Duration(0.0)
while (not World.has_objects() and
wait_time < RECOGNITION_TIMEOUT_SECONDS):
rospy.sleep(PAUSE_SECONDS)
wait_time += PAUSE_SECONDS
if not World.has_objects():
rospy.logerr('Timeout waiting for a recognition result.')
return False
else:
rospy.loginfo('Got the object list.')
return True
else:
rospy.logerr('Could not recognize.')
return False
def clear_all_objects(self):
'''Removes all objects from the world.'''
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
rospy.sleep(PAUSE_SECONDS)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Successfully reset object localization pipeline.')
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
'''Returns the nearest object, if one exists.
Args:
arm_pose (Pose): End-effector pose.
Returns:
Object|None: As in Object.msg, the nearest object (if it
is close enough), or None if there were none close
enough.
'''
# First, find which object is the closest.
distances = []
for wobj in World.objects:
dist = World.pose_distance(wobj.object.pose, arm_pose)
distances.append(dist)
# Then, see if the closest is actually below our threshhold for
# a 'closest object.'
if len(distances) > 0:
if min(distances) < OBJ_NEAREST_DIST_THRESHHOLD:
chosen = distances.index(min(distances))
return World.objects[chosen].object
# We didn't have any objects or none were close enough.
return None
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Clicked on object ' + str(feedback.marker_name))
rospy.loginfo('Number of objects ' + str(len(World.objects)))
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
def update(self):
'''Update function called in a loop.
Returns:
bool: Whether any tracked objects were removed, AKA "is
world changed."
'''
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if World.has_objects():
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(
World.objects[i].object.pose,
World.objects[i].get_name(),
BASE_LINK
)
if World.objects[i].is_removed:
to_remove = i
if to_remove is not None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _reset_objects(self):
'''Removes all objects.'''
self._lock.acquire()
for wobj in World.objects:
self._im_server.erase(wobj.int_marker.name)
self._im_server.applyChanges()
if self.surface is not None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
'''Maybe add a new object with the specified properties to our
object list.
It might not be added if too similar of an object already
exists (and has been added).
Args:
pose (Pose)
dimensions (Vector3)
is_recognized (bool)
mesh (Mesh, optional): A mesh, if it exists. Default is
None.
Returns:
bool: Whether the object was actually added.
'''
to_remove = None
if is_recognized:
# TODO(mbforbes): Re-implement object recognition or remove
# this dead code.
return False
# # Check if there is already an object
# for i in range(len(World.objects)):
# distance = World.pose_distance(
# World.objects[i].object.pose, pose)
# if distance < OBJ_ADD_DIST_THRESHHOLD:
# if World.objects[i].is_recognized:
# rospy.loginfo(
# 'Previously recognized object at the same ' +
# 'location, will not add this object.')
# return False
# else:
# rospy.loginfo(
# 'Previously unrecognized object at the same ' +
# 'location, will replace it with the recognized '+
# 'object.')
# to_remove = i
# break
# # Remove any duplicate objects.
# if to_remove is not None:
# self._remove_object(to_remove)
# # Actually add the object.
# self._add_new_object_internal(
# pose, dimensions, is_recognized, mesh)
# return True
else:
# Whether whether we already have an object at ~ the same
# location (and if so, don't add).
for wobj in World.objects:
if (World.pose_distance(wobj.object.pose, pose)
< OBJ_ADD_DIST_THRESHHOLD):
rospy.loginfo(
'Previously detected object at the same location, ' +
'will not add this object.')
return False
# Actually add the object.
self._add_new_object_internal(
pose, dimensions, is_recognized, mesh)
return True
def _add_new_object_internal(self, pose, dimensions, is_recognized, mesh):
'''Does the 'internal' adding of an object with the passed
properties. Call _add_new_object to do all pre-requisite checks
first (it then calls this function).
Args:
pose (Pose)
dimensions (Vector3)
is_recognized (bool)
mesh (Mesh|None): A mesh, if it exists (can be None).
'''
n_objects = len(World.objects)
World.objects.append(WorldObject(
pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(
self._im_server, int_marker.name)
self._im_server.applyChanges()
def _remove_object(self, to_remove):
'''Remove an object by index.
Args:
to_remove (int): Index of the object to remove in
World.objects.
'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# TODO(mbforbes): Re-implement object recognition or remove
# this dead code.
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index > obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index > obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
'''Function to request removing surface (from IM).'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
'''Generate and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=MARKER_DURATION,
scale=World.objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=World.objects[index].object.pose
)
if mesh is not None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (
World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
int_marker.controls.append(button_control)
return int_marker
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object named name with pose pose and
parent reference frame parent.
Args:
pose (Pose): The object's pose.
name (str): The object's name.
parent (str): The parent reference frame.
'''
if pose is not None:
pp = pose.position
po = pose.orientation
pos = (pp.x, pp.y, pp.z)
rot = (po.x, po.y, po.z, po.w)
# TODO(mbforbes): Is it necessary to change the position
# and orientation into tuples to send to TF?
self._tf_broadcaster.sendTransform(
pos, rot, rospy.Time.now(), name, parent)
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
bb_service_name = 'find_cluster_bounding_box'
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name,
FindClusterBoundingBox)
rospy.Subscriber('interactive_object_recognition_result',
GraspableObjectList, self.receieve_object_info)
self._object_action_client = actionlib.SimpleActionClient(
'object_detection_user_command', UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo('Interactive object detection action ' +
'server has responded.')
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber('tabletop_segmentation_markers', Marker,
self.receieve_table_marker)
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface, self.marker_feedback_cb)
self._im_server.applyChanges()
def receieve_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), True,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) +
'\n' + 'In ref frame' +
str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims,
False)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose'''
rotation = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(
arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation), arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list):
'''Finds the most similar object in the world'''
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if (dist < best_dist):
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn('Did not find a similar object..')
return None
else:
print 'Object dissimilarity is --- ', best_dist
if best_dist > 0.075:
rospy.logwarn('Found some objects, but not similar enough.')
return None
else:
rospy.loginfo('Most similar to new object ' +
str(chosen_obj_index))
return ref_frame_list[chosen_obj_index]
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
if (is_recognized):
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose,
pose)
if (distance < dist_threshold):
if (World.objects[i].is_recognized):
rospy.loginfo(
'Previously recognized object at ' +
'the same location, will not add this object.')
return False
else:
rospy.loginfo(
'Previously unrecognized object at ' +
'the same location, will replace it with the ' +
'recognized object.')
to_remove = i
break
if (to_remove != None):
self._remove_object(to_remove)
n_objects = len(World.objects)
World.objects.append(
WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose) <
dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
n_objects = len(World.objects)
World.objects.append(
WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
'''Function that returns the list of ref. frames'''
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Distance between two objects'''
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(
array([dims1.x, dims1.y, dims1.z]) -
array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name,
'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link',
ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(
arm_frame.ee_pose, arm_frame.refFrameObject.name,
ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(
from_frame, to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(
to_frame, pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
rospy.logwarn('One of the frame objects might not exist: ' +
from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(),
name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE):
time.sleep(0.1)
if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Looking at table now.')
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects()
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Table segmentation is complete.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not segment.')
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Objects on the table have been recognized.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
# Record the result
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
wait_time = 0
total_wait_time = 5
while (not World.has_objects() and wait_time < total_wait_time):
time.sleep(0.1)
wait_time += 0.1
if (not World.has_objects()):
rospy.logerr('Timeout waiting for a recognition result.')
return False
else:
rospy.loginfo('Got the object list.')
return True
else:
rospy.logerr('Could not recognize.')
return False
def clear_all_objects(self):
'''Removes all objects from the world'''
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('Successfully reset object localization pipeline.')
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
'''Gives a pointed to the nearest object'''
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Distance between two world poses'''
if pose1 == [] or pose2 == []:
return 0.0
else:
if (is_on_table):
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array(
[pose1.position.x, pose1.position.y, pose1.position.z])
arr2 = array(
[pose2.position.x, pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Clicked on object ' + str(feedback.marker_name))
rospy.loginfo('Number of objects ' + str(len(World.objects)))
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def update(self):
'''Update function called in a loop'''
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if (World.has_objects()):
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose,
World.objects[i].get_name(), 'base_link')
if (World.objects[i].is_removed):
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
class IntCollisionEnv(CollisionEnvServices):
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_server.applyChanges()
def ignored_cb(self, feedback):
pass
def d_menu_clear_cb(self, f):
pass
def global_menu_det_clear_all_cb(self, f):
for name in self.clear_all_det():
self.im_server.erase(name)
self.im_server.applyChanges()
def global_menu_pause_cb(self, f):
handle = f.menu_entry_id
state = self.global_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.global_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
self.paused = False
else:
self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.paused = True
self.global_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def global_menu_save_all_cb(self, f):
self.save_primitives()
def global_menu_add_prim_cb(self, feedback):
p = CollisionPrimitive()
p.name = self._generate_name()
p.bbox.type = SolidPrimitive.BOX
p.bbox.dimensions = [0.1, 0.1, 0.1]
p.pose.header.frame_id = self.world_frame
p.pose.pose.orientation.w = 1
p.pose.pose.position.z = 0.5
p.setup = self.setup
self.add_primitive(p)
def global_menu_reload_cb(self, feedback):
self.reload()
def global_menu_clear_all_cb(self, feedback):
self.clear_all(permanent=False)
def global_menu_clear_all_perm_cb(self, feedback):
self.clear_all()
def menu_save_cb(self, f):
self.save_primitive(f.marker_name)
def menu_scaleable_cb(self, f):
handle = f.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# TODO
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
# TODO
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_moveable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# if feedback.marker_name in self.moveable:
# self.moveable.remove(feedback.marker_name)
self.im_server.erase(feedback.marker_name)
self.im_server.insert(
make_def(self.artificial_objects[feedback.marker_name]),
self.process_im_feedback)
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.make_interactive(
self.artificial_objects[feedback.marker_name])
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_remove_cb(self, feedback):
self.remove_name(feedback.marker_name)
def process_im_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
ps = PoseStamped()
ps.header = feedback.header
ps.pose = feedback.pose
if feedback.marker_name in self.artificial_objects:
self.set_primitive_pose(feedback.marker_name, ps)
elif feedback.marker_name:
# detected objects
pass
def make_interactive(self, p):
im = self.im_server.get(p.name)
for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0,
1))):
im.controls.append(rotate_axis_control("rotate_" + v[0], v[1]))
im.controls.append(move_axis_control("move_" + v[0], v[1]))
def remove_name(self, name):
super(IntCollisionEnv, self).remove_name(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def add_primitive(self, p):
super(IntCollisionEnv, self).add_primitive(p)
self.im_server.insert(make_def(p), self.process_im_feedback)
self.a_obj_menu_handler.apply(self.im_server, p.name)
self.im_server.applyChanges()
def reload(self):
self.im_server.clear()
self.create_menus()
super(IntCollisionEnv, self).reload()
def add_detected(self, name, ps, object_type):
super(IntCollisionEnv, self).add_detected(name, ps, object_type)
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = object_type.bbox
self.im_server.insert(make_def(p, color=(0, 0, 1)),
self.process_im_feedback)
self.im_server.applyChanges()
def clear_detected(self, name):
super(IntCollisionEnv, self).clear_detected(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def check_attached_timer_cb(self, evt):
for name, ps, bbox in self.get_attached():
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = bbox
self.im_server.insert(make_def(p, color=(1, 0, 0)),
self.process_im_feedback)
self.im_server.applyChanges()
class TrajectoryAnalyzer():
def __init__(self, marker_name):
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self._client = pymongo.MongoClient(host, port)
self._traj = dict()
self._retrieve_logs(marker_name)
self._server = InteractiveMarkerServer(marker_name)
def _retrieve_logs(self, marker_name):
#logs = self._client.message_store.people_perception_marathon_uob.find()
logs = self._client.message_store.people_perception.find()
for log in logs:
#print "logs: " + repr(log)
#print "log keys: " + repr(log.keys())
for i, uuid in enumerate(log['uuids']):
#if uuid not in ['21c75fa0-2ed9-5359-b4db-250142fe0f5d', '89c29b5f-e568-56ea-bca2-f3e59ddff3f7', '0824a8d9-cf9c-5aca-89fc-03e08c14275f']:
# continue
if uuid not in self._traj:
t = Trajectory(uuid)
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
self._traj[uuid] = t
else:
t = self._traj[uuid]
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
#print "pose x,y: " + repr(t.uuid) + repr(t.pose[0]['position'][u'x']) + ", " + repr( t.pose[0]['position']['y'])
#print ""
#sys.exit(1)
def visualize_trajectories(self, mode="all", average_length=0,
longest_length=0):
counter = 0
for uuid in self._traj:
if len(self._traj[uuid].pose) > 1:
if mode == "average":
if abs(self._traj[uuid].length - average_length) \
< (average_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "longest":
if abs(self._traj[uuid].length - longest_length) \
< (longest_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "shortest":
if self._traj[uuid].length < 1:
self.visualize_trajectory(self._traj[uuid])
counter += 1
else:
self.visualize_trajectory(self._traj[uuid])
#print "uuid: " + repr(uuid)
#raw_input("Press 'Enter' for the next trajectory.")
#self.delete_trajectory(self._traj[uuid])
counter += 1
rospy.loginfo("Total Trajectories: " + str(len(self._traj)))
rospy.loginfo("Printed trajectories: " + str(counter))
self.delete_trajectory(self._traj[uuid])
def _update_cb(self, feedback):
return
def visualize_trajectory(self, traj):
int_marker = self.create_trajectory_marker(traj)
self._server.insert(int_marker, self._update_cb)
self._server.applyChanges()
def delete_trajectory(self, traj):
self._server.erase(traj.uuid)
self._server.applyChanges()
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
# int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
# for i in range(len(traj.pose)):
# print "Velocity: ", traj.vel[i]
# print "X,Y: ", traj.pose[i]['position']['x'],\
# traj.pose[i]['position']['y']
# print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i])
# print traj.max_vel, traj.length
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
if traj.max_vel == 0:
val = vel / 0.01
else:
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
