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)