def run(self, poses): # determines order of traversal and orientation at each waypoint # while has remaining waypoints: # determines start, goal # evaluate time budget # call service to ROSPlannerFinal and retrieve a new path # push new path to queue if poses is None: raise RuntimeError('No waypoints is read.') # initialize states self.next_paths = Queue.Queue() # convert poses to ROS Pose and Marker ros_poses = [] for i in range(len(poses)): rpose = util.particle_to_pose(poses[i]) ros_poses.append(rpose) marker = self.make_marker(rpose, i, "goalpoint") self.rp_waypoints.publish(marker) assert len(ros_poses) == len(poses) first_flag = True sstart = 0 max_cnt = 5 for i in range(1, len(ros_poses)): cnt = 1 print 'Computing path from point {} to point {}...'.format( sstart, i) resp = self.planner(ros_poses[sstart], ros_poses[i]) while not resp.success and cnt <= max_cnt: # perturb heading and keep runing perturb_pose = poses[i] perturb_pose[2] += cnt / max_cnt * (2 * np.pi) #print 'check', ros_poses[i], perturb_pose rpose = util.particle_to_pose(perturb_pose) test_plantime = rospy.get_time() resp = self.planner(ros_poses[sstart], rpose) print rospy.get_time() - test_plantime print 'Failed to find path.' cnt += 1 if not resp.success: print 'Failed to find path with all sampled headings.' continue print 'Path found.' sstart = i path = resp.path if first_flag: first_flag = False success = self.controller(path) print 'sent first path' else: self.next_paths.put(path)
def run(self): ros_poses = [] for i in range(len(self.waypoints)): rpose = util.particle_to_pose(self.waypoints[i]) ros_poses.append(rpose) marker = make_marker(rpose, i, "goalpoint") self.rp_waypoints.publish(marker) assert len(ros_poses) == len(self.waypoints) self.waypoints = util.world_to_map(self.waypoints, self.map.info) resp = self.controller(self.main_XYHV) print 'Main Trajectory sent.', resp.success rate = rospy.Rate(5) while len(self.rem_points) > 0: curpose = self.curpose for wid in self.rem_points: if np.linalg.norm(curpose[:2] - self.waypoints[wid, :2]) > self.TRY_DISTANCE: continue dubins_path, length = self.try_dubins(curpose, self.waypoints[wid]) if length > 0: self.hit_waypoints(dubins_path) self.rem_points.remove(wid) break rate.sleep()
def run_one_pose(self, pose, backward=False): if pose is None: raise RuntimeError('No waypoints is read.') # initialize states self.next_paths = Queue.Queue() # convert poses to ROS Pose and Marker rpose = util.particle_to_pose(pose) cur_pose = util.particle_to_pose( self.current_pose) # might be outdated marker = make_marker(rpose, 0, "goalpoint") self.rp_waypoints.publish(marker) first_flag = True sstart = 0 max_cnt = 8 cnt = 1 print 'Computing path from key points to waypoint...' resp = self.planner(cur_pose, rpose, backward) path = resp.path while not resp.success and cnt <= max_cnt: # perturb heading and keep runing perturb_pose = pose perturb_pose[2] += cnt / max_cnt * (2 * np.pi) rpose = util.particle_to_pose(perturb_pose) test_plantime = rospy.get_time() resp = self.planner(cur_pose, rpose, backward) print rospy.get_time() - test_plantime print 'Failed to find path.' cnt += 1 if not resp.success: print 'Failed to find path with all sampled headings.' return print 'Path found.' path = resp.path if path is not None: self.controller(path) backward_path = path.waypoints backward_path = [[p.x, p.y, p.h, -p.v] for p in backward_path[::-1]] backward_path = toXYHVPath(backward_path) self.main_path = False i = 0 while not self.main_path: i += 1 print i self.controller(backward_path)
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 run(self): rospy.sleep(1.0) self.num_hit = 0 self.on_main_path = False self.returned = False ros_poses = [] for i in range(len(self.waypoints)): rpose = util.particle_to_pose(self.waypoints[i]) ros_poses.append(rpose) marker = make_marker(rpose, i, "goalpoint") self.rp_waypoints.publish(marker) assert len(ros_poses) == len(self.waypoints) self.waypoints = util.world_to_map(self.waypoints, self.map.info) # start to main init_pose = self.curpose mnt_point_idx = np.argsort( np.linalg.norm(init_pose[:2] - self.main_path[:, :2], axis=-1)) for i in range(50, mnt_point_idx.shape[0]): mnt_point = self.main_path[mnt_point_idx[i]] bridge, bridge_len = self.try_dubins( init_pose, mnt_point, headings=np.linspace(-0.3, 0.3, 7, endpoint=True) + mnt_point[2]) if bridge is not None: bridge = util.map_to_world(bridge, self.map.info) break resp = self.controller(toXYHVPath(bridge, desired_speed=self.SPEED), 'on_bridge') print 'start ! ' # hit way points rate = rospy.Rate(5) while self.num_hit < len(self.waypoints): if not self.on_main_path: continue curpose = self.curpose for wid in self.rem_points: if np.linalg.norm(curpose[:2] - self.waypoints[wid, :2]) > self.TRY_DISTANCE: continue dubins_path, length = self.try_dubins(curpose, self.waypoints[wid]) if length > 0: self.on_main_path = False self.hit_waypoints(dubins_path) self.rem_points.remove(wid) marker = Marker() marker.ns = "waypoints" marker.id = wid marker.action = 2 self.rp_waypoints.publish(marker) break rate.sleep() rospy.sleep(5.0) # main to start while True: if np.linalg.norm(self.curpose - init_pose) <= 0.7 * self.TRY_DISTANCE: # find returned path to init_pose bridge, bridge_len = self.try_dubins(self.curpose, init_pose) # if found, sent path to controller print 'aaaaaaaaaaaaa' if bridge is not None: print 'bbbbbbbbbbbbbbb' end_path = toXYHVPath(bridge, desired_speed=self.SPEED) #IPython.embed() break rate.sleep() rospy.sleep(5.0) resp = self.controller(end_path, 'off_bridge') while not self.returned: rate.sleep()