def __init__(self, point_ids, locations, neighbors, landmark_ids,
landmark_positions, landmark_angles):
# listen for frame transformations
TfTransformer.__init__(self)
# store raw tag data, data in the odom frame, and data in the base frame
self.tags = {}
self.tags_base = {}
self.tags_odom = {}
# set up the transformer between the map and ekf
self._transform = self._transform = {
"map_pos": Point(0, 0, 0),
"map_angle": 0,
"odom_pos": Point(0, 0, 0),
"odom_angle": 0
}
self.floorplan = FloorPlan(point_ids, locations, neighbors,
landmark_ids, landmark_positions,
landmark_angles)
# smooth data by selectively sampling
self._prev_odom = [0, 0, 0, 0, 0, 0, 1]
# set up logger and csv logging
self._logger = Logger("Localization")
# subscribe to raw tag data
rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
self._tagCallback,
queue_size=1)
def __init__(self, jerky=False, walking_speed=1):
# listen for frame transformations
TfTransformer.__init__(self)
# initialize motion component of navigation
self._motion = Motion()
self._sensors = Sensors()
self._jerky = jerky
self._walking_speed = min(abs(walking_speed), 1)
self._logger = Logger("Navigation")
# set up obstacle avoidance
self._avoiding = False
self._obstacle = False
self._bumped = False
self._bumper = 0
# we're going to send the turtlebot to a point a quarter meter ahead of itself
self._avoid_goto = PointStamped()
self._avoid_goto.header.frame_id = "/base_footprint"
self._avoid_goto.point.x = self._AVOID_DIST
self._avoid_target = None
self._avoid_turn = None
# subscibe to the robot_pose_ekf odometry information
self.p = None
self.q = None
self.angle = 0
self._target_turn_delta = 0
rospy.Subscriber('/robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped, self._ekfCallback)
# set up navigation to destination data
self._reached_goal = True
# set up the odometry reset publisher (publishing Empty messages here will reset odom)
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=1)
# reset odometry (these messages take about a second to get through)
timer = time()
while time() - timer < 1 or self.p is None:
reset_odom.publish(Empty())