コード例 #1
0
class Joystick(object):
    def __init__(self):
        self.force_scale = rospy.get_param("~force_scale", 600)
        self.torque_scale = rospy.get_param("~torque_scale", 500)

        self.remote = RemoteControl("joystick", "/wrench/rc")
        self.reset()
        rospy.Subscriber("joy", Joy, self.joy_recieved)

    def reset(self):
        '''
        Used to reset the state of the controller. Sometimes when it
        disconnects then comes back online, the settings are all out of whack.
        '''
        self.last_raise_kill = False
        self.last_clear_kill = False
        self.last_station_hold_state = False
        self.last_rc_control = False
        self.last_emergency_control = False
        self.last_keyboard_control = False
        self.last_back = False
        self.last_auto_control = False
        self.thruster_deploy_count = 0
        self.thruster_retract_count = 0

        self.start_count = 0
        self.last_joy = None
        self.active = False
        self.remote.clear_wrench()

    def check_for_timeout(self, joy):
        if self.last_joy is None:
            self.last_joy = joy
            return

        if joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons:

            # No change in state
            if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(
                    15 * 60):

                # The controller times out after 15 minutes
                if self.active:
                    rospy.logwarn(
                        "Controller Timed out. Hold start to resume.")
                    self.reset()

        else:
            joy.header.stamp = rospy.Time.now(
            )  # In the sim, stamps weren't working right
            self.last_joy = joy

    def joy_recieved(self, joy):
        self.check_for_timeout(joy)

        # Assigns readable names to the buttons that are used
        start = joy.buttons[7]
        back = joy.buttons[6]
        raise_kill = bool(joy.buttons[1])  # B
        clear_kill = bool(joy.buttons[2])  # X
        station_hold = bool(joy.buttons[0])  # A
        rc_control = bool(joy.buttons[11])  # d-pad left
        emergency_control = bool(joy.buttons[13])  # d-pad up
        keyboard_control = bool(joy.buttons[14])  # d-pad down
        auto_control = bool(joy.buttons[12])  # d-pad right
        thruster_retract = bool(joy.buttons[4])  # LB
        thruster_deploy = bool(joy.buttons[5])  # RB

        if back and not self.last_back:
            rospy.loginfo('Back pressed. Going inactive')
            self.reset()
            return

        # Reset controller state if only start is pressed down about 1 second
        self.start_count += start
        if self.start_count > 5:
            rospy.loginfo("Resetting controller state")
            self.reset()
            self.active = True

        if not self.active:
            self.remote.clear_wrench()
            return

        if thruster_retract:
            self.thruster_retract_count += 1
        else:
            self.thruster_retract_count = 0
        if thruster_deploy:
            self.thruster_deploy_count += 1
        else:
            self.thruster_deploy_count = 0

        if self.thruster_retract_count > 10:
            self.remote.retract_thrusters()
            self.thruster_retract_count = 0
        elif self.thruster_deploy_count > 10:
            self.remote.deploy_thrusters()
            self.thruster_deploy_count = 0

        if raise_kill and not self.last_raise_kill:
            self.remote.kill()

        if clear_kill and not self.last_clear_kill:
            self.remote.clear_kill()

        if station_hold and not self.last_station_hold_state:
            self.remote.station_hold()

        if rc_control and not self.last_rc_control:
            self.remote.select_rc_control()

        if emergency_control and not self.last_emergency_control:
            self.remote.select_emergency_control()

        if keyboard_control and not self.last_keyboard_control:
            self.remote.select_keyboard_control()

        if auto_control and not self.last_auto_control:
            self.remote.select_autonomous_control()

        self.last_back = back
        self.last_raise_kill = raise_kill
        self.last_clear_kill = clear_kill
        self.last_station_hold_state = station_hold
        self.last_rc_control = rc_control
        self.last_emergency_control = emergency_control
        self.last_keyboard_control = keyboard_control
        self.last_auto_control = auto_control

        # Scale joystick input to force and publish a wrench
        x = joy.axes[1] * self.force_scale
        y = joy.axes[0] * self.force_scale
        rotation = joy.axes[3] * self.torque_scale
        self.remote.publish_wrench(x, y, rotation, rospy.Time.now())
コード例 #2
0
class Joystick(object):

    def __init__(self):
        self.force_scale = rospy.get_param("~force_scale", 600)
        self.torque_scale = rospy.get_param("~torque_scale", 500)

        self.remote = RemoteControl("joystick", "/wrench/rc")
        self.reset()
        rospy.Subscriber("joy", Joy, self.joy_recieved)

        self.active = False

    def reset(self):
        '''
        Used to reset the state of the controller. Sometimes when it
        disconnects then comes back online, the settings are all out of whack.
        '''
        self.last_kill = False
        self.last_station_hold_state = False
        self.last_rc_control = False
        self.last_emergency_control = False
        self.last_keyboard_control = False
        self.last_auto_control = False
        self.last_change_mode = False
