Merge remote-tracking branch 'upstream/master'

software/ground_station_setup.sh

@@ -12,6 +12,7 @@ folders_to_link=(
     nimbro_topic_transport
     rover_main
     rover_camera
+    rover_status
 )
 
 # Print heading

software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py

@@ -45,6 +45,19 @@ class DriveCoordinator(object):
 
         self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
 
+        # Drive data
+        self.drive_command_data = {
+            "iris": {
+                "message": DriveCommandMessage(),
+                "last_time": time()
+            },
+
+            "ground_station": {
+                "message": DriveCommandMessage(),
+                "last_time": time()
+            }
+        }
+
         # ########## Class Variables ##########
         self.iris_drive_command_subscriber = rospy.Subscriber(self.iris_drive_command_topic,
                                                               DriveCommandMessage,
@@ -58,17 +71,6 @@ class DriveCoordinator(object):
         self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
         self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
 
-        self.drive_command_data = {
-            "iris": {
-                "message": DriveCommandMessage(),
-                "last_time": time()
-            },
-
-            "ground_station": {
-                "message": DriveCommandMessage(),
-                "last_time": time()
-            }
-        }
 
         self.last_message_time = time()
 

software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch

@@ -47,5 +47,9 @@
         <node name="end_effector_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
             <param name="port" value="17012" />
         </node>
+
+        <node name="rover_status" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+            <param name="port" value="17013" />
+        </node>
     </group>
 </launch>

software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch

@@ -14,12 +14,12 @@
             <param name="destination_addr" value="$(arg target)" />
             <param name="destination_port" value="17021" />
             <rosparam param="topics">
-                [
+                [{name: "/cameras/chassis/camera_control", compress: true, rate: 5.0},
-                {name: "/cameras/chassis/camera_control", compress: true, rate: 5.0},
                 {name: "/cameras/undercarriage/camera_control", compress: true, rate: 5.0},
                 {name: "/cameras/main_navigation/camera_control", compress: true, rate: 5.0},
-                {name: "/cameras/end_effector/camera_control", compress: true, rate: 5.0}
+                {name: "/cameras/end_effector/camera_control", compress: true, rate: 5.0},
-                ]
+                {name: "/cameras/chassis/camera_control", compress: true, rate: 1.0},
+                {name: "/rover_status/update_requested", compress: true, rate: 1.0}]
             </rosparam>
         </node>
     </group>

software/ros_packages/rover_main/launch/rover.launch

@@ -6,6 +6,7 @@
     <include file="$(find rover_main)/launch/rover/cameras.launch"/>
 
     <!-- ########## Start System Status Monitoring Nodes ########## -->
+    <include file="$(find rover_main)/launch/rover/status.launch"/>
 
     <!-- ########## Start Nimbro Topic Transport Nodes ########## -->
     <include file="$(find rover_main)/launch/rover/topic_transport_senders.launch"/>

software/ros_packages/rover_main/launch/rover/status.launch

@@ -0,0 +1,5 @@
+<launch>
+    <group ns="rover_status">
+        <node name="rover_status" pkg="rover_status" type="system_statuses_node.py" output="screen"/>
+    </group>
+</launch>

software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch

@@ -97,5 +97,18 @@
                 [{name: "/cameras/end_effector/image_256x144/compressed", compress: false, rate: 0.0}]
             </rosparam>
         </node>
+
+        <node name="rover_tcp_topics" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
+            <param name="destination_addr" value="$(arg target)" />
+            <param name="destination_port" value="17013" />
+            <rosparam param="topics">
+                [{name: "/rover_status/bogie_status", compress: true, rate: 1.0},
+                {name: "/rover_status/camera_status", compress: true, rate: 1.0},
+                {name: "/rover_status/frsky_status", compress: true, rate: 1.0},
+                {name: "/rover_status/gps_status", compress: true, rate: 1.0},
+                {name: "/rover_status/jetson_status", compress: true, rate: 5.0},
+                {name: "/rover_status/misc_status", compress: true, rate: 1.0}]
+            </rosparam>
+        </node>
     </group>
 </launch>

software/ros_packages/rover_status/msg/BogieStatuses.msg

@@ -1,3 +1,3 @@
-int8 bogie_connection_1
+bool bogie_connection_1
-int8 bogie_connection_2
+bool bogie_connection_2
-int8 bogie_connection_3
+bool bogie_connection_3

software/ros_packages/rover_status/msg/CameraStatuses.msg

@@ -1,4 +1,4 @@
-int8 camera_zed
+bool camera_zed
-int8 camera_undercarriage
+bool camera_undercarriage
-int8 camera_chassis
+bool camera_chassis
-int8 camera_main_navigation
+bool camera_main_navigation

software/ros_packages/rover_status/msg/FrSkyStatus.msg

@@ -1 +1 @@
-int8 FrSky_controller_connection_status
+bool FrSky_controller_connection_status

software/ros_packages/rover_status/msg/MiscStatuses.msg

@@ -1,5 +1,5 @@
-int8 arm_connection_status
+bool arm_connection_status
-int8 arm_end_effector_connection_statuses
+bool arm_end_effector_connection_statuses
-int8 sample_containment_connection_status
+bool sample_containment_connection_status
-int8 tower_connection_status
+bool tower_connection_status
-int8 chassis_pan_tilt_connection_status
+bool chassis_pan_tilt_connection_status

software/ros_packages/rover_status/src/rover_statuses.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python
 import rospy
-from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
+from rover_status.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
-
 
 # THIS IS A SUPER ROUGH EXAMPLE OF HOW TO PULL THE DATA
 # You can create your own message formats in the msg folder
@@ -9,8 +8,8 @@ from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSI
 # and storing them into self values.
 # The ground control code sounds like it'll be fairly different in format.
 
-class RoverStatuses:
 
+class RoverStatuses:
     def __init__(self):
 
         rospy.init_node('RoverStatuses')
@@ -74,7 +73,6 @@ class RoverStatuses:
         rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback)
         rospy.spin()
 
