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

Got more statuses working, mainly GPS. Played with odom a bit more. Map now shows live GPS location!

Browse Source
This commit is contained in:
Corwin Perren
2018-07-15 22:35:50 -07:00
parent 1b7f5437b9
commit a573f72c1c
8 changed files with 405 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py
View File

@@ -18,7 +18,7 @@ GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
DEFAULT_PAN_TILT_COMMAND_TOPIC = "/rover_control/pan_tilt/control"
DRIVE_COMMAND_HERTZ = 15
DRIVE_COMMAND_HERTZ = 20
Y_AXIS_DEADBAND = 0.05
X_AXIS_DEADBAND = 0.05
@@ -32,7 +32,7 @@ GUI_ELEMENT_CHANGE_TIME = 0.2
CAMERA_TOGGLE_CHANGE_TIME = 0.35
PAN_TILT_X_AXIS_SCALAR = 3
PAN_TILT_Y_AXIS_SCALAR = 10
PAN_TILT_Y_AXIS_SCALAR = 20
#####################################
@@ -169,6 +169,9 @@ class JoystickControlSender(QtCore.QThread):
self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_PAN_TILT_COMMAND_TOPIC, TowerPanTiltControlMessage, queue_size=1)
self.last_hat_x_was_movement = False
self.last_hat_y_was_movement = False
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
self.drive_paused = True

47
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

