Esempio n. 1
0
    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)
Esempio n. 2
0
    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())