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)
