def main():
global tf
global command, limbs
global pub
# Initialize the ROS node
rospy.init_node('kdl_kinect')
# Create a transform listener
tf = TransformListener()
# Retrieve raw robot parameters from rosmaster
robot_string = rospy.get_param("robot_description", None)
if not robot_string:
raise Exception('Robot model not specified')
# Load URDF model of robot description locally
robot_urdf = URDF.parse_xml_string(robot_string)
# Load URDF model of robot description into KDL
robot_kdl = kdl_parser.kdl_tree_from_urdf_model(robot_urdf)
# Publish Atlas commands
pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand)
# Subscribe to hydra and atlas updates
rospy.Subscriber("/arms/hydra_calib", Hydra, hydra_callback, queue_size = 1)
rospy.Subscriber("/skeleton", Skeleton, skeleton_callback, queue_size = 1)
rospy.Subscriber("/atlas/atlas_state", AtlasState, atlas_callback, queue_size = 1)
# Start main event handling loop
rospy.loginfo('Started kdl_hydra node...')
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
r.sleep()
rospy.loginfo('Stopping kdl_hydra node...')
