updat

sktometometo · Jul 4, 2024 · 6d9d12e · 6d9d12e
1 parent 01b63dc
commit 6d9d12e
Show file tree

Hide file tree

Showing 6 changed files with 344 additions and 3 deletions.
diff --git a/python/smart_device_protocol/smart_device_protocol_interface.py b/python/smart_device_protocol/smart_device_protocol_interface.py
@@ -301,6 +301,17 @@ def __init__(
 
     def _callback_data_for_interface(self, src_address, frame):
         interface_description = frame.interface_description
+        if src_address in self._device_interfaces:
+            if "broadcast_interfaces" not in self._device_interfaces[src_address]:
+                self._device_interfaces[src_address]["broadcast_interfaces"] = []
+            if (
+                interface_description
+                not in self._device_interfaces[src_address]["broadcast_interfaces"]
+            ):
+                self._device_interfaces[src_address]["broadcast_interfaces"].append(
+                    interface_description
+                )
+            self._device_interfaces[src_address]["last_stamp"] = rospy.Time.now()
         if interface_description in self._interface_callbacks:
             self._interface_callbacks[interface_description](src_address, frame)
 

diff --git a/ros_lib/ros_lib/std_srvs/Empty.h b/ros_lib/ros_lib/std_srvs/Empty.h
@@ -0,0 +1,70 @@
+#ifndef _ROS_SERVICE_Empty_h
+#define _ROS_SERVICE_Empty_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_srvs
+{
+
+static const char EMPTY[] = "std_srvs/Empty";
+
+  class EmptyRequest : public ros::Msg
+  {
+    public:
+
+    EmptyRequest()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    virtual const char * getType() override { return EMPTY; };
+    virtual const char * getMD5() override { return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class EmptyResponse : public ros::Msg
+  {
+    public:
+
+    EmptyResponse()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    virtual const char * getType() override { return EMPTY; };
+    virtual const char * getMD5() override { return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class Empty {
+    public:
+    typedef EmptyRequest Request;
+    typedef EmptyResponse Response;
+  };
+
+}
+#endif
diff --git a/ros_lib/ros_lib/std_srvs/SetBool.h b/ros_lib/ros_lib/std_srvs/SetBool.h
@@ -0,0 +1,123 @@
+#ifndef _ROS_SERVICE_SetBool_h
+#define _ROS_SERVICE_SetBool_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_srvs
+{
+
+static const char SETBOOL[] = "std_srvs/SetBool";
+
+  class SetBoolRequest : public ros::Msg
+  {
+    public:
+      typedef bool _data_type;
+      _data_type data;
+
+    SetBoolRequest():
+      data(0)
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    virtual const char * getType() override { return SETBOOL; };
+    virtual const char * getMD5() override { return "8b94c1b53db61fb6aed406028ad6332a"; };
+
+  };
+
+  class SetBoolResponse : public ros::Msg
+  {
+    public:
+      typedef bool _success_type;
+      _success_type success;
+      typedef const char* _message_type;
+      _message_type message;
+
+    SetBoolResponse():
+      success(0),
+      message("")
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.real = this->success;
+      *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->success);
+      uint32_t length_message = strlen(this->message);
+      varToArr(outbuffer + offset, length_message);
+      offset += 4;
+      memcpy(outbuffer + offset, this->message, length_message);
+      offset += length_message;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.base = 0;
+      u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->success = u_success.real;
+      offset += sizeof(this->success);
+      uint32_t length_message;
+      arrToVar(length_message, (inbuffer + offset));
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_message; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_message-1]=0;
+      this->message = (char *)(inbuffer + offset-1);
+      offset += length_message;
+     return offset;
+    }
+
+    virtual const char * getType() override { return SETBOOL; };
+    virtual const char * getMD5() override { return "937c9679a518e3a18d831e57125ea522"; };
+
+  };
+
+  class SetBool {
+    public:
+    typedef SetBoolRequest Request;
+    typedef SetBoolResponse Response;
+  };
+
+}
+#endif
diff --git a/ros_lib/ros_lib/std_srvs/Trigger.h b/ros_lib/ros_lib/std_srvs/Trigger.h
@@ -0,0 +1,105 @@
+#ifndef _ROS_SERVICE_Trigger_h
+#define _ROS_SERVICE_Trigger_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_srvs
+{
+
+static const char TRIGGER[] = "std_srvs/Trigger";
+
+  class TriggerRequest : public ros::Msg
+  {
+    public:
+
+    TriggerRequest()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    virtual const char * getType() override { return TRIGGER; };
+    virtual const char * getMD5() override { return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class TriggerResponse : public ros::Msg
+  {
+    public:
+      typedef bool _success_type;
+      _success_type success;
+      typedef const char* _message_type;
+      _message_type message;
+
+    TriggerResponse():
+      success(0),
+      message("")
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.real = this->success;
+      *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->success);
+      uint32_t length_message = strlen(this->message);
+      varToArr(outbuffer + offset, length_message);
+      offset += 4;
+      memcpy(outbuffer + offset, this->message, length_message);
+      offset += length_message;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer) override
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.base = 0;
+      u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->success = u_success.real;
+      offset += sizeof(this->success);
+      uint32_t length_message;
+      arrToVar(length_message, (inbuffer + offset));
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_message; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_message-1]=0;
+      this->message = (char *)(inbuffer + offset-1);
+      offset += length_message;
+     return offset;
+    }
+
+    virtual const char * getType() override { return TRIGGER; };
+    virtual const char * getMD5() override { return "937c9679a518e3a18d831e57125ea522"; };
+
+  };
+
+  class Trigger {
+    public:
+    typedef TriggerRequest Request;
+    typedef TriggerResponse Response;
+  };
+
+}
+#endif
diff --git a/scripts/update_ros_lib.sh b/scripts/update_ros_lib.sh
@@ -20,6 +20,7 @@ rosrun rosserial_arduino make_libraries.py .
 cp -r ros_lib/ros $TARGET_DIR/ros_lib/
 cp -r ros_lib/smart_device_protocol $TARGET_DIR/ros_lib/
 cp -r ros_lib/std_msgs $TARGET_DIR/ros_lib/
