Пример #1
0
class PatrolTask(Task):
    """
    A task which manages movement through a sequence of waypoints, potentially running sub-tasks at each waypoint.
    """

    ACCEL_TIME = 0.1

    def __init__(self, waypoints, loop=False, linear_offset=30, angular_offset=0.2, max_power=1.0):
        """
        Create a new Patrol task, specifying a sequence of waypoints, whether to patrol continuously, and tolerances
        used to determine when we've hit a waypoint and should start executing the waypoint's task.

        :param waypoints:
            List of :class:`triangula.navigation.TaskWaypoint` defining the patrol route.
        :param loop:
            Whether to patrol continuously, defaults to False in which case this task will return an ExitTask when it
            has completed all its waypoints. If True the task will not exit, and will repeatedly run through its list
            of TaskWaypoint targets until otherwise interrupted.
        :param linear_offset:
            Maximum linear distance away from the target Pose for each waypoint before we consider that we've hit it.
            Specified in mm, defaults to 20
        :param angular_offset:
            Maximum angular distance away from the target Pose for each waypoint before we consider that we've hit it.
            Specified in radians, defaults to 0.1
        :param max_power:
            A scale applied to motor speeds being sent to the chassis, defaults to 1.0 to move as fast as possible,
            lower values might be helpful when testing!
        """
        super(PatrolTask, self).__init__(task_name='Patrol', requires_compass=False)
        self.waypoints = waypoints
        self.loop = loop
        self.linear_offset = linear_offset
        self.angular_offset = angular_offset
        self.active_waypoint_index = None
        self.active_subtask = None
        self.dead_reckoning = None
        self.motion_limit = None
        self.pose_update_interval = IntervalCheck(interval=0.001)
        self.subtask_tick = 0
        self.max_power = max_power

    def init_task(self, context):
        self.active_waypoint_index = 0
        self.dead_reckoning = DeadReckoning(chassis=context.chassis, counts_per_revolution=3310)
        self.motion_limit = MotionLimit(
            linear_acceleration_limit=context.chassis.get_max_translation_speed() / PatrolTask.ACCEL_TIME,
            angular_acceleration_limit=context.chassis.get_max_rotation_speed() / PatrolTask.ACCEL_TIME)

    def poll_task(self, context, tick):
        # Check to see whether the minimum interval between dead reckoning updates has passed
        # Do this whether we're navigating or not, as it'll also register motion performed during the execution of a
        # task while at a waypoint.
        if self.pose_update_interval.should_run():
            self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
            # print self.dead_reckoning.pose

        waypoint = self.waypoints[self.active_waypoint_index]

        # If we don't have an active sub-task, we're in waypoint seeking mode.
        if self.active_subtask is None:
            target_pose = waypoint.pose
            # Are we close enough?
            if self.dead_reckoning.pose.is_close_to(target_pose, max_distance=self.linear_offset,
                                                    max_orientation_difference=self.angular_offset):
                # Close enough, do we have to come to a complete stop first?
                print 'Waypoint reached - pose is {}, target is {}'.format(self.dead_reckoning.pose, target_pose)
                if waypoint.stop:
                    braking_start_time = time.time()
                    while time.time() - braking_start_time <= PatrolTask.ACCEL_TIME:
                        self._set_motion(motion=Motion(), context=context)
                        if self.pose_update_interval.should_run():
                            self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
                    # Stop full, this should already have happened but in case it didn't we don't want the
                    # robot to be moving while it runs the intermediate tasks.
                    context.arduino.set_motor_power(0, 0, 0)
                # Stopped or not, we now pick the waypoint task and start running it
                self.active_subtask = waypoint.task
                print 'Task is {}'.format(self.active_subtask)
                if self.active_subtask is None:
                    self.active_subtask = ExitTask()
                self.active_subtask.init_task(context=context)
                self.subtask_tick = 0
            else:
                # Not close enough, move towards waypoint
                print 'Moving towards waypoint'
                motion = self.dead_reckoning.pose.pose_to_pose_motion(to_pose=target_pose, time_seconds=0.01)
                scale = context.chassis.get_wheel_speeds(motion=motion).scaling
                motion = Motion(translation=motion.translation * scale, rotation=motion.rotation * scale)
                self._set_motion(motion=motion, context=context)
        else:
            # We have a sub-task, should probably run it or something. Check it's not an ExitTask first though
            if isinstance(self.active_subtask, ExitTask):
                print 'self.active_subtask is an ExitTask, setting to None'
                self.active_subtask = None
            else:
                print 'Polling subtask, tick {}'.format(self.subtask_tick)
                task_result = self.active_subtask.poll_task(context=context, tick=self.subtask_tick)
                self.subtask_tick += 1
                if task_result is not None:
                    self.subtask_tick = 0
                    self.active_subtask = task_result
                    self.active_subtask.init_task(context=context)
            if self.active_subtask is None:
                print 'self.active_subtask is None, moving to next waypoint'
                # A previous sub-task returned an ExitTask, so we're done here. Move to the next waypoint, or exit
                # if we've hit all of them and we're not looping
                self.active_waypoint_index += 1
                if self.active_waypoint_index >= len(self.waypoints):
                    if self.loop:
                        self.active_waypoint_index = 0
                    else:
                        return ExitTask()

    def _set_motion(self, motion, context):
        """
        Using the motion limit traction control, apply the specified motion to the chassis.
        """
        motion = self.motion_limit.limit_and_return(motion=motion)
        wheel_speeds = context.chassis.get_wheel_speeds(motion=motion)
        speeds = wheel_speeds.speeds
        power = [speeds[i] / context.chassis.wheels[i].max_speed for i in range(0, 3)]
        context.arduino.set_motor_power(power[0] * self.max_power,
                                        power[1] * self.max_power,
                                        power[2] * self.max_power)
