RobotService接口

本节介绍使用RobotService相关的接口，具体包含以下接口：

模拟RobotServer服务

模拟RobotServer服务通常用于主控机器人，但无法通过RobotServer控制机器人，而是创建RobotService以模拟RobotServer的作用。当RobotService注册在Mech-Viz后，Mech-Viz会以与真实机器人连接的同等方式与RobotService进行信息交互。

模拟RobotServer服务应用于以下场景：

获取机器人关节角用于Mech-Vision计算（Eye In Hand）；
从真实机器人获取位姿传输给RobotService服务。

示例

在Adapter子类中调用方式如下所示。

def _register_service(self):
       """
       register_service
       :return:
       """
       if self.robot_service:
           return

       self.robot_service = RobotService(self)
       other_info = {'robot_type': self.robot_service.service_name}
       self.server, _ = register_service(self.hub_caller, self.robot_service, other_info)

       self.robot_service.setJ([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

RobotService类如下所示。

class RobotService(JsonService):
    service_type = "robot"
    service_name = "robot"
    jps = [0, 0, 0, 0, 0, 0]
    pose = [0, 0, 0, 1, 0, 0, 0]

    def getJ(self, *_):
        return {"joint_positions": self.jps}

    def setJ(self, jps):
        logging.info("setJ:{}".format(jps))
        self.jps = jps

    def getL(self, *_):
        return {"tcp_pose": self.pose}

    def getFL(self, *_):
        return {"flange_pose": self.pose}

    def setL(self, pose):
        logging.info("setL:{}".format(pose))
        self.pose = pose

    def moveXs(self, params, _):
        pass

    def stop(self, *_):
        pass

    def setTcp(self, *_):
        pass

    def setDigitalOut(self, params, _):
        pass

    def getDigitalIn(self, *_):
        pass

    def switchPauseContinue(self, *_):
        pass

getJ

getJ()用于Mech-Viz和Mech-Vision获取当前关节角，具体如下所示。通常先通过setJ()设置当前关节角，然后再调用getJ()。

示例

def getJ(self, *_):
     pose = {"joint_positions": self._jps}
     return pose

Eye In Hand：将机器人发送的关节角写入setJ()。
```
def setJ(self, jps):
  assert len(jps) = 6
  for i in range(6):
      jps[i] = deg2rad(float(jps[i]))
  self._jps = jps
  logging.info("SetJ:{}".format(self._jps))
```
其中，jps是机器人发送过来的关节角数据，将其赋值给self._jps，以被getJ()调用。由于getJ()需要获取弧度格式的数据，此处需注意单位转化。
Eye To Hand：无需根据机器人当前关节角设置，但模拟真实机器人状态，需要设置一个安全的目标点用作初始点（通常为Home点），否则会以随机数赋值，易出错。
```
def getJ(self, *_):
     return {"joint_positions": [1.246689,-0.525868,-0.789761,-1.330814, 0.922581, 4.364021]}
```

getFL

getFL()用于提供机器人拍照时的法兰位姿，由于在Eye In Hand方式下标定的是法兰和相机的位置关系，但最终使用的数据是基坐标系下的数据，因此需要提供拍照时的法兰位姿。

def getFL(self, *_):
    return {"flange_pose": self.pose}

使用Eye In Hand方式提供视觉点时，需注意以下几点：

若机器人返回拍照时的JPs，则直接在RobotService中调用setJ()即可（单位是弧度），此函数将返回[]。
若机器人返回法兰位姿，则需进行以下操作：
- 确保Mech-Vision工程中外参文件extri_param.json中“is_eye_in_hand”为true；
- 调用RobotService的setFL()（单位是米，位姿类型是四元数）。

moveXs

Adapter 创建的 RobotService 服务使用 moveXs() 接收 Mech-Viz 规划后的路径点数据，具体如下所示。

函数定义

def moveXs(self, params, _):
     with self.lock:
         for move in params["moves"]:
             self.targets.append(move)
     return {"finished": True}

其中，params 传入了 Mech-Viz 工程中全部参数，通过 params[“moves”] 可获取全部运动点的位姿，包括视觉移动、相对移动等，位姿默认以关节角方式返回，将位姿传入 self.targets 便于后续调用。

示例

通常，此功能需要搭配通知共同使用，当 Adapter 获取到通知的消息时，将 self.targets 传入函数转化并打包后发送给机器人，示例如下所示。

def notify(self, request, _):
    msg = request["notify_message"]
    logging.info("{} notify message:{}".format(self.service_name, msg))
    if msg = "started":
        with self.lock:
            self.move_targets.clear()
    elif msg = "finished":
        with self.lock:
            targets = self.move_targets[:]
            self.move_targets.clear()
        self.send_moves(targets)

当收到通知消息为 “started” 时，将目标点列表清零（防止 Mech-Viz 错误中断导致前后两次移动点叠加）；当收到通知消息为 “finished” 时，将目标点列表传入 pack_move 函数，用于数据整合发送。

def pack_move(self, move_param):
     move_list = []
     for i, move in enumerate(move_param):
         target = move["target"]
         move_list.append(target)
     logging.info("move list num:{}".format(len(move_list)))
     logging.info("move list:{}".format(*move_list))
     motion_cmd = pack('>24f', *move_list)
     self.send(motion_cmd)

根据现场需要，可发送 Mech-Viz 中全部运动点，也可按照 index 选取其中几点发送；pack_move() 一般根据各品牌机器人所需格式对数据进行整合（通常会在通信协议中提前设定）。

RobotService接口

模拟RobotServer服务

getJ

getFL

moveXs

我们重视您的隐私