+cp -r ros_lib/std_srvs $TARGET_DIR/ros_lib/
 cp -r ros_lib/rosserial_msgs $TARGET_DIR/ros_lib/
 cp -r ros_lib/*.h $TARGET_DIR/ros_lib/
 cp -r ros_lib/*.cpp $TARGET_DIR/ros_lib/

diff --git a/sketchbooks/smart_device_protocol_ros_interface/src/main.cpp b/sketchbooks/smart_device_protocol_ros_interface/src/main.cpp
@@ -12,6 +12,7 @@
 // ROS
 #include <smart_device_protocol/Packet.h>
 #include <smart_device_protocol/UWBDistance.h>
+#include <std_srvs/SetBool.h>
 
 #include "ros/node_handle.h"
 #if defined(M5STACKATOMS3)
@@ -33,6 +34,7 @@
 #include "devices/uwb_module_util.h"
 #include "sdp/esp_now.h"
 
+void uwb_toggle(const std_srvs::SetBoolRequest &, std_srvs::SetBoolResponse &);
 void messageCb(const smart_device_protocol::Packet &);
 
 // ESP-NOW
@@ -50,6 +52,36 @@ ros::NodeHandle_<ArduinoHardware, 25, 25, 2048, 2048> nh;
 ros::Publisher publisher("/smart_device_protocol/recv", &msg_recv_packet);
 ros::Publisher publisher_uwb("/smart_device_protocol/uwb", &msg_uwb);
 ros::Subscriber<smart_device_protocol::Packet> subscriber("/smart_device_protocol/send", &messageCb);
+ros::ServiceServer<std_srvs::SetBoolRequest, std_srvs::SetBoolResponse> uwb_toggle_service(
+    "/smart_device_protocol/uwb_toggle", &uwb_toggle);
+
+void uwb_toggle(const std_srvs::SetBoolRequest &req, std_srvs::SetBoolResponse &res) {
+  if (req.data) {
+    if (uwb_initialized) {
+      res.success = true;
+      res.message = "UWB is already enabled.";
+    } else {
+      uwb_initialized = initUWB(true, 1, Serial2);
+      if (uwb_initialized) {
+        res.success = true;
+        res.message = "UWB is enabled.";
+      } else {
+        res.success = false;
+        res.message = "UWB is not enabled.";
+      }
+    }
+  } else {
+    if (uwb_initialized) {
+      resetUWB(Serial2);
+      uwb_initialized = false;
+      res.success = true;
+      res.message = "UWB is disabled.";
+    } else {
+      res.success = true;
+      res.message = "UWB is already disabled.";
+    }
+  }
+}
 
 void messageCb(const smart_device_protocol::Packet &msg) {
   if (msg.mac_address_length != 6) {
@@ -124,9 +156,8 @@ void setup() {
 
   // Subscribe and Publish
   nh.advertise(publisher);
-  if (uwb_initialized) {
-    nh.advertise(publisher_uwb);
-  }
+  nh.advertise(publisher_uwb);
+  nh.advertiseService(uwb_toggle_service);
   nh.subscribe(subscriber);
   while (not nh.connected()) {
     delay(1000);