drl_reader.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 Jin Hyuk Gong (jinhyuk.gong@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 ROBOT_ID = "dsr01"
16 ROBOT_MODEL = "m1013"
17 import DR_init
18 DR_init.__dsr__id = ROBOT_ID
19 DR_init.__dsr__model = ROBOT_MODEL
20 from DSR_ROBOT import *
21 
22 
23 def shutdown():
24  print "shutdown time!"
25  print "shutdown time!"
26  print "shutdown time!"
27  print("DRL stop!!!!!")
28  drl_script_stop(stop_mode=STOP_TYPE_QUICK)
29  set_robot_mode(ROBOT_MODE_MANUAL)
30  pub_stop.publish(stop_mode=STOP_TYPE_QUICK)
31  return 0
32 
33 def fileRead():
34  #fd = open("/home/dra/catkin_ws/doosan-robot-drl-reader/common/servicepack/scripts/movej_test.drl", 'r')
35  file_dir = os.path.abspath(__file__)
36  print(file_dir)
37  fd = open("drl_files/movej_test.drl", 'r')
38  drl_code = ""
39  while True:
40  line = fd.readline()
41  drl_code += (line)
42  if not line:
43  break
44  fd.close()
45  return drl_code
46 
47 
48 def msgRobotState_cb(msg):
49  msgRobotState_cb.count += 1
50 
51  if (0==(msgRobotState_cb.count % 100)):
52  rospy.loginfo("________ ROBOT STATUS ________")
53  print(" robot_state : %d" % (msg.robot_state))
54  print(" robot_state_str : %s" % (msg.robot_state_str))
55  print(" actual_mode : %d" % (msg.actual_mode))
56  print(" actual_space : %d" % (msg.actual_space))
57  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]))
58  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]))
59  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]))
60  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]))
61  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]))
62  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]))
63  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]))
64  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]))
65  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]))
66  print(" solution_space : %d" % (msg.solution_space))
67  sys.stdout.write(" rotation_matrix : ")
68  for i in range(0 , 3):
69  sys.stdout.write( "dim : [%d]"% i)
70  sys.stdout.write(" [ ")
71  for j in range(0 , 3):
72  sys.stdout.write("%d " % msg.rotation_matrix[i].data[j])
73  sys.stdout.write("] ")
74  print ##end line
75  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]))
76  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]))
77  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]))
78  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]))
79  print(" sync_time : %7.3f" % (msg.sync_time))
80  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]))
81  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]))
82  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]))
83  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]))
84 
85  #print digital i/o
86  sys.stdout.write(" ctrlbox_digital_input : ")
87  for i in range(0 , 16):
88  sys.stdout.write("%d " % msg.ctrlbox_digital_input[i])
89  print ##end line
90  sys.stdout.write(" ctrlbox_digital_output: ")
91  for i in range(0 , 16):
92  sys.stdout.write("%d " % msg.ctrlbox_digital_output[i])
93  print
94  sys.stdout.write(" flange_digital_input : ")
95  for i in range(0 , 6):
96  sys.stdout.write("%d " % msg.flange_digital_input[i])
97  print
98  sys.stdout.write(" flange_digital_output : ")
99  for i in range(0 , 6):
100  sys.stdout.write("%d " % msg.flange_digital_output[i])
101  print
102  #print modbus i/o
103  sys.stdout.write(" modbus_state : " )
104  if len(msg.modbus_state) > 0:
105  for i in range(0 , len(msg.modbus_state)):
106  sys.stdout.write("[" + msg.modbus_state[i].modbus_symbol)
107  sys.stdout.write(", %d] " % msg.modbus_state[i].modbus_value)
108  print
109 
110  print(" access_control : %d" % (msg.access_control))
111  print(" homming_completed : %d" % (msg.homming_completed))
112  print(" tp_initialized : %d" % (msg.tp_initialized))
113  print(" mastering_need : %d" % (msg.mastering_need))
114  print(" drl_stopped : %d" % (msg.drl_stopped))
115  print(" disconnected : %d" % (msg.disconnected))
116 msgRobotState_cb.count = 0
117 
118 def thread_subscriber():
119  rospy.Subscriber('/'+ROBOT_ID +ROBOT_MODEL+'/state', RobotState, msgRobotState_cb)
120  print("thread_subscriber")
121  rospy.spin()
122  #rospy.spinner(2)
123 
124 if __name__ == "__main__":
125  rospy.init_node('single_robot_simple_py')
126  rospy.on_shutdown(shutdown)
127  #set_robot_mode = rospy.ServiceProxy('/'+ROBOT_ID +ROBOT_MODEL+'/system/set_robot_mode', SetRobotMode)
128  #get_robot_mode = rospy.ServiceProxy('/'+ROBOT_ID +ROBOT_MODEL+'/system/get_robot_mode', GetRobotMode)
129  #get_drl_state = rospy.ServiceProxy('/'+ROBOT_ID +ROBOT_MODEL+'/drl/get_drl_state', GetDrlState)
130  #t1 = threading.Thread(target=thread_subscriber)
131  #t1.daemon = True
132  #t1.start()
133 
134  pub_stop = rospy.Publisher('/'+ROBOT_ID +ROBOT_MODEL+'/stop', RobotStop, queue_size=10)
135  print(fileRead())
136  # if get_robot_mode() != ROBOT_MODE_AUTONOMOUS :
137  set_robot_mode(ROBOT_MODE_AUTONOMOUS)
138  res = drl_script_run(ROBOT_SYSTEM_REAL, fileRead())
139  #print("#################" + str(res) + "###############")
140 
141  while not rospy.is_shutdown():
142  pass
143  #print(get_drl_state())
144  #print(get_robot_mode())
145  #print(fileRead())
146  #drl_script_stop(stop_mode=STOP_TYPE_QUICK)
147  print 'good bye!'
drl_reader.msgRobotState_cb
def msgRobotState_cb(msg)
Definition: drl_reader.py:48
drl_reader.thread_subscriber
def thread_subscriber()
Definition: drl_reader.py:118
drl_reader.fileRead
def fileRead()
Definition: drl_reader.py:33
drl_reader.shutdown
def shutdown()
Definition: drl_reader.py:23
single_robot_simple.set_robot_mode
set_robot_mode
Definition: single_robot_simple.py:108

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