@@ -12,12 +12,15 @@ import rospy
# Custom Imports
import RoverMap
from sensor_msgs.msg import NavSatFix
#####################################
# Global Variables
#####################################
# put some stuff here later so you can remember
GPS_POSITION_TOPIC = "/rover_odometry/fix"
class RoverMapCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal()
@@ -36,6 +39,8 @@ class RoverMapCoordinator(QtCore.QThread):
self.logger = logging.getLogger("groundstation")
self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix, self.gps_position_updated_callback)
self.run_thread_flag = True
self.setup_map_flag = True
@@ -47,6 +52,9 @@ class RoverMapCoordinator(QtCore.QThread):
self.map_pixmap = None
self.last_map_pixmap_cache_key = None
self.longitude = None
self.latitude = None
def run(self):
self.logger.debug("Starting Map Coordinator Thread")
@@ -72,13 +80,13 @@ class RoverMapCoordinator(QtCore.QThread):
def _map_setup(self):
self.google_maps_object = RoverMap.GMapsStitcher(1280,
720,
44.567161,
-123.278432,
18,
44.5675721667,
-123.2750535,
20, # FIXME: Used to be 18
'satellite',
None, 20)
self.overlay_image_object = (
RoverMap.OverlayImage(44.567161, -123.278432,
RoverMap.OverlayImage(44.5675721667, -123.2750535,
self.google_maps_object.northwest,
self.google_maps_object.southeast,
self.google_maps_object.big_image.size[0],
@@ -136,15 +144,22 @@ class RoverMapCoordinator(QtCore.QThread):
return temp_list
def update_overlay(self):
longitude = 44.567161
latitude = -123.278432
navigation_list = self._get_table_elements(self.navigation_label)
# landmark_list = self._get_table_elements(self.landmark_label)
landmark_list = []
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
70,
navigation_list,
landmark_list)
# self.overlay_image.save("something.png")
if self.latitude and self.longitude:
longitude = self.latitude
latitude = self.longitude
print self.longitude, " : ", self.latitude
navigation_list = self._get_table_elements(self.navigation_label)
# landmark_list = self._get_table_elements(self.landmark_label)
landmark_list = []
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
70,
navigation_list,
landmark_list)
# self.overlay_image.save("something.png")
def gps_position_updated_callback(self, data):
self.latitude = data.latitude
self.longitude = data.longitude

50
software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
View File

@@ -7,6 +7,8 @@ from PyQt5 import QtWidgets, QtCore, QtGui, uic
from std_msgs.msg import Empty
import PIL.Image
from PIL.ImageQt import ImageQt
from std_msgs.msg import UInt16
# import Timer
REQUEST_UPDATE_TOPIC = "/rover_status/update_requested"
@@ -19,6 +21,7 @@ GPS_TOPIC_NAME = "/rover_status/gps_status"
JETSON_TOPIC_NAME = "/rover_status/jetson_status"
MISC_TOPIC_NAME = "/rover_status/misc_status"
BATTERY_TOPIC_NAME = "/rover_status/battery_status"
CO2_TOPIC_NAME = "/rover_control/tower/status/co2"
COLOR_GREEN = "background-color: darkgreen; border: 1px solid black;"
COLOR_ORANGE = "background-color: orange; border: 1px solid black;"
@@ -40,6 +43,13 @@ class SensorCore(QtCore.QThread):
bogie_connection_2_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
bogie_connection_3_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
gps_fix_update_ready__signal = QtCore.pyqtSignal(str)
gps_heading_valid_update_ready__signal = QtCore.pyqtSignal(str)
gps_num_satellites_update_ready__signal = QtCore.pyqtSignal(str)
gps_accuracy_update_ready__signal = QtCore.pyqtSignal(str)
co2_levels_update_ready__signal = QtCore.pyqtSignal(str)
camera_zed_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
camera_under_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
camera_chassis_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
@@ -70,7 +80,10 @@ class SensorCore(QtCore.QThread):
self.frsky = self.screen_main_window.frsky # type: QtWidgets.QLabel
self.nav_mouse = self.screen_main_window.nav_mouse # type: QtWidgets.QLabel
self.joystick = self.screen_main_window.joystick # type: QtWidgets.QLabel
self.gps = self.screen_main_window.gps # type: QtWidgets.QLabel
self.gps_fix_label = self.screen_main_window.gps_fix_label # type: QtWidgets.QLabel
self.gps_heading_valid_label = self.screen_main_window.gps_heading_valid_label # type: QtWidgets.QLabel
self.gps_num_satellites_label = self.screen_main_window.gps_num_satellites_label # type: QtWidgets.QLabel
self.gps_accuracy_label = self.screen_main_window.gps_accuracy_label # type: QtWidgets.QLabel
self.zed = self.screen_main_window.zed # type: QtWidgets.QLabel
self.main_cam = self.screen_main_window.main_cam # type: QtWidgets.QLabel
self.chassis_cam = self.screen_main_window.chassis_cam # type: QtWidgets.QLabel
@@ -81,6 +94,7 @@ class SensorCore(QtCore.QThread):
self.gpu_temp = self.screen_main_window.gpu_temp # type: QtWidgets.QLabel
self.emmc = self.screen_main_window.emmc # type: QtWidgets.QLabel
self.battery = self.screen_main_window.battery_voltage_status_label # type: QtWidgets.QLabel
self.co2_levels_label = self.screen_main_window.co2_levels_label # type: QtWidgets.QLabel
# ########## subscriptions pulling data from system_statuses_node.py ##########
self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback)
@@ -89,6 +103,7 @@ class SensorCore(QtCore.QThread):
self.jetson_status = rospy.Subscriber(JETSON_TOPIC_NAME, JetsonInfo, self.__jetson_callback)
self.misc_status = rospy.Subscriber(MISC_TOPIC_NAME, MiscStatuses, self.__misc_callback)
self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback)
self.co2_status = rospy.Subscriber(CO2_TOPIC_NAME, UInt16, self.__co2_callback)
self.camera_msg = CameraStatuses()
self.bogie_msg = None # BogieStatuses()
@@ -185,13 +200,23 @@ class SensorCore(QtCore.QThread):
self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_GREEN)
def __gps_callback(self, data):
self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time
if not data.GPS_connection_status:
self.gps_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
if data.gps_connected:
self.gps_fix_update_ready__signal.emit("GPS Fix\nTrue")
self.gps_stylesheet_change_ready__signal.emit(COLOR_GREEN)
else:
self.gps_stylesheet_change_ready__signal.emit(COLOR_RED)
self.gps_fix_update_ready__signal.emit("GPS Fix\nFalse")
if data.gps_heading != -1:
self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nTrue")
else:
self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nFalse")
self.gps_num_satellites_update_ready__signal.emit("GPS Satellites\n%s" % data.num_satellites)
self.gps_accuracy_update_ready__signal.emit("GPS Accuracy\n%2.2f m" % data.horizontal_dilution)
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
@@ -209,6 +234,12 @@ class SensorCore(QtCore.QThread):
self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V")
def __co2_callback(self, data):
if data.data != 9999:
self.co2_levels_update_ready__signal.emit("CO2 Levels\n%d ppm" % data.data)
else:
self.co2_levels_update_ready__signal.emit("CO2 Levels\n--- ppm")
def __display_time(self):
time = QtCore.QTime.currentTime()
temp = time.toString('hh:mm')
@@ -233,9 +264,16 @@ class SensorCore(QtCore.QThread):
self.camera_under_stylesheet_change_ready__signal.connect(self.under_cam.setStyleSheet)
self.camera_chassis_stylesheet_change_ready__signal.connect(self.chassis_cam.setStyleSheet)
self.camera_main_stylesheet_change_ready__signal.connect(self.main_cam.setStyleSheet)
self.gps_stylesheet_change_ready__signal.connect(self.gps.setStyleSheet)
self.gps_stylesheet_change_ready__signal.connect(self.gps_fix_label.setStyleSheet)
self.frsky_stylesheet_change_ready__signal.connect(self.frsky.setStyleSheet)
self.gps_fix_update_ready__signal.connect(self.gps_fix_label.setText)
self.gps_heading_valid_update_ready__signal.connect(self.gps_heading_valid_label.setText)
self.gps_num_satellites_update_ready__signal.connect(self.gps_num_satellites_label.setText)
self.gps_accuracy_update_ready__signal.connect(self.gps_accuracy_label.setText)
self.co2_levels_update_ready__signal.connect(self.co2_levels_label.setText)
self.battery_voltage_update_ready__signal.connect(self.battery.setText)
self.battery_voltage_stylesheet_change_ready__signal.connect(self.battery.setStyleSheet)

