add envSensors support

BluewhaleRobot · Jul 13, 2019 · 36bacc7 · 36bacc7
1 parent 9b01e7b
commit 36bacc7
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 5 deletions.
diff --git a/remote_server.py b/remote_server.py
@@ -96,11 +96,13 @@ def init_sub_pubs():
 if __name__ == "__main__":
     pubs = init_sub_pubs()
     rate = rospy.Rate(10)
+
+    useEnvSensors = rospy.get_param("~useEnvSensors", 0) #默认为0 表示不使用环境监测数据
     # 配置udp广播
     s = socket(AF_INET, SOCK_DGRAM)
     s.bind(('', 0))
     s.setsockopt(SOL_SOCKET, SO_BROADCAST, 1)
-    
+
     # 配置全局系统状态
     galileo_status = GalileoStatus()
     galileo_status_lock = threading.Lock()
@@ -116,7 +118,7 @@ def init_sub_pubs():
 
     # UDP系统状态发布线程
     udp_status_service = UDPStatusService(monitor_server, galileo_status_service.galileo_status,
-                                          galileo_status_service.galileo_status_lock)
+                                          galileo_status_service.galileo_status_lock,useEnvSensors)
     udp_status_service.start()
 
     broadcast_count = 10  # 每1秒播放一次声音

diff --git a/utils/udp_status_service.py b/utils/udp_status_service.py
@@ -34,32 +34,59 @@
 from galileo_serial_server.msg import GalileoStatus
 from geometry_msgs.msg import Pose, PoseStamped, Twist
 from nav_msgs.msg import Odometry
+from bw_env_sensors.msg import EnvSensors
 
 from config import BROADCAST_PORT, ROS_PACKAGE_PATH, TF_ROT, TF_TRANS
 
 
 class UDPStatusService(threading.Thread):
 
-    def __init__(self, monitor_server, galileo_status, galileo_status_lock):
+    def __init__(self, monitor_server, galileo_status, galileo_status_lock, useEnvSensors):
         super(UDPStatusService, self).__init__()
         # stop flag
         self._stop = threading.Event()
         self._stop.set()
         self.listener = tf.TransformListener(True, rospy.Duration(10.0))
         self.send_data = bytearray(
-            [205, 235, 215, 36, 0x00, 0x00, 0x00, 0x00,
+            [205, 235, 215, 40, 0x00, 0x00, 0x00, 0x00,
              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
              0x00, 0x00, 0x00, 0x00,
              ])
+        self.send_data2 = bytearray(
+            [205, 235, 215, 32, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00,
+             ])
         self.monitor_server = monitor_server
         self.robot_pose_stamped = None
         self.galileo_status = galileo_status
         self.galileo_status_lock = galileo_status_lock
-        self.odom_flag = False
 
+        self.env_sensor_data = EnvSensors()
+
+        def update_envSensor(sensorData):
+            self.env_sensor_data.header = sensorData.header
+            self.env_sensor_data.temperature = sensorData.temperature #Centigrade
+            self.env_sensor_data.rh = sensorData.rh #relative humidity %RH
+            self.env_sensor_data.smoke = sensorData.smoke #ppm
+            self.env_sensor_data.pm1_0 = sensorData.pm1_0 #ug/m^3
+            self.env_sensor_data.pm2_5 = sensorData.pm2_5 #ug/m^3
+            self.env_sensor_data.pm10 = sensorData.pm10 #ug/m^3
+            self.env_sensor_data.illuminance = sensorData.illuminance #lx
+            self.env_sensor_data.noise = sensorData.noise #db
+
+        if useEnvSensors == 0:
+            self.envSensor_flag = False
+        else:
+            self.envSensor_sub = rospy.Subscriber("/bw_env_sensors/EnvSensorData", EnvSensors, update_envSensor)
+            self.envSensor_flag = True
+
+        self.odom_flag = False
         def update_odom(odom):
             self.odom_flag = True
             self.robot_pose_stamped = PoseStamped()
@@ -78,6 +105,7 @@ def stopped(self):
     def run(self):
         rate = rospy.Rate(30)
         self._stop.clear()
+        num_i = 0
         while not self.stopped() and not rospy.is_shutdown():
             with self.galileo_status_lock:
                 self.send_data[4:8] = map(ord, struct.pack('f', self.galileo_status.currentPosX))
@@ -124,4 +152,20 @@ def run(self):
                 statu2 + statu3 + status4 + status5
             self.send_data[3] = len(self.send_data) - 4
             self.monitor_server.sendto(bytes(self.send_data))
+
+            if self.envSensor_flag and  num_i >= 30:
+                num_i = 0
+                self.send_data2[4:8] = map(ord, struct.pack('f', self.env_sensor_data.temperature))
+                self.send_data2[8:12] = map(ord, struct.pack('f', self.env_sensor_data.rh))
+                self.send_data2[12:16] = map(ord, struct.pack('f', self.env_sensor_data.smoke))
+                self.send_data2[16:20] = map(ord, struct.pack('f', self.env_sensor_data.pm1_0))
+                self.send_data2[20:24] = map(ord, struct.pack('f', self.env_sensor_data.pm2_5))
+                self.send_data2[24:28] = map(ord, struct.pack('f', self.env_sensor_data.pm10))
+                self.send_data2[28:32] = map(ord, struct.pack('f', self.env_sensor_data.illuminance))
+                self.send_data2[32:36] = map(ord, struct.pack('f', self.env_sensor_data.noise))
+                self.send_data2[3] = len(self.send_data2) - 4
+                self.monitor_server.sendto(bytes(self.send_data2))
+                # l = [hex(int(j)) for j in self.send_data2]
+                # print(" ".join(l))
+            num_i = num_i +1
             rate.sleep()