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