00001
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
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
00141 max_elev_pos=0.4
00142 home_ok=0
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, queue_size=1)
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, queue_size=1)
00176
00177 while not rospy.is_shutdown():
00178 msg.header.stamp = rospy.Time.now()
00179 pub.publish(msg)
00180
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