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 battery status monitoring. Also made wheel status senders/recievers.

Browse Source
This commit is contained in:
Corwin Perren
2018-04-21 17:01:27 -07:00
parent 7b7efbe37a
commit 72994f9c9c
7 changed files with 47 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/ros_packages/rover_control/src/drive_control/drive_control.py
View File

@@ -147,7 +147,7 @@ class DriveControl(object):
self.second_motor.write_registers(0, second_motor_register_data)
except Exception, error:
print "Error occurred:", error
pass
self.new_control_message = False

13
software/ros_packages/rover_control/src/iris_controller/iris_controller.py
View File

@@ -91,11 +91,13 @@ class IrisController(object):
self.drive_command_publisher = rospy.Publisher(self.drive_command_publisher_topic, DriveCommandMessage,
queue_size=1)
self.iris
self.iris_status_publisher = rospy.Publisher(self.iris_status_publisher_topic, IrisStatusMessage,
queue_size=1)
self.registers = []
self.iris_connected = False
self.run()
def __setup_minimalmodbus_for_485(self):
@@ -125,7 +127,7 @@ class IrisController(object):
try:
self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
except Exception, error:
print error
self.iris_connected = False
def broadcast_drive_if_current_mode(self):
if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
@@ -161,7 +163,10 @@ class IrisController(object):
print "Arm"
def broadcast_iris_status(self):
print self.registers[MODBUS_REGISTERS["VOLTAGE_24V"]]
status_message = IrisStatusMessage()
status_message.iris_connected = True
status_message.voltage_24v = self.registers[MODBUS_REGISTERS["VOLTAGE_24V"]]
self.iris_status_publisher.publish(status_message)
if __name__ == "__main__":

2
software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch
View File

@@ -75,7 +75,7 @@
<param name="port" value="17018" />
</node>
<node name="gps_status_udp" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
<node name="udp_statuses" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17019" />
</node>

9
software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
View File

@@ -102,7 +102,7 @@
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17013" />
<rosparam param="topics">
[{name: "/rover_status/bogie_status", compress: false, rate: 1.0}]
[{name: "/rover_status/wheel_status", compress: false, rate: 1.0}]
</rosparam>
</node>
@@ -146,11 +146,14 @@
</rosparam>
</node>
<node name="gps_status_udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="udp_statuses_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17019" />
<rosparam param="topics">
[{name: "/rover_status/gps_status", compress: true, rate: 5.0}]
[
{name: "/rover_status/gps_status", compress: true, rate: 5.0},
{name: "/rover_status/battery_status", compress: false, rate: 1.0}
]
</rosparam>
</node>
</group>

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

@@ -56,6 +56,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
BatteryStatusMessage.msg
RoverSysStatus.msg
WheelStatuses.msg
FrSkyStatus.msg

1
software/ros_packages/rover_status/msg/BatteryStatusMessage.msg Normal file
View File

@@ -0,0 +1 @@
float32 battery_voltage

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