-
 if __name__ == '__main__':
     rover_statuses = RoverStatuses()
     rover_statuses.run()

software/ros_packages/rover_status/src/system_statuses_node.py

@@ -1,13 +1,34 @@
 #!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
 import rospy
 import os.path
 import psutil
 import subprocess
-from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
+from rover_status.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
 from rover_control.msg import DriveCommandMessage
+from std_msgs.msg import Empty
 
+
+#####################################
+# Global Variables
+#####################################
+DEFAULT_CAMERA_TOPIC_NAME = "camera_status"
+DEFAULT_BOGIE_TOPIC_NAME = "bogie_status"
+DEFAULT_FRSKY_TOPIC_NAME = "frsky_status"
+DEFAULT_GPS_TOPIC_NAME = "gps_status"
+DEFAULT_JETSON_TOPIC_NAME = "jetson_status"
+DEFAULT_MISC_TOPIC_NAME = "misc_status"
+
+DEFAULT_REQUEST_UPDATE_TOPIC_NAME = "update_requested"
+
+
+#####################################
+# SystemStatuses Class Definition
+#####################################
 class SystemStatuses:
-
     def __init__(self):
         # Camera path locations
         self.system_path_locations = [
@@ -26,13 +47,31 @@ class SystemStatuses:
 
         rospy.init_node('SystemStatuses')
 
+        # Get Topic Names
+        self.camera_topic_name = rospy.get_param("~camera_status_topic", DEFAULT_CAMERA_TOPIC_NAME)
+        self.bogie_topic_name = rospy.get_param("~bogie_status_topic", DEFAULT_BOGIE_TOPIC_NAME)
+        self.frsky_topic_name = rospy.get_param("~frsky_status_topic", DEFAULT_FRSKY_TOPIC_NAME)
+        self.gps_topic_name = rospy.get_param("~gps_status_topic", DEFAULT_GPS_TOPIC_NAME)
+        self.jetson_topic_name = rospy.get_param("~jetson_status_topic", DEFAULT_JETSON_TOPIC_NAME)
+        self.misc_topic_name = rospy.get_param("~misc_status_topic", DEFAULT_MISC_TOPIC_NAME)
+
+        self.request_update_topic_name = rospy.get_param("~request_update_status_topic",
+                                                         DEFAULT_REQUEST_UPDATE_TOPIC_NAME)
+
         # init all publisher functions
-        self.pub_camera = rospy.Publisher('camera_system_status_chatter', CameraStatuses, queue_size=10)
+        self.pub_camera = rospy.Publisher(self.camera_topic_name, CameraStatuses, queue_size=1)
-        self.pub_bogie = rospy.Publisher('bogie_system_status_chatter', BogieStatuses, queue_size=10)
+        self.pub_bogie = rospy.Publisher(self.bogie_topic_name, BogieStatuses, queue_size=1)
-        self.pub_FrSky = rospy.Publisher('FrSky_system_status_chatter', FrSkyStatus, queue_size=10)
+        self.pub_FrSky = rospy.Publisher(self.frsky_topic_name, FrSkyStatus, queue_size=1)
-        self.pub_GPS = rospy.Publisher('GPS_system_status_chatter', GPSInfo, queue_size=10)
+        self.pub_GPS = rospy.Publisher(self.gps_topic_name, GPSInfo, queue_size=1)
-        self.pub_jetson = rospy.Publisher('jetson_system_status_chatter', JetsonInfo, queue_size=10)
+        self.pub_jetson = rospy.Publisher(self.jetson_topic_name, JetsonInfo, queue_size=1)
-        self.pub_Misc = rospy.Publisher('misc_system_status_chatter', MiscStatuses, queue_size=10)
+        self.pub_Misc = rospy.Publisher(self.misc_topic_name, MiscStatuses, queue_size=1)
+
+        # Subscribers
+        self.request_update_subscriber = rospy.Subscriber(self.request_update_topic_name, Empty,
+                                                          self.on_update_requested)
+
+        # Manual update variable
+        self.manual_update_requested = False
 
         # init all message variables
         self.camera_msg = CameraStatuses()
@@ -116,8 +155,8 @@ class SystemStatuses:
         try:
             sensor_temperatures = subprocess.check_output("sensors | grep temp", shell=True)
             parsed_temps = sensor_temperatures.replace("\xc2\xb0C","").replace("(crit = ","").replace("temp1:","")\
-                .replace("\n","").replace("+","").split()
+                .replace("\n", "").replace("+", "").split()
-            self.jetson_msg.jetson_GPU_temp = parsed_temps[4]
+            self.jetson_msg.jetson_GPU_temp = float(parsed_temps[4])
         except subprocess.CalledProcessError:
             print 'sensors call failed (potential reason: on VM)'
             self.jetson_msg.jetson_GPU_temp = -1.0
@@ -182,6 +221,9 @@ class SystemStatuses:
         self.previous_sample_containment_connection_status = self.misc_msg.sample_containment_connection_status
         self.previous_tower_connection_status = self.misc_msg.tower_connection_status
 
+    def on_update_requested(self,  _):
+        self.manual_update_requested = True
+
     def run(self):
         r = rospy.Rate(10)  # 10Hz
         while not rospy.is_shutdown():
@@ -193,7 +235,8 @@ class SystemStatuses:
             if (self.camera_msg.camera_zed != self.previous_camera_zed or
                     self.camera_msg.camera_undercarriage != self.previous_camera_undercarriage or
                     self.camera_msg.camera_chassis != self.previous_camera_chassis or
-                    self.camera_msg.camera_main_navigation != self.previous_camera_main_navigation):
+                    self.camera_msg.camera_main_navigation != self.previous_camera_main_navigation or
+                    self.manual_update_requested):
                 self.__set_previous_camera_values()
                 self.pub_camera.publish(self.camera_msg)
 
