Proto

CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 3.16)
-project(robot_monitoring VERSION 2.6.0)
+project(robot_monitoring VERSION 2.7.5)
 
 add_subdirectory(xbot2_gui)

proto/generic.proto

@@ -0,0 +1,16 @@
+syntax = "proto3";
+
+import "jointstate.proto";
+import "text.proto";
+import "process_output.proto";
+import "theora_packet.proto";
+
+message Message {
+
+    int32 seq = 1;
+    JointState jointstate = 2;
+    Text text = 3;
+    ProcessOutput process_output = 4;
+    TheoraPacket theora_packet = 5;
+
+}

proto/jointstate.proto

@@ -0,0 +1,27 @@
+syntax = "proto3";
+
+message FloatArray {
+    repeated float value = 1;
+}
+
+message JointState {
+
+    repeated float linkPos = 1;
+    repeated float motPos = 2;
+    repeated float linkVel = 3;
+    repeated float motVel = 4;
+    repeated float tor = 5;
+    repeated float motorTemp = 6;
+    repeated float driverTemp = 7;
+
+    repeated float posRef = 8;
+    repeated float velRef = 9;
+    repeated float torRef = 10;
+    repeated float k = 11;
+    repeated float d = 12;
+
+    map<string, FloatArray> aux = 13;
+
+    float vbatt = 14;
+    float ibatt = 15;
+}

proto/process_output.proto

@@ -0,0 +1,8 @@
+syntax = "proto3";
+
+message ProcessOutput {
+
+    string name = 1;
+    string out = 2;
+    string err = 3;
+}

proto/text.proto

@@ -0,0 +1,6 @@
+syntax = "proto3";
+
+message Text {
+
+    string text = 1;
+}

proto/theora_packet.proto

@@ -0,0 +1,11 @@
+syntax = "proto3";
+
+message TheoraPacket {
+    string stream_name = 1;
+    bytes data = 2;
+    int32 b_o_s = 3;
+    int32 e_o_s = 4;
+    int32 granulepos = 5;
+    int32 packetno = 6;
+}
+

proto/update_proto.bash

@@ -0,0 +1 @@
+protoc *.proto --python_out=../server/src/xbot2_gui_server/proto

server/setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = xbot2_gui_server
-version = 2.6.0
+version = 2.7.3
 author = Arturo Laurenzi
 author_email = arturo.laurenzi@iit.it
 description = Backend for XBot2 QML GUI
@@ -23,6 +23,7 @@ python_requires = >=3.8
 install_requires =
     aiohttp
     asyncudp
+    protobuf>=5.29.0,<5.29.9
 
 
 [options.packages.find]
@@ -31,3 +32,4 @@ where = src
 [options.entry_points]
 console_scripts =
     xbot2_gui_server = xbot2_gui_server.main:main
+    xbot2_gui = xbot2_gui_server.xbot2_gui:main

server/src/xbot2_gui_server/cosecosecose.py

@@ -0,0 +1,5 @@
+from ecat_repl.stuff import read_sdo, set_uri
+
+set_uri('amax-5580.local:5555')
+an = read_sdo(['Assigned_name'], [1])[1]['Assigned_name']
+print(an)

server/src/xbot2_gui_server/ecat.py

@@ -100,6 +100,13 @@ async def get_sdo_list(self, req: web.Request):
             print('cache is empty, refreshing')
             self.ctx.update_cache()
 
+        for id in ids:
+            if id not in self.ctx.sdo_dict.keys():
+                print(f'esc id {id} not found in cache, refreshing...')
+                await utils.to_thread(self.ctx.update_cache)
+                print('...done')
+                break
+
         # fill sdo list
         sdos = []
         for id in ids:

server/src/xbot2_gui_server/joint_states.py

@@ -9,14 +9,15 @@
 
 
 import rospy 
+from sensor_msgs.msg import JointState as StdJointState
 from xbot_msgs.msg import JointState, Fault, JointCommand, CustomState
 from std_msgs.msg import Float32
 from urdf_parser_py import urdf as urdf_parser
 
 from .server import ServerBase
 from . import utils
 
-# from .proto import joint_states_pb2
+from .proto import jointstate_pb2, generic_pb2
 
 ## limit float precision in json serialization
 class RoundingFloat(float):
@@ -40,6 +41,7 @@ def __init__(self, srv: ServerBase, config=dict()) -> None:
         self.srv.schedule_task(self.run())
         self.srv.register_ws_coroutine(self.handle_ws_msg)
         self.srv.add_route('GET', '/joint_states/info', self.get_joint_info_handler, 'get_joint_info')
+        self.srv.add_route('GET', '/joint_states/grippers', self.get_grippers_handler, 'get_grippers')
         self.srv.add_route('GET', '/joint_states/urdf', self.get_urdf_handler, 'get_urdf')
         self.srv.add_route('GET', '/joint_states/connected', self.robot_connected_handler, 'get_connected')
         self.srv.add_route('PUT', '/joint_command/goto/{joint_name}', self.command_handler, 'command')
@@ -76,6 +78,11 @@ def __init__(self, srv: ServerBase, config=dict()) -> None:
         self.cmd_guard = JointStateHandler.CommandGuard(self.command_acquire, self.command_release)
         self.cmd_should_stop = True
 
+        # grippers
+        self.gripper_state_sub = dict()
+        self.gripper_cmd_pub = dict()
+        self.gripper_state_msg = dict()
+
         # config
         self.rate = config.get('rate', 30.0)
 
