def publish_selected_pose(self, pose):
p = PoseStamped()
p.header = Header()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "map"
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.orientation = utils.angle_to_rosquaternion(pose[2])
self.rp_waypoint.publish(p)
def visualize_path(self, path):
marker = self.make_marker(path[0], 0, "start")
self.rp_waypoints.publish(marker)
poses = []
for i in range(1, len(path)):
p = Pose()
p.position.x = path[i].x
p.position.y = path[i].y
p.orientation = utils.angle_to_rosquaternion(path[i].h)
poses.append(p)
pa = PoseArray()
pa.header = Header()
pa.header.stamp = rospy.Time.now()
pa.header.frame_id = "map"
pa.poses = poses
self.rp_path_viz.publish(pa)
def visualize_selected_traj(self, poses):
#rospy.loginfo("poses length" + str(poses.shape))
path_poses = []
assert self.T*self.pose_resol+1 == poses.shape[0]
time_now = rospy.Time.now()
for i in xrange(poses.shape[0]):
pose = PoseStamped()
#pose.header = Header()
#pose.header.stamp = time_now
#pose.header.frame_id = "map"
pose.pose.position.x = poses[i,0]
pose.pose.position.y = poses[i,1]
pose.pose.position.z = 0
pose.pose.orientation = utils.angle_to_rosquaternion(poses[i,2])
path_poses.append(pose)
path = Path()
path.header = Header()
path.header.stamp = time_now
path.header.frame_id = "map"
path.poses = path_poses
self.traj_pub.publish(path)
def pub_heat_map(self):
m = Marker()
m.header.frame_id = "map"
m.header.stamp = rospy.Time.now()
m.id = 1
m.type = m.POINTS
m.action = m.ADD
m.pose.position.x = self.map_data.origin_x
m.pose.position.y = self.map_data.origin_y
m.pose.position.z = 0
m.pose.orientation = utils.angle_to_rosquaternion(self.map_data.angle)
m.color.a = 1.0
m.color.g = 1.0
m.scale.x = 0.5
rospoints = []
for i in range(150, self.map_data.width - 150, 50):
for j in range(150, self.map_data.height - 150, 50):
rospoints.append(self.dtype([i, j]).mul_(self.map_data.resolution))
print(self.map_data.resolution)
rospoints = torch.stack(rospoints)
print(rospoints)
print(self.map_data.height, self.map_data.width)
K = self.params.get_int("K")
T = self.params.get_int("T")
# Filter colliding points
collisions = self.dtype(K * T, 3)
for i in range(0, len(rospoints), K * T):
end = min(len(rospoints) - i, K * T)
collisions[:end, :2] = rospoints[i : i + end]
col = self.rhctrl.cost.world_rep.collisions(collisions)
for p, c in zip(collisions[:end], col[:end]):
if c == 0:
m.points.append(p)
points = self.dtype(K, 3)
colors = []
for i in range(0, len(m.points), K):
end = min(len(m.points) - i, K)
points[:end, 0] = self.dtype(map(lambda p: p.x, m.points[i : i + end]))
points[:end, 1] = self.dtype(map(lambda p: p.y, m.points[i : i + end]))
c2g = self.rhctrl.cost.value_fn.get_value(points)
colors.extend(map(float, list(c2g)[:end]))
print(colors)
norm = mplcolors.Normalize(vmin=min(colors), vmax=max(colors))
cmap = cm.get_cmap("coolwarm")
def colorfn(cost):
col = cmap(norm(cost))
r, g, b, a = col[0], col[1], col[2], 1.0
if len(col) > 3:
a = col[3]
return ColorRGBA(r=r, g=g, b=b, a=a)
m.colors = map(colorfn, colors)
self.value_heat_map_pub.publish(m)