src/fsrobo_r_driver/cc_client.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # FSRobo-R Package BSDL
5 # ---------
6 # Copyright (C) 2019 FUJISOFT. All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without modification,
9 # are permitted provided that the following conditions are met:
10 # 1. Redistributions of source code must retain the above copyright notice,
11 # this list of conditions and the following disclaimer.
12 # 2. Redistributions in binary form must reproduce the above copyright notice,
13 # this list of conditions and the following disclaimer in the documentation and/or
14 # other materials provided with the distribution.
15 # 3. Neither the name of the copyright holder nor the names of its contributors
16 # may be used to endorse or promote products derived from this software without
17 # specific prior written permission.
18 #
19 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 # IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 # INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27 # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 # ---------
30 
31 import time
32 import socket
33 import json
34 from struct import pack, unpack
35 
36 class CCClient:
37  # クラス変数
38  _SOCKET_IP_ADDRESS = "192.168.0.23"
39  _SOCKET_PORT_NUMBER = 5500
40  # ロボット操作コマンド
41  _CMD_PROGRAM = 0x000
42  _CMD_HOME = 0x100
43  _CMD_JSPEED = 0x103
44  _CMD_P2J = 0x105
45  _CMD_QJMOVE = 0x106
46  _CMD_SETTOOL = 0x107
47  _CMD_SETPOSTURE = 0x10B
48  _CMD_GETPOSTURE = 0x10C
49  _CMD_ABORTM = 0x10F
50  _CMD_SYSSTS = 0x110
51  # I/O操作コマンド
52  _CMD_SETIO = 0x200
53  _CMD_GETIO = 0x201
54  _CMD_SETADC = 0x202
55  _CMD_GETADC = 0x203
56  # その他コマンド
57  _CMD_NOCOMMAND = 0xFFF
58 
59  _CONNECTING_PROCCES_ROS = 0x02
60  # データ種別
61  _DATA_TYPE_CMD = 0x00
62  _DATA_TYPE_PROGRAM = 0x01
63  _DATA_TYPE_CONNECT_CHECK = 0x02
64  _DATA_TYPE_OPERATION_GET = 0x03
65 
66  _SOCKET_RECV_BUFF_SIZE = 4096
67 
68  def __init__(self, ip_address=_SOCKET_IP_ADDRESS):
69  # ソケットを作成してデータの送受信を実施
70  self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
71  self._sock.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
72  self._sock.connect((ip_address, self._SOCKET_PORT_NUMBER))
73  #self.open()
74 
75  def _send_data(self, send_data, return_data = {}):
76  #print "_send_data execution"
77  self._sock.sendall(send_data)
78  # ティーチからの送信データを受信
79  rec_msg = self._sock.recv(self._SOCKET_RECV_BUFF_SIZE)
80  recv_data = json.loads(rec_msg)
81  err_code = recv_data["RE"]
82  return_data["DA"] = json.loads(recv_data["DA"])
83  return err_code
84 
85  def _create_send_data(self, cmd_id, data, data_type=_DATA_TYPE_CMD):
86  send_json = {
87  "CD": cmd_id,
88  "PR": self._CONNECTING_PROCCES_ROS,
89  "DT": data_type,
90  "DA": json.dumps(data)
91  }
92  send_char_data = json.dumps(send_json)
93  pack_format = str(len(send_char_data)) + "s"
94  send_data = pack(pack_format, send_char_data)
95  return send_data
96 
98  data = {}
99  send_data = self._create_send_data(self._CMD_NOCOMMAND, data, self._DATA_TYPE_CONNECT_CHECK)
100  return_data = self._send_data(send_data)
101  return return_data
102 
104  data = {}
105  send_data = self._create_send_data(self._CMD_NOCOMMAND, data, self._DATA_TYPE_OPERATION_GET)
106  return_data = self._send_data(send_data)
107  return return_data
108 
109  def exec_program(self, path, param='', delete_flag=0):
110  data = {
111  "PATH": path,
112  "PAR": param,
113  "DEL": delete_flag
114  }
115  send_data = self._create_send_data(self._CMD_PROGRAM, data, self._DATA_TYPE_PROGRAM)
116  return_data = self._send_data(send_data)
117  return return_data
118 
119  def qjmove(self, ax1, ax2, ax3, ax4, ax5, ax6, speed=None):
120  print "qjmove execution"
121  data = {
122  "J1": ax1,
123  "J2": ax2,
124  "J3": ax3,
125  "J4": ax4,
126  "J5": ax5,
127  "J6": ax6
128  }
129  if speed is not None:
130  data['SP'] = speed
131  send_data = self._create_send_data(self._CMD_QJMOVE, data)
132  return_data = self._send_data(send_data)
133  print "qjmove success"
134  print "return:" + str(return_data)
135  return return_data
136 
137  def abortm(self):
138  print "abortm execution"
139  data = {}
140  send_data = self._create_send_data(self._CMD_ABORTM, data)
141  return_data = self._send_data(send_data)
142  print "abortm success"
143  print "return:" + str(return_data)
144  return return_data
145 
146  def syssts(self, stat_type, output_data):
147  #print "syssts execution"
148  data = {
149  "TYPE": stat_type
150  }
151  send_data = self._create_send_data(self._CMD_SYSSTS, data)
152  return_data = self._send_data(send_data, output_data)
153  #print "syssts success"
154  #print "return:" + str(return_data)
155  return return_data
156 
157  def set_jspeed(self, s):
158  print "set_jspeed execution"
159  data = {
160  "SP": s * 100
161  }
162  send_data = self._create_send_data(self._CMD_JSPEED, data)
163  print "speed設定送信"
164  return_data = self._send_data(send_data)
165  print "set_jspeed success"
166  print "return:" + str(return_data)
167  return return_data
168 
169  def set_posture(self, posture):
170  data = {
171  "P": posture
172  }
173  send_data = self._create_send_data(self._CMD_SETPOSTURE, data)
174  return_data = self._send_data(send_data)
175  print "set_posture success"
176  print "return:" + str(return_data)
177  return return_data
178 
179  def get_posture(self, output_data):
180  data = {
181  }
182  send_data = self._create_send_data(self._CMD_GETPOSTURE, data)
183  return_data = self._send_data(send_data, output_data)
184  print "get_posture success"
185  print "return:" + str(return_data)
186  print "output:" + str(output_data)
187  return return_data
188 
189  def position_to_joint(self, x, y, z, rx, ry, rz, output_data):
190  data = {
191  "X": x,
192  "Y": y,
193  "Z": z,
194  "Rx": rx,
195  "Ry": ry,
196  "Rz": rz,
197  "P": -1
198  }
199  send_data = self._create_send_data(self._CMD_P2J, data)
200  return_data = self._send_data(send_data, output_data)
201  print "position_to_joint success"
202  print "return:" + str(return_data)
203  return return_data
204 
205  def set_io(self, address, signal):
206  data = {
207  "AD": address,
208  "SL": signal
209  }
210  send_data = self._create_send_data(self._CMD_SETIO, data)
211  return_data = self._send_data(send_data)
212  print "set_io success"
213  print "return:" + str(return_data)
214  return return_data
215 
216  def get_io(self, start_addr, end_addr, output_data):
217  data = {
218  "SA": start_addr,
219  "EA": end_addr
220  }
221  send_data = self._create_send_data(self._CMD_GETIO, data)
222  return_data = self._send_data(send_data, output_data)
223  print "get_io success"
224  print "return:" + str(return_data)
225  print "output_data:" + str(output_data)
226  return return_data
227 
228  def set_adc(self, ch, mode):
229  data = {
230  "CH": ch,
231  "MO": mode
232  }
233  send_data = self._create_send_data(self._CMD_SETADC, data)
234  return_data = self._send_data(send_data)
235  print "set_adc success"
236  print "return:" + str(return_data)
237  return return_data
238 
239  def get_adc(self, output_data):
240  data = {}
241  send_data = self._create_send_data(self._CMD_GETADC, data)
242  return_data = self._send_data(send_data, output_data)
243  print "get_adc success"
244  print "return:" + str(return_data)
245  print "output_data:" + str(output_data)
246  return return_data
247 
248  def close(self):
249  self._sock.close()
250 
251  def set_tool(self, x, y, z, rz, ry, rx):
252  data = {
253  "X": x,
254  "Y": y,
255  "Z": z,
256  "Rx": rx,
257  "Ry": ry,
258  "Rz": rz,
259  }
260  send_data = self._create_send_data(self._CMD_SETTOOL, data)
261  return_data = self._send_data(send_data)
262  print "set_tool success"
263  print "return:" + str(return_data)
264  return return_data
265 
266 if __name__ == '__main__':
267  api = CCClient()
268  print "check_connect_status():"
269  print api.check_connect_status()
270  print "get_operation_permission():"
271  print api.get_operation_permission()
272  # print api.set_jspeed(0.5)
273  # print api.qjmove(30.5, 30.5, 40.5, 50.5, 60.5, 70.5)
274  # print api.qjmove(28.4, 28.5, 38.5, 44.5, 60.5, 70.5)
275  # #time.sleep(10)
276  # output_data = {}
277  # print api.set_io(10, "11111")
278  # print api.get_io(10, 14, output_data)
279  # print "output_data:" + str(output_data)
280  # print api.set_adc(1, 1)
281  # output_data2 = {}
282  # print api.get_adc(output_data2)
283  # print "output_data2:" + str(output_data2)
def syssts(self, stat_type, output_data)
def get_io(self, start_addr, end_addr, output_data)
def position_to_joint(self, x, y, z, rx, ry, rz, output_data)
def _create_send_data(self, cmd_id, data, data_type=_DATA_TYPE_CMD)
def _send_data(self, send_data, return_data={})
def exec_program(self, path, param='', delete_flag=0)
def qjmove(self, ax1, ax2, ax3, ax4, ax5, ax6, speed=None)
def __init__(self, ip_address=_SOCKET_IP_ADDRESS)


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