dsr_service_io_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 ROBOT_ID = "dsr01"
19 ROBOT_MODEL = "m1013"
20 import DR_init
21 DR_init.__dsr__id = ROBOT_ID
22 DR_init.__dsr__model = ROBOT_MODEL
23 from DSR_ROBOT import *
24 
25 def shutdown():
26  print "shutdown time!"
27  print "shutdown time!"
28  print "shutdown time!"
29 
30  pub_stop.publish(stop_mode=STOP_TYPE_QUICK)
31  return 0
32 
33 def msgRobotState_cb(msg):
34  msgRobotState_cb.count += 1
35 
36  if (0==(msgRobotState_cb.count % 100)):
37  rospy.loginfo("________ ROBOT STATUS ________")
38  print(" robot_state : %d" % (msg.robot_state))
39  print(" robot_state_str : %s" % (msg.robot_state_str))
40  print(" actual_mode : %d" % (msg.actual_mode))
41  print(" actual_space : %d" % (msg.actual_space))
42  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]))
43  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]))
44  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]))
45  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]))
46  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]))
47  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]))
48  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]))
49  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]))
50  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]))
51  print(" solution_space : %d" % (msg.solution_space))
52  sys.stdout.write(" rotation_matrix : ")
53  for i in range(0 , 3):
54  sys.stdout.write( "dim : [%d]"% i)
55  sys.stdout.write(" [ ")
56  for j in range(0 , 3):
57  sys.stdout.write("%d " % msg.rotation_matrix[i].data[j])
58  sys.stdout.write("] ")
59  print ##end line
60  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]))
61  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]))
62  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]))
63  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]))
64  print(" sync_time : %7.3f" % (msg.sync_time))
65  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]))
66  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]))
67  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]))
68  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]))
69 
70  #print digital i/o
71  sys.stdout.write(" ctrlbox_digital_input : ")
72  for i in range(0 , 16):
73  sys.stdout.write("%d " % msg.ctrlbox_digital_input[i])
74  print ##end line
75  sys.stdout.write(" ctrlbox_digital_output: ")
76  for i in range(0 , 16):
77  sys.stdout.write("%d " % msg.ctrlbox_digital_output[i])
78  print
79  sys.stdout.write(" flange_digital_input : ")
80  for i in range(0 , 6):
81  sys.stdout.write("%d " % msg.flange_digital_input[i])
82  print
83  sys.stdout.write(" flange_digital_output : ")
84  for i in range(0 , 6):
85  sys.stdout.write("%d " % msg.flange_digital_output[i])
86  print
87  #print modbus i/o
88  sys.stdout.write(" modbus_state : " )
89  if len(msg.modbus_state) > 0:
90  for i in range(0 , len(msg.modbus_state)):
91  sys.stdout.write("[" + msg.modbus_state[i].modbus_symbol)
92  sys.stdout.write(", %d] " % msg.modbus_state[i].modbus_value)
93  print
94 
95  print(" access_control : %d" % (msg.access_control))
96  print(" homming_completed : %d" % (msg.homming_completed))
97  print(" tp_initialized : %d" % (msg.tp_initialized))
98  print(" mastering_need : %d" % (msg.mastering_need))
99  print(" drl_stopped : %d" % (msg.drl_stopped))
100  print(" disconnected : %d" % (msg.disconnected))
101 msgRobotState_cb.count = 0
102 
103 def thread_subscriber():
104  rospy.Subscriber('/'+ROBOT_ID +ROBOT_MODEL+'/state', RobotState, msgRobotState_cb)
105  rospy.spin()
106  #rospy.spinner(2)
107 
108 if __name__ == "__main__":
109  rospy.init_node('dsr_simple_test_py')
110  rospy.on_shutdown(shutdown)
111 
112  t1 = threading.Thread(target=thread_subscriber)
113  t1.daemon = True
114  t1.start()
115 
116  pub_stop = rospy.Publisher('/'+ROBOT_ID +ROBOT_MODEL+'/stop', RobotStop, queue_size=10)
117 
118  set_digital_output(2, ON)
119  if get_digital_input(2) == 1 :
120  set_digital_output(11, ON)
121  while not rospy.is_shutdown():
122  pass
123 
124  print 'good bye!'
dsr_service_io_simple.msgRobotState_cb
def msgRobotState_cb(msg)
Definition: dsr_service_io_simple.py:33
dsr_service_io_basic.get_digital_input
get_digital_input
Definition: dsr_service_io_basic.py:60
dsr_service_io_simple.shutdown
def shutdown()
Definition: dsr_service_io_simple.py:25
dsr_service_io_basic.set_digital_output
set_digital_output
Definition: dsr_service_io_basic.py:59
dsr_service_io_simple.thread_subscriber
def thread_subscriber()
Definition: dsr_service_io_simple.py:103

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