Пример #2
0
class PatrolTask(Task):
    """
    A task which manages movement through a sequence of waypoints, potentially running sub-tasks at each waypoint.
    """

    ACCEL_TIME = 0.1

    def __init__(self,
                 waypoints,
                 loop=False,
                 linear_offset=30,
                 angular_offset=0.2,
                 max_power=1.0):
        """
        Create a new Patrol task, specifying a sequence of waypoints, whether to patrol continuously, and tolerances
        used to determine when we've hit a waypoint and should start executing the waypoint's task.

        :param waypoints:
            List of :class:`triangula.navigation.TaskWaypoint` defining the patrol route.
        :param loop:
            Whether to patrol continuously, defaults to False in which case this task will return an ExitTask when it
            has completed all its waypoints. If True the task will not exit, and will repeatedly run through its list
            of TaskWaypoint targets until otherwise interrupted.
        :param linear_offset:
            Maximum linear distance away from the target Pose for each waypoint before we consider that we've hit it.
            Specified in mm, defaults to 20
        :param angular_offset:
            Maximum angular distance away from the target Pose for each waypoint before we consider that we've hit it.
            Specified in radians, defaults to 0.1
        :param max_power:
            A scale applied to motor speeds being sent to the chassis, defaults to 1.0 to move as fast as possible,
            lower values might be helpful when testing!
        """
        super(PatrolTask, self).__init__(task_name='Patrol',
                                         requires_compass=False)
        self.waypoints = waypoints
        self.loop = loop
        self.linear_offset = linear_offset
        self.angular_offset = angular_offset
        self.active_waypoint_index = None
        self.active_subtask = None
        self.dead_reckoning = None
        self.motion_limit = None
        self.pose_update_interval = IntervalCheck(interval=0.001)
        self.subtask_tick = 0
        self.max_power = max_power

    def init_task(self, context):
        self.active_waypoint_index = 0
        self.dead_reckoning = DeadReckoning(chassis=context.chassis,
                                            counts_per_revolution=3310)
        self.motion_limit = MotionLimit(
            linear_acceleration_limit=context.chassis.
            get_max_translation_speed() / PatrolTask.ACCEL_TIME,
            angular_acceleration_limit=context.chassis.get_max_rotation_speed(
            ) / PatrolTask.ACCEL_TIME)

    def poll_task(self, context, tick):
        # Check to see whether the minimum interval between dead reckoning updates has passed
        # Do this whether we're navigating or not, as it'll also register motion performed during the execution of a
        # task while at a waypoint.
        if self.pose_update_interval.should_run():
            self.dead_reckoning.update_from_counts(
                context.arduino.get_encoder_values())
            # print self.dead_reckoning.pose

        waypoint = self.waypoints[self.active_waypoint_index]

        # If we don't have an active sub-task, we're in waypoint seeking mode.
        if self.active_subtask is None:
            target_pose = waypoint.pose
            # Are we close enough?
            if self.dead_reckoning.pose.is_close_to(
                    target_pose,
                    max_distance=self.linear_offset,
                    max_orientation_difference=self.angular_offset):
                # Close enough, do we have to come to a complete stop first?
                print 'Waypoint reached - pose is {}, target is {}'.format(
                    self.dead_reckoning.pose, target_pose)
                if waypoint.stop:
                    braking_start_time = time.time()
                    while time.time(
                    ) - braking_start_time <= PatrolTask.ACCEL_TIME:
                        self._set_motion(motion=Motion(), context=context)
                        if self.pose_update_interval.should_run():
                            self.dead_reckoning.update_from_counts(
                                context.arduino.get_encoder_values())
                    # Stop full, this should already have happened but in case it didn't we don't want the
                    # robot to be moving while it runs the intermediate tasks.
                    context.arduino.set_motor_power(0, 0, 0)
                # Stopped or not, we now pick the waypoint task and start running it
                self.active_subtask = waypoint.task
                print 'Task is {}'.format(self.active_subtask)
                if self.active_subtask is None:
                    self.active_subtask = ExitTask()
                self.active_subtask.init_task(context=context)
                self.subtask_tick = 0
            else:
                # Not close enough, move towards waypoint
                print 'Moving towards waypoint'
                motion = self.dead_reckoning.pose.pose_to_pose_motion(
                    to_pose=target_pose, time_seconds=0.01)
                scale = context.chassis.get_wheel_speeds(motion=motion).scaling
                motion = Motion(translation=motion.translation * scale,
                                rotation=motion.rotation * scale)
                self._set_motion(motion=motion, context=context)
        else:
            # We have a sub-task, should probably run it or something. Check it's not an ExitTask first though
            if isinstance(self.active_subtask, ExitTask):
                print 'self.active_subtask is an ExitTask, setting to None'
                self.active_subtask = None
            else:
                print 'Polling subtask, tick {}'.format(self.subtask_tick)
                task_result = self.active_subtask.poll_task(
                    context=context, tick=self.subtask_tick)
                self.subtask_tick += 1
                if task_result is not None:
                    self.subtask_tick = 0
                    self.active_subtask = task_result
                    self.active_subtask.init_task(context=context)
            if self.active_subtask is None:
                print 'self.active_subtask is None, moving to next waypoint'
                # A previous sub-task returned an ExitTask, so we're done here. Move to the next waypoint, or exit
                # if we've hit all of them and we're not looping
                self.active_waypoint_index += 1
                if self.active_waypoint_index >= len(self.waypoints):
                    if self.loop:
                        self.active_waypoint_index = 0
                    else:
                        return ExitTask()

    def _set_motion(self, motion, context):
        """
        Using the motion limit traction control, apply the specified motion to the chassis.
        """
        motion = self.motion_limit.limit_and_return(motion=motion)
        wheel_speeds = context.chassis.get_wheel_speeds(motion=motion)
        speeds = wheel_speeds.speeds
        power = [
            speeds[i] / context.chassis.wheels[i].max_speed
            for i in range(0, 3)
        ]
        context.arduino.set_motor_power(power[0] * self.max_power,
                                        power[1] * self.max_power,
                                        power[2] * self.max_power)
