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()