@@ -8,8 +8,8 @@ import os.path
import psutil
import pynmea2
import subprocess
from rover_status.msg import CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
from rover_control.msg import DriveCommandMessage, DriveStatusMessage
from rover_status.msg import BatteryStatusMessage, CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
from rover_control.msg import DriveCommandMessage, DriveStatusMessage, IrisStatusMessage
from std_msgs.msg import Empty
from nmea_msgs.msg import Sentence
from time import time
@@ -19,6 +19,7 @@ from time import time
# Global Variables
#####################################
# Publishers
DEFAULT_BATTERY_TOPIC_NAME = "battery_status"
DEFAULT_CAMERA_TOPIC_NAME = "camera_status"
DEFAULT_WHEEL_TOPIC_NAME = "wheel_status"
DEFAULT_FRSKY_TOPIC_NAME = "frsky_status"
@@ -27,9 +28,11 @@ DEFAULT_JETSON_TOPIC_NAME = "jetson_status"
DEFAULT_MISC_TOPIC_NAME = "misc_status"
MAX_JETSON_UPDATE_HERTZ = 0.2
MAX_IRIS_UPDATE_HERTZ = 0.2
# Subscribers
DEFAULT_REQUEST_UPDATE_TOPIC_NAME = "update_requested"
DEFAULT_IRIS_STATUS_TOPIC_NAME = "/rover_control/iris_status"
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'
@@ -59,6 +62,7 @@ class SystemStatuses:
rospy.init_node('SystemStatuses')
# Get Topic Names
self.battery_topic_name = rospy.get_param("~pub_battery_status_topic", DEFAULT_BATTERY_TOPIC_NAME)
self.camera_topic_name = rospy.get_param("~pub_camera_status_topic", DEFAULT_CAMERA_TOPIC_NAME)
self.wheel_topic_name = rospy.get_param("~pub_wheel_status_topic", DEFAULT_WHEEL_TOPIC_NAME)
self.frsky_topic_name = rospy.get_param("~pub_frsky_status_topic", DEFAULT_FRSKY_TOPIC_NAME)
@@ -69,12 +73,15 @@ class SystemStatuses:
# Subscribers
self.request_update_topic_name = rospy.get_param("~sub_request_update_status_topic",
DEFAULT_REQUEST_UPDATE_TOPIC_NAME)
self.iris_status_topic_name = rospy.get_param("~sub_iris_status_topic", DEFAULT_IRIS_STATUS_TOPIC_NAME)
self.bogie_left_topic_name = rospy.get_param("~sub_bogie_left_topic", DEFAULT_BOGIE_LEFT_TOPIC_NAME)
self.bogie_right_topic_name = rospy.get_param("~sub_bogie_right_topic", DEFAULT_BOGIE_RIGHT_TOPIC_NAME)
self.bogie_rear_topic_name = rospy.get_param("~sub_bogie_rear_topic", DEFAULT_BOGIE_REAR_TOPIC_NAME)
self.gps_nmea_topic_name = rospy.get_param("~sub_gps_nmea_topic", DEFAULT_GPS_NMEA_TOPIC_NAME)
# init all publisher functions
self.pub_battery = rospy.Publisher(self.battery_topic_name, BatteryStatusMessage, queue_size=1)
self.pub_camera = rospy.Publisher(self.camera_topic_name, CameraStatuses, 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)
@@ -90,6 +97,7 @@ class SystemStatuses:
self.manual_update_requested = False
# init all message variables
self.battery_message = BatteryStatusMessage()
self.camera_msg = CameraStatuses()
self.wheel_msg = WheelStatuses()
self.FrSky_msg = FrSkyStatus()
@@ -106,6 +114,7 @@ class SystemStatuses:
self.__update_all_previous_values()
self.last_jetson_message_sent = time()
self.last_iris_message_sent = time()
# init all RoverSysMessage values
def __pull_new_message_values(self):
@@ -138,6 +147,9 @@ class SystemStatuses:
# Instantiates all subscriber methods
def __instantiate_subscribers(self):
# Iris Status Subscriber
self.iris_status_sub = rospy.Subscriber(self.iris_status_topic_name, IrisStatusMessage, self.__iris_status_callback)
# Bogie Wheel Subscribers
self.bogie_left_sub = rospy.Subscriber(self.bogie_left_topic_name, DriveStatusMessage, self.__left_wheel_callback)
self.bogie_right_sub = rospy.Subscriber(self.bogie_right_topic_name, DriveStatusMessage, self.__right_wheel_callback)
@@ -145,6 +157,9 @@ class SystemStatuses:
# GPS NMEA subscriber
self.gps_nmea_sub = rospy.Subscriber(self.gps_nmea_topic_name, Sentence, self.__set_gps_info)
def __iris_status_callback(self, data):
self.battery_message.battery_voltage = data.voltage_24v
def __left_wheel_callback(self, data):
self.wheel_msg.front_left = data.first_motor_connected
self.wheel_msg.middle_left = data.second_motor_connected
@@ -228,6 +243,7 @@ class SystemStatuses:
# Used mainly for init, sets all previous values in one go
def __update_all_previous_values(self):
self.__set_previous_battery_values()
self.__set_previous_camera_values()
self.__set_previous_jetson_values()
self.__set_previous_frsky_value()
@@ -274,6 +290,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 __set_previous_battery_values(self):
self.previous_battery_voltage = self.battery_message.battery_voltage
def on_update_requested(self, _):
self.manual_update_requested = True
@@ -283,6 +302,13 @@ class SystemStatuses:
# Update all message values
self.__pull_new_message_values()
if (self.battery_message.battery_voltage != self.previous_battery_voltage or
self.manual_update_requested):
if (time() - self.last_iris_message_sent) > (1.0 / MAX_IRIS_UPDATE_HERTZ):
self.__set_previous_battery_values()
self.pub_battery.publish(self.battery_message)
self.last_iris_message_sent = time()
# 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