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 __init__(self, mainpoints_file): rospy.init_node('final_planner', anonymous=True) # parse main way points self.CURVATURE = 0.05 #0.075 self.TRY_DISTANCE = 50 self.SAMPLED_HEADINGS = np.linspace(0, 2 * np.pi, 16, endpoint=False) self.SPEED = 0.5 self.main_path = _parse_mainpoints(mainpoints_file) self.main_XYHV = toXYHVPath(self.main_path, desired_speed=1.5 * self.SPEED) self.waypoints = None self.rem_points = None pose_topic = rospy.get_param("/final_planner/pose_topic", "/sim_car_pose/pose") self.curpose = None self.map = self.get_map() self.map_x = self.map.info.origin.position.x self.map_y = self.map.info.origin.position.y self.map_angle = util.rosquaternion_to_angle( self.map.info.origin.orientation) self.map_c = np.cos(self.map_angle) self.map_s = np.sin(self.map_angle) self.map_data = self.load_permissible_region(self.map) self.main_path = util.world_to_map(self.main_path, self.map.info) self.pose_sub = rospy.Subscriber(pose_topic, PoseStamped, self.pose_cb) self.rp_waypoints = rospy.Publisher('xx_waypoints', Marker, queue_size=20) self.planning_env = DubinsMapEnvironment(self.map_data.transpose(), curvature=self.CURVATURE) rospy.wait_for_message(pose_topic, PoseStamped) self.PathCompletedService = rospy.Service( "/final_planner/path_complete", SignalPathComplete, self.path_completed_cb) self.RunWaypointsService = rospy.Service( "/final_planner/run_waypoints", ReadFile, self.waypoints_received) self.controller = rospy.ServiceProxy("/controller/follow_path", StampedFollowPath()) self.on_main_path = False self.returned = False self.num_hit = 0 # self.rp_waypoints = rospy.Publisher('xx_waypoints', Marker, queue_size=100) rospy.spin()
def pose_cb(self, msg): pose = util.rospose_to_posetup(msg.pose) self.curpose = (util.world_to_map(np.array([pose]), self.map.info)).reshape(3)
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()