def waypoints_received(self, req):
plt.subplot(111)
ax = plt.gca()
req_waypoints, scores = _parse_waypoints(req.filename)
curr_pose = util.rospose_to_posetup(self.current_pose)
waypoints = _get_roundtrip_waypoints((curr_pose[0], curr_pose[1]), req_waypoints)
headings = fitdubins.fit_heading_spline(waypoints, ax)
ax.plot(waypoints[:, 0], waypoints[:, 1], 'o')
headings[0] = curr_pose[2]
fitdubins.visualize_dubins(waypoints, headings, 1/1.6, ax)
plt.savefig('../plots/plan.png')
plt.clf()
self.run(np.c_[waypoints, headings])
def waypoints_received(self, req):
plt.subplot(111)
ax = plt.gca()
req_waypoints, scores = _parse_waypoints(req.filename)
curr_pose = self.current_pose
waypoints = _get_roundtrip_waypoints((curr_pose[0], curr_pose[1]),
req_waypoints)
headings = fitdubins.fit_heading_spline(waypoints, ax)
headings[0] = curr_pose[2]
self.waypoints = np.c_[waypoints, headings]
for i in range(self.waypoints.shape[0]):
rpose = util.particle_to_pose(self.waypoints[i])
marker = make_marker(rpose, i, "goalpoint")
self.rp_waypoints.publish(marker)
self.run()
def _parse_mainpoints(filename):
points = []
with open("../waypoints/" + filename, 'r') as wf:
line = wf.readline()
while line:
points.append(tuple([float(x) for x in line.strip().split(' ')]))
line = wf.readline()
points.append(points[0])
mainpoints = np.array(points)
plt.subplot(111)
ax = plt.gca()
headings = fitdubins.fit_heading_spline(mainpoints, ax)
poses = fitdubins.visualize_dubins(mainpoints, headings,
np.tan(0.34) / 0.3, ax)
ax.plot(mainpoints[:, 0], mainpoints[:, 1], 'o')
ax.set_aspect('equal')
ax.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.0)
plt.savefig('../plots/main_traj.png')
# plt.show()
return poses