def create_cache(self):
self._cache = dict()
for goal in self._goals_svc().goals:
position = Point.from_pose2d(goal.position)
orientation = Quaternion(0, 0, 0, 1)
pose = Pose(position, orientation)
self._cache[goal.name] = pose
def _feedback_cb(self, feedback):
cur_pos = Pose.from_pose_stamped(feedback.base_position)
target_pos = self._req.target_pose.pose
# Check if we've arrived within the accepted radius of our target
if cur_pos.distance_to(target_pos) < self._target_radius:
self._stop()
self._goal_status = GoalStatus.SUCCEEDED
self._action_finished = True
def __distance_cb(self, data):
# Create a Pose message to validate our input data
goal = Pose.from_pose(data.goal)
# Get a path to the given goal
path = Path.from_posearray(self._plan_svc(goal).path)
# Calculate distance if a path was found, or return inf
distance = path.length if path else float('inf')
return DistanceToGoalResponse(distance=distance)
def _start(self, tick):
if not self._req:
coords = tick.blackboard.get('locations')[self._target_location]
position = Point(*coords)
orientation = Quaternion(0, 0, 0, 1)
pose = Pose(position, orientation)
self._req = MoveBaseGoal()
self._req.target_pose.header.frame_id = 'map'
self._req.target_pose.header.stamp = rospy.Time.now()
self._req.target_pose.pose = pose
self._cli.send_goal(self._req,
done_cb=self._done_cb,
active_cb=self._active_cb,
feedback_cb=self._feedback_cb)
def _feedback_cb(self, feedback):
cur_pos = Pose.from_pose_stamped(feedback.base_position)
target_pos = self._req.target_pose.pose
# Check if we've arrived within the accepted radius of our target
if cur_pos.distance_to(target_pos) < self._target_radius:
self._stop()
self._goal_status = GoalStatus.SUCCEEDED
self._action_finished = True
return
# Check if we've made progress towards our target
# Restart the counter if we have
target_pos = self._req.target_pose.pose
distance = self._dist_svc(target_pos).distance
if self._target_distance > distance:
self._target_distance = distance
rospy.loginfo("Progress made. New distance: %d" %
self._target_distance)
self._timeout_counter.shutdown()
self._timeout_counter = rospy.Timer(rospy.Duration(15),
self._timeout)