Namespace in ReflexHand and Motor

README.md

@@ -1,3 +1,8 @@
+# OAkyildiz/reflex-ros-pkg
+fork of RightHandRobotics/reflex-ros-pkg
+This fork aims to fix the catkin build order problems and namespace issues 
+
+
 reflex-ros-pkg
 ======
 

reflex/launch/reflex_plus.launch

@@ -23,5 +23,5 @@
                 --port reflex_plus_port 
                 reflex_plus_f1 reflex_plus_f2 reflex_plus_f3 reflex_plus_preshape"
           output="screen"/>
-    <node name="reflex_plus_hand" pkg="reflex" type="reflex_usb_hand.py" required="true" output="screen"/>
+    <node name="reflex_plus_hand" pkg="reflex" type="reflex_usb_hand.py" args="reflex_plus" required="true" output="screen"/>
 </launch>

reflex/launch/reflex_sf.launch

@@ -22,5 +22,5 @@
                 --port reflex_sf_port 
                 reflex_sf_f1 reflex_sf_f2 reflex_sf_f3 reflex_sf_preshape"
           output="screen"/>
-    <node name="reflex_sf_hand" pkg="reflex" type="reflex_usb_hand.py" required="true" output="screen"/>
+    <node name="reflex_sf_hand" pkg="reflex" type="reflex_usb_hand.py" args="reflex_sf" required="true" output="screen"/>
 </launch>

reflex/src/reflex/motor.py