360
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -251,7 +251,7 @@
<property name="spacing">
<number>0</number>
</property>
<item row="5" column="4">
<item row="5" column="3">
<widget class="QLabel" name="emmc">
<property name="font">
<font>
@@ -275,7 +275,7 @@
</property>
</widget>
</item>
<item row="13" column="5">
<item row="13" column="4">
<widget class="QLabel" name="chassis_cam">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -320,7 +320,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="13" column="4">
<item row="13" column="3">
<widget class="QLabel" name="under_cam">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -452,7 +452,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="0" column="4">
<item row="0" column="3">
<widget class="QLabel" name="nav_mouse">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -494,7 +494,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="0" column="5">
<item row="0" column="4">
<widget class="QLabel" name="joystick">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -578,49 +578,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="gps">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="text">
<string>GPS
Fix</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="8" column="4">
<item row="8" column="3">
<widget class="QLabel" name="middle_left_wheel_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -662,7 +620,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="8" column="5">
<item row="8" column="4">
<widget class="QLabel" name="rear_left_wheel_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -812,7 +770,7 @@ Disconnected</string>
</property>
</widget>
</item>
<item row="5" column="5">
<item row="5" column="4">
<widget class="QLabel" name="gpu_temp">
<property name="font">
<font>
@@ -878,49 +836,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="battery_voltage_status_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>Battery Voltage
N/A</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="10" column="4">
<item row="10" column="3">
<widget class="QLabel" name="middle_right_wheel_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -986,7 +902,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="10" column="5">
<item row="10" column="4">
<widget class="QLabel" name="rear_right_wheel_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -1070,7 +986,7 @@ Connected</string>
</property>
</widget>
</item>
<item row="2" column="4">
<item row="2" column="3">
<widget class="QLabel" name="radio_rates_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -1154,7 +1070,7 @@ TX Rate: --- Mbps</string>
</property>
</widget>
</item>
<item row="2" column="5">
<item row="2" column="4">
<widget class="QLabel" name="radio_latency_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -1196,6 +1112,258 @@ RX Latency: ----- ms</string>
</property>
</widget>
</item>
<item row="3" column="4">
<widget class="QLabel" name="gps_accuracy_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="text">
<string>GPS Accuracy
-- m</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="3">
<widget class="QLabel" name="gps_num_satellites_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="text">
<string>GPS Satellites
0</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="gps_heading_valid_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="text">
<string>GPS Heading Valid
False</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="gps_fix_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="text">
<string>GPS Fix
False</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="battery_voltage_status_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>Battery Voltage
N/A</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="co2_levels_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>CO2 Levels
--- ppm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>

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

@@ -7,7 +7,7 @@
<param name="destination_port" value="17100" />
<rosparam param="topics">
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
{name: "/rover_control/pan_tilt/control", compress: true, rate: 15.0}]
{name: "/rover_control/pan_tilt/control", compress: true, rate: 20.0}]
</rosparam>
</node>

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

