example_change_scene.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 #
4 # Example of ChangeScene for COBOTTA PRO
5 # Change Scene(0,0) from default Scene.
6 # RC9: v1.4.0 or later
7 # denso_robot_ros: v3.3.0 or later
8 #
9 # Usage:
10 # $ roslaunch denso_robot_bringup <robot_name>_bringup.launch sim:=false \
11 # ip_address:=192.168.0.1 send_format:=0 recv_format:=2
12 # $ roslaunch bcap_service bcap_service.launch ip_address:=192.168.0.1
13 # $ rosrun denso_robot_bringup example_change_scene.py _robot_name:=<robot_name>
14 #
15 # Reference:
16 # - b-CAP_Guide_RC9_en.pdf
17 # - RC9_ProvGuide_en.pdf
18 # - COBOTTA PRO User Manuals
19 #
20 # Copyright (C) 2023 DENSO WAVE INCORPORATED
21 import sys
22 import math
23 import ctypes
24 from enum import IntEnum
25 
26 import rosnode
27 import rospy
28 import moveit_commander
29 
30 from std_msgs.msg import Int32
31 from bcap_service.srv import bcap, bcapRequest
32 from bcap_service.msg import variant
33 
34 # Example Target Scene "<Scene Number>,<Sub-Scene Number>"
35 _EXAMPLE_SCENE_NUMBER = "0,0"
36 # Example Target Position
37 _EXAMPLE_POSITION = [0, -45, 135, 0, 45, 0]
38 # b-CAP slvMove Speed [default:5%]
39 _SLVMOVE_SPEED = 0.05
40 # b-CAP Slave Mode
41 _bcap_slave_mode = 0
42 
43 
44 # b-CAP function ID
45 # cf. b-CAP_Guide_RC9_en.pdf
46 class BcapFuncId(IntEnum):
47  CONTROLLER_CONNECT = 3
48  CONTROLLER_DISCONNECT = 4
49  CONTROLLER_GETROBOT = 7
50  CONTROLLER_EXECUTE = 17
51  ROBOT_EXECUTE = 64
52  ROBOT_RELEASE = 84
53  ROBOT_MOVE = 72
54 
55 
56 # CaoRobot::Move() interpolation
57 # cf. RC9_ProvGuide_en.pdf
58 class CaoRobotMove(IntEnum):
59  MOVE_P = 1
60  MOVE_L = 2
61  MOVE_C = 3
62  MOVE_S = 4
63 
64 
65 # Variant type of ORiN2/RAC
66 class VarType(IntEnum):
67  VT_I2 = 2
68  VT_I4 = 3
69  VT_R4 = 4
70  VT_R8 = 5
71  VT_CY = 6
72  VT_DATE = 7
73  VT_BSTR = 8
74  VT_BOOL = 11
75  VT_VARIANT = 12
76  VT_U11 = 17
77  VT_ARRAY = 0x2000
78 
79 
80 def usage():
81  rospy.logerr("Usage:\n"
82  " roslaunch denso_robot_bringup <robot_name>_bringup.launch"
83  " sim:=false ip_address:=<IP address> send_format:=0 recv_format:=2\n"
84  " roslaunch bcap_service bcap_service.launch ip_address:=<IP address>\n"
85  " rosrun denso_robot_bringup example_change_scene.py"
86  " _robot_name:=<robot_name>")
87 
88 
89 def convert_error_code(hresult):
90  return hex(ctypes.c_uint(hresult).value)
91 
92 
93 def callback_curmode(message):
94  global _bcap_slave_mode
95  _bcap_slave_mode = int(message.data)
96 
97 
98 def change_bcap_slave_mode(pub_changemode, mode):
99  if _bcap_slave_mode == mode:
100  return
101 
102  pub_changemode.publish(mode)
103  while (not rospy.is_shutdown()) and (_bcap_slave_mode != mode):
104  rospy.sleep(0.5) # 500ms
105 
106 
107 # GetErrorDescription
108 # b-CAP: Robot_Execute(GetErrorDescription)
109 # ORiN2: CaoController::Execute(GetErrorDescription)
110 def print_error_description(bcap_service, rc9, hresult):
111  bcap_request = bcapRequest()
112  bcap_request.func_id = BcapFuncId.CONTROLLER_EXECUTE
113  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9),
114  variant(vt=VarType.VT_BSTR, value='GetErrorDescription'),
115  variant(vt=VarType.VT_I4, value=str(hresult))]
116  res = bcap_service(bcap_request)
117  if res.HRESULT < 0:
118  return
119 
120  rospy.logerr("%s: %s", convert_error_code(hresult), res.vntRet.value)
121 
122 
123 def get_current_scene(bcap_service, rc9, rc9robot):
124  # CurScene: Get current Scene Number. Number 11 is default Scene.
125  # b-CAP: Robot_Execute(CurScene)
126  # ORiN2: CaoRobot::Execute(CurScene)
127  bcap_request = bcapRequest()
128  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
129  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
130  variant(vt=VarType.VT_BSTR, value='CurScene'),
131  variant(vt=VarType.VT_BSTR, value='')]
132  res = bcap_service(bcap_request)
133  if res.HRESULT < 0:
134  print_error_description(bcap_service, rc9, res.HRESULT)
135  raise Exception("bcap_service: RC9Robot: Failed to execute CurScene. error="
136  + convert_error_code(res.HRESULT))
137  scene_number = res.vntRet.value
138 
139  # CurSubScene: Get current Sub-Scene Number
140  # b-CAP: Robot_Execute(CurSubScene)
141  # ORiN2: CaoRobot::Execute(CurSubScene)
142  bcap_request = bcapRequest()
143  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
144  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
145  variant(vt=VarType.VT_BSTR, value='CurSubScene'),
146  variant(vt=VarType.VT_BSTR, value='')]
147  res = bcap_service(bcap_request)
148  if res.HRESULT < 0:
149  print_error_description(bcap_service, rc9, res.HRESULT)
150  raise Exception("bcap_service: RC9Robot: Failed to execute CurSubScene. error="
151  + convert_error_code(res.HRESULT))
152  sub_scene_number = res.vntRet.value
153 
154  return scene_number, sub_scene_number
155 
156 
157 def main():
158  global _bcap_slave_mode
159  _bcap_slave_mode = 0
160  rc9 = 0
161  rc9robot = 0
162  take_arm = False
163 
164  rospy.init_node('example_change_scene')
165  rospy.loginfo("Start Example of ChangeScene...")
166 
167  robot_name = rospy.get_param("~robot_name", "")
168  if not robot_name or len(robot_name) <= 0:
169  rospy.logerr("Invalid parameter: robot_name")
170  usage()
171  sys.exit(1)
172  rospy.loginfo("robot_name: %s", robot_name)
173 
174  rospy.loginfo("Check if /%s/denso_robot_control is up...", robot_name)
175  node_list = rosnode.get_node_names(namespace=robot_name)
176  if not '/' + robot_name + '/denso_robot_control' in node_list:
177  rospy.logerr("No such node: /%s/denso_robot_control", robot_name)
178  usage()
179  sys.exit(1)
180 
181  moveit_commander.roscpp_initialize(sys.argv)
182  move_group = moveit_commander.MoveGroupCommander('arm')
183  pub_changemode = rospy.Publisher('/' + robot_name + '/ChangeMode',
184  Int32, queue_size=1)
185  sub_curmode = rospy.Subscriber('/' + robot_name + '/CurMode',
186  Int32, callback_curmode)
187  while (pub_changemode.get_num_connections() < 1)\
188  and (sub_curmode.get_num_connections() < 1):
189  rospy.sleep(0.5)
190 
191  rospy.loginfo("Waiting for /bcap_service...")
192  rospy.wait_for_service('/bcap_service')
193  try:
194  rospy.loginfo("Connecting to /bcap_service...")
195  bcap_service = rospy.ServiceProxy('/bcap_service', bcap)
196 
197  # Connect to RC9 controller using b-CAP Service
198  # - b-CAP: Controller_Connect()
199  # - ORiN2: CaoWorkspace::AddController()
200  rospy.loginfo("bcap_service: Controller_Connect()...")
201  bcap_request = bcapRequest()
202  bcap_request.func_id = BcapFuncId.CONTROLLER_CONNECT
203  bcap_request.vntArgs = [variant(vt=VarType.VT_BSTR, value='example_controller_name'),
204  variant(vt=VarType.VT_BSTR, value='CaoProv.DENSO.VRC9'),
205  variant(vt=VarType.VT_BSTR, value='localhost'),
206  variant(vt=VarType.VT_BSTR, value='@IfNotMember')]
207  res = bcap_service(bcap_request)
208  if res.HRESULT < 0:
209  raise Exception("bcap_service: Failed to connect to RC9 controller. error="
210  + convert_error_code(res.HRESULT))
211  rc9 = res.vntRet.value
212  rospy.loginfo("bcap_service: Connected to RC9. Controller handle is %s", rc9)
213 
214  # Get a robot Handle from controller
215  # - b-CAP: Controller_GetRobot()
216  # - ORiN2: CaoController::AddRobot()
217  rospy.loginfo("bcap_service: RC9: Controller_GetRobot()...")
218  bcap_request = bcapRequest()
219  bcap_request.func_id = BcapFuncId.CONTROLLER_GETROBOT
220  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9),
221  variant(vt=VarType.VT_BSTR, value='example_robot_name'),
222  variant(vt=VarType.VT_BSTR, value='@IfNotMember')]
223  res = bcap_service(bcap_request)
224  if res.HRESULT < 0:
225  raise Exception("bcap_service: RC9: Failed to get the robot handle. error="
226  + convert_error_code(res.HRESULT))
227  rc9robot = res.vntRet.value
228  rospy.loginfo("bcap_service: RC9: Robot handle is %s", rc9robot)
229 
230  # slvGetMode: Acquire the current setting of the Slave Mode
231  # - b-CAP: Robot_Execute(slvGetMode)
232  # - ORiN2: CaoRobot::Execute(slvGetMode)
233  rospy.loginfo("bcap_service: RC9Robot: Executing slvGetMode...")
234  bcap_request = bcapRequest()
235  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
236  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
237  variant(vt=VarType.VT_BSTR, value='slvGetMode'),
238  variant(vt=VarType.VT_BSTR, value='')]
239  res = bcap_service(bcap_request)
240  if res.HRESULT < 0:
241  print_error_description(bcap_service, rc9, res.HRESULT)
242  raise Exception("bcap_service: RC9Robot: Failed to execute slvGetMode. error="
243  + convert_error_code(res.HRESULT))
244  _bcap_slave_mode = int(res.vntRet.value)
245  rospy.loginfo("bcap_service: RC9Robot: Current b-CAP Slave Mode is 0x%x",
246  _bcap_slave_mode)
247 
248  # Stop b-CAP Slave Mode
249  rospy.loginfo("Stopping b-CAP Slave Mode...")
250  change_bcap_slave_mode(pub_changemode, 0)
251 
252  # ClearError: Clear an error that occurs in the controller
253  # b-CAP: Controller_Execute(ClearError)
254  # ORiN2: CaoController::Execute(ClearError)
255  rospy.loginfo("bcap_service: RC9: Executing ClearError...")
256  bcap_request = bcapRequest()
257  bcap_request.func_id = BcapFuncId.CONTROLLER_EXECUTE
258  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9),
259  variant(vt=VarType.VT_BSTR, value='ClearError'),
260  variant(vt=VarType.VT_BSTR, value='')]
261  res = bcap_service(bcap_request)
262  if res.HRESULT < 0:
263  print_error_description(bcap_service, rc9, res.HRESULT)
264  raise Exception("bcap_service: RC9: Failed to execute ClearError. error="
265  + convert_error_code(res.HRESULT))
266 
267  # ManualReset: Clear safety-state
268  # b-CAP: Controller_Execute(ManualReset)
269  # ORiN2: CaoController::Execute(ManualReset)
270  rospy.loginfo("bcap_service: RC9: Executing ManualReset...")
271  bcap_request = bcapRequest()
272  bcap_request.func_id = BcapFuncId.CONTROLLER_EXECUTE
273  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9),
274  variant(vt=VarType.VT_BSTR, value='ManualReset'),
275  variant(vt=VarType.VT_BSTR, value='')]
276  res = bcap_service(bcap_request)
277  if res.HRESULT < 0:
278  print_error_description(bcap_service, rc9, res.HRESULT)
279  raise Exception("bcap_service: RC9: Failed to execute ManualReset. error="
280  + convert_error_code(res.HRESULT))
281 
282  # Motor: Turn on the motor
283  # b-CAP: Robot_Execute(Motor)
284  # ORiN2: CaoRobot::Execute(Motor)
285  rospy.loginfo("bcap_service: RC9Robot: Running Motor...")
286  bcap_request = bcapRequest()
287  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
288  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
289  variant(vt=VarType.VT_BSTR, value='Motor'),
290  variant(vt=VarType.VT_I4, value='1')]
291  res = bcap_service(bcap_request)
292  if res.HRESULT < 0:
293  print_error_description(bcap_service, rc9, res.HRESULT)
294  raise Exception("bcap_service: RC9Robot: Failed to run motor. error="
295  + convert_error_code(res.HRESULT))
296 
297  # TakeArm: Request to get control authority
298  # b-CAP: Robot_Execute(TakeArm)
299  # ORiN2: CaoRobot::Execute(TakeArm)
300  rospy.loginfo("bcap_service: RC9Robot: Executing TakeArm...")
301  bcap_request = bcapRequest()
302  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
303  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
304  variant(vt=VarType.VT_BSTR, value='TakeArm'),
305  variant(vt=(VarType.VT_ARRAY | VarType.VT_I4),
306  value='0,1')]
307  res = bcap_service(bcap_request)
308  if res.HRESULT < 0:
309  print_error_description(bcap_service, rc9, res.HRESULT)
310  raise Exception("bcap_service: RC9Robot: Failed to execute TakeArm. error="
311  + convert_error_code(res.HRESULT))
312  take_arm = True
313 
314  # CurScene/SurSubScene: Get current Scene Number and Sub-Scene Number
315  rospy.loginfo("bcap_service: RC9Robot: Executing CurScene and CurSubScene...")
316  scene_number, sub_scene_number = get_current_scene(bcap_service, rc9, rc9robot)
317  rospy.loginfo("bcap_service: RC9Robot: Current Scene is (%s,%s)",
318  scene_number, sub_scene_number)
319 
320  # Move SafetyP0: Required before ChangeScene
321  # b-CAP: Robot_Move(MOVE P)
322  # ORiN2: CaoRobot::Move(MOVE P)
323  # If Tool Number and Work Number are specified in Scene Parameter Setting,
324  # change them with CaoRobot::Change().
325  # Select a motion option of Move command suitable for Scene Parameter Setting.
326  rospy.loginfo("bcap_service: RC9Robot: Moving to SafetyP0 position before ChangeScene...")
327  bcap_request = bcapRequest()
328  bcap_request.func_id = BcapFuncId.ROBOT_MOVE
329  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
330  variant(vt=VarType.VT_I4, value=str(CaoRobotMove.MOVE_P.value)),
331  variant(vt=VarType.VT_BSTR, value='SafetyP0'),
332  variant(vt=VarType.VT_BSTR, value='')]
333  res = bcap_service(bcap_request)
334  if res.HRESULT < 0:
335  print_error_description(bcap_service, rc9, res.HRESULT)
336  raise Exception("bcap_service: RC9Robot: Failed to move SafetyP0. error="
337  + convert_error_code(res.HRESULT))
338 
339  # ChangeScene: Change current Scene
340  # b-CAP: Robot_Execute(ChangeScene)
341  # ORiN2: CaoRobot::Execute(ChangeScene)
342  rospy.loginfo("bcap_service: RC9Robot: Executing ChangeScene(%s)...",
343  _EXAMPLE_SCENE_NUMBER)
344  bcap_request = bcapRequest()
345  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
346  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
347  variant(vt=VarType.VT_BSTR, value='ChangeScene'),
348  variant(vt=(VarType.VT_ARRAY | VarType.VT_I4),
349  value=_EXAMPLE_SCENE_NUMBER)]
350  res = bcap_service(bcap_request)
351  if res.HRESULT < 0:
352  print_error_description(bcap_service, rc9, res.HRESULT)
353  raise Exception("bcap_service: RC9Robot: Failed to execute ChangeScene. error="
354  + convert_error_code(res.HRESULT))
355 
356  # GiveArm: Request to release control authority
357  # b-CAP: Robot_Execute(GiveArm)
358  # ORiN2: CaoRobot::Execute(GiveArm)
359  rospy.loginfo("bcap_service: RC9Robot: Executing GiveArm...")
360  bcap_request = bcapRequest()
361  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
362  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
363  variant(vt=VarType.VT_BSTR, value='GiveArm'),
364  variant(vt=VarType.VT_BSTR, value='')]
365  res = bcap_service(bcap_request)
366  if res.HRESULT < 0:
367  print_error_description(bcap_service, rc9, res.HRESULT)
368  raise Exception("bcap_service: RC9Robot: Failed to execute GiveArm. error=%x"
369  + convert_error_code(res.HRESULT))
370  take_arm = False
371 
372  # Start b-Cap Slave
373  rospy.loginfo("Starting b-CAP Slave Mode...")
374  change_bcap_slave_mode(pub_changemode, 0x202)
375 
376  # Robot moves by MoveIt!
377  rospy.loginfo("Move(slvMove) to " + str(_EXAMPLE_POSITION) + "...")
378  move_group.set_max_acceleration_scaling_factor(_SLVMOVE_SPEED)
379  move_group.set_max_velocity_scaling_factor(_SLVMOVE_SPEED)
380  pose = [x / 180.0 * math.pi for x in _EXAMPLE_POSITION]
381  move_group.go(pose, wait=True)
382  move_group.stop()
383 
384  except rospy.ServiceException as se:
385  rospy.logerr("rospy.ServiceException: %s", se)
386 
387  except Exception as e:
388  rospy.logerr("%s: %s", e.__class__.__name__, e.message)
389 
390  finally:
391  rospy.loginfo("Cleaning up...")
392  # GiveArm
393  if rc9robot > 0 and take_arm:
394  rospy.loginfo("bcap_service: RC9Robot: Executing GiveArm...")
395  bcap_request = bcapRequest()
396  bcap_request.func_id = BcapFuncId.ROBOT_EXECUTE
397  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot),
398  variant(vt=VarType.VT_BSTR, value='GiveArm'),
399  variant(vt=VarType.VT_BSTR, value='')]
400  bcap_service(bcap_request)
401 
402  if rc9robot > 0:
403  # Release robot handle
404  # b-CAP: Robot_Release()
405  # ORiN2: CaoRobots::Remove()
406  rospy.loginfo("bcap_service: RC9Robot: Release robot handle...")
407  bcap_request = bcapRequest()
408  bcap_request.func_id = BcapFuncId.ROBOT_RELEASE
409  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9robot)]
410  bcap_service(bcap_request)
411 
412  if rc9 > 0:
413  # Disconnect from the controller
414  # b-CAP: Controller_Disconnect()
415  # ORiN2: CaoWorkspace::Controllers::Remove()
416  rospy.loginfo("bcap_service: RC9: Disconnect from the controller...")
417  bcap_request = bcapRequest()
418  bcap_request.func_id = BcapFuncId.CONTROLLER_DISCONNECT
419  bcap_request.vntArgs = [variant(vt=VarType.VT_I4, value=rc9)]
420  bcap_service(bcap_request)
421  rospy.loginfo("bcap_service: RC9: Disconnected.")
422 
423 
424 if __name__ == "__main__":
425  main()
example_change_scene.convert_error_code
def convert_error_code(hresult)
Definition: example_change_scene.py:89
example_change_scene.callback_curmode
def callback_curmode(message)
Definition: example_change_scene.py:93
example_change_scene.change_bcap_slave_mode
def change_bcap_slave_mode(pub_changemode, mode)
Definition: example_change_scene.py:98
example_change_scene.CaoRobotMove
Definition: example_change_scene.py:58
example_change_scene.usage
def usage()
Definition: example_change_scene.py:80
example_change_scene.BcapFuncId
Definition: example_change_scene.py:46
example_change_scene.get_current_scene
def get_current_scene(bcap_service, rc9, rc9robot)
Definition: example_change_scene.py:123
example_change_scene.VarType
Definition: example_change_scene.py:66
example_change_scene.print_error_description
def print_error_description(bcap_service, rc9, hresult)
Definition: example_change_scene.py:110
example_change_scene.main
def main()
Definition: example_change_scene.py:157

denso_robot_bringup
Author(s): DENSO WAVE INCORPORATED
autogenerated on Sat Feb 18 2023 04:06:00