Esempio n. 1
0
    def handle_footstep_plan(self, channel, msg):
        if isinstance(msg, str):
            msg = drc.footstep_plan_t.decode(msg)

        footsteps, opts = decode_footstep_plan(msg)
        self.last_params = msg.params

        if len(footsteps) <= 2:
            # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op
            print 'BDI step translator: Empty plan recieved. Not executing.'
            return

        behavior = opts['behavior']
        #if behavior == Behavior.BDI_WALKING:
        #    # duration = 0.6
        #    if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2:
        #        print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation'
        #        return
        #elif behavior != Behavior.BDI_STEPPING:
        #    print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior"
        #    return

        self.behavior = behavior

        now = time.time()
        if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT:
            self.executing = False
        self.last_footstep_plan_time = now

        if self.mode == Mode.plotting:
            self.draw(footsteps)
        else:
            #if not self.executing:
            print "Starting new footstep plan"
            self.bdi_step_queue_in = footsteps
            self.send_params(1)
            if not self.safe:
                m = "BDI step translator: Steps received; transitioning to {:s}".format(
                    "BDI_STEP" if self.behavior ==
                    Behavior.BDI_STEPPING else "BDI_WALK")
                print m
                ut.send_status(6, 0, 0, m)
                time.sleep(1)
                self.executing = True
                self.send_behavior()
            else:
                m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format(
                    "BDI_STEP" if self.behavior ==
                    Behavior.BDI_STEPPING else "BDI_WALK")
                print m
                ut.send_status(6, 0, 0, m)
Esempio n. 2
0
    def handle_footstep_plan(self, channel, msg):
        if isinstance(msg, str):
            msg = drc.footstep_plan_t.decode(msg)

        footsteps, opts = decode_footstep_plan(msg)
        self.last_params = msg.params

        if len(footsteps) <= 2:
            # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op
            print 'BDI step translator: Empty plan recieved. Not executing.'
            return

        behavior = opts['behavior']
        #if behavior == Behavior.BDI_WALKING:
        #    # duration = 0.6
        #    if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2:
        #        print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation'
        #        return
        #elif behavior != Behavior.BDI_STEPPING:
        #    print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior"
        #    return

        self.behavior = behavior

        now = time.time()
        if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT:
            self.executing = False
        self.last_footstep_plan_time = now

        if self.mode == Mode.plotting:
            self.draw(footsteps)
        else:
            #if not self.executing:
            print "Starting new footstep plan"
            self.bdi_step_queue_in = footsteps
            self.send_params(1)
            if not self.safe:
                m = "BDI step translator: Steps received; transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK")
                print m
                ut.send_status(6,0,0,m)
                time.sleep(1)
                self.executing = True
                self.send_behavior()
            else:
                m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK")
                print m
                ut.send_status(6,0,0,m)
Esempio n. 3
0
    def handle_footstep_plan(self, channel, msg):
        if isinstance(msg, str):
            try:
                msg = drc.deprecated_footstep_plan_t.decode(msg)
            except ValueError:
                msg = drc.footstep_plan_t.decode(msg)
        if isinstance(msg, drc.deprecated_footstep_plan_t):
            footsteps, opts = decode_deprecated_footstep_plan(msg)
        elif isinstance(msg, drc.footstep_plan_t):
            footsteps, opts = decode_footstep_plan(msg)
            self.last_params = msg.params
        else:
            raise ValueError(
                "Can't decode footsteps: not a drc.footstep_plan_t or drc.deprecated_footstep_plan_t"
            )

        if len(footsteps) <= 2:
            # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op
            print 'BDI step translator: Empty plan recieved. Not executing.'
            return

        behavior = opts['behavior']
        if behavior == Behavior.BDI_WALKING:
            # duration = 0.6
            if len(footsteps) < NUM_REQUIRED_WALK_STEPS + 2:
                print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation'
                return
        elif behavior != Behavior.BDI_STEPPING:
            print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior"
            return

        self.behavior = behavior

        now = time.time()
        if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT:
            self.executing = False
        self.last_footstep_plan_time = now

        if self.mode == Mode.plotting:
            self.draw(footsteps)
        else:
            if not self.executing:
                print "Starting new footstep plan"
                self.bdi_step_queue_in = footsteps
                self.send_params(1)
                if not self.safe:
                    m = "BDI step translator: Steps received; transitioning to {:s}".format(
                        "BDI_STEP" if self.behavior ==
                        Behavior.BDI_STEPPING else "BDI_WALK")
                    print m
                    ut.send_status(6, 0, 0, m)
                    time.sleep(1)
                    self.executing = True
                    self.send_behavior()
                else:
                    m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format(
                        "BDI_STEP" if self.behavior ==
                        Behavior.BDI_STEPPING else "BDI_WALK")
                    print m
                    ut.send_status(6, 0, 0, m)

            else:
                print "Got updated footstep plan"
                if self.bdi_step_queue_in[
                        self.delivered_index -
                        1].is_right_foot == footsteps[0].is_right_foot:
                    print "Re-aligning new footsteps to current plan"
                    self.bdi_step_queue_in = self.bdi_step_queue_in[:self.
                                                                    delivered_index
                                                                    -
                                                                    1] + footsteps
                else:
                    print "Can't align the updated plan to the current plan"
                    return
Esempio n. 4
0
    def handle_footstep_plan(self, channel, msg):
        if isinstance(msg, str):
            try:
                msg = drc.deprecated_footstep_plan_t.decode(msg)
            except ValueError:
                msg = drc.footstep_plan_t.decode(msg)
        if isinstance(msg, drc.deprecated_footstep_plan_t):
            footsteps, opts = decode_deprecated_footstep_plan(msg)
        elif isinstance(msg, drc.footstep_plan_t):
            footsteps, opts = decode_footstep_plan(msg)
            self.last_params = msg.params
        else:
            raise ValueError("Can't decode footsteps: not a drc.footstep_plan_t or drc.deprecated_footstep_plan_t")

        if len(footsteps) <= 2:
            # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op
            print 'BDI step translator: Empty plan recieved. Not executing.'
            return

        behavior = opts['behavior']
        if behavior == Behavior.BDI_WALKING:
            # duration = 0.6
            if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2:
                print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation'
                return
        elif behavior != Behavior.BDI_STEPPING:
            print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior"
            return

        self.behavior = behavior

        now = time.time()
        if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT:
            self.executing = False
        self.last_footstep_plan_time = now

        if self.mode == Mode.plotting:
            self.draw(footsteps)
        else:
            if not self.executing:
                print "Starting new footstep plan"
                self.bdi_step_queue_in = footsteps
                self.send_params(1)
                if not self.safe:
                    m = "BDI step translator: Steps received; transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK")
                    print m
                    ut.send_status(6,0,0,m)
                    time.sleep(1)
                    self.executing = True
                    self.send_behavior()
                else:
                    m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK")
                    print m
                    ut.send_status(6,0,0,m)

            else:
                print "Got updated footstep plan"
                if self.bdi_step_queue_in[self.delivered_index-1].is_right_foot == footsteps[0].is_right_foot:
                    print "Re-aligning new footsteps to current plan"
                    self.bdi_step_queue_in = self.bdi_step_queue_in[:self.delivered_index-1] + footsteps
                else:
                    print "Can't align the updated plan to the current plan"
                    return