@@ -202,25 +245,29 @@ class SystemStatuses:
                     self.jetson_msg.jetson_RAM != self.previous_jetson_RAM or
                     self.jetson_msg.jetson_EMMC != self.previous_jetson_EMMC or
                     self.jetson_msg.jetson_NVME_SSD != self.previous_jetson_NVME_SSD or
-                    self.jetson_msg.jetson_GPU_temp != self.previous_jetson_GPU_temp):
+                    self.jetson_msg.jetson_GPU_temp != self.previous_jetson_GPU_temp or
+                    self.manual_update_requested):
                 self.__set_previous_jetson_values()
                 self.pub_jetson.publish(self.jetson_msg)
 
             # Placeholder FrSky Controller Check
-            if self.FrSky_msg.FrSky_controller_connection_status != self.previous_FrSky_controller_connection_status:
+            if (self.FrSky_msg.FrSky_controller_connection_status != self.previous_FrSky_controller_connection_status or
+                    self.manual_update_requested):
                 self.__set_previous_frsky_value()
                 self.pub_FrSky.publish(self.FrSky_msg)
 
             # Placeholder bogie status check
             if (self.bogie_msg.bogie_connection_1 != self.previous_bogie_connection_1 or
                     self.bogie_msg.bogie_connection_2 != self.previous_bogie_connection_2 or
-                    self.bogie_msg.bogie_connection_3 != self.previous_bogie_connection_3):
+                    self.bogie_msg.bogie_connection_3 != self.previous_bogie_connection_3 or
+                    self.manual_update_requested):
                 self.__set_previous_bogie_values()
                 self.pub_bogie.publish(self.bogie_msg)
 
             # Placeholder GPS Information check
             if (self.GPS_msg.UTC_GPS_time != self.previous_UTC_GPS_time or
-                    self.GPS_msg.UTC_GPS_time != self.previous_GPS_connection_status):
+                    self.GPS_msg.UTC_GPS_time != self.previous_GPS_connection_status or
+                    self.manual_update_requested):
                 self.__set_previous_gps_values()
                 self.pub_GPS.publish(self.GPS_msg)
 
@@ -233,12 +280,14 @@ class SystemStatuses:
                     self.previous_chassis_pan_tilt_connection_status or
                     self.misc_msg.sample_containment_connection_status !=
                     self.previous_sample_containment_connection_status or
-                    self.misc_msg.tower_connection_status !=
+                    self.misc_msg.tower_connection_status != self.previous_tower_connection_status or
-                    self.previous_tower_connection_status):
+                    self.manual_update_requested):
                 self.__set_previous_misc_values()
                 self.pub_Misc.publish(self.misc_msg)
 
-            # rospy.loginfo(self.msg)
+            if self.manual_update_requested:
+                self.manual_update_requested = False
+
             r.sleep()