diff --git a/software/ros_packages/rover_status/CMakeLists.txt b/software/ros_packages/rover_status/CMakeLists.txt index 38b10c5..0000223 100644 --- a/software/ros_packages/rover_status/CMakeLists.txt +++ b/software/ros_packages/rover_status/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(rover_status) +project(rover_statuses) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -8,7 +8,10 @@ project(rover_status) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + roscpp rospy + std_msgs + message_generation ) ## System dependencies are found with CMake's conventions @@ -51,6 +54,17 @@ find_package(catkin REQUIRED COMPONENTS # Message2.msg # ) +add_message_files( + FILES + RoverSysStatus.msg + BogieStatuses.msg + FrSkyStatus.msg + JetsonInfo.msg + CameraStatuses.msg + GPSInfo.msg + MiscStatuses.msg +) + ## Generate services in the 'srv' folder # add_service_files( # FILES @@ -66,10 +80,10 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -102,8 +116,8 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES rover_status -# CATKIN_DEPENDS rospy +# LIBRARIES system_statuses +# CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -120,7 +134,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/rover_status.cpp +# src/${PROJECT_NAME}/system_statuses.cpp # ) ## Add cmake target dependencies of the library @@ -131,7 +145,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/rover_status_node.cpp) +# add_executable(${PROJECT_NAME}_node src/system_statuses_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -188,7 +202,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_status.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_system_statuses.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp b/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp new file mode 100644 index 0000000..54ffbfe Binary files /dev/null and b/software/ros_packages/rover_status/msg/.RoverSysStatus.msg.kate-swp differ diff --git a/software/ros_packages/rover_status/msg/BogieStatuses.msg b/software/ros_packages/rover_status/msg/BogieStatuses.msg new file mode 100644 index 0000000..d9694fe --- /dev/null +++ b/software/ros_packages/rover_status/msg/BogieStatuses.msg @@ -0,0 +1,3 @@ +int8 bogie_connection_1 +int8 bogie_connection_2 +int8 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 new file mode 100644 index 0000000..2e50e77 --- /dev/null +++ b/software/ros_packages/rover_status/msg/CameraStatuses.msg @@ -0,0 +1,4 @@ +int8 camera_zed +int8 camera_undercarriage +int8 camera_chassis +int8 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 new file mode 100644 index 0000000..72a7868 --- /dev/null +++ b/software/ros_packages/rover_status/msg/FrSkyStatus.msg @@ -0,0 +1 @@ +int8 FrSky_controller_connection_status \ No newline at end of file diff --git a/software/ros_packages/rover_status/msg/GPSInfo.msg b/software/ros_packages/rover_status/msg/GPSInfo.msg new file mode 100644 index 0000000..2844c32 --- /dev/null +++ b/software/ros_packages/rover_status/msg/GPSInfo.msg @@ -0,0 +1,2 @@ +int8 UTC_GPS_time +int8 GPS_connection_status \ No newline at end of file diff --git a/software/ros_packages/rover_status/msg/JetsonInfo.msg b/software/ros_packages/rover_status/msg/JetsonInfo.msg new file mode 100644 index 0000000..92506cb --- /dev/null +++ b/software/ros_packages/rover_status/msg/JetsonInfo.msg @@ -0,0 +1,5 @@ +float64 jetson_CPU +float64 jetson_RAM +float64 jetson_EMMC +float64 jetson_NVME_SSD +float64 jetson_GPU_temp \ 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 new file mode 100644 index 0000000..f6e87b0 --- /dev/null +++ b/software/ros_packages/rover_status/msg/MiscStatuses.msg @@ -0,0 +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 diff --git a/software/ros_packages/rover_status/msg/RoverSysStatus.msg b/software/ros_packages/rover_status/msg/RoverSysStatus.msg new file mode 100644 index 0000000..d181327 --- /dev/null +++ b/software/ros_packages/rover_status/msg/RoverSysStatus.msg @@ -0,0 +1,19 @@ +int8 UTC_GPS_time +int8 bogie_connection_1 +int8 bogie_connection_2 +int8 bogie_connection_3 +int8 arm_connection_status +int8 arm_end_effector_connection_statuses +int8 camera_zed +int8 camera_undercarriage +int8 camera_chassis +int8 camera_main_navigation +int8 sample_containment_connection_status +int8 tower_connection_status +int8 chassis_pan_tilt_connection_status +int8 GPS_connection_status +int8 jetson_CPU +int8 jetson_RAM +int8 jetson_EMMC +int8 jetson_NVME_SSD +int8 FrSky_controller_connection_status \ No newline at end of file diff --git a/software/ros_packages/rover_status/package.xml b/software/ros_packages/rover_status/package.xml index c40314d..0750d55 100644 --- a/software/ros_packages/rover_status/package.xml +++ b/software/ros_packages/rover_status/package.xml @@ -1,25 +1,25 @@ - rover_status + system_statuses 0.0.0 - The rover_status package + The system_statuses package - caperren + matcurtay - TODO + BSD - + @@ -37,21 +37,27 @@ - + message_generation - + message_runtime catkin + roscpp rospy + std_msgs + roscpp rospy + std_msgs + roscpp rospy + std_msgs diff --git a/software/ros_packages/rover_status/src/camera_2_updater.py b/software/ros_packages/rover_status/src/camera_2_updater.py new file mode 100755 index 0000000..6f77347 --- /dev/null +++ b/software/ros_packages/rover_status/src/camera_2_updater.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python + +# import rospy +# from system_statuses.msg import Camera2Changer + + +# def camera_2_changer(): +# pub = rospy.Publisher('camera_2_changer_chatter', Camera2Changer, queue_size=10) +# rospy.init_node('camera_2_changer_talker', anonymous=True) +# # r = rospy.sleep(10) # 10hz +# msg = Camera2Changer() +# msg.camera_2_value = 0 + +# while not rospy.is_shutdown(): +# msg.camera_2_value = (msg.camera_2_value + 1) % 2 +# rospy.loginfo(msg) +# pub.publish(msg) + # r.sleep() +## rospy.sleep(2.) + + +#if __name__ == '__main__': +# try: +# camera_2_changer() +# except rospy.ROSInterruptException: +# pass diff --git a/software/ros_packages/rover_status/src/rover_statuses.py b/software/ros_packages/rover_status/src/rover_statuses.py new file mode 100755 index 0000000..5a8e12b --- /dev/null +++ b/software/ros_packages/rover_status/src/rover_statuses.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python +import rospy +from system_statuses.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 +# This is a simple example of pulling data from system_statuses_node.py +# and storing them into self values. +# The ground control code sounds like it'll be fairly different in format. + +class RoverStatuses: + + def __init__(self): + + rospy.init_node('RoverStatuses') + + # self.pub = rospy.Publisher('rover_statuses_chatter', RoverSysStatus, queue_size=10) + + # Subscription examples on pulling data from system_statuses_node.py + rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback) + rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback) + rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback) + rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback) + rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback) + rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback) + + self.camera_msg = CameraStatuses() + self.bogie_msg = BogieStatuses() + self.FrSky_msg = FrSkyStatus() + self.GPS_msg = GPSInfo() + self.jetson_msg = JetsonInfo() + self.misc_msg = MiscStatuses() + + def __camera_callback(self, data): + self.camera_msg.camera_zed = data.camera_zed + self.camera_msg.camera_undercarriage = data.camera_undercarriage + self.camera_msg.camera_chassis = data.camera_chassis + self.camera_msg.camera_main_navigation = data.camera_main_navigation + + def __frsky_callback(self, data): + self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status + + def __bogie_callback(self, data): + self.bogie_msg.bogie_connection_1 = data.bogie_connection_1 + self.bogie_msg.bogie_connection_2 = data.bogie_connection_2 + self.bogie_msg.bogie_connection_3 = data.bogie_connection_3 + + def __jetson_callback(self, data): + self.jetson_msg.jetson_CPU = data.jetson_CPU + self.jetson_msg.jetson_RAM = data.jetson_RAM + self.jetson_msg.jetson_EMMC = data.jetson_EMMC + self.jetson_msg.jetson_NVME_SSD = data.jetson_NVME_SSD + self.jetson_msg.jetson_GPU_temp = data.jetson_GPU_temp + rospy.loginfo(self.jetson_msg) + + def __gps_callback(self, data): + self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time + self.GPS_msg.GPS_connection_status = data.GPS_connection_status + + def __misc_callback(self, data): + self.misc_msg.arm_connection_status = data.arm_connection_status + self.misc_msg.arm_end_effector_connection_statuses = data.arm_end_effector_connection_statuses + self.misc_msg.sample_containment_connection_status = data.sample_containment_connection_status + self.misc_msg.tower_connection_status = data.tower_connection_status + self.misc_msg.chassis_pan_tilt_connection_status = data.chassis_pan_tilt_connection_status + + def run(self): + rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback) + rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback) + rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback) + rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback) + rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback) + 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 new file mode 100755 index 0000000..0d663b4 --- /dev/null +++ b/software/ros_packages/rover_status/src/system_statuses_node.py @@ -0,0 +1,244 @@ +#!/usr/bin/env python +import rospy +import os.path +import psutil +import subprocess +from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo + + +class SystemStatuses: + + def __init__(self): + # Camera path locations + self.system_path_locations = [ + '/dev/rover/camera_zed', + '/dev/rover/camera_undercarriage', + '/dev/rover/camera_chassis', + '/dev/rover/camera_main_navigation' + ] + + # filesystem paths for EMMC [0] and NVME_SSD [1] + # -- UPDATE [1] FOR JETSON -- + self.file_systems_EMMC_NVMe_SSD = [ + '/', + '/dev/shm' + ] + + rospy.init_node('SystemStatuses') + + # 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) + + # init all message variables + self.camera_msg = CameraStatuses() + self.bogie_msg = BogieStatuses() + self.FrSky_msg = FrSkyStatus() + self.GPS_msg = GPSInfo() + self.jetson_msg = JetsonInfo() + self.misc_msg = MiscStatuses() + + # init all message values + self.__pull_new_message_values() + + # init all previous values + self.__update_all_previous_values() + + # init all RoverSysMessage values + def __pull_new_message_values(self): + self.__set_gps_info() + self.__set_bogie_connection_statuses() + self.__set_arm_connection_status() + self.__set_arm_end_effector_connection_statuses() + self.__set_cameras() + self.__set_sample_containment_connection_status() + self.__set_tower_connection_status() + self.__set_chassis_pan_tilt_connection_status() + self.__set_jetson_usage_information() + self.__set_frsky_controller_connection_status() + + # Pulls the UTC GPS Time (WIP) + def __set_gps_info(self): + self.GPS_msg.UTC_GPS_time = 0 + self.GPS_msg.GPS_connection_status = 0 + + # Pulls bogie connection statuses (WIP) + def __set_bogie_connection_statuses(self): + self.bogie_msg.bogie_connection_1 = 0 + self.bogie_msg.bogie_connection_2 = 0 + self.bogie_msg.bogie_connection_3 = 0 + + # Checks arm connection status (WIP) + def __set_arm_connection_status(self): + self.misc_msg.arm_connection_status = 0 + + # Checks Arm End Effector Connection Statuses (WIP) + def __set_arm_end_effector_connection_statuses(self): + self.misc_msg.arm_end_effector_connection_statuses = 0 + + # Sets the Camera values (zed, undercarriage, chassis, and main_nav + def __set_cameras(self): + # Check if camera_zed is found + self.camera_msg.camera_zed = 1 if os.path.exists(self.system_path_locations[0]) else 0 + # Check if camera_undercarriage is found + self.camera_msg.camera_undercarriage = 1 if os.path.exists(self.system_path_locations[1]) else 0 + # Check if camera_chassis is found + self.camera_msg.camera_chassis = 1 if os.path.exists(self.system_path_locations[2]) else 0 + # Check if camera_main_navigation is found + self.camera_msg.camera_main_navigation = 1 if os.path.exists(self.system_path_locations[3]) else 0 + + # Checks Sample Containment Connection Status (WIP) + def __set_sample_containment_connection_status(self): + self.misc_msg.sample_containment_connection_status = 0 + + def __set_tower_connection_status(self): + self.misc_msg.tower_connection_status = 0 + + # Checks Tower Connection Status (WIP) + def __set_chassis_pan_tilt_connection_status(self): + self.misc_msg.chassis_pan_tilt_connection_status = 0 + + # Get Jetson Statuses (WIP) + def __set_jetson_usage_information(self): + self.jetson_msg.jetson_CPU = psutil.cpu_percent() + mem = psutil.virtual_memory() + self.jetson_msg.jetson_RAM = mem.percent + self.jetson_msg.jetson_EMMC = self.__used_percent_fs(self.file_systems_EMMC_NVMe_SSD[0]) + self.jetson_msg.jetson_NVME_SSD = self.__used_percent_fs(self.file_systems_EMMC_NVMe_SSD[1]) + + # Temperature + # This try except causes a bunch of annoying messages, but lets it run on non-jetson devices + # sets to -1.0 if sensor fails to give it a default value notifying failure to pull + 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] + except subprocess.CalledProcessError: + print 'sensors call failed (potential reason: on VM)' + self.jetson_msg.jetson_GPU_temp = -1.0 + + # EMMC and NVMe_SSD used % calculation + def __used_percent_fs(self, pathname): + statvfs = os.statvfs(pathname) + # percentage :: USED: + # used amount: blocks - bfree + # used%: used_amount / (used_amount + bavail) + used_available = (statvfs.f_frsize * statvfs.f_blocks / 1024) - (statvfs.f_frsize * statvfs.f_bfree / 1024.0) + used_percent = used_available / (used_available + (statvfs.f_frsize * statvfs.f_bavail / 1024.0)) + # Round 4 for 2 decimal accuracy + value = 100 * round(used_percent, 4) + return value + + # Check FrSky Controller Connection Status (WIP) + def __set_frsky_controller_connection_status(self): + self.FrSky_msg.FrSky_controller_connection_status = 0 + + # Used mainly for init, sets all previous values in one go + def __update_all_previous_values(self): + self.__set_previous_camera_values() + self.__set_previous_jetson_values() + self.__set_previous_frsky_value() + self.__set_previous_bogie_values() + self.__set_previous_gps_values() + self.__set_previous_misc_values() + + def __set_previous_camera_values(self): + self.previous_camera_zed = self.camera_msg.camera_zed + self.previous_camera_undercarriage = self.camera_msg.camera_undercarriage + self.previous_camera_chassis = self.camera_msg.camera_chassis + self.previous_camera_main_navigation = self.camera_msg.camera_main_navigation + + def __set_previous_jetson_values(self): + self.previous_jetson_CPU = self.jetson_msg.jetson_CPU + self.previous_jetson_RAM = self.jetson_msg.jetson_RAM + self.previous_jetson_EMMC = self.jetson_msg.jetson_EMMC + self.previous_jetson_NVME_SSD = self.jetson_msg.jetson_NVME_SSD + self.previous_jetson_GPU_temp = self.jetson_msg.jetson_GPU_temp + + def __set_previous_frsky_value(self): + self.previous_FrSky_controller_connection_status = self.FrSky_msg.FrSky_controller_connection_status + + def __set_previous_bogie_values(self): + self.previous_bogie_connection_1 = self.bogie_msg.bogie_connection_1 + self.previous_bogie_connection_2 = self.bogie_msg.bogie_connection_2 + self.previous_bogie_connection_3 = self.bogie_msg.bogie_connection_3 + + def __set_previous_gps_values(self): + self.previous_UTC_GPS_time = self.GPS_msg.UTC_GPS_time + self.previous_GPS_connection_status = self.GPS_msg.GPS_connection_status + + def __set_previous_misc_values(self): + self.previous_arm_connection_status = self.misc_msg.arm_connection_status + self.previous_arm_end_effector_connection_statuses = self.misc_msg.arm_end_effector_connection_statuses + self.previous_chassis_pan_tilt_connection_status = self.misc_msg.chassis_pan_tilt_connection_status + 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 run(self): + r = rospy.Rate(10) # 10Hz + while not rospy.is_shutdown(): + # Update all message values + self.__pull_new_message_values() + + # Camera Check -- if current value is now different from previous value, + # update the previous value for cameras and publish to listening Subscribers + 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.__set_previous_camera_values() + self.pub_camera.publish(self.camera_msg) + + # Placeholder Jetson Info Check + if (self.jetson_msg.jetson_CPU != self.previous_jetson_CPU or + 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.__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: + 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.__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.__set_previous_gps_values() + self.pub_GPS.publish(self.GPS_msg) + + # Placeholder Misc Information check + if (self.misc_msg.arm_connection_status != + self.previous_arm_connection_status or + self.misc_msg.arm_end_effector_connection_statuses != + self.previous_arm_end_effector_connection_statuses or + self.misc_msg.chassis_pan_tilt_connection_status != + 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.__set_previous_misc_values() + self.pub_Misc.publish(self.misc_msg) + + # rospy.loginfo(self.msg) + r.sleep() + + +if __name__ == '__main__': + system_status = SystemStatuses() + system_status.run()