@@ -101,7 +108,11 @@ def on_aux_recv(self, msg: CustomState, aux: list):
             if not math.isnan(msg.value[i]):
                 aux[i] = msg.value[i]
 
-
+
+    def on_gripper_state_recv(self, msg: StdJointState, gname):
+        self.gripper_state_msg[gname] = msg
+
+
     @utils.handle_exceptions
     async def get_urdf_handler(self, request: web.Request):
         print('retrieving robot description..')
@@ -184,6 +195,43 @@ async def get_joint_info_handler(self, request: web.Request):
         return web.Response(text=json.dumps(joint_info))
 
 
+    @utils.handle_exceptions
+    async def get_grippers_handler(self, req: web.Request):
+
+        # get topic names from ros master
+        topic_name_type_list = await utils.to_thread(rospy.get_published_topics)
+
+        # filter /xbotcore/gripper/GRIPPERNAME/state
+        gripper_names = set()
+        for tname, ttype in topic_name_type_list:
+            print(tname, ttype)
+            if ttype != 'sensor_msgs/JointState':
+                continue
+            tokens = tname.strip('/').split('/')
+            if len(tokens) == 4 and tokens[1] == 'gripper' and tokens[3] == 'state':
+                gripper_names.add(tokens[2])
+
+        # register topics
+        for gname in gripper_names:
+            state = f'xbotcore/gripper/{gname}/state'
+            cmd = f'xbotcore/gripper/{gname}/command'
+            self.gripper_state_msg[gname] = None
+            self.gripper_state_sub[gname] = rospy.Subscriber(state, StdJointState, 
+                self.on_gripper_state_recv, gname, queue_size=1)
+            self.gripper_cmd_pub[gname] = rospy.Publisher(cmd, StdJointState, queue_size=1)
+            print(f'connecting to gripper {gname}...')
+
+
+        # reply
+        res = {
+            'success': True,
+            'message': '',
+            'gripper_names': sorted(list(gripper_names))
+        }
+
+        return web.Response(text=json.dumps(res))
+
+
     async def run(self):
 
         t0 = time.time()
@@ -207,30 +255,48 @@ async def run(self):
             # check js received
             if self.msg is None:
                 continue
-
-            # convert to dict
-            js_msg_to_send = self.js_msg_to_dict(self.msg)
-
-            # test: avoid sending names to save bw
-            del js_msg_to_send['name']
-
-            # add pow data and seq id
-            js_msg_to_send['vbatt'] = self.vbatt
-            js_msg_to_send['iload'] = self.iload
-            js_msg_to_send['seq'] = self.js_seq
-            self.js_seq += 1
-
-            # serialize msg to json
-            js_str = json.dumps(js_msg_to_send)      
 
-            # send to all connected clients
-            await self.srv.udp_send_to_all(js_str)
+            # pb js
+            # TBD aux support
+            try:
+                msgpb = generic_pb2.Message()
+                msgpb.jointstate.linkPos.extend(self.msg.link_position)
+                msgpb.jointstate.motPos.extend(self.msg.motor_position)
+                msgpb.jointstate.motVel.extend(self.msg.motor_velocity)
+                msgpb.jointstate.velRef.extend(self.msg.velocity_reference)
+                msgpb.jointstate.torRef.extend(self.msg.effort_reference)
+                msgpb.jointstate.tor.extend(self.msg.effort)
+                msgpb.jointstate.posRef.extend(self.msg.position_reference)
+                msgpb.jointstate.k.extend(self.msg.stiffness)
+                msgpb.jointstate.d.extend(self.msg.damping)
+                msgpb.jointstate.motorTemp.extend(self.msg.temperature_motor)
+                msgpb.jointstate.driverTemp.extend(self.msg.temperature_driver)
+                msgpb.jointstate.vbatt = self.vbatt
+                msgpb.jointstate.ibatt = self.iload
+                await self.srv.udp_send_to_all(msgpb)
+            except Exception as e:
+                # print traceback  
+                import traceback
+                traceback.print_exc()
+
 
             # clear to avoid sending duplicates
             self.msg = None
             for v in self.aux_map.values():
                 v.clear()
 
+            # grippers
+            for gname, gmsg in self.gripper_state_msg.items():
+                if gmsg is None:
+                    continue 
+                gmsg = {
+                    'type': 'gripper_state',
+                    'q': gmsg.position[0],
+                    'tau': gmsg.effort
+                }
+                await self.srv.ws_send_to_all(json.dumps(gmsg))
+                self.gripper_state_msg[gname] = None
+
 
     def command_acquire(self):
         if self.cmd_busy:
@@ -376,6 +442,7 @@ def js_msg_to_dict(self, msg: JointState):
 
 
     async def handle_ws_msg(self, msg, proto, ws):
+
         if msg['type'] == 'joint_cmd':
             cmdmsg = JointCommand()
             cmdmsg.name = msg['joint_names']
@@ -388,3 +455,12 @@ async def handle_ws_msg(self, msg, proto, ws):
             else:
                 raise ValueError(f'unknown control mode {msg["ctrl"]}')
             self.cmd_pub.publish(cmdmsg)
+
+        if msg['type'] == 'gripper_cmd':
+            cmdmsg = StdJointState()
+            gname = msg['name']
+            if msg['action'] == 'open':
+                cmdmsg.position = [0.0]
+            elif msg['action'] == 'close':
+                cmdmsg.effort = [msg['effort']]
+            self.gripper_cmd_pub[gname].publish(cmdmsg)