#        self.last_shooter_load = False
#        self.last_shooter_fire = False
        self.last_shooter_cancel = False

        self.start_count = 0
        self.last_joy = None
        self.active = False

        self.remote.clear_wrench()

    def check_for_timeout(self, joy):
        if self.last_joy is None:
            self.last_joy = joy
            return

        if joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons:

            # No change in state
            if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60):

                # The controller times out after 15 minutes
                if self.active:
                    rospy.logwarn("Controller Timed out. Hold start to resume.")
                    self.reset()

        else:
            joy.header.stamp = rospy.Time.now()  # In the sim, stamps weren't working right
            self.last_joy = joy

    def joy_recieved(self, joy):
        self.check_for_timeout(joy)

        # Assigns readable names to the buttons that are used
        start = joy.buttons[7]
        kill = bool(joy.buttons[2])  # X
        station_hold = bool(joy.buttons[0])  # A
        rc_control = bool(joy.buttons[11])  # d-pad left
        emergency_control = bool(joy.buttons[13])  # d-pad up
        keyboard_control = bool(joy.buttons[14])  # d-pad down
        auto_control = bool(joy.buttons[12])  # d-pad right
        change_mode = bool(joy.buttons[3])  # Y
#        shooter_load = bool(joy.buttons[4])
#        shooter_fire = bool(joy.axes[5] < -0.9)
        shooter_cancel = bool(joy.buttons[5])

        # Reset controller state if only start is pressed down about 3 seconds
        self.start_count += start
        if self.start_count > 10:
            rospy.loginfo("Resetting controller state")
            self.reset()
            self.active = True
            self.remote.clear_kill()

        if not self.active:
            return

        if kill and not self.last_kill:
            self.remote.toggle_kill()

        if station_hold and not self.last_station_hold_state:
            self.remote.station_hold()

        if rc_control and not self.last_rc_control:
            self.remote.select_rc_control()

        if emergency_control and not self.last_emergency_control:
            self.remote.select_emergency_control()

        if keyboard_control and not self.last_keyboard_control:
            self.remote.select_keyboard_control()

        if auto_control and not self.last_auto_control:
            self.remote.select_autonomous_control()

        if change_mode and not self.last_change_mode:
            self.remote.select_next_control()

#        if shooter_load and not self.last_shooter_load:
#            self.remote.shooter_load()

#        if shooter_fire and not self.last_shooter_fire:
#            self.remote.shooter_fire()

        if shooter_cancel and not self.last_shooter_cancel:
            self.remote.shooter_cancel()

        self.last_kill = kill
        self.last_station_hold_state = station_hold
        self.last_rc_control = rc_control
        self.last_emergency_control = emergency_control
        self.last_keyboard_control = keyboard_control
        self.last_auto_control = auto_control
        self.last_change_mode = change_mode
#        self.last_shooter_load = shooter_load
#        self.last_shooter_fire = shooter_fire
        self.last_shooter_cancel = shooter_cancel

        # Scale joystick input to force and publish a wrench
        x = joy.axes[1] * self.force_scale
        y = joy.axes[0] * self.force_scale
        rotation = joy.axes[3] * self.torque_scale
        self.remote.publish_wrench(x, y, rotation, joy.header.stamp)
