dsr_service_drl_simple.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # ##
4 # @brief [py example simple] motion basic test for doosan robot
5 # @author Kab Kyoum Kim (kabkyoum.kim@doosan.com)
6 
7 import rospy
8 import os
9 import threading, time
10 import sys
11 sys.dont_write_bytecode = True
12 sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__),"../../../../common/imp")) ) # get import pass : DSR_ROBOT.py
13 
14 # for single robot
15 
16 ROBOT_SYSTEM_VIRTUAL = 1
17 ROBOT_SYSTEM_REAL = 0
18 
19 ROBOT_ID = "dsr01"
20 ROBOT_MODEL = "m0609"
21 import DR_init
22 DR_init.__dsr__id = ROBOT_ID
23 DR_init.__dsr__model = ROBOT_MODEL
24 from DSR_ROBOT import *
25 
26 def shutdown():
27  print "shutdown time!"
28  print "shutdown time!"
29  print "shutdown time!"
30 
31  pub_stop.publish(stop_mode=STOP_TYPE_QUICK)
32  return 0
33 
34 def msgRobotState_cb(msg):
35  msgRobotState_cb.count += 1
36 
37  if (0==(msgRobotState_cb.count % 100)):
38  rospy.loginfo("________ ROBOT STATUS ________")
39  print(" robot_state : %d" % (msg.robot_state))
40  print(" robot_state_str : %s" % (msg.robot_state_str))
41  print(" actual_mode : %d" % (msg.actual_mode))
42  print(" actual_space : %d" % (msg.actual_space))
43  print(" current_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posj[0],msg.current_posj[1],msg.current_posj[2],msg.current_posj[3],msg.current_posj[4],msg.current_posj[5]))
44  print(" current_velj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_velj[0],msg.current_velj[1],msg.current_velj[2],msg.current_velj[3],msg.current_velj[4],msg.current_velj[5]))
45  print(" joint_abs : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.joint_abs[0],msg.joint_abs[1],msg.joint_abs[2],msg.joint_abs[3],msg.joint_abs[4],msg.joint_abs[5]))
46  print(" joint_err : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.joint_err[0],msg.joint_err[1],msg.joint_err[2],msg.joint_err[3],msg.joint_err[4],msg.joint_err[5]))
47  print(" target_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.target_posj[0],msg.target_posj[1],msg.target_posj[2],msg.target_posj[3],msg.target_posj[4],msg.target_posj[5]))
48  print(" target_velj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.target_velj[0],msg.target_velj[1],msg.target_velj[2],msg.target_velj[3],msg.target_velj[4],msg.target_velj[5]))
49  print(" current_posx : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posx[0],msg.current_posx[1],msg.current_posx[2],msg.current_posx[3],msg.current_posx[4],msg.current_posx[5]))
50  print(" current_velx : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_velx[0],msg.current_velx[1],msg.current_velx[2],msg.current_velx[3],msg.current_velx[4],msg.current_velx[5]))
51  print(" task_err : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.task_err[0],msg.task_err[1],msg.task_err[2],msg.task_err[3],msg.task_err[4],msg.task_err[5]))
52  print(" solution_space : %d" % (msg.solution_space))
53  sys.stdout.write(" rotation_matrix : ")
54  for i in range(0 , 3):
55  sys.stdout.write( "dim : [%d]"% i)
56  sys.stdout.write(" [ ")
57  for j in range(0 , 3):
58  sys.stdout.write("%d " % msg.rotation_matrix[i].data[j])
59  sys.stdout.write("] ")
60  print ##end line
61  print(" dynamic_tor : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.dynamic_tor[0],msg.dynamic_tor[1],msg.dynamic_tor[2],msg.dynamic_tor[3],msg.dynamic_tor[4],msg.dynamic_tor[5]))
62  print(" actual_jts : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_jts[0],msg.actual_jts[1],msg.actual_jts[2],msg.actual_jts[3],msg.actual_jts[4],msg.actual_jts[5]))
63  print(" actual_ejt : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_ejt[0],msg.actual_ejt[1],msg.actual_ejt[2],msg.actual_ejt[3],msg.actual_ejt[4],msg.actual_ejt[5]))
64  print(" actual_ett : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_ett[0],msg.actual_ett[1],msg.actual_ett[2],msg.actual_ett[3],msg.actual_ett[4],msg.actual_ett[5]))
65  print(" sync_time : %7.3f" % (msg.sync_time))
66  print(" actual_bk : %d %d %d %d %d %d" % (msg.actual_bk[0],msg.actual_bk[1],msg.actual_bk[2],msg.actual_bk[3],msg.actual_bk[4],msg.actual_bk[5]))
67  print(" actual_bt : %d %d %d %d %d " % (msg.actual_bt[0],msg.actual_bt[1],msg.actual_bt[2],msg.actual_bt[3],msg.actual_bt[4]))
68  print(" actual_mc : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_mc[0],msg.actual_mc[1],msg.actual_mc[2],msg.actual_mc[3],msg.actual_mc[4],msg.actual_mc[5]))
69  print(" actual_mt : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_mt[0],msg.actual_mt[1],msg.actual_mt[2],msg.actual_mt[3],msg.actual_mt[4],msg.actual_mt[5]))
70 
71  #print digital i/o
72  sys.stdout.write(" ctrlbox_digital_input : ")
73  for i in range(0 , 16):
74  sys.stdout.write("%d " % msg.ctrlbox_digital_input[i])
75  print ##end line
76  sys.stdout.write(" ctrlbox_digital_output: ")
77  for i in range(0 , 16):
78  sys.stdout.write("%d " % msg.ctrlbox_digital_output[i])
79  print
80  sys.stdout.write(" flange_digital_input : ")
81  for i in range(0 , 6):
82  sys.stdout.write("%d " % msg.flange_digital_input[i])
83  print
84  sys.stdout.write(" flange_digital_output : ")
85  for i in range(0 , 6):
86  sys.stdout.write("%d " % msg.flange_digital_output[i])
87  print
88  #print modbus i/o
89  sys.stdout.write(" modbus_state : " )
90  if len(msg.modbus_state) > 0:
91  for i in range(0 , len(msg.modbus_state)):
92  sys.stdout.write("[" + msg.modbus_state[i].modbus_symbol)
93  sys.stdout.write(", %d] " % msg.modbus_state[i].modbus_value)
94  print
95 
96  print(" access_control : %d" % (msg.access_control))
97  print(" homming_completed : %d" % (msg.homming_completed))
98  print(" tp_initialized : %d" % (msg.tp_initialized))
99  print(" mastering_need : %d" % (msg.mastering_need))
100  print(" drl_stopped : %d" % (msg.drl_stopped))
101  print(" disconnected : %d" % (msg.disconnected))
102 msgRobotState_cb.count = 0
103 
104 def thread_subscriber():
105  rospy.Subscriber('/'+ROBOT_ID +ROBOT_MODEL+'/state', RobotState, msgRobotState_cb)
106  rospy.spin()
107  #rospy.spinner(2)
108 
109 if __name__ == "__main__":
110  rospy.init_node('dsr_service_drl_simple_py')
111  rospy.on_shutdown(shutdown)
112 
113  t1 = threading.Thread(target=thread_subscriber)
114  t1.daemon = True
115  t1.start()
116 
117  pub_stop = rospy.Publisher('/'+ROBOT_ID +ROBOT_MODEL+'/stop', RobotStop, queue_size=10)
118 
119  drlCodeMove = "set_velj(50)\nset_accj(50)\nmovej([0,0,90,0,90,0])\n"
120  drlCodeReset = "movej([0,0,0,0,0,0])\n"
121  drl_script_run(ROBOT_SYSTEM_REAL, drlCodeMove + drlCodeReset)
122  while not rospy.is_shutdown():
123  pass
124  print 'good bye!'
dsr_service_drl_simple.shutdown
def shutdown()
Definition: dsr_service_drl_simple.py:26
dsr_service_drl_simple.msgRobotState_cb
def msgRobotState_cb(msg)
Definition: dsr_service_drl_simple.py:34
dsr_service_drl_simple.thread_subscriber
def thread_subscriber()
Definition: dsr_service_drl_simple.py:104

py
Author(s):
autogenerated on Sat May 18 2019 02:32:55