Пример #3
0
    def poll_task(self, context, tick):
        # Check to see whether the minimum interval between dead reckoning updates has passed
        # Do this whether we're navigating or not, as it'll also register motion performed during the execution of a
        # task while at a waypoint.
        if self.pose_update_interval.should_run():
            self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
            # print self.dead_reckoning.pose

        waypoint = self.waypoints[self.active_waypoint_index]

        # If we don't have an active sub-task, we're in waypoint seeking mode.
        if self.active_subtask is None:
            target_pose = waypoint.pose
            # Are we close enough?
            if self.dead_reckoning.pose.is_close_to(target_pose, max_distance=self.linear_offset,
                                                    max_orientation_difference=self.angular_offset):
                # Close enough, do we have to come to a complete stop first?
                print 'Waypoint reached - pose is {}, target is {}'.format(self.dead_reckoning.pose, target_pose)
                if waypoint.stop:
                    braking_start_time = time.time()
                    while time.time() - braking_start_time <= PatrolTask.ACCEL_TIME:
                        self._set_motion(motion=Motion(), context=context)
                        if self.pose_update_interval.should_run():
                            self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
                    # Stop full, this should already have happened but in case it didn't we don't want the
                    # robot to be moving while it runs the intermediate tasks.
                    context.arduino.set_motor_power(0, 0, 0)
                # Stopped or not, we now pick the waypoint task and start running it
                self.active_subtask = waypoint.task
                print 'Task is {}'.format(self.active_subtask)
                if self.active_subtask is None:
                    self.active_subtask = ExitTask()
                self.active_subtask.init_task(context=context)
                self.subtask_tick = 0
            else:
                # Not close enough, move towards waypoint
                print 'Moving towards waypoint'
                motion = self.dead_reckoning.pose.pose_to_pose_motion(to_pose=target_pose, time_seconds=0.01)
                scale = context.chassis.get_wheel_speeds(motion=motion).scaling
                motion = Motion(translation=motion.translation * scale, rotation=motion.rotation * scale)
                self._set_motion(motion=motion, context=context)
        else:
            # We have a sub-task, should probably run it or something. Check it's not an ExitTask first though
            if isinstance(self.active_subtask, ExitTask):
                print 'self.active_subtask is an ExitTask, setting to None'
                self.active_subtask = None
            else:
                print 'Polling subtask, tick {}'.format(self.subtask_tick)
                task_result = self.active_subtask.poll_task(context=context, tick=self.subtask_tick)
                self.subtask_tick += 1
                if task_result is not None:
                    self.subtask_tick = 0
                    self.active_subtask = task_result
                    self.active_subtask.init_task(context=context)
            if self.active_subtask is None:
                print 'self.active_subtask is None, moving to next waypoint'
                # A previous sub-task returned an ExitTask, so we're done here. Move to the next waypoint, or exit
                # if we've hit all of them and we're not looping
                self.active_waypoint_index += 1
                if self.active_waypoint_index >= len(self.waypoints):
                    if self.loop:
                        self.active_waypoint_index = 0
                    else:
                        return ExitTask()