コード例 #3
0
class Joystick(object):
    def __init__(self):
        self.force_scale = rospy.get_param("~force_scale", 600)
        self.torque_scale = rospy.get_param("~torque_scale", 500)

        self.remote = RemoteControl("joystick", "/wrench/rc")
        rospy.Subscriber("joy", Joy, self.joy_recieved)

        self.active = False
        self.reset()

    def reset(self):
        '''
        Used to reset the state of the controller. Sometimes when it
        disconnects then comes back online, the settings are all out of whack.
        '''
        self.last_kill = False
        self.last_station_hold_state = False
        self.last_change_mode = False
        self.last_auto_control = False
        self.last_rc_control = False
        self.last_keyboard_control = False
        self.last_shooter_load = False
        self.last_shooter_fire = False
        self.last_shooter_cancel = False

        self.start_count = 0
        self.last_joy = None
        self.active = False

        self.remote.clear_wrench()

    def check_for_timeout(self, joy):
        if self.last_joy is None:
            self.last_joy = joy
            return

        if joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons:

            # No change in state
            if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(
                    15 * 60):

                # The controller times out after 15 minutes
                if self.active:
                    rospy.logwarn(
                        "Controller Timed out. Hold start to resume.")
                    self.reset()

        else:
            joy.header.stamp = rospy.Time.now(
            )  # In the sim, stamps weren't working right
            self.last_joy = joy

    def joy_recieved(self, joy):
        self.check_for_timeout(joy)

        # Assigns readable names to the buttons that are used
        start = joy.buttons[7]
        change_mode = bool(joy.buttons[3])  # Y
        kill = bool(joy.buttons[2])  # X
        station_hold = bool(joy.buttons[0])  # A
        rc_control = bool(joy.buttons[11])  # d-pad left
        auto_control = bool(joy.buttons[12])  # d-pad right
        keyboard_control = bool(joy.buttons[14])  # d-pad down
        shooter_load = bool(joy.buttons[4])
        shooter_fire = bool(joy.axes[5] < -0.9)
        shooter_cancel = bool(joy.buttons[5])

        # Reset controller state if only start is pressed down about 3 seconds
        self.start_count += start
        if self.start_count > 10:
            rospy.loginfo("Resetting controller state")
            self.reset()
            self.active = True
            self.remote.clear_kill()

        if not self.active:
            return

        if kill and not self.last_kill:
            self.remote.toggle_kill()

        if station_hold and not self.last_station_hold_state:
            self.remote.station_hold()

        if change_mode and not self.last_change_mode:
            self.remote.select_next_control()

        if auto_control and not self.last_auto_control:
            self.remote.select_autonomous_control()

        if rc_control and not self.last_rc_control:
            self.remote.select_rc_control()

        if keyboard_control and not self.last_keyboard_control:
            self.remote.select_keyboard_control()

        if shooter_load and not self.last_shooter_load:
            self.remote.shooter_load()

        if shooter_fire and not self.last_shooter_fire:
            self.remote.shooter_fire()

        if shooter_cancel and not self.last_shooter_cancel:
            self.remote.shooter_cancel()

        self.last_kill = kill
        self.last_station_hold_state = station_hold
        self.last_change_mode = change_mode
        self.last_auto_control = auto_control
        self.last_rc_control = rc_control
        self.last_keyboard_control = keyboard_control
        self.last_shooter_load = shooter_load
        self.last_shooter_fire = shooter_fire
        self.last_shooter_cancel = shooter_cancel

        # Scale joystick input to force and publish a wrench
        x = joy.axes[1] * self.force_scale
        y = joy.axes[0] * self.force_scale
        rotation = joy.axes[3] * self.torque_scale
        self.remote.publish_wrench(x, y, rotation, joy.header.stamp)
コード例 #4
0
class KeyboardServer(object):

    def __init__(self):
        self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600)
        self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500)

        self.remote = RemoteControl("keyboard", "/wrench/keyboard")
        rospy.Service("/keyboard_control", KeyboardControl, self.key_recieved)

        # Initialize this to a random UUID so that a client without a UUID cannot authenticate
        self.locked_uuid = uuid.uuid4().hex

        # This maps each key to a remote control function
        self.key_mappings = {ord('k'): lambda: self.remote.toggle_kill(),
                             ord('K'): lambda: self.remote.kill(),
                             ord('h'): lambda: self.remote.station_hold(),
                             ord('j'): lambda: self.remote.select_rc_control(),
                             ord('e'): lambda: self.remote.select_emergency_control(),
                             ord('b'): lambda: self.remote.select_keyboard_control(),
                             ord('u'): lambda: self.remote.select_autonomous_control(),
                             ord('c'): lambda: self.remote.select_next_control(),
                             ord('r'): lambda: self.remote.shooter_load(),
                             ord('f'): lambda: self.remote.shooter_fire(),
                             ord('t'): lambda: self.remote.shooter_cancel(),
                             ord('w'): lambda: self.remote.publish_wrench(self.force_scale, 0, 0),
                             ord('s'): lambda: self.remote.publish_wrench(-self.force_scale, 0, 0),
                             ord('a'): lambda: self.remote.publish_wrench(0, self.force_scale, 0),
                             ord('d'): lambda: self.remote.publish_wrench(0, -self.force_scale, 0),
                             curses.KEY_LEFT: lambda: self.remote.publish_wrench(0, 0, self.torque_scale),
                             curses.KEY_RIGHT: lambda: self.remote.publish_wrench(0, 0, -self.torque_scale)
                             }
        self.movement_keys = [ord('w'), ord('s'), ord('a'), ord('d'), curses.KEY_LEFT, curses.KEY_RIGHT]

    def key_recieved(self, req):
        '''
        This function handles the process of locking control of the service to
        one client. If an 'L' is received, a UUID is generated for the client
        and control of the service is locked to it.
        '''

        # If the key pressed was L, locks control of the service to the clinet's UUID
        if (req.keycode == 76):

            # Generates a new UUID for the client if it does not already have one
            if (req.uuid is ''):
                self.locked_uuid = uuid.uuid4().hex
            else:
                self.locked_uuid = req.uuid
            return {"generated_uuid": self.locked_uuid, "is_locked": True}

        # If the key was from the client with locked control, pass it to execution
        elif (req.uuid == self.locked_uuid):
            self.execute_key(req.keycode)
            return {"generated_uuid": '', "is_locked": True}

        # Ignore keys sent by a client that has not locked control of the service
        else:
            return {"generated_uuid": '', "is_locked": False}

    def execute_key(self, key):
        '''
        Executes a remote control action based on the key that was received.
        '''
        if (key in self.key_mappings):
            self.key_mappings[key]()

        # If no motion key was received, clear the wrench
        if (key not in self.movement_keys):
            self.remote.clear_wrench()