diff --git a/software/ground_station_setup.sh b/software/ground_station_setup.sh index 11f5db9..57f5f1d 100755 --- a/software/ground_station_setup.sh +++ b/software/ground_station_setup.sh @@ -12,6 +12,7 @@ folders_to_link=( nimbro_topic_transport rover_main rover_camera + rover_status ) # Print heading diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index 51b482f..89ea46b 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/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() diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch index 33af17e..2c62436 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch @@ -47,5 +47,9 @@ + + + + diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index 7df2eac..3daa5ec 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -14,12 +14,12 @@ - [ - {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}] diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index e3af5e8..4352782 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -6,6 +6,7 @@ + diff --git a/software/ros_packages/rover_main/launch/rover/status.launch b/software/ros_packages/rover_main/launch/rover/status.launch new file mode 100644 index 0000000..9fece35 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/status.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 585af33..7891458 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/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}] + + + + + + [{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}] + + diff --git a/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp b/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp deleted file mode 100644 index 54ffbfe..0000000 Binary files a/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp and /dev/null differ diff --git a/software/ros_packages/rover_status/msg/BogieStatuses.msg b/software/ros_packages/rover_status/msg/BogieStatuses.msg index d9694fe..749cfad 100644 --- a/software/ros_packages/rover_status/msg/BogieStatuses.msg +++ b/software/ros_packages/rover_status/msg/BogieStatuses.msg @@ -1,3 +1,3 @@ -int8 bogie_connection_1 -int8 bogie_connection_2 -int8 bogie_connection_3 \ No newline at end of file +bool bogie_connection_1 +bool bogie_connection_2 +bool bogie_connection_3 \ No newline at end of file diff --git a/software/ros_packages/rover_status/msg/CameraStatuses.msg b/software/ros_packages/rover_status/msg/CameraStatuses.msg index 2e50e77..2b9496e 100644 --- a/software/ros_packages/rover_status/msg/CameraStatuses.msg +++ b/software/ros_packages/rover_status/msg/CameraStatuses.msg @@ -1,4 +1,4 @@ -int8 camera_zed -int8 camera_undercarriage -int8 camera_chassis -int8 camera_main_navigation \ No newline at end of file +bool camera_zed +bool camera_undercarriage +bool camera_chassis +bool camera_main_navigation \ No newline at end of file diff --git a/software/ros_packages/rover_status/msg/FrSkyStatus.msg b/software/ros_packages/rover_status/msg/FrSkyStatus.msg index 72a7868..3d1acde 100644 --- a/software/ros_packages/rover_status/msg/FrSkyStatus.msg +++ b/software/ros_packages/rover_status/msg/FrSkyStatus.msg @@ -1 +1 @@ -int8 FrSky_controller_connection_status \ No newline at end of file +bool FrSky_controller_connection_status \ No newline at end of file diff --git a/software/ros_packages/rover_status/msg/MiscStatuses.msg b/software/ros_packages/rover_status/msg/MiscStatuses.msg index f6e87b0..c7f93be 100644 --- a/software/ros_packages/rover_status/msg/MiscStatuses.msg +++ b/software/ros_packages/rover_status/msg/MiscStatuses.msg @@ -1,5 +1,5 @@ -int8 arm_connection_status -int8 arm_end_effector_connection_statuses -int8 sample_containment_connection_status -int8 tower_connection_status -int8 chassis_pan_tilt_connection_status \ No newline at end of file +bool arm_connection_status +bool arm_end_effector_connection_statuses +bool sample_containment_connection_status +bool tower_connection_status +bool chassis_pan_tilt_connection_status \ No newline at end of file diff --git a/software/ros_packages/rover_status/src/rover_statuses.py b/software/ros_packages/rover_status/src/rover_statuses.py index 5a8e12b..b6bc6d2 100755 --- a/software/ros_packages/rover_status/src/rover_statuses.py +++ b/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() diff --git a/software/ros_packages/rover_status/src/system_statuses_node.py b/software/ros_packages/rover_status/src/system_statuses_node.py index d10a9f8..da09ba7 100755 --- a/software/ros_packages/rover_status/src/system_statuses_node.py +++ b/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_bogie = rospy.Publisher('bogie_system_status_chatter', BogieStatuses, queue_size=10) - self.pub_FrSky = rospy.Publisher('FrSky_system_status_chatter', FrSkyStatus, queue_size=10) - self.pub_GPS = rospy.Publisher('GPS_system_status_chatter', GPSInfo, queue_size=10) - self.pub_jetson = rospy.Publisher('jetson_system_status_chatter', JetsonInfo, queue_size=10) - self.pub_Misc = rospy.Publisher('misc_system_status_chatter', MiscStatuses, queue_size=10) + self.pub_camera = rospy.Publisher(self.camera_topic_name, CameraStatuses, queue_size=1) + self.pub_bogie = rospy.Publisher(self.bogie_topic_name, BogieStatuses, queue_size=1) + self.pub_FrSky = rospy.Publisher(self.frsky_topic_name, FrSkyStatus, queue_size=1) + self.pub_GPS = rospy.Publisher(self.gps_topic_name, GPSInfo, queue_size=1) + self.pub_jetson = rospy.Publisher(self.jetson_topic_name, JetsonInfo, queue_size=1) + 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() - self.jetson_msg.jetson_GPU_temp = parsed_temps[4] + .replace("\n", "").replace("+", "").split() + 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.previous_tower_connection_status): + self.misc_msg.tower_connection_status != self.previous_tower_connection_status or + 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()