@@ -33,7 +33,15 @@ def __init__(self, name):
         Assumes that "name" is the name of the controller with a preceding
         slash, e.g. /reflex_takktile_f1 or /reflex_sf_f1
         '''
-        self.name = name[1:]
+        # self.name = name[1:] # prone-tofail
+        # this is better if RHR wants to keep it:
+        # self.name=name[1:] if name[0] ='/' else name
+
+        '''
+        But why do that, and then manually add slash? Instead:
+        '''
+        self.name=name     
+
         self._DEFAULT_MOTOR_SPEED = rospy.get_param(name + '/default_motor_speed')
         self._MAX_MOTOR_SPEED = rospy.get_param(name + '/max_motor_speed')
         self._MAX_MOTOR_TRAVEL = rospy.get_param(name + '/max_motor_travel')

reflex/src/reflex/reflex_usb_hand.py

@@ -40,10 +40,12 @@
 motor_names = ['_f1', '_f2', '_f3', '_preshape']
 
 class ReflexUSBHand(ReflexHand):
-    def __init__(self):
-        self.usb_hand_type = rospy.get_param('usb_hand_type')  
-        self.init_namespace = '/' + self.usb_hand_type
+    def __init__(self,name):
+        self.usb_hand_type = rospy.get_param('usb_hand_type')
+        self.init_namespace =  name
+
         super(ReflexUSBHand, self).__init__(self.init_namespace, ReflexUSBMotor)
+
         self.hand_state_pub = rospy.Publisher(self.namespace + '/hand_state',
                                               reflex_msgs.msg.Hand, queue_size=10)
         self.encoder_last_value = [0, 0, 0]  #This will be updated constantly in _receive_enc_state_cb()
@@ -57,10 +59,10 @@ def __init__(self):
         if (self.usb_hand_type == "reflex_plus"):
             self.enc_subscriber = rospy.Subscriber('/encoder_states', Encoder, self._receive_enc_state_cb)
             rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_auto)
-            self.encoder_zero_point = rospy.get_param('/enc_zero_points')
+            self.encoder_zero_point = rospy.get_param('/enc_zero_points') #remove slash?
         else:
             rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_manual)
-    
+
     def _receive_enc_state_cb(self, data):
         #Receives and processes the encoder state
         #print("encoder 1: " + str(data.encoders[0]) + " encoder 2: " + str(data.encoders[1]) + " encoder 3: " + str(data.encoders[2]))
@@ -74,7 +76,7 @@ def _receive_enc_state_cb(self, data):
             #motor_angles[i] = raw_motor_angle
             self.distal_approx[i] = self.calc_distal_angle(motor_joint_angle, self.proximal_angle[i])
         #print motor_angles
-    
+
     def _receive_cmd_cb(self, data):
         self.disable_force_control()
         self.set_speeds(data.velocity)
@@ -143,7 +145,7 @@ def calibrate_auto(self, data=None):
             j=0
             while(1):
                 enc_pos = self.encoder_last_value[i]
-                if (j==0): 
+                if (j==0):
                     j=1
                     last = enc_pos
                 if ((abs(enc_pos-last) < self.calibration_error)):
@@ -172,20 +174,20 @@ def _write_zero_point_data_to_file(self, filename, data):
             outfile.write(yaml.dump(data))
 
     def _zero_current_pose(self):
-        data = dict(
-            reflex_sf_f1=dict(zero_point=self.motors[self.namespace + '_f1'].get_current_raw_motor_angle()),
-            reflex_sf_f2=dict(zero_point=self.motors[self.namespace + '_f2'].get_current_raw_motor_angle()),
-            reflex_sf_f3=dict(zero_point=self.motors[self.namespace + '_f3'].get_current_raw_motor_angle()),
-            reflex_sf_preshape=dict(zero_point=self.motors[self.namespace + '_preshape'].get_current_raw_motor_angle())
-        )
+        data = dict()
+        data[self.namespace+"_f1"]=dict(zero_point=self.motors[self.namespace + "_f1"].get_current_raw_motor_angle())
+        data[self.namespace+"_f2"]=dict(zero_point=self.motors[self.namespace + "_f2"].get_current_raw_motor_angle())
+        data[self.namespace+"_f3"]=dict(zero_point=self.motors[self.namespace + "_f3"].get_current_raw_motor_angle())
+        data[self.namespace+"_preshape"]=dict(zero_point=self.motors[self.namespace + "_preshape"].get_current_raw_motor_angle())
+
         if (self.usb_hand_type == 'reflex_plus'):
             data = dict(
-                reflex_plus_f1 = data['reflex_sf_f1'],
-                reflex_plus_f2 = data['reflex_sf_f2'],
-                reflex_plus_f3 = data['reflex_sf_f3'],
-                reflex_plus_preshape = data['reflex_sf_preshape']
+                reflex_plus_f1 = data[self.namespace+'_f1'],
+                reflex_plus_f2 = data[self.namespace+'_f2'],
+                reflex_plus_f3 = data[self.namespace+'_f3'],
+                reflex_plus_preshape = data[self.namespace+'_preshape']
             )
-        self._write_zero_point_data_to_file(self.usb_hand_type + '_motor_zero_points.yaml', data)
+        self._write_zero_point_data_to_file(self.namespace + '_motor_zero_points.yaml', data)
 
     #Encoder data processing functions are based off encoder functions used
     #For the reflex_takktile hand as in reflex_driver_node.cpp
@@ -195,12 +197,12 @@ def calibrate_encoders_locally(self, data):
             self.encoder_zero_point[i] = data[i]*self.enc_scale
             self.encoder_offset[i] = 0;
         data = dict(enc_zero_points = self.encoder_zero_point)
-        self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data)    
+        self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data)
 
     def update_encoder_offset(self, raw_value):
         #Given a raw and past (self.encoder_last_value) value, track encoder wrapes (self.enc_offset)
         offset = self.encoder_offset[:]
-        
+
         for i in range(0,3):
             if (offset[i]==-1):
                 #This happens at start up
@@ -211,7 +213,7 @@ def update_encoder_offset(self, raw_value):
                     offset[i] = offset[i] + 16383
                 elif (self.encoder_last_value[i] - raw_value[i] < -5000):
                     offset[i] = offset[i] - 16383
-        
+
         self.encoder_offset = offset[:]
 
     def calc_proximal_angle(self, raw_value, zero, offset):
@@ -230,9 +232,9 @@ def calc_distal_angle(self, joint_angle, proximal):
         else:
             return diff
 
-def main():
+def main(name):
     rospy.sleep(4.0)  # To allow services and parameters to load
-    hand = ReflexUSBHand()
+    hand = ReflexUSBHand(name)
     rospy.on_shutdown(hand.disable_torque)
     r = rospy.Rate(20)
     while not rospy.is_shutdown():
@@ -241,4 +243,7 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
+    import sys
+    #this needs to be slightly revised as and empty 'args' parameter in roslaunch causes problems
+    nm='reflex_sf' if len(sys.argv)<=1 else sys.argv[1]
+    main(nm)

reflex/src/reflex/reflex_usb_motor.py

@@ -33,7 +33,7 @@
 class ReflexUSBMotor(Motor):
     def __init__(self, name):
         super(ReflexUSBMotor, self).__init__(name)
-        self.zero_point = rospy.get_param(self.name + '/zero_point')
+        self.zero_point = rospy.get_param(self.name + "/zero_point")
         self.MOTOR_TO_JOINT_GEAR_RATIO = rospy.get_param(self.name + '/motor_to_joint_gear_ratio')
         self.MOTOR_TO_JOINT_INVERTED = rospy.get_param(self.name + '/motor_to_joint_inverted')
         self.motor_cmd_pub = rospy.Publisher(name + '/command', Float64, queue_size=10)

reflex/yaml/left_hand_motor_zero_points.yaml

@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 20.95724552409363}
+reflex_sf_f2: {zero_point: 20.28075999663606}
+reflex_sf_f3: {zero_point: 24.543692606170257}
+reflex_sf_preshape: {zero_point: 21.764119418521478}

reflex/yaml/right_hand_motor_zero_points.yaml

@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 22.929944817314563}
+reflex_sf_f2: {zero_point: 20.469439633545996}
+reflex_sf_f3: {zero_point: 22.88545937446588}
+reflex_sf_preshape: {zero_point: 23.58955655610539}

reflex/yaml/this_is_left_hand.yaml

@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 20.773167829547354}
+reflex_sf_f2: {zero_point: 20.49244934536428}
+reflex_sf_f3: {zero_point: 25.615945176902322}
+reflex_sf_preshape: {zero_point: 21.567769877672117}

reflex_visualizer/launch/reflex_visualizer.launch

@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
 	<param name="robot_description" command="$(find xacro)/xacro.py '$(find reflex_visualizer)/urdf/full_reflex_model.urdf.xacro'" />
 	<node name="hand_visualizer" pkg="reflex_visualizer" type="hand_visualizer" output="screen"/>