def callback_lf(self, data): """ The callback function for the little finger: called each time the tactile sensor publishes a message. If no action is currently running, we reset the arm and hand to their starting position @param data: the pressure value """ #if we're already performing an action, don't do anything if not self.action_running.testandset(): return #filters the noise: check when the finger is being pressed if data.data < .1: self.action_running.unlock() return #ok finger pressed p = subprocess.Popen('beep') rospy.loginfo("LF touched, going to start position.") #wait 1s for the user to release the sensor time.sleep(.2) #send the start position to the hand self.hand_publisher.publish(sendupdate(len(self.start_pos_hand), self.start_pos_hand)) #send the start position to the arm self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm)) #wait before next possible action time.sleep(.2) self.action_running.unlock()
def __init__(self): smach.State.__init__(self, outcomes=['success','failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.iteration = 0 self.opened = True self.finger_opened = sendupdate(1, [joint(joint_name="FFJ0", joint_target=0)]) self.finger_closed = sendupdate(1, [joint(joint_name="FFJ0", joint_target=160)])
def __init__(self): smach.State.__init__(self, outcomes=['success', 'failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.iteration = 0 self.big_muscles_start = False self.target_1 = sendupdate(len(self.pos.big_muscles_end), self.pos.big_muscles_end) self.target_2 = sendupdate(len(self.pos.big_muscles_start), self.pos.big_muscles_start)
def __init__(self): smach.State.__init__( self, outcomes=['success', 'failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.iteration = 0 self.opened = True self.finger_opened = sendupdate( 1, [joint(joint_name="FFJ0", joint_target=0)]) self.finger_closed = sendupdate( 1, [joint(joint_name="FFJ0", joint_target=160)])
def __init__(self): smach.State.__init__( self, outcomes=['success', 'failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.iteration = 0 self.big_muscles_start = False self.target_1 = sendupdate(len(self.pos.big_muscles_end), self.pos.big_muscles_end) self.target_2 = sendupdate(len(self.pos.big_muscles_start), self.pos.big_muscles_start)
def lf_pressed(self, data): """ The little finger was pressed. If no action is currently running, we reset the arm and hand to their starting position @param data: the pressure value (pdc) """ #if we're already performing an action, don't do anything if not self.action_running.testandset(): return #ok finger pressed p = subprocess.Popen('beep') rospy.loginfo("LF touched, going to start position.") #wait 1s for the user to release the sensor time.sleep(.2) #send the start position to the hand self.hand_publish( self.start_pos_hand ) #send the start position to the arm self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm)) #wait before next possible action time.sleep(.2) self.action_running.unlock()
def sendupdate_from_dict(self, dicti): """ @param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target. Sends new targets to the hand from a dictionnary """ self.sendupdate_lock.acquire() if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"): for join in dicti.keys(): if join not in self.eth_publishers: topic = "sh_" + \ join.lower() + self.topic_ending + "/command" self.eth_publishers[join] = rospy.Publisher( topic, Float64, latch=True) msg_to_send = Float64() msg_to_send.data = math.radians(float(dicti[join])) self.eth_publishers[join].publish(msg_to_send) elif self.hand_type == "CANhand": message = [] for join in dicti.keys(): message.append( joint(joint_name=join, joint_target=dicti[join])) self.pub.publish(sendupdate(len(message), message)) self.sendupdate_lock.release()
def __init__(self, joints_dict): joints_table = [] for name in joints_dict.keys(): joints_table.append(joint(joint_name = name, joint_target = joints_dict[name])) self.msg = sendupdate(len(joints_table), joints_table)
def __init__(self): smach.State.__init__( self, outcomes=['success', 'failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.targets = sendupdate(len(self.pos.first_pos_msg), self.pos.first_pos_msg)
def sendupdate_arm(self, jointName, angle=0): """ @param jointName: Name of the joint to update @param angle: Target of the joint, 0 if not set Sends a new target for the specified joint """ message = [joint(joint_name=jointName, joint_target=angle)] self.pub_arm.publish(sendupdate(len(message), message))
def sendupdate_arm_from_dict(self, dicti): """ @param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target Sends new targets to the hand from a dictionnary """ message = [] for join in dicti.keys(): message.append(joint(joint_name=join, joint_target=dicti[join])) self.pub_arm.publish(sendupdate(len(message), message))
def __init__(self): #send the start position to the hand self.hand_publisher.publish(sendupdate(len(self.start_pos_hand), self.start_pos_hand)) #send the start position to the arm self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm)) #wait for the node to be initialized and then go to the starting position time.sleep(1) rospy.loginfo("OK, ready for the demo") # We have one subscriber per tactile sensor # This way we can easily have a different action mapped to # each tactile sensor # NB: The tactile sensor on the ring finger is not mapped in this example self.sub_ff = rospy.Subscriber("/sr_tactile/touch/ff", Float64, self.callback_ff, queue_size=1) self.sub_mf = rospy.Subscriber("/sr_tactile/touch/mf", Float64, self.callback_mf, queue_size=1) self.sub_lf = rospy.Subscriber("/sr_tactile/touch/lf", Float64, self.callback_lf, queue_size=1) self.sub_th = rospy.Subscriber("/sr_tactile/touch/th", Float64, self.callback_th, queue_size=1)
def callback_mf(self, data): """ The callback function for the middle finger: called each time the tactile sensor publishes a message. If no action is currently running, we set the ShoulderJRotate to a positive value proportional to the pressure received. @param data: the pressure value """ #if we're already performing an action, don't do anything if not self.action_running.testandset(): return #filters the noise: check when the finger is being pressed if data.data < .1: self.action_running.unlock() return #ok finger was pressed p = subprocess.Popen('beep') #rotate the trunk to (data_received * min_pos) data.data /= 2. if data.data > 1.: data.data = 1. target_sh_rot = 0. + data.data * (45.0) target_sh_swing = 20. + data.data * (20.0) target_elb_swing = 90. + data.data * (20.0) target_elb_rot = 0. + data.data * (45.0) rospy.loginfo("MF touched, going to new target ") #wait 1s for the user to release the sensor time.sleep(.2) message = [joint(joint_name = "ShoulderJRotate", joint_target = target_sh_rot), joint(joint_name = "ShoulderJSwing", joint_target = target_sh_swing), joint(joint_name = "ElbowJRotate", joint_target = target_elb_rot), joint(joint_name = "ElbowJSwing", joint_target = target_elb_swing) ] self.arm_publisher.publish(sendupdate(len(message), message)) #wait before next possible action time.sleep(.2) self.action_running.unlock()
def ff_pressed(self,data): """ The first finger was pressed. If no action is currently running, we set the ShoulderJRotate to a negative value proportional to the pressure received. @param data: the pressure value (pdc) """ #if we're already performing an action, don't do anything if not self.action_running.testandset(): return #ok the finger sensor was pressed p = subprocess.Popen('beep') #rotate the trunk to (data_received * min_pos) # convert data to be in [0., 1.] data /= 4000. if data > 1.: data = 1. target_sh_rot = 0. + data * (-45.0) target_sh_swing = 20. + data * (-10.0) target_elb_swing = 90. + data * (-10.0) target_elb_rot = 0. + data * (-45.0) rospy.loginfo("FF touched, going to new target ") #wait 1s for the user to release the sensor time.sleep(.2) message = [joint(joint_name = "ShoulderJRotate", joint_target = target_sh_rot), joint(joint_name = "ShoulderJSwing", joint_target = target_sh_swing), joint(joint_name = "ElbowJRotate", joint_target = target_elb_rot), joint(joint_name = "ElbowJSwing", joint_target = target_elb_swing) ] self.arm_publisher.publish(sendupdate(len(message), message)) #send the start position to the hand self.hand_publish( self.extended_pos_hand ) #wait before next possible action time.sleep(.2) self.action_running.unlock()
def callback(data): """ The callback function: called each time a message is received on the topic /srh/shadowhand_data @param data: the message """ message=[] if data.joints_list_length == 0: return # loop on the joints in the message for joint_data in data.joints_list: # if its the parent joint, read the target and send it to the child if joint_data.joint_name == parent_name: message.append(joint(joint_name=child_name, joint_target=joint_data.joint_target)) # publish the message to the /srh/sendupdate topic. pub = rospy.Publisher('srh/sendupdate', sendupdate) pub.publish(sendupdate(len(message), message))
def sendupdate_from_dict(self, dicti): """ @param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target Sends new targets to the hand from a dictionnary """ #print(dicti) if (self.check_hand_type() == "etherCAT") or (self.check_hand_type() == "gazebo"): for join in dicti.keys(): if not self.eth_publishers.has_key(join): topic = "/sh_"+ join.lower() + self.topic_ending+"/command" self.eth_publishers[join] = rospy.Publisher(topic, Float64) msg_to_send = Float64() msg_to_send.data = math.radians( float( dicti[join] ) ) self.eth_publishers[join].publish(msg_to_send) elif self.check_hand_type() == "CANhand": message = [] for join in dicti.keys(): message.append(joint(joint_name=join, joint_target=dicti[join])) self.pub.publish(sendupdate(len(message), message))
def sendupdate(self, jointName, angle=0): """ @param jointName: Name of the joint to update @param angle: Target of the joint, 0 if not set Sends a new target for the specified joint """ self.sendupdate_lock.acquire() if (self.check_hand_type() == "etherCAT") or (self.check_hand_type() == "gazebo"): if not self.eth_publishers.has_key(jointName): topic = "/sh_"+ jointName.lower() + self.topic_ending + "/command" self.eth_publishers[jointName] = rospy.Publisher(topic, Float64) msg_to_send = Float64() msg_to_send.data = math.radians( float( angle ) ) self.eth_publishers[jointName].publish(msg_to_send) elif self.check_hand_type() == "CANhand": message = [joint(joint_name=jointName, joint_target=angle)] self.pub.publish(sendupdate(len(message), message)) self.sendupdate_lock.release()
def sendupdate(self, jointName, angle=0): """ @param jointName: Name of the joint to update @param angle: Target of the joint, 0 if not set Sends a new target for the specified joint """ self.sendupdate_lock.acquire() if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"): if jointName not in self.eth_publishers: topic = "sh_" + \ jointName.lower() + self.topic_ending + "/command" self.eth_publishers[jointName] = rospy.Publisher( topic, Float64, latch=True) msg_to_send = Float64() msg_to_send.data = math.radians(float(angle)) self.eth_publishers[jointName].publish(msg_to_send) elif self.hand_type == "CANhand": message = [joint(joint_name=jointName, joint_target=angle)] self.pub.publish(sendupdate(len(message), message)) self.sendupdate_lock.release()
def __init__(self): #A vector containing the different callbacks, in the same order # as the tactiles. self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed, self.lf_pressed, self.th_pressed] #The hand publishers: # we use a dictionnary of publishers, because on the etherCAT hand # you have one publisher per controller. self.hand_publishers = self.create_hand_publishers() #send the start position to the hand self.hand_publish(self.start_pos_hand) #send the start position to the arm self.arm_publisher.publish( sendupdate(len(self.start_pos_arm), self.start_pos_arm) ) #wait for the node to be initialized and then go to the starting position time.sleep(1) rospy.loginfo("OK, ready for the demo") # We subscribe to the data being published by the biotac sensors. self.sub_biotacs = rospy.Subscriber("tactiles", BiotacAll, self.callback_biotacs, queue_size=1) self.sub_psts = rospy.Subscriber("tactile", ShadowPST, self.callback_psts, queue_size=1)
def __init__(self): #A vector containing the different callbacks, in the same order # as the tactiles. self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed, self.lf_pressed, self.th_pressed] #The hand publishers: # we use a dictionnary of publishers, because on the etherCAT hand # you have one publisher per controller. self.hand_publishers = self.create_hand_publishers() #send the start position to the hand self.hand_publish(self.start_pos_hand) #send the start position to the arm self.arm_publisher.publish( sendupdate(len(self.start_pos_arm), self.start_pos_arm) ) #wait for the node to be initialized and then go to the starting position time.sleep(1) rospy.loginfo("OK, ready for the demo") # We subscribe to the data being published by the biotac sensors. self.sub_biotacs = rospy.Subscriber("/tactiles", BiotacAll, self.callback_biotacs, queue_size=1) self.sub_psts = rospy.Subscriber("/tactile", ShadowPST, self.callback_psts, queue_size=1)
def talker(): """ The Publisher publishes to the different topics on which the sr_subscriber subscribes. It sends commands to the hand. Please set the message you want to test. """ test_what = "sendupdate" # choose sendupdate or contrlr """ Test the sendupdate command """ if test_what == "sendupdate": pub1 = rospy.Publisher('srh/sendupdate', sendupdate) rospy.init_node('shadowhand_command_publisher_python') new_target = 0 time.sleep(1) print "publishing" data_to_send = [ joint(joint_name="FFJ0", joint_target=new_target), joint(joint_name="FFJ3", joint_target=new_target) ] pub1.publish(sendupdate(len(data_to_send), data_to_send)) """ Tests the contrlr command """ if test_what == "contrlr": pub2 = rospy.Publisher('contrlr', contrlr) data_to_send = ["p:0","i:0"] pub2.publish( contrlr( contrlr_name="smart_motor_ff2" , list_of_parameters = data_to_send, length_of_list = len(data_to_send) ) )
def __init__(self): smach.State.__init__(self, outcomes=['success','failed'], output_keys=['index_counter', 'max_counter', 'last_targets']) self.pos = Position() self.targets = sendupdate(len(self.pos.first_pos_msg), self.pos.first_pos_msg)
'ffj3': 0, 'ffj4': 0, 'rfj0': 40.0, 'rfj3': 0, 'rfj4': 0, 'thj1': 20, 'thj2': 31, 'thj3': 0, 'thj4': 25, 'thj5': -29, 'wrj1': 0, 'wrj2': 0 } print "Start" sendupdate(start_pose) rospy.sleep(2) # FF curl sendupdate({'ffj3': 90}) rospy.sleep(3) sendupdate({'ffj0': 120}) rospy.sleep(2) sendupdate(start_pose) rospy.sleep(4) # RF curl sendupdate({'rfj0': 120, 'rfj3': 90}) rospy.sleep(4) sendupdate(start_pose) rospy.sleep(4)