komodo_arm.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ric_robot')
00003 import rospy
00004 import math
00005 import sys
00006 from dynamixel_msgs.msg import JointState as dxl_JointState
00007 from sensor_msgs.msg import JointState
00008 from math import *
00009 from ric_robot.srv import *
00010 from ric_robot.msg import *
00011 from std_msgs.msg import Float64
00012 
00013 def br_callback(data):       
00014     global msg,ns
00015     msg.name[0]=ns+"_"+data.name
00016     msg.position[0]=data.current_pos
00017     msg.velocity[0]=data.velocity
00018     msg.effort[0]=data.load
00019 
00020 def sh_callback(data):       
00021     global msg,ns
00022     msg.name[1]=ns+"_"+data.name
00023     msg.position[1]=data.current_pos
00024     msg.velocity[1]=data.velocity
00025     msg.effort[1]=data.load
00026 
00027 def e1_callback(data):       
00028     global msg,ns
00029     msg.name[2]=ns+"_"+data.name
00030     msg.position[2]=data.current_pos
00031     msg.velocity[2]=data.velocity
00032     msg.effort[2]=data.load
00033 
00034 def e2_callback(data):       
00035     global msg,ns
00036     msg.name[3]=ns+"_"+data.name
00037     msg.position[3]=data.current_pos
00038     msg.velocity[3]=data.velocity
00039     msg.effort[3]=data.load
00040 
00041 def wr_callback(data):       
00042     global msg,ns
00043     msg.name[4]=ns+"_"+data.name
00044     msg.position[4]=data.current_pos
00045     msg.velocity[4]=data.velocity
00046     msg.effort[4]=data.load
00047 
00048 def lf_callback(data):       
00049     global msg,ns
00050     msg.name[5]=ns+"_"+data.name
00051     msg.position[5]=data.current_pos
00052     msg.velocity[5]=data.velocity
00053     msg.effort[5]=data.load
00054 
00055 def rf_callback(data):       
00056     global msg,ns
00057     msg.name[6]=ns+"_"+data.name
00058     msg.position[6]=data.current_pos
00059     msg.velocity[6]=data.velocity
00060     msg.effort[6]=data.load
00061 
00062 def elev_callback(data):       
00063     global msg,pre_pos,pre_epos,final_epos,ns
00064     msg.name[7]=ns+"_"+data.name
00065     epos=data.current_pos*elev_rad2m
00066     msg.velocity[7]=data.velocity*elev_rad2m
00067     msg.effort[7]=data.load
00068     dp=epos-pre_epos
00069     if dp>0.0005:
00070        final_epos-=math.pi*2*elev_rad2m
00071     elif dp<-0.0005:
00072        final_epos+=math.pi*2*elev_rad2m
00073     pre_epos=epos
00074     final_epos+=dp
00075     msg.position[7]=final_epos
00076 
00077 def handle_elev_set(req):
00078     global final_epos,elevpub,home_ok,elev_move
00079     if (home_ok==1) or (home_ok==0): 
00080        home_ok=2
00081        rospy.loginfo("Homing is done")
00082     elevpub.publish(0.0)
00083     elev_move=False
00084        #rospy.sleep(0.5)
00085     final_epos=req.pos
00086     rospy.loginfo("Elevator position is: %.3f",final_epos)
00087     return True
00088 
00089 def epos_callback(data): 
00090     global elev_goal_pos,elev_move,elevpub,final_epos,epos_tol,max_elev_speed,home_ok, min_elev_pos,max_elev_pos
00091     if home_ok==2:
00092        elev_goal_pos=data.pos
00093        if elev_goal_pos>(max_elev_pos+0.000001) or elev_goal_pos<(min_elev_pos-0.000001):
00094           rospy.loginfo("Required goal (%.3f) is outside of the elevator range of motion [%.3f-%.3f]",elev_goal_pos,min_elev_pos,max_elev_pos)
00095        else:
00096           epos_err=elev_goal_pos-final_epos
00097           if abs(epos_err)>epos_tol:
00098              elev_move=True
00099     elif home_ok==1:
00100        rospy.logwarn("Cannot accept commands! Home service is running")
00101     elif home_ok==0:
00102        rospy.logwarn("Cannot accept commands! Please run the home service first")
00103 
00104 
00105 def handle_elev_home(req):
00106     global home_ok,elev_move
00107     if (req.dir>0):
00108       if (final_epos<max_elev_pos+0.005):
00109         elev_move=False
00110         elevpub.publish(max_elev_speed/elev_rad2m)
00111         home_ok=1
00112         rospy.loginfo("Elevator is homing up")    
00113         return True
00114       else:
00115         return False
00116     elif(req.dir<0):
00117       if (final_epos>min_elev_pos-0.005):
00118         elev_move=False
00119         elevpub.publish(-max_elev_speed/elev_rad2m)
00120         home_ok=1
00121         rospy.loginfo("Elevator is homing down")
00122         return True
00123       else:
00124         return False
00125     else:
00126        rospy.loginfo("Please choose homing direction (1 - up / -1 - down)")
00127        return False
00128        
00129        
00130     
00131 def komodo_arm():
00132     global pub,msg,ns
00133     global elev_rad2m
00134     global pre_epos,final_epos,elev_goal_pos,elev_move,epos_tol,max_elev_speed, min_elev_pos,max_elev_pos,elevpub
00135     global home_ok
00136     fr=0
00137     ns=rospy.get_namespace()
00138     ns=ns[1:-1]
00139     elev_rad2m=0.00175/(math.pi*2)
00140     min_elev_pos=0.0 #TODO: get as parameter?
00141     max_elev_pos=0.4 #TODO: get as parameter?
00142     home_ok=0 #0-not homed, 1-homing  2-homed
00143     epos_tol=0.001
00144     max_elev_speed=0.01
00145     elev_goal_pos=0
00146     elev_move=False
00147     pre_epos=0
00148     final_epos=0
00149     msg = JointState()
00150     rospy.init_node('komodo_arm', anonymous=True)
00151     have_elevator=rospy.get_param('have_elevator')
00152     for i in range(7):
00153         msg.name.append("")
00154         msg.position.append(0.0)
00155         msg.velocity.append(0.0)
00156         msg.effort.append(0.0)
00157     if have_elevator:
00158        msg.name.append("")
00159        msg.position.append(0.0)
00160        msg.velocity.append(0.0)
00161        msg.effort.append(0.0)
00162        rospy.Subscriber("/"+ns+"/elevator_controller/state", dxl_JointState, elev_callback)
00163        elevpub = rospy.Publisher("/"+ns+"/elevator_controller/command", Float64)
00164        set_serv = rospy.Service("/"+ns+"/elevator_controller/set_position", set_elevator, handle_elev_set)
00165        home_serv = rospy.Service("/"+ns+"/elevator_controller/home", home_elevator, handle_elev_home)
00166        rospy.Subscriber("/"+ns+"/elevator_controller/pos_command", ric_elevator_command, epos_callback)
00167        rospy.loginfo("Seting up elevator...")
00168     rospy.Subscriber("/"+ns+"/base_rotation_controller/state", dxl_JointState, br_callback)
00169     rospy.Subscriber("/"+ns+"/shoulder_controller/state", dxl_JointState, sh_callback)
00170     rospy.Subscriber("/"+ns+"/elbow1_controller/state", dxl_JointState, e1_callback)
00171     rospy.Subscriber("/"+ns+"/elbow2_controller/state", dxl_JointState, e2_callback)
00172     rospy.Subscriber("/"+ns+"/left_finger_controller/state", dxl_JointState, lf_callback)
00173     rospy.Subscriber("/"+ns+"/right_finger_controller/state", dxl_JointState, rf_callback)
00174     rospy.Subscriber("/"+ns+"/wrist_controller/state", dxl_JointState, wr_callback)
00175     pub = rospy.Publisher('joint_states', JointState)
00176    
00177     while not rospy.is_shutdown():
00178         msg.header.stamp = rospy.Time.now()
00179         pub.publish(msg)
00180         #rospy.loginfo("Elevator position is: %.3f",final_epos)
00181         if have_elevator:
00182            epos_err=elev_goal_pos-final_epos
00183            if elev_move==True and abs(epos_err)<epos_tol:
00184               elev_move=False
00185               rospy.loginfo("Elevator reached goal position: %.3f",elev_goal_pos)
00186               elevpub.publish(0.0)
00187            elif elev_move==True:
00188               if epos_err>0: speed=max_elev_speed
00189               elif epos_err<0: speed=-max_elev_speed
00190               elevpub.publish(speed/elev_rad2m)
00191         rospy.sleep(0.05)
00192 
00193 
00194 if __name__ == '__main__':
00195     komodo_arm()
00196 


ric_robot
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:40:59