def handle_mushr_global_planner(self, request):
print("Start Position", request.start.position)
print("Goal Position", request.goal.position)
self.pub_start.publish(request.start)
self.pub_goal.publish(request.goal)
start = utils.rospose_to_posetup(request.start, self.map_metadata)
goal = utils.rospose_to_posetup(request.goal, self.map_metadata)
if str(start)+str(goal) in self.path_dict and self.reuse_plans:
print("Reusing plan from cache")
print("waiting for service call...")
return self.path_dict[str(start)+str(goal)]
else:
success, path = self.global_planner.plan(start, goal, request.turning_radius, request.planning_time)
pose_array = PoseArray()
pose_array.header.frame_id = 'map'
if success:
pose_array.poses = []
for posetup in path:
pose_array.poses.append(utils.posetup_to_rospose(posetup, self.map_metadata))
response = MushrGlobalPlannerResponse()
response.success.data = success
response.path = pose_array
self.pub_path.publish(response.path)
self.path_dict[str(start)+str(goal)] = response
print("waiting for service call...")
return response
def cb_goal(self, msg):
goal = self.dtype(utils.rospose_to_posetup(msg.pose))
self.ready_event.wait()
if not self.rhctrl.set_goal(goal):
self.logger.err("That goal is unreachable, please choose another")
return
else:
self.logger.info("Goal set")
self.goal_event.set()
def cb_initialpose(self, msg):
self.init_pose = self.dtype(utils.rospose_to_posetup(msg.pose.pose))
self.logger.info("Got initial pose")
if self.debug_current_path:
# If the current path already exists, delete it.
self.current_path.action = self.current_path.DELETE
self.current_path_pub.publish(self.current_path)
self.current_path.action = self.current_path.ADD
if self.debug_rollouts:
if self.goal is not None:
# There is viz_logic in here, so don't do anything with the return
self.rhctrl.step(self.init_pose)
else:
self.logger.info("No goal set")
def cb_pose(self, msg):
self.set_inferred_pose(self.dtype(utils.rospose_to_posetup(msg.pose)))
if self.cur_rollout is not None and self.cur_rollout_ip is not None:
m = Marker()
m.header.frame_id = "map"
m.type = m.LINE_STRIP
m.action = m.ADD
with self.traj_pub_lock:
pts = (self.cur_rollout[:, :2] -
self.cur_rollout_ip[:2]) + self.inferred_pose()[:2]
m.points = map(lambda xy: Point(x=xy[0], y=xy[1]), pts)
r, g, b = 0x36, 0xCD, 0xC4
m.colors = [
ColorRGBA(r=r / 255.0, g=g / 255.0, b=b / 255.0, a=0.7)
] * len(m.points)
m.scale.x = 0.05
self.traj_chosen_pub.publish(m)
def cb_goal(self, msg):
goal = self.dtype(utils.rospose_to_posetup(msg.pose))
self.logger.info("Got goal")
if self.rhctrl is not None:
if not self.rhctrl.set_goal(goal):
self.logger.err("That goal is unreachable, please choose another")
else:
self.logger.info("Goal set")
self.goal = goal
m = Marker()
m.header.frame_id = "map"
m.header.stamp = rospy.Time.now()
m.id = 1
m.type = m.ARROW
m.action = m.ADD
m.pose = msg.pose
m.color.r = 1.0
m.color.b = 1.0
m.scale.x = 1
m.scale.y = 0.1
m.scale.z = 0.1
self.goal_pub.publish(m)
def cb_map_metadata(self, msg):
default_map_name = "default"
map_file = self.params.get_str("map_file",
default=default_map_name,
global_=True)
name = os.path.splitext(os.path.basename(map_file))[0]
if name is default_map_name:
rospy.logwarn(
"Default map name being used, will be corrupted on map change. "
+ "To fix, set '/map_file' parameter with map_file location")
x, y, angle = utils.rospose_to_posetup(msg.origin)
self.map_data = types.MapData(
name=name,
resolution=msg.resolution,
origin_x=x,
origin_y=y,
orientation_angle=angle,
width=msg.width,
height=msg.height,
get_map_data=self.get_map,
)
def cb_inferred_pose(self, msg):
if self.init_pose is not None:
self.current_path.header.stamp = rospy.Time.now()
self.current_path.points.append(msg.pose.position)
self.current_path_pub.publish(self.current_path)
self.inferred_pose = self.dtype(utils.rospose_to_posetup(msg.pose))
def cb_odom(self, msg):
self.inferred_pose = utils.rospose_to_posetup(msg.pose.pose)