@@ -2,7 +2,7 @@
<group ns="sender_transports">
<arg name="target" default="192.168.1.15" />
<node name="chassis_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="chassis_1280x720" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<rosparam param="topics">
@@ -10,7 +10,7 @@
</rosparam>
</node>
<node name="undercarriage_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="undercarriage_1280x720" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17002" />
<rosparam param="topics">
@@ -18,7 +18,7 @@
</rosparam>
</node>
<node name="main_navigation_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="main_navigation_1280x720" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17003" />
<rosparam param="topics">
@@ -26,7 +26,7 @@
</rosparam>
</node>
<node name="end_effector_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="end_effector_1280x720" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17004" />
<rosparam param="topics">
@@ -34,7 +34,7 @@
</rosparam>
</node>
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17005" />
<rosparam param="topics">
@@ -42,7 +42,7 @@
</rosparam>
</node>
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17006" />
<rosparam param="topics">
@@ -50,7 +50,7 @@
</rosparam>
</node>
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17007" />
<rosparam param="topics">
@@ -58,7 +58,7 @@
</rosparam>
</node>
<node name="end_effector_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="end_effector_640x360" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17008" />
<rosparam param="topics">
@@ -66,7 +66,7 @@
</rosparam>
</node>
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17009" />
<rosparam param="topics">
@@ -74,7 +74,7 @@
</rosparam>
</node>
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17010" />
<rosparam param="topics">
@@ -82,7 +82,7 @@
</rosparam>
</node>
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17011" />
<rosparam param="topics">
@@ -90,7 +90,7 @@
</rosparam>
</node>
<node name="end_effector_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="end_effector_256x144" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17012" />
<rosparam param="topics">
@@ -98,7 +98,7 @@
</rosparam>
</node>
<node name="bogie_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="bogie_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17013" />
<rosparam param="topics">
@@ -106,7 +106,7 @@
</rosparam>
</node>
<node name="camera_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="camera_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17014" />
<rosparam param="topics">
@@ -114,7 +114,7 @@
</rosparam>
</node>
<node name="frsky_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="frsky_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17015" />
<rosparam param="topics">
@@ -122,15 +122,16 @@
</rosparam>
</node>
<node name="gps_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="gps_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17016" />
<rosparam param="topics">
[{name: "/rover_status/gps_status", compress: false, rate: 1.0}]
[{name: "/rover_status/gps_status", compress: false, rate: 1.0},
{name: "/rover_odometry/fix", compress: false, rate: 5.0}]
</rosparam>
</node>
<node name="jetson_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="jetson_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17017" />
<rosparam param="topics">
@@ -138,7 +139,7 @@
</rosparam>
</node>
<node name="misc_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<node name="misc_status_tcp" pkg="nimbro_topic_transport" type="tcp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17018" />
<rosparam param="topics">
@@ -146,13 +147,13 @@
</rosparam>
</node>
<node name="udp_statuses_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<node name="udp_statuses_sender" pkg="nimbro_topic_transport" type="udp_sender">
<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/battery_status", compress: false, rate: 1.0}
{name: "/rover_status/battery_status", compress: false, rate: 1.0},
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}
]
</rosparam>
</node>

32
software/ros_packages/rover_odometry/src/odometry.py
View File

@@ -7,6 +7,7 @@ import rospy
import serial
from time import time, sleep
import json
import re
from nmea_msgs.msg import Sentence
from sensor_msgs.msg import Imu
@@ -75,13 +76,42 @@ class Odometry(object):
imu = temp.get('imu', None)
if gps:
print gps
# ###### THIS IS HERE TO DEAL WITH UBLOX GPS #####
if "GNGGA" in gps:
gps = gps.replace("GNGGA", "GPGGA")
gps = gps[:-2] + str(self.chksum_nmea(gps))[2:]
# print gps
# #####
self.broadcast_gps(gps)
if imu:
# print imu
self.broadcast_imu(imu)
@staticmethod
def chksum_nmea(sentence):
# String slicing: Grabs all the characters
# between '$' and '*' and nukes any lingering
# newline or CRLF
chksumdata = re.sub("(\n|\r\n)", "", sentence[sentence.find("$") + 1:sentence.find("*")])
# Initializing our first XOR value
csum = 0
# For each char in chksumdata, XOR against the previous
# XOR'd char. The final XOR of the last char will be our
# checksum to verify against the checksum we sliced off
# the NMEA sentence
for c in chksumdata:
# XOR'ing value of csum against the next char in line
# and storing the new XOR value in csum
csum ^= ord(c)
# Do we have a validated sentence?
return hex(csum)
def broadcast_gps(self, gps):
message = Sentence()
message.header.frame_id = "gps"

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

@@ -36,7 +36,7 @@ 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'
DEFAULT_GPS_NMEA_TOPIC_NAME = '/odometry/gps/sentence'
DEFAULT_GPS_NMEA_TOPIC_NAME = '/rover_odometry/gps/sentence'
#####################################
@@ -130,7 +130,11 @@ class SystemStatuses:
# Pulls the UTC GPS Information (WIP v2.0)
def __set_gps_info(self, data):
self.GPS_msg.gps_connected = True
self.Nmea_Message = pynmea2.parse(data.sentence)
try:
self.Nmea_Message = pynmea2.parse(data.sentence)
except:
return
if self.Nmea_Message.sentence_type == 'GGA':
if int(self.Nmea_Message.gps_qual) != 0:
self.GPS_msg.gps_fix = True