def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for left finger self.left_finger_frame = 'arm_left_finger_link' self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64) rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger) rospy.wait_for_message('finger_left_controller/state', JointControllerState) # Initialize publisher & subscriber for right finger self.right_finger_frame = 'arm_right_finger_link' self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64) rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger) rospy.wait_for_message('finger_right_controller/state', JointControllerState) # Initialize action server self.result = SmartArmGripperResult() self.feedback = SmartArmGripperFeedback() self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback) # Reset gripper position rospy.sleep(1) self.reset_gripper_position() rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): # Initialize constants self.JOINTS_COUNT = 4 # Number of joints to manage self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME + 'server', anonymous=True) # Initialize publisher & subscriber for shoulder pan self.shoulder_pan_frame = 'arm_shoulder_pan_link' self.shoulder_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64) rospy.Subscriber('shoulder_pan_controller/state', JointControllerState, self.read_shoulder_pan) rospy.wait_for_message('shoulder_pan_controller/state', JointControllerState) # Initialize publisher & subscriber for shoulder tilt self.shoulder_tilt_frame = 'arm_shoulder_tilt_link' self.shoulder_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.shoulder_tilt_pub = rospy.Publisher('shoulder_tilt_controller/command', Float64) rospy.Subscriber('shoulder_tilt_controller/state', JointControllerState, self.read_shoulder_tilt) rospy.wait_for_message('shoulder_tilt_controller/state', JointControllerState) # Initialize publisher & subscriber for elbow tilt self.elbow_tilt_frame = 'arm_elbow_tilt_link' self.elbow_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64) rospy.Subscriber('elbow_tilt_controller/state', JointControllerState, self.read_elbow_tilt) rospy.wait_for_message('elbow_tilt_controller/state', JointControllerState) # Initialize publisher & subscriber for shoulder tilt self.wrist_rotate_frame = 'arm_wrist_rotate_link' self.wrist_rotate = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_rotate_pub = rospy.Publisher('wrist_rotate_controller/command', Float64) rospy.Subscriber('wrist_rotate_controller/state', JointControllerState, self.read_wrist_rotate) rospy.wait_for_message('wrist_rotate_controller/state', JointControllerState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize joints action server self.result = SmartArmResult() self.feedback = SmartArmFeedback() self.feedback.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_rotate.process_value] self.server = SimpleActionServer(NAME, SmartArmAction, self.execute_callback) # Reset arm position rospy.sleep(1) self.reset_arm_position() rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works) self.TIMEOUT_THRESHOLD = rospy.Duration( 15.0 ) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for pan self.head_pan_frame = 'head_pan_link' self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64) rospy.Subscriber('head_pan_controller/state', JointControllerState, self.read_current_pan) rospy.wait_for_message('head_pan_controller/state', JointControllerState) # Initialize publisher & subscriber for tilt self.head_tilt_frame = 'head_tilt_link' self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64) rospy.Subscriber('head_tilt_controller/state', JointControllerState, self.read_current_tilt) rospy.wait_for_message('head_tilt_controller/state', JointControllerState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize point action server self.result = WubbleHeadResult() self.feedback = WubbleHeadFeedback() self.feedback.head_position = [ self.head_pan.process_value, self.head_tilt.process_value ] self.server = SimpleActionServer(NAME, WubbleHeadAction, self.execute_callback) # Reset head position rospy.sleep(1) self.reset_head_position() rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): # Initialize constants self.error_threshold = 0.0175 # Report success if error reaches below threshold self.signal = 1 # Initialize new node rospy.init_node(NAME, anonymous=True) controller_name = rospy.get_param('~controller') # Initialize publisher & subscriber for tilt self.laser_tilt = JointControllerState() self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64) self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal) self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True) rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state) rospy.wait_for_message(controller_name + '/state', JointControllerState) # Initialize tilt action server self.result = HokuyoLaserTiltResult() self.feedback = HokuyoLaserTiltFeedback() self.feedback.tilt_position = self.laser_tilt.process_value self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback) rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): rospy.init_node('dynamixel_to_pr2_state_msgs', anonymous=True) self.robot_controllers = ( 'shoulder_pitch_controller', 'shoulder_pan_controller', 'upperarm_roll_controller', 'elbow_flex_controller', 'forearm_roll_controller', 'wrist_pitch_controller', 'wrist_roll_controller', 'left_finger_controller', 'right_finger_controller', 'head_pan_controller', 'head_tilt_controller', 'neck_tilt_controller') self.joint_to_publisher_state = {} for controller in self.robot_controllers: joint_name = rospy.get_param(controller + '/joint') publisher = rospy.Publisher(controller + '/state_pr2_msgs', JointControllerState) state = JointControllerState() self.joint_to_publisher_state[joint_name] = { 'publisher': publisher, 'state': state } [ rospy.Subscriber(controller + '/state', JointState, self.handle_controller_state) for controller in self.robot_controllers ] [rospy.wait_for_message(controller + '/state', JointState)]
def handle_head_state(self, msg): jcs = JointControllerState(set_point=msg.goal_pos, process_value=msg.current_pos, process_value_dot=msg.velocity, error=msg.error, command=msg.load) jcs.header.stamp = msg.header.stamp name = msg.name.replace('_joint', '_controller', 1) self.head_controllers[name].publish(jcs)
def handle_laser_state(self, msg): jcs = JointControllerState(set_point=msg.goal_pos, process_value=msg.current_pos, process_value_dot=msg.velocity, error=msg.error, command=msg.load) jcs.header.stamp = msg.header.stamp name = 'laser_tilt_controller' self.laser_controllers[name].publish(jcs)