Пример #4
0
    def poll_task(self, context, tick):
        # Check to see whether the minimum interval between dead reckoning updates has passed
        # Do this whether we're navigating or not, as it'll also register motion performed during the execution of a
        # task while at a waypoint.
        if self.pose_update_interval.should_run():
            self.dead_reckoning.update_from_counts(
                context.arduino.get_encoder_values())
            # print self.dead_reckoning.pose

        waypoint = self.waypoints[self.active_waypoint_index]

        # If we don't have an active sub-task, we're in waypoint seeking mode.
        if self.active_subtask is None:
            target_pose = waypoint.pose
            # Are we close enough?
            if self.dead_reckoning.pose.is_close_to(
                    target_pose,
                    max_distance=self.linear_offset,
                    max_orientation_difference=self.angular_offset):
                # Close enough, do we have to come to a complete stop first?
                print 'Waypoint reached - pose is {}, target is {}'.format(
                    self.dead_reckoning.pose, target_pose)
                if waypoint.stop:
                    braking_start_time = time.time()
                    while time.time(
                    ) - braking_start_time <= PatrolTask.ACCEL_TIME:
                        self._set_motion(motion=Motion(), context=context)
                        if self.pose_update_interval.should_run():
                            self.dead_reckoning.update_from_counts(
                                context.arduino.get_encoder_values())
                    # Stop full, this should already have happened but in case it didn't we don't want the
                    # robot to be moving while it runs the intermediate tasks.
                    context.arduino.set_motor_power(0, 0, 0)
                # Stopped or not, we now pick the waypoint task and start running it
                self.active_subtask = waypoint.task
                print 'Task is {}'.format(self.active_subtask)
                if self.active_subtask is None:
                    self.active_subtask = ExitTask()
                self.active_subtask.init_task(context=context)
                self.subtask_tick = 0
            else:
                # Not close enough, move towards waypoint
                print 'Moving towards waypoint'
                motion = self.dead_reckoning.pose.pose_to_pose_motion(
                    to_pose=target_pose, time_seconds=0.01)
                scale = context.chassis.get_wheel_speeds(motion=motion).scaling
                motion = Motion(translation=motion.translation * scale,
                                rotation=motion.rotation * scale)
                self._set_motion(motion=motion, context=context)
        else:
            # We have a sub-task, should probably run it or something. Check it's not an ExitTask first though
            if isinstance(self.active_subtask, ExitTask):
                print 'self.active_subtask is an ExitTask, setting to None'
                self.active_subtask = None
            else:
                print 'Polling subtask, tick {}'.format(self.subtask_tick)
                task_result = self.active_subtask.poll_task(
                    context=context, tick=self.subtask_tick)
                self.subtask_tick += 1
                if task_result is not None:
                    self.subtask_tick = 0
                    self.active_subtask = task_result
                    self.active_subtask.init_task(context=context)
            if self.active_subtask is None:
                print 'self.active_subtask is None, moving to next waypoint'
                # A previous sub-task returned an ExitTask, so we're done here. Move to the next waypoint, or exit
                # if we've hit all of them and we're not looping
                self.active_waypoint_index += 1
                if self.active_waypoint_index >= len(self.waypoints):
                    if self.loop:
                        self.active_waypoint_index = 0
                    else:
                        return ExitTask()