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