caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added Bogie Wheel recognition in System Statuses, tested with detection results (not necessarily correct name/location) on rover

Browse Source
This commit is contained in:
MatthewTaylor24
2018-04-14 17:22:02 -07:00
parent 4daf40a229
commit 7b7f174d01
4 changed files with 56 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/ros_packages/rover_status/CMakeLists.txt
View File

@@ -57,7 +57,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files( add_message_files(
FILES FILES
RoverSysStatus.msg RoverSysStatus.msg
BogieStatuses.msg WheelStatuses.msg
FrSkyStatus.msg FrSkyStatus.msg
JetsonInfo.msg JetsonInfo.msg
CameraStatuses.msg CameraStatuses.msg

3
software/ros_packages/rover_status/msg/BogieStatuses.msg
View File

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

6
software/ros_packages/rover_status/msg/WheelStatuses.msg Normal file
View File

@@ -0,0 +1,6 @@
bool front_left
bool middle_left
bool rear_left
bool front_right
bool middle_right
bool rear_right

71
software/ros_packages/rover_status/src/system_statuses_node.py
View File

@@ -7,8 +7,8 @@ import rospy
import os.path import os.path
import psutil import psutil
import subprocess import subprocess
from rover_status.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo from rover_status.msg import CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
from rover_control.msg import DriveCommandMessage from rover_control.msg import DriveCommandMessage, DriveStatusMessage
from std_msgs.msg import Empty from std_msgs.msg import Empty
from time import time from time import time
@@ -17,7 +17,7 @@ from time import time
# Global Variables # Global Variables
##################################### #####################################
DEFAULT_CAMERA_TOPIC_NAME = "camera_status" DEFAULT_CAMERA_TOPIC_NAME = "camera_status"
DEFAULT_BOGIE_TOPIC_NAME = "bogie_status" DEFAULT_WHEEL_TOPIC_NAME = "wheel_status"
DEFAULT_FRSKY_TOPIC_NAME = "frsky_status" DEFAULT_FRSKY_TOPIC_NAME = "frsky_status"
DEFAULT_GPS_TOPIC_NAME = "gps_status" DEFAULT_GPS_TOPIC_NAME = "gps_status"
DEFAULT_JETSON_TOPIC_NAME = "jetson_status" DEFAULT_JETSON_TOPIC_NAME = "jetson_status"
@@ -27,6 +27,10 @@ DEFAULT_REQUEST_UPDATE_TOPIC_NAME = "update_requested"
MAX_JETSON_UPDATE_HERTZ = 0.2 MAX_JETSON_UPDATE_HERTZ = 0.2
DEFAULT_BOGIE_LEFT_TOPIC_NAME = '/rover_control/drive_status/left'
DEFAULT_BOGIE_RIGHT_TOPIC_NAME = '/rover_control/drive_status/right'
DEFAULT_BOGIE_REAR_TOPIC_NAME = '/rover_control/drive_status/rear'
##################################### #####################################
# SystemStatuses Class Definition # SystemStatuses Class Definition
@@ -52,7 +56,7 @@ class SystemStatuses:
# Get Topic Names # Get Topic Names
self.camera_topic_name = rospy.get_param("~camera_status_topic", DEFAULT_CAMERA_TOPIC_NAME) 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.wheel_topic_name = rospy.get_param("~wheel_status_topic", DEFAULT_WHEEL_TOPIC_NAME)
self.frsky_topic_name = rospy.get_param("~frsky_status_topic", DEFAULT_FRSKY_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.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.jetson_topic_name = rospy.get_param("~jetson_status_topic", DEFAULT_JETSON_TOPIC_NAME)
@@ -60,10 +64,13 @@ class SystemStatuses:
self.request_update_topic_name = rospy.get_param("~request_update_status_topic", self.request_update_topic_name = rospy.get_param("~request_update_status_topic",
DEFAULT_REQUEST_UPDATE_TOPIC_NAME) DEFAULT_REQUEST_UPDATE_TOPIC_NAME)
self.bogie_left_topic_name = rospy.get_param("~bogie_left_topic", DEFAULT_BOGIE_LEFT_TOPIC_NAME)
self.bogie_right_topic_name = rospy.get_param("~bogie_right_topic", DEFAULT_BOGIE_RIGHT_TOPIC_NAME)
self.bogie_rear_topic_name = rospy.get_param("~bogie_rear_topic", DEFAULT_BOGIE_REAR_TOPIC_NAME)
# init all publisher functions # init all publisher functions
self.pub_camera = rospy.Publisher(self.camera_topic_name, CameraStatuses, queue_size=1) 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_wheel = rospy.Publisher(self.wheel_topic_name, WheelStatuses, queue_size=1)
self.pub_FrSky = rospy.Publisher(self.frsky_topic_name, FrSkyStatus, 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_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_jetson = rospy.Publisher(self.jetson_topic_name, JetsonInfo, queue_size=1)
@@ -78,7 +85,7 @@ class SystemStatuses:
# init all message variables # init all message variables
self.camera_msg = CameraStatuses() self.camera_msg = CameraStatuses()
self.bogie_msg = BogieStatuses() self.wheel_msg = WheelStatuses()
self.FrSky_msg = FrSkyStatus() self.FrSky_msg = FrSkyStatus()
self.GPS_msg = GPSInfo() self.GPS_msg = GPSInfo()
self.jetson_msg = JetsonInfo() self.jetson_msg = JetsonInfo()
@@ -87,6 +94,8 @@ class SystemStatuses:
# init all message values # init all message values
self.__pull_new_message_values() self.__pull_new_message_values()
self.__set_bogie_subscribers()
# init all previous values # init all previous values
self.__update_all_previous_values() self.__update_all_previous_values()
@@ -95,9 +104,9 @@ class SystemStatuses:
# init all RoverSysMessage values # init all RoverSysMessage values
def __pull_new_message_values(self): def __pull_new_message_values(self):
self.__set_gps_info() self.__set_gps_info()
self.__set_bogie_connection_statuses()
self.__set_arm_connection_status() self.__set_arm_connection_status()
self.__set_arm_end_effector_connection_statuses() self.__set_arm_end_effector_connection_statuses()
#self.__set_bogie_subscribers()
self.__set_cameras() self.__set_cameras()
self.__set_sample_containment_connection_status() self.__set_sample_containment_connection_status()
self.__set_tower_connection_status() self.__set_tower_connection_status()
@@ -110,11 +119,23 @@ class SystemStatuses:
self.GPS_msg.UTC_GPS_time = 0 self.GPS_msg.UTC_GPS_time = 0
self.GPS_msg.GPS_connection_status = 0 self.GPS_msg.GPS_connection_status = 0
# Pulls bogie connection statuses (WIP) # Pulls bogie connection statuses
def __set_bogie_connection_statuses(self): def __set_bogie_subscribers(self):
self.bogie_msg.bogie_connection_1 = 0 self.bogie_left_sub = rospy.Subscriber(self.bogie_left_topic_name, DriveStatusMessage, self.__left_wheel_callback)
self.bogie_msg.bogie_connection_2 = 0 self.bogie_right_sub = rospy.Subscriber(self.bogie_right_topic_name, DriveStatusMessage, self.__right_wheel_callback)
self.bogie_msg.bogie_connection_3 = 0 self.bogie_rear_sub = rospy.Subscriber(self.bogie_rear_topic_name, DriveStatusMessage, self.__rear_wheel_callback)
def __left_wheel_callback(self, data):
self.wheel_msg.front_left = data.first_motor_connected
self.wheel_msg.middle_left = data.second_motor_connected
def __right_wheel_callback(self, data):
self.wheel_msg.front_right = data.first_motor_connected
self.wheel_msg.middle_right = data.second_motor_connected
def __rear_wheel_callback(self, data):
self.wheel_msg.rear_left = data.first_motor_connected
self.wheel_msg.rear_right = data.second_motor_connected
# Checks arm connection status (WIP) # Checks arm connection status (WIP)
def __set_arm_connection_status(self): def __set_arm_connection_status(self):
@@ -190,7 +211,7 @@ class SystemStatuses:
self.__set_previous_camera_values() self.__set_previous_camera_values()
self.__set_previous_jetson_values() self.__set_previous_jetson_values()
self.__set_previous_frsky_value() self.__set_previous_frsky_value()
self.__set_previous_bogie_values() self.__set_previous_wheel_values()
self.__set_previous_gps_values() self.__set_previous_gps_values()
self.__set_previous_misc_values() self.__set_previous_misc_values()
@@ -210,10 +231,13 @@ class SystemStatuses:
def __set_previous_frsky_value(self): def __set_previous_frsky_value(self):
self.previous_FrSky_controller_connection_status = self.FrSky_msg.FrSky_controller_connection_status self.previous_FrSky_controller_connection_status = self.FrSky_msg.FrSky_controller_connection_status
def __set_previous_bogie_values(self): def __set_previous_wheel_values(self):
self.previous_bogie_connection_1 = self.bogie_msg.bogie_connection_1 self.previous_wheel_front_left = self.wheel_msg.front_left
self.previous_bogie_connection_2 = self.bogie_msg.bogie_connection_2 self.previous_wheel_middle_left = self.wheel_msg.middle_left
self.previous_bogie_connection_3 = self.bogie_msg.bogie_connection_3 self.previous_wheel_rear_left = self.wheel_msg.rear_left
self.previous_wheel_front_right = self.wheel_msg.front_right
self.previous_wheel_middle_right = self.wheel_msg.middle_right
self.previous_wheel_rear_right = self.wheel_msg.rear_right
def __set_previous_gps_values(self): def __set_previous_gps_values(self):
self.previous_UTC_GPS_time = self.GPS_msg.UTC_GPS_time self.previous_UTC_GPS_time = self.GPS_msg.UTC_GPS_time
@@ -264,12 +288,15 @@ class SystemStatuses:
self.pub_FrSky.publish(self.FrSky_msg) self.pub_FrSky.publish(self.FrSky_msg)
# Placeholder bogie status check # Placeholder bogie status check
if (self.bogie_msg.bogie_connection_1 != self.previous_bogie_connection_1 or if (self.wheel_msg.front_left != self.previous_wheel_front_left or
self.bogie_msg.bogie_connection_2 != self.previous_bogie_connection_2 or self.wheel_msg.middle_left != self.previous_wheel_middle_left or
self.bogie_msg.bogie_connection_3 != self.previous_bogie_connection_3 or self.wheel_msg.rear_left != self.previous_wheel_rear_left or
self.wheel_msg.front_right != self.previous_wheel_front_right or
self.wheel_msg.middle_right != self.previous_wheel_middle_right or
self.wheel_msg.rear_right != self.previous_wheel_rear_right or
self.manual_update_requested): self.manual_update_requested):
self.__set_previous_bogie_values() self.__set_previous_wheel_values()
self.pub_bogie.publish(self.bogie_msg) self.pub_wheel.publish(self.wheel_msg)
# Placeholder GPS Information check # Placeholder GPS Information check
if (self.GPS_msg.UTC_GPS_time != self.previous_UTC_GPS_time or if (self.GPS_msg.UTC_GPS_time != self.previous_UTC_GPS_time or