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)