io_gripper_action.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 import rospy
4 import actionlib
5 from control_msgs.msg import GripperCommandAction, GripperCommandResult
6 from fsrobo_r_driver.io_interface import IOInterface
7 
8 
10  def __init__(self, action_name):
11  rospy.init_node(action_name)
12  rospy.loginfo('gripper_action server started')
13 
14  self._tool_io_param = rospy.get_param('tool_io')
15  robot_ns = self._tool_io_param.get('robot_ns')
16 
17  rospy.loginfo('wating I/O interface')
18  self._io = IOInterface(robot_ns, init_node=False)
19  rospy.loginfo('I/O found')
20  self.action_server = actionlib.SimpleActionServer(action_name, GripperCommandAction, self.execute_action, False)
21 
22  def run(self):
23  self.action_server.start()
24  rospy.loginfo('action server started')
25  rospy.spin()
26 
27  def _open(self):
28  return self._io_operation('open')
29 
30  def _close(self):
31  return self._io_operation('close')
32 
33  def _io_operation(self, name):
34  param = self._tool_io_param.get(name)
35  print('_io_operation: {} {}'.format(name, param))
36 
37  for x in param['trigger']:
38  addr, data = self._translate_io_param(x)
39  self._io.set_digital_val(addr, *data)
40  for x in param['state']:
41  addr, data = self._translate_io_param(x)
42  print('wait state: {} {}'.format(addr, data))
43  result = self._io.wait_digital_val(addr, data, time_out=1.0)
44  print('result: {}'.format(result))
45  if not result:
46  return False
47  return True
48 
49  def _translate_io_param(self, param):
50  start_addr = min(map(int, param.keys()))
51  end_addr = max(map(int, param.keys()))
52 
53  data = [param.get(str(k), -1) for k in range(start_addr, end_addr + 1)]
54 
55  return start_addr, data
56 
57  def execute_action(self, goal):
58  print('------------------execute_action')
59  print(goal)
60  if goal.command.position > 0:
61  success = self._open()
62  else:
63  success = self._close()
64 
65  if success:
66  result = GripperCommandResult()
67  result.position = goal.command.position
68  result.effort = goal.command.max_effort
69  result.reached_goal = True
70  print(result)
71  self.action_server.set_succeeded(result)
72  print('------------------set_succeeded')
73  else:
74  rospy.loginfo('tool operation aboarted')
75  self.action_server.set_aborted()
76  print('------------------set_aborted')
77 
78 if __name__ == '__main__':
79  server = IOGripperActionServer('gripper_action')
80  server.run()


fsrobo_r_driver
Author(s): F-ROSROBO
autogenerated on Sun Feb 9 2020 03:58:29