手柄控制高斯机械臂

#!/usr/bin/env python

import rospy, actionlib

from std_msgs.msg import Bool
from sensor_msgs.msg import Joy

from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from gauss_msgs.srv import GetInt
from gauss_msgs.srv import SetInt
from gauss_msgs.srv import CurrentTool
from gauss_msgs.msg import ToolAction
from gauss_msgs.msg import ToolGoal

from gauss_msgs.msg import RobotMoveAction
from gauss_msgs.msg import RobotMoveGoal

from gauss_commander.command_status import CommandStatus
from gauss_commander.robot_commander_exception import RobotCommanderException
from actionlib_msgs.msg import GoalStatus

from gauss_commander.command_type import CommandType as MoveCommandType
# AXES index table
AXE_JOY_L_H = 0
AXE_JOY_L_V = 1
AXE_LT      = 2
AXE_JOY_R_H = 3
AXE_JOY_R_V = 4
AXE_RT      = 5
AXE_ARROW_H = 6
AXE_ARROW_V = 7

# BUTTON index table
BUTTON_A      = 0
BUTTON_B      = 1
BUTTON_X      = 2
BUTTON_Y      = 3
BUTTON_LB     = 4
BUTTON_RB     = 5
BUTTON_BACK   = 6
BUTTON_START  = 7
BUTTON_SELECT = 8
BUTTON_JOY_L  = 9
BUTTON_JOY_R  = 10

POSITION_MODE = 100
JOINT_MODE    = 101

MIN_DELTA_XYZ = 0.01
MAX_DELTA_XYZ = 0.05
MIN_DELTA_RPY = 0.1
MAX_DELTA_RPY = 0.5

MAX_MULTIPLIER = 0.15
MIN_MULTIPLIER = 0.01
DEFAULT_MULTIPLIER = 0.07
STEP_MULTIPLIER = 0.01

class JointMode:

    def __init__(self):

        # Get params from rosparams
        self.timer_rate = rospy.get_param("~joystick_timer_rate_sec")
        self.validation = rospy.get_param("/gauss/robot_command_validation")
        self.joint_mode_timer = None
        
        self.synchronization_needed = True
        self.is_enabled = False

        self.joint_state_subscriber = rospy.Subscriber('/joint_states', 
                JointState, self.callback_joint_states)
        
        self.learning_mode_subscriber = rospy.Subscriber(
                '/gauss/learning_mode', Bool, self.callback_learning_mode)

        self.joint_trajectory_publisher = rospy.Publisher(
                '/gauss_follow_joint_trajectory_controller/command',
                JointTrajectory, queue_size=10)

        self.axes = [0,0,0,0,0,0,0,0]
        self.buttons = [0,0,0,0,0,0,0,0,0,0,0]
        self.joint_states = JointState()
        self.joint_cmd = [0,0,0,0,0,0]
        self.multiplier = DEFAULT_MULTIPLIER
        self.learning_mode_on = True
        
        self.joint_mode_timer = rospy.Timer(rospy.Duration(self.timer_rate), self.send_joint_trajectory)
        self.time_debounce_start_button = rospy.Time.now()

        self.joystick_tool_commander = JoystickToolCommander()
        try:
            rospy.wait_for_service('/gauss/current_tool', 1)
            self.current_tool_client = rospy.ServiceProxy('/gauss/current_tool', CurrentTool)            
        except Exception as e:
            print e
        
    def increase_speed(self):
        self.multiplier += STEP_MULTIPLIER
        if self.multiplier > MAX_MULTIPLIER:
            self.multiplier = MAX_MULTIPLIER