def get_msg(self):
# type: () -> PointMsg
return PointMsg(
x=self.x,
y=self.y,
z=self.z,
)
def pixel_msg_to_ground_msg(self, point_msg) -> PointMsg:
"""
Creates a :py:class:`ground_projection.Point` object from a normalized point message from an
unrectified
image. It converts it to pixel coordinates and rectifies it. Then projects it to the ground plane and
converts it to a ROS Point message.
Args:
point_msg (:obj:`geometry_msgs.msg.Point`): Normalized point coordinates from an unrectified
image.
Returns:
:obj:`geometry_msgs.msg.Point`: Point coordinates in the ground reference frame.
"""
# normalized coordinates to pixel:
norm_pt = Point.from_message(point_msg)
pixel = self.ground_projector.vector2pixel(norm_pt)
# rectify
rect = self.rectifier.rectify_point(pixel)
# convert to Point
rect_pt = Point.from_message(rect)
# project on ground
ground_pt = self.ground_projector.pixel2ground(rect_pt)
# point to message
ground_pt_msg = PointMsg()
ground_pt_msg.x = ground_pt.x
ground_pt_msg.y = ground_pt.y
ground_pt_msg.z = ground_pt.z
return ground_pt_msg
def target_pose_along_path(self, pose_s):
current_point = np.array(_a(pose_s.pose.position))
point, yaw = self.target_along_path(current_point)
q = quaternion_from_euler(0, 0, yaw)
msg = PoseStamped(header=Header(frame_id=self.path.header.frame_id),
pose=Pose(position=PointMsg(point[0], point[1], 0),
orientation=Quaternion(*q)))
return msg
def build_zone_markers(zone):
# type: (ZoneDocument) -> typing.Sequence[Marker]
markers = list() # type: typing.List[Marker]
if zone.zone_type == ZoneMsg.EXCLUSION_ZONE:
color = ColorRGBA(1.0, 0.0, 0.2, 0.2)
elif zone.zone_type == ZoneMsg.DRIVABLE_ZONE:
color = ColorRGBA(0.0, 1.0, 0.2, 0.1)
elif zone.zone_type == ZoneMsg.AVOID_ZONE:
color = ColorRGBA(1.0, 0.6, 0.0, 0.2)
else:
color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
polygon_points = [(point.x, point.y, point.z) for point in zone.polygon]
triangles = tess.triangulate(polygon_points)
triangle_marker = Marker(type=Marker.TRIANGLE_LIST,
frame_locked=True,
scale=Vector3Msg(1.0, 1.0, 1.0),
color=color)
for t in triangles:
# Forward triangle
triangle_marker.points.append(PointMsg(*polygon_points[t[0]]))
triangle_marker.points.append(PointMsg(*polygon_points[t[1]]))
triangle_marker.points.append(PointMsg(*polygon_points[t[2]]))
# Reverse triangle
triangle_marker.points.append(PointMsg(*polygon_points[t[0]]))
triangle_marker.points.append(PointMsg(*polygon_points[t[2]]))
triangle_marker.points.append(PointMsg(*polygon_points[t[1]]))
markers.append(triangle_marker)
return markers
def build_marker_array(map_obj):
# type: (MapDocument) -> MarkerArray
assert isinstance(map_obj, MapDocument)
marker_array = MarkerArray()
marker_array.markers.append(Marker(type=Marker.DELETEALL))
for zone in map_obj.zones:
marker_array.markers += build_zone_markers(zone=zone)
nodes = {}
for node in map_obj.nodes:
nodes[node.id] = node
marker_array.markers.append(
Marker(type=Marker.SPHERE,
pose=PoseMsg(position=PointMsg(x=node.point.x,
y=node.point.y,
z=0),
orientation=QuaternionMsg(w=1)),
frame_locked=True,
scale=Vector3Msg(0.06, 0.06, 0.06),
color=ColorRGBA(0.0, 0.0, 1.0, 1.0)))
for path in map_obj.paths:
for i in range(len(path.nodes) - 1):
start_node = nodes[path.nodes[i]]
end_node = nodes[path.nodes[i + 1]]
marker_array.markers.append(
build_cylinder_marker(
start_point=Vector3(start_node.point.x, start_node.point.y,
0),
end_point=Vector3(end_node.point.x, end_node.point.y, 0),
color=ColorRGBA(0.0, 0.0, 1.0, 1.0),
radius=0.02))
print marker_array
for marker in map_obj.markers:
marker_array.markers += build_pose_markers(pose=marker.pose.get_msg(),
size=0.2)
now = rospy.Time(0)
for i, marker in enumerate(marker_array.markers):
assert isinstance(marker, Marker)
marker.id = i
marker.header.frame_id = 'map'
marker.header.stamp = now
return marker_array
def build_pose_markers(pose, size=0.05):
# type: (PoseMsg, float) -> typing.Sequence[Marker]
quat_m3d = Quaternion(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z)
mat = quat_m3d.matrix()
return [
Marker(type=Marker.ARROW,
points=[
pose.position,
PointMsg(pose.position.x + mat[:, 0][0] * size,
pose.position.y + mat[:, 0][1] * size,
pose.position.z + mat[:, 0][2] * size)
],
frame_locked=True,
scale=Vector3Msg(size / 6, size / 3, 0),
color=ColorRGBA(1.0, 0, 0, 1.0)),
Marker(type=Marker.ARROW,
points=[
pose.position,
PointMsg(pose.position.x + mat[:, 1][0] * size,
pose.position.y + mat[:, 1][1] * size,
pose.position.z + mat[:, 1][2] * size)
],
frame_locked=True,
scale=Vector3Msg(size / 6, size / 3, 0),
color=ColorRGBA(0.0, 1.0, 0, 1.0)),
Marker(type=Marker.ARROW,
points=[
pose.position,
PointMsg(pose.position.x + mat[:, 2][0] * size,
pose.position.y + mat[:, 2][1] * size,
pose.position.z + mat[:, 2][2] * size)
],
frame_locked=True,
scale=Vector3Msg(size / 6, size / 3, 0),
color=ColorRGBA(0.0, 0.0, 1.0, 1.0))
]
def target_pose_along_path(self, pose_s):
if not self.following:
return None
current_point = array_from_msg(pose_s.pose.position)
# rospy.loginfo('current_point %s', current_point)
current_z = pose_s.pose.position.z
point, yaw, z, v = self.target_along_path(current_point, current_z)
if self.flat:
z = current_z
# rospy.loginfo('target_point %s', point)
q = quaternion_from_euler(0, 0, yaw)
msg = PoseStamped(header=Header(frame_id=self.path.header.frame_id),
pose=Pose(position=PointMsg(point[0], point[1], z),
orientation=Quaternion(*q)))
return msg
def auto_encode(data):
t = type(data)
if t in ts.matrix_types:
m = data if t in ts.symengine_matrix_types else data.to_sym_matrix()
if m.shape == (4, 4):
return encode_pose(m)
elif m.shape == (3, 3):
return encode_rotation(m)
elif m.shape == (3, 1):
return encode_point(m)
elif m.shape == (4, 1):
return encode_vector(m) if m[3] == 0 else encode_point(m)
else:
raise Exception(
'No encoding known for matrices of shape {}'.format(m.shape))
elif t == list or t == tuple:
if len(data) == 0:
raise Exception('Can not encode empty list.')
s_t = type(data[0])
if s_t is float or s_t is int or s_t is list or s_t is tuple:
return auto_encode(se.Matrix(data))
raise Exception('Can not encode list with inner type {}'.format(s_t))
elif t == pb.Transform:
out = PoseMsg()
out.position.x = data.origin.x
out.position.y = data.origin.y
out.position.z = data.origin.z
out.orientation.x = data.rotation.x
out.orientation.y = data.rotation.y
out.orientation.z = data.rotation.z
out.orientation.w = data.rotation.w
return out
elif t == pb.Quaternion:
out = QuaternionMsg()
out.x = data.x
out.y = data.y
out.z = data.z
out.w = data.w
return out
elif t == pb.Vector3:
out = PointMsg()
out.x = data.x
out.y = data.y
out.z = data.z
return out
else:
raise Exception('Can not encode data of type {}'.format(t))
def build_cylinder_marker(start_point, end_point, color, radius):
# type: (Vector3, Vector3, ColorRGBA, float) -> Marker
vec = Vector3(end_point - start_point)
center = (start_point + end_point) * (1 / 2.0)
axis_vec = end_point - center
axis_vec.normalize()
up_vec = Vector3(0, 0, 1)
right_axis_vector = axis_vec.cross(up_vec)
right_axis_vector.normalize()
theta = axis_vec * up_vec
angle_rotation = -1.0 * math.acos(theta)
qt = Quaternion.from_axis_angle(axis=right_axis_vector,
angle=angle_rotation)
return Marker(type=Marker.CYLINDER,
pose=PoseMsg(position=PointMsg(*center.data()),
orientation=QuaternionMsg(
qt.x, qt.y, qt.z, qt.w)),
frame_locked=True,
scale=Vector3Msg(radius, radius, vec.length()),
color=color)
def fake_observation():
if base_tf is None:
print('Can not generate observation, base_tf is None')
return []
if camera_base_tf is None:
print('Can not generate observation, camera_base_tf is None')
return []
visualizer.begin_draw_cycle('frustum', 'aabbs')
camera_tf = base_tf.dot(camera_base_tf)
frustum = camera_tf.dot(frustum_vertices)
frust_min = frustum.min(axis=1)
frust_max = frustum.max(axis=1)
frust_aabb_center = (frust_min + frust_max) * 0.5
visualizer.draw_lines('frustum', pb.Transform.identity(), 0.05,
camera_tf.dot(frustum_lines).T.tolist())
visualizer.draw_mesh(
'frustum',
pb.Transform(frust_aabb_center[0], frust_aabb_center[1],
frust_aabb_center[2]), (frust_max - frust_min),
'package://gebsyas/meshes/bounding_box.dae')
objects = world.overlap_aabb(pb.Vector3(*frust_min[:3]),
pb.Vector3(*frust_max[:3]))
aabbs = [o.aabb for o in objects]
centers = [a * 0.5 + b * 0.5 for a, b in aabbs]
ray_starts = [pb.Vector3(*camera_tf[:3, 3])] * len(centers)
ray_results = world.closest_ray_test_batch(ray_starts, centers)
visible_set = {r.collision_object
for r in ray_results}.intersection(set(objects))
#print(sorted([inv_phy_obj[o] for o in visible_set]))
i_camera_tf = np.eye(4)
i_camera_tf[:3, :3] = camera_tf[:3, :3].T
i_camera_tf[:3, 3] = -i_camera_tf[:3, :3].dot(camera_tf[:3, 3])
bounding_boxes = []
for o in visible_set:
aabb_min, aabb_max = o.aabb
aabb_min = np.array([aabb_min[x] for x in range(3)] + [1])
aabb_max = np.array([aabb_max[x] for x in range(3)] + [1])
aabb_dim = aabb_max - aabb_min
aabb_corners = (aabb_vertex_template * aabb_dim + aabb_min +
0.5 * aabb_dim).T
aabb_center = aabb_min + 0.5 * aabb_dim
visualizer.draw_points('aabbs', pb.Transform.identity(), 0.05,
aabb_corners.T.tolist())
visualizer.draw_cube('aabbs',
pb.Transform(*aabb_center[:3]),
pb.Vector3(*aabb_dim[:3]),
g=1,
a=0.5)
corners_in_camera = i_camera_tf.dot(aabb_corners)
projected_corners = projection_matrix.dot(corners_in_camera)
projected_corners[2, :] = np.abs(projected_corners[2, :]).clip(
1e-12, 1e12)
projected_corners = np.divide(projected_corners,
projected_corners[2, :])
screen_bb_min = projected_corners.min(axis=1)
screen_bb_max = projected_corners.max(axis=1)
if screen_bb_max.min() <= -1 or screen_bb_min.max() > 1:
print(
'Bounding box for object {} is off screen.\n Min: {}\n Max: {}'
.format(inv_phy_obj[o], screen_bb_min, screen_bb_max))
else:
pixel_bb_min = screen_translation.dot(screen_bb_min.clip(-1, 1))
pixel_bb_max = screen_translation.dot(screen_bb_max.clip(-1, 1))
print('Bounding box for object {}.\n Min: {}\n Max: {}'.format(
inv_phy_obj[o], pixel_bb_min, pixel_bb_max))
bounding_boxes.append(
(i_camera_tf.dot(aabb_min + 0.5 * aabb_dim)[2],
phy_to_label[o], pixel_bb_min, pixel_bb_max, aabb_center))
min_box_width = 5
visible_boxes = []
for _, label, bmin, bmax, location in reversed(sorted(bounding_boxes)):
new_visible_boxes = [(label, bmin, bmax, location)]
for vlabel, vmin, vmax, vlocation in visible_boxes:
rmin = vmin - bmin
rmax = vmax - bmax
new_visible_boxes.append((vlabel, vmin, vmax, vlocation))
continue # AFTER THIS BOX CUTTING
if rmin.min() > 0: # Min corner is outside v-box
if rmax.max() < 0: # Max corner is outside of v-box
print('Box for {} is completely occluded.'.format(vlabel))
elif rmax.min() > 0: # Max corner is inside v-box -> L-shape
top_bar = (vlabel, np.array([vmin[0], bmax[1]]), vmax)
right_bar = (vlabel, np.array([bmax[0], vmin[1]]),
np.array([vmax[0], bmax[1]]))
new_visible_boxes.extend([top_bar, right_bar])
else: # I/bar shape
if rmax[0] >= 0: # bar shape
top_bar = (vlabel, np.array([vmin[0], bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # I shape
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
new_visible_boxes.append(right_bar)
elif rmin.max() < 0: # Min corner is inside v-box
left_bar = (vlabel, vmin, np.array([bmin[0], vmax[1]]))
bot_bar = (vlabel, np.array([bmin[0], vmin[1]]),
np.array([vmax[0], bmin[1]]))
new_visible_boxes.extend([left_bar,
bot_bar]) # These always go in
if rmax.max() < 0: # Max corner is outside v-box -> L-shape
pass
elif rmax.min() > 0: # Max corner is inside v-box -> O-shape
right_bar = (vlabel, np.array([bmax[0], bmin[1]]),
np.array([vmax[0], bmax[1]]))
top_bar = (vlabel, np.array([bmin[0], bmax[1]]), vmax)
new_visible_boxes.extend([right_bar, top_bar])
else: # C/U shape
if rmax[0] > 0: # U shape
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # C shape
top_bar = (vlabel, np.array([bmin[0], bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # Min corner is partially overlapping v-box
left_bar = (vlabel, vmin, np.array([bmin[0], vmax[1]]))
bot_bar = (vlabel, vmin, np.array([vmax[0], bmin[1]]))
if rmin[0] < 0: # I/II/angle/cap
new_visible_boxes.append(left_bar)
if rmax.max(
) < 0: # Max corner is outside shape -> I shape
pass
elif rmax.min() > 0: # Max corner is inside shape -> Cap
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
top_bar = (vlabel, np.array([bmin[0], bmax[1]]),
np.array([bmax[0], vmax[1]]))
new_visible_boxes.extend([right_bar, top_bar])
else: # Max corner is overlapping -> II/Angle
if rmax[0] > 0: # II shape
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # Angle shape
top_bar = (vlabel, np.array([bmin[0],
bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # bar/=/angle
new_visible_boxes.append(bot_bar)
if rmax.max(
) < 0: # Max corner is outside shape -> bar shape
pass
elif rmax.min(
) > 0: # Max corner is inside shape -> reversed C
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
top_bar = (vlabel, np.array([vmin[0], bmax[1]]),
np.array([bmax[0], vmax[1]]))
new_visible_boxes.extend([right_bar, top_bar])
else: # Max corner is overlapping -> =/Angle
if rmax[0] > 0: # Angle shape
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # = shape
top_bar = (vlabel, np.array([vmin[0],
bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
visible_boxes = [(vlabel, vmin, vmax, vlocation)
for vlabel, vmin, vmax, vlocation in new_visible_boxes
if (vmax - vmin).min() >= min_box_width]
bboxes = {}
for label, bmin, bmax, location in visible_boxes:
if label not in bboxes:
bboxes[label] = ObjectInfoMsg(label=label)
msg = bboxes[label]
msg.bbox_xmin.append(bmin[0])
msg.bbox_ymin.append(bmin[1])
msg.bbox_xmax.append(bmax[0])
msg.bbox_ymax.append(bmax[1])
msg.location.append(
PointMsg(x=location[0], y=location[1], z=location[2]))
msg.score.append(1.0)
visualizer.render('frustum', 'aabbs')
return bboxes.values()
def encode_point(iterable):
out = PointMsg()
out.x = iterable[0]
out.y = iterable[1]
out.z = iterable[2]
return out
def odomCallback(data, tfListener):
global markerArray
horizFOV = 54.0 * math.pi / 180.0
vertFOV = 45.0 * math.pi / 180.0
depth = 1.0
horizRadsPerPixel = horizFOV / 640.0
vertRadsPerPixel = vertFOV / 480.0
angles = np.array([horizRadsPerPixel, vertRadsPerPixel])
endPixel1 = np.array([-320, -240])
endPixel2 = np.array([320, 240])
endAngs1 = angles * endPixel1
endAngs2 = angles * endPixel2
endRay1 = [math.sin(endAngs1[0]), -43]
endRay2 = [math.sin(endAngs2[0]), -43]
h1 = math.sqrt(1 + endRay1[0]**2)
h2 = math.sqrt(1 + endRay2[0]**2)
endRay1[1] = h1 * math.sin(endAngs1[1])
endRay2[1] = h2 * math.sin(endAngs2[1])
pos = data.pose.pose.position
startPoint = PointStampedMsg()
endPoint1 = PointStampedMsg()
endPoint2 = PointStampedMsg()
while True:
try:
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 0
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
startPoint = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = endRay1[0]
point.point.y = endRay1[1]
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
endPoint1 = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = endRay2[0]
point.point.y = endRay2[1]
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
endPoint2 = tfListener.transformPoint("/odom", point)
print "Good!"
break
except:
print "Waiting..."
markerArray = MarkerArrayMsg()
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint1.point.x
marker.points[1].y = endPoint1.point.y
marker.points[1].z = endPoint1.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
markerArray.markers.append(marker)
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint2.point.x
marker.points[1].y = endPoint2.point.y
marker.points[1].z = endPoint2.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
markerArray.markers.append(marker)
print markerArray
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
def transformImgTo3d(points, tfListener):
global markerArray, markerPublisher
print "And going back, from ", points
markerArray = MarkerArrayMsg()
horizFOV = cameraFOV[0]
vertFOV = cameraFOV[1]
depth = 1.0
horizRadsPerPixel = horizFOV / 640.0
vertRadsPerPixel = vertFOV / 480.0
angles = np.array([horizRadsPerPixel, vertRadsPerPixel])
while True:
try:
stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
pos, quat = tfListener.lookupTransform("/odom", "/head_camera_rgb_optical_frame", stamp)
matTransform = tfListener.fromTranslationRotation(pos, quat)
startPoint = np.array([0, 0, 0, 1])
startPoint = np.matmul(matTransform, startPoint)
for point in points:
endPixel = np.array(point)
endAngs = angles * endPixel
endRay = [math.sin(endAngs[0]), -43]
h1 = math.sqrt(1 + endRay[0]**2)
endRay[1] = h1 * math.sin(endAngs[1])
endPoint = np.array([endRay[0], endRay[1], 1, 1])
endPoint = np.matmul(matTransform, endPoint)
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint[0]
marker.points[0].y = startPoint[1]
marker.points[0].z = startPoint[2]
marker.points[1].x = endPoint[0]
marker.points[1].y = endPoint[1]
marker.points[1].z = endPoint[2]
marker.scale.x = 0.005
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
markerArray.markers.append(marker)
break
except:
print "Waiting for transform..."
# print markerArray
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
markerPublisher.publish(markerArray)
def update(self, current):
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = self.frame
pc.channels = [ChannelFloat32()]
pc.channels[0].name = "weight"
for particle in self.pf.particles:
pc.points.append(
Point32(particle.pos[0], particle.pos[1], particle.pos[2]))
pc.channels[0].values.append(particle.getWeight())
prediction = self.pf.predict()
r = self.markerColor[0]
g = self.markerColor[1]
b = self.markerColor[2]
a = self.markerColor[3]
markerArray = MarkerArrayMsg()
marker = MarkerMsg()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = a
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.pose.position.x = prediction[0]
marker.pose.position.y = prediction[1]
marker.pose.position.z = prediction[2]
markerArray.markers.append(marker)
startPoint = current[0]
for endPoint in current[1]:
marker = MarkerMsg()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint[0]
marker.points[0].y = startPoint[1]
marker.points[0].z = startPoint[2]
marker.points[1].x = endPoint[0]
marker.points[1].y = endPoint[1]
marker.points[1].z = endPoint[2]
marker.scale.x = 0.0025
marker.scale.y = 0.0025
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
markerArray.markers.append(marker)
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
self.pcPub.publish(pc)
self.markerPub.publish(markerArray)
def odomCallback(data, tfListener):
global markerArray
pos = data.pose.pose.position
startPoint = PointStampedMsg()
endPoint = PointStampedMsg()
while True:
try:
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 0
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime(
"/head_camera_rgb_optical_frame", "/odom")
startPoint = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime(
"/head_camera_rgb_optical_frame", "/odom")
endPoint = tfListener.transformPoint("/odom", point)
print "Good!"
break
except:
print "Waiting..."
# marker = MarkerMsg()
# marker.header.frame_id = "/odom"
# marker.type = marker.ARROW
# marker.action = marker.ADD
# marker.scale.x = 0.05
# marker.scale.y = 0.05
# marker.scale.z = 0.05
# marker.color.a = 1
# marker.color.r = 1
# marker.color.g = 1
# marker.color.b = 1
# marker.pose.position.x = endPoint.point.x
# marker.pose.position.y = endPoint.point.y
# marker.pose.position.z = endPoint.point.z
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint.point.x
marker.points[1].y = endPoint.point.y
marker.points[1].z = endPoint.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
markerArray = MarkerArrayMsg()
markerArray.markers.append(marker)
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
def generate_cartographer_map(cartographer_configuration_directory,
world_sdf_path,
resolution=0.02,
map_origin=(-30, -10),
map_size=(64, 64),
submap_locations=((30, 22, 64, 32), (46, 22, 64,
32))):
assert os.path.isdir(cartographer_configuration_directory)
assert os.path.isfile(world_sdf_path)
temp_png_f = tempfile.NamedTemporaryFile()
temp_pb_f = tempfile.NamedTemporaryFile()
cmd = [
'rosrun',
'cartographer_ros',
'sdf_to_pbstream',
'--configuration_directory',
cartographer_configuration_directory,
'--pbstream',
temp_pb_f.name,
'--map_png',
temp_png_f.name,
'--world_sdf',
world_sdf_path,
'--map_origin', # bottom left corner position
'{},{}'.format(*map_origin),
'--map_size',
'{},{}'.format(*map_size)
]
for s in submap_locations:
cmd += ['--submap_location', ','.join([str(_s) for _s in s])]
str_cmd = ' '.join(cmd)
subprocess.check_call(str_cmd, shell=True, stderr=subprocess.STDOUT)
temp_pb_f.seek(0)
pb_bytes = temp_pb_f.read()
temp_png_f.seek(0)
im_bytes = temp_png_f.read()
xml_file = open(world_sdf_path, 'r')
sdf_xml = xml_file.read()
parser = etree.XMLParser(remove_blank_text=True)
original_xml_element = etree.XML(sdf_xml, parser=parser)
world = original_xml_element.find('world')
#
# Extract the zones
#
zones = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
zone_type = get_zone_type(layer)
if zone_type != Zone.UNKNOWN:
collision = link.find('visual')
geometry = collision.find('geometry')
polyline = geometry.find('polyline')
points = [[float(t) for t in point.text.split(' ')]
for point in polyline.findall('point')]
zones.append(
Zone(name=link.attrib['name']
if 'name' in link.attrib else layer,
zone_type=zone_type,
polygon=PolygonMsg(points=[
Point32Msg(point[0], point[1], 0)
for point in points
])))
#
# Extract the path nodes
#
nodes = []
for model in world.findall('model'):
for node in model.findall('node'):
position = node.find('position')
node_id = node.attrib['id']
points = [float(t) for t in position.text.split(' ')]
nodes.append(Node(id=node_id, x=points[0], y=points[1]))
#
# Extract the paths
#
paths = []
for model in world.findall('model'):
for path in model.findall('path'):
node_ids = [ni.text for ni in path.findall('node')]
paths.append(Path(
name='',
nodes=node_ids,
))
#
# Extract the markers
#
markers = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
if layer == 'marker':
pose = link.find('pose')
values = [float(f) for f in pose.text.split(' ')]
if len(values) != 6:
raise Exception('Invalid Pose: {}'.format(values))
qt = Quaternion.from_euler_extrinsic(*values[3:])
markers.append(
Marker(name=link.attrib['name']
if 'name' in link.attrib else layer,
marker_type=0,
pose=PoseMsg(position=PointMsg(
values[0], values[1], values[2]),
orientation=QuaternionMsg(x=qt.x,
y=qt.y,
z=qt.z,
w=qt.w))))
#
# Save the map
#
add_map_srv = rospy.ServiceProxy(name='/map_manager/add_map',
service_class=AddMap)
add_map_srv.wait_for_service(timeout=10)
add_response = add_map_srv.call(
AddMapRequest(map=Map(info=MapInfo(
name='sim_map',
meta_data=MapMetaData(
resolution=resolution,
width=map_size[0] / resolution,
height=map_size[1] / resolution,
origin=PoseMsg(
position=PointMsg(x=map_origin[0], y=map_origin[1])))),
markers=markers,
zones=zones,
paths=paths,
nodes=nodes,
default_zone=Zone.EXCLUSION_ZONE),
occupancy_grid=CompressedImage(format='png',
data=im_bytes),
map_data=pb_bytes)) # type: AddMapResponse
if not add_response.success:
raise Exception('Failed to save map: {}'.format(add_response.message))
