def get_goal(self, msg):
"""
- plan in map frame - execute in world frame
don't forget tranform back to world frame
"""
print("Got a new goal\n{}".format(msg.pose))
goal_pose = util.rospose_to_posetup(msg.pose)
self.goal = self.world2map(np.array([goal_pose]))[0]
start_pose = util.rospose_to_posetup(self.current_pose)
self.start = self.world2map(np.array([start_pose]))[0]
print("goal and start:", self.goal, self.start)
map_points = None
if self.multi_goals:
if self.route_sent:
self.route_sent = False
self.goals = []
self.goals += [self.goal.copy()]
if len(self.goals) == self.num_goals:
print(
"Got the final goal for mutli goal. Planning for multiple goals"
)
map_points = self.plan_multi_goals(self.start)
else:
print("Got {}/{} goals.".format(len(self.goals),
self.num_goals))
else:
print("start single goal")
map_points = self.plan_to_goal(self.start, self.goal)
if map_points is not None and len(map_points) > 0:
world_points = self.map2world(map_points)
success = self.send_path(np.array(world_points))
print("Sent path")
def get_goal(self, msg):
print("Got a new goal\n{}".format(msg.pose))
goal_pose = util.rospose_to_posetup(msg.pose)
self.goal = self.world2map(np.array([goal_pose]))[0]
start_pose = util.rospose_to_posetup(self.current_pose)
self.start = self.world2map(np.array([start_pose]))[0]
world_points = None
if self.multi_goals:
if self.route_sent:
self.route_sent = False
self.goals = []
self.goals += [self.goal.copy()]
if len(self.goals) == self.num_goals:
print(
"Got the final goal for mutli goal. Planning for multiple goals"
)
world_points = self.plan_multi_goals(self.start)
else:
print("Got {}/{} goals.".format(len(self.goals),
self.num_goals))
else:
world_points = self.plan_to_goal(self.start, self.goal)
if world_points is not None and len(world_points) > 0:
success = self.send_path(world_points)
print("Sent path")
def waypoints_received(self, req):
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)
poses = fitdubins.fit_spline(waypoints)
# fitdubins.visualize_dubins(waypoints, headings, 1/1.6, ax)
self.run(poses)
return ReadFileResponse()
def gen_path(self, req):
"""
Plan a path from start to goal
Return a path
"""
# Implement here
# Plan with lazy_astar
start = self.world2map(np.array([util.rospose_to_posetup(req.start)]))
goal = self.world2map(np.array([util.rospose_to_posetup(req.goal)]))
start, goal = tuple(start[0]), tuple(goal[0])
map_points = self.plan_to_goal(start, goal)
if map_points is not None and len(map_points) > 0:
world_points = self.map2world(map_points)
path = self.toXYHVPath(world_points)
return GeneratePathResponse(path, True)
else:
return GeneratePathResponse(None, False)
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 update_state(self, msg):
pose = np.array(util.rospose_to_posetup(msg.pose)).reshape([
3,
])
self.current_pose = pose
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)
