def make_interactive_marker(pose, model):
int_marker = InteractiveMarker()
int_marker.name = model
int_marker.description = model
int_marker.header.frame_id = "map"
int_marker.pose = pose
int_marker.pose.position.z = max(int_marker.pose.position.z,
0.01) # ensure marker is above ground
int_marker.pose.orientation = quaternion_msg_from_yaw(
yaw(pose)) # discard all but yaw to ensure marker is flat
int_marker.scale = 1
make_model_control(int_marker, model)
control = InteractiveMarkerControl()
control.name = "drag"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalize_quaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, process_feedback)
menu_handler.apply(server, int_marker.name)
return int_marker
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 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