def callback_ethercat_states(self, data, jointName): """ @param data: The ROS message received which called the callback If the message is the first received, initializes the dictionnaries Else, it updates the lastMsg @param joint: a Joint object that contains the name of the joint that we receive data from """ joint_data = joint(joint_name=jointName, joint_target=math.degrees( float(data.set_point)), joint_position=math.degrees(float(data.process_value))) # update the dictionary of joints self.dict_ethercat_joints[joint_data.joint_name] = joint_data # Build a CAN hand style lastMsg with the latest info in # self.dict_ethercat_joints self.lastMsg = joints_data() for joint_name in self.dict_ethercat_joints.keys(): self.lastMsg.joints_list.append( self.dict_ethercat_joints[joint_name]) # self.lastMsg.joints_list_length++ # TODO This is not the right way to do it. We should check that messages from all the joints have been # received (but we don't know if we have all the joints, e.g three # finger hand) self.isReady = True
def callback_ethercat_states(self, data, jointName): """ @param data: The ROS message received which called the callback If the message is the first received, initializes the dictionnaries Else, it updates the lastMsg @param joint: a Joint object that contains the name of the joint that we receive data from """ joint_data = joint(joint_name=jointName, joint_target=math.degrees(float(data.set_point)), joint_position=math.degrees(float(data.process_value))) # update the dictionary of joints self.dict_ethercat_joints[joint_data.joint_name]=joint_data # Build a CAN hand style lastMsg with the latest info in self.dict_ethercat_joints self.lastMsg = joints_data() for joint_name in self.dict_ethercat_joints.keys() : self.lastMsg.joints_list.append(self.dict_ethercat_joints[joint_name]) #self.lastMsg.joints_list_length++ #TODO This is not the right way to do it. We should check that messages from all the joints have been received (but we don't know if we have all the joints, e.g three finger hand) self.isReady = True '''
def __init__(self): """ Builds the library, creates the communication node in ROS and initializes the hand publisher and subscriber to the default values of shadowhand_data and sendupdate """ self.allJoints = [Joint("THJ1", "smart_motor_th1", 0, 90), Joint("THJ2", "smart_motor_th2", -40, 40), Joint("THJ3", "smart_motor_th3", -15, 15), Joint("THJ4", "smart_motor_th4", 0, 75), Joint("THJ5", "smart_motor_th5", -60, 60), Joint("FFJ0", "smart_motor_ff2", 0, 180), Joint("FFJ3", "smart_motor_ff3"), Joint("FFJ4", "smart_motor_ff4", -20, 20), Joint("MFJ0", "smart_motor_mf2", 0, 180), Joint("MFJ3", "smart_motor_mf3"), Joint("MFJ4", "smart_motor_mf4", -20, 20), Joint("RFJ0", "smart_motor_rf2", 0, 180), Joint("RFJ3", "smart_motor_rf3"), Joint("RFJ4", "smart_motor_rf4", -20, 20), Joint("LFJ0", "smart_motor_lf2", 0, 180), Joint("LFJ3", "smart_motor_lf3"), Joint("LFJ4", "smart_motor_lf4", -20, 20), Joint("LFJ5", "smart_motor_lf5", 0, 45), Joint("WRJ1", "smart_motor_wr1", -45, 30), Joint("WRJ2", "smart_motor_wr2", -30, 10), ] self.handJoints = [] self.armJoints = [Joint("ShoulderJRotate", "", -45, 60), Joint("ShoulderJSwing", "", 0, 80), Joint("ElbowJSwing", "", 0, 120), Joint("ElbowJRotate", "", -80, 80) ] self.lastMsg = joints_data() self.lastArmMsg = joints_data() self.cyberglove_pub = 0 self.cyberglove_sub = 0 self.cybergrasp_pub = 0 self.cyberglove_sub = 0 self.isFirstMessage = True self.isFirstMessageArm = True self.isReady = False self.liste = 0 self.hasarm = 0 self.dict_pos = {} self.dict_tar = {} self.dict_arm_pos = {} self.dict_arm_tar = {} self.dict_ethercat_joints = {} self.eth_publishers = {} self.eth_subscribers = {} # rospy.init_node('python_hand_library') self.sendupdate_lock = threading.Lock() self.joint_states_lock = threading.Lock() # contains the ending for the topic depending on which controllers are # loaded self.topic_ending = "" # EtherCAT hand self.activate_etherCAT_hand() # # Grasps self.grasp_parser = GraspParser() self.rootPath = rospkg.RosPack().get_path('sr_hand') self.grasp_parser.parse_tree( self.rootPath + "/scripts/sr_hand/grasps.xml") self.grasp_interpoler = 0 self.pub = rospy.Publisher( 'srh/sendupdate', sendupdate, queue_size=1, latch=True) self.pub_arm = rospy.Publisher( 'sr_arm/sendupdate', sendupdate, queue_size=1, latch=True) self.sub_arm = rospy.Subscriber( 'sr_arm/shadowhand_data', joints_data, self.callback_arm) self.sub = rospy.Subscriber( 'srh/shadowhand_data', joints_data, self.callback) self.hand_type = self.check_hand_type() self.hand_velocity = {} self.hand_effort = {} if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"): self.joint_states_listener = rospy.Subscriber( "joint_states", JointState, self.joint_states_callback) # Initialize the command publishers here, to avoid the delay caused # when initializing them in the sendupdate for jnt in self.allJoints: if jnt.name not in self.eth_publishers: topic = "sh_" + \ jnt.name.lower() + self.topic_ending + "/command" self.eth_publishers[jnt.name] = rospy.Publisher( topic, Float64, queue_size=1, latch=True) self.tactile_receiver = TactileReceiver() threading.Thread(None, rospy.spin)
def __init__(self): """ Builds the library, creates the communication node in ROS and initializes the hand publisher and subscriber to the default values of shadowhand_data and sendupdate """ self.allJoints = [ Joint("THJ1", "smart_motor_th1", 0, 90), Joint("THJ2", "smart_motor_th2", -40, 40), Joint("THJ3", "smart_motor_th3", -15, 15), Joint("THJ4", "smart_motor_th4", 0, 75), Joint("THJ5", "smart_motor_th5", -60, 60), Joint("FFJ0", "smart_motor_ff2", 0, 180), Joint("FFJ3", "smart_motor_ff3"), Joint("FFJ4", "smart_motor_ff4", -20, 20), Joint("MFJ0", "smart_motor_mf2", 0, 180), Joint("MFJ3", "smart_motor_mf3"), Joint("MFJ4", "smart_motor_mf4", -20, 20), Joint("RFJ0", "smart_motor_rf2", 0, 180), Joint("RFJ3", "smart_motor_rf3"), Joint("RFJ4", "smart_motor_rf4", -20, 20), Joint("LFJ0", "smart_motor_lf2", 0, 180), Joint("LFJ3", "smart_motor_lf3"), Joint("LFJ4", "smart_motor_lf4", -20, 20), Joint("LFJ5", "smart_motor_lf5", 0, 45), Joint("WRJ1", "smart_motor_wr1", -45, 30), Joint("WRJ2", "smart_motor_wr2", -30, 10), ] self.handJoints = [] self.armJoints = [ Joint("ShoulderJRotate", "", -45, 60), Joint("ShoulderJSwing", "", 0, 80), Joint("ElbowJSwing", "", 0, 120), Joint("ElbowJRotate", "", -80, 80) ] self.lastMsg = joints_data() self.lastArmMsg = joints_data() self.cyberglove_pub = 0 self.cyberglove_sub = 0 self.cybergrasp_pub = 0 self.cyberglove_sub = 0 self.isFirstMessage = True self.isFirstMessageArm = True self.isReady = False self.liste = 0 self.hasarm = 0 self.dict_pos = {} self.dict_tar = {} self.dict_arm_pos = {} self.dict_arm_tar = {} self.dict_ethercat_joints = {} self.eth_publishers = {} self.eth_subscribers = {} #rospy.init_node('python_hand_library') self.sendupdate_lock = threading.Lock() #contains the ending for the topic depending on which controllers are loaded self.topic_ending = "" ##EtherCAT hand self.activate_etherCAT_hand() ### # Grasps self.grasp_parser = GraspParser() self.rootPath = rospkg.RosPack().get_path('sr_hand') self.grasp_parser.parse_tree(self.rootPath + "/scripts/sr_hand/grasps.xml") self.grasp_interpoler = 0 self.pub = rospy.Publisher('srh/sendupdate', sendupdate, latch=True) self.pub_arm = rospy.Publisher('sr_arm/sendupdate', sendupdate, latch=True) self.sub_arm = rospy.Subscriber('sr_arm/shadowhand_data', joints_data, self.callback_arm) self.sub = rospy.Subscriber('srh/shadowhand_data', joints_data, self.callback) self.hand_type = self.check_hand_type() threading.Thread(None, rospy.spin)
def __init__(self): """ Builds the library, creates the communication node in ROS and initializes the hand publisher and subscriber to the default values of shadowhand_data and sendupdate """ self.allJoints = [Joint("THJ1", "smart_motor_th1"), Joint("THJ2", "smart_motor_th2", -30, 30), Joint("THJ3", "smart_motor_th3",-15, 15), Joint("THJ4", "smart_motor_th4",0, 75), Joint("THJ5", "smart_motor_th5",-60, 60), Joint("FFJ0", "smart_motor_ff2", 0, 180), Joint("FFJ3", "smart_motor_ff3"), Joint("FFJ4", "smart_motor_ff4", -25, 25), Joint("MFJ0", "smart_motor_mf2", 0, 180), Joint("MFJ3", "smart_motor_mf3"), Joint("MFJ4", "smart_motor_mf4", -25, 25), Joint("RFJ0", "smart_motor_rf2", 0, 180), Joint("RFJ3", "smart_motor_rf3"), Joint("RFJ4", "smart_motor_rf4", -25,25), Joint("LFJ0", "smart_motor_lf2", 0, 180), Joint("LFJ3", "smart_motor_lf3"), Joint("LFJ4", "smart_motor_lf4", -25, 25), Joint("LFJ5", "smart_motor_lf5", 0, 45), Joint("WRJ1", "smart_motor_wr1", -30, 40), Joint("WRJ2", "smart_motor_wr2", -30, 10), ] self.handJoints = [] self.armJoints = [Joint("ShoulderJRotate", "", -45, 60), Joint("ShoulderJSwing", "", 0, 80), Joint("ElbowJSwing", "", 0,120), Joint("ElbowJRotate", "", -80,80) ] self.lastMsg = joints_data() self.lastArmMsg = joints_data() self.cyberglove_pub = 0 self.cyberglove_sub = 0 self.cybergrasp_pub = 0 self.cyberglove_sub = 0 self.isFirstMessage = True self.isFirstMessageArm = True self.isReady = False self.liste = 0 self.hasarm = 0 self.dict_pos = {} self.dict_tar = {} self.dict_arm_pos = {} self.dict_arm_tar = {} self.dict_ethercat_joints = {} self.eth_publishers = {} self.eth_subscribers = {} #rospy.init_node('python_hand_library') self.sendupdate_lock = threading.Lock() #contains the ending for the topic depending on which controllers are loaded self.topic_ending = "" ##EtherCAT hand self.activate_etherCAT_hand() ### # Grasps self.grasp_parser = GraspParser() process = subprocess.Popen("rospack find sr_hand".split(), stdout=subprocess.PIPE) self.rootPath = process.communicate()[0] self.rootPath = self.rootPath.split('\n') self.rootPath = self.rootPath[0] #print "path : "+self.rootPath self.grasp_parser.parse_tree(self.rootPath+"/python_lib/grasp/grasps.xml") self.grasp_interpoler = 0 self.pub = rospy.Publisher('srh/sendupdate',sendupdate) self.pub_arm = rospy.Publisher('sr_arm/sendupdate',sendupdate) self.sub_arm = rospy.Subscriber('sr_arm/shadowhand_data', joints_data,self.callback_arm) self.sub = rospy.Subscriber('srh/shadowhand_data', joints_data ,self.callback) threading.Thread(None, rospy.spin)