diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index 045fd69..4ef8dc8 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -2,9 +2,6 @@ #include #include #include -#include "FastLED.h" - -#include /* Imu/data (Imu) @@ -44,7 +41,7 @@ enum HARDWARE { IMU_SDA = 18, IMU_SCL = 19, - WS2812_DATA = 11, + WHITE_LED_CONTROL = 11, C02_SENSOR = A7, MISC_PIN = A8, @@ -52,7 +49,16 @@ enum HARDWARE { }; enum MODBUS_REGISTERS { - DIRECTION = 0, // Input + LED_CONTROL = 0, // Input + CO2_READING_PPM = 1 +}; + +enum LIGHT_STATES { + NO_CHANGE = 0, + LIGHT_OFF = 1, + LIGHT_FLASH = 2, + LIGHT_MED = 3, + LIGHT_HIGH = 4 }; #define GPS_SERIAL_PORT Serial3 @@ -88,13 +94,12 @@ Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN); Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_IMM); -NMEAGPS gps; - const char baud115200[] = "PUBX,41,1,3,3,115200,0"; void setup() { // Debugging Serial.begin(9600); +// while (!Serial); // Raw pin setup setup_hardware(); @@ -134,6 +139,8 @@ void loop() { set_leds(); send_imu_stream_line(root); process_gps_and_send_if_ready(root); + process_white_led_command(); + get_co2_data(); // Print JSON and newline root.printTo(GPS_IMU_STREAMING_PORT); @@ -144,13 +151,14 @@ void loop() { void setup_hardware() { // Setup pins as inputs / outputs - pinMode(HARDWARE::WS2812_DATA, OUTPUT); + pinMode(HARDWARE::WHITE_LED_CONTROL, OUTPUT); pinMode(HARDWARE::C02_SENSOR, INPUT); pinMode(HARDWARE::MISC_PIN, OUTPUT); pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); // Set default pin states + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); // Set teensy to increased analog resolution @@ -217,6 +225,52 @@ void process_gps_and_send_if_ready(JsonObject &root) { } +void process_white_led_command() { + uint16_t light_command = modbus_data[MODBUS_REGISTERS::LED_CONTROL]; + + + if (light_command == LIGHT_STATES::LIGHT_MED) { + digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH); + delay(100); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); + delay(100); // wait for a second + } else if (light_command == LIGHT_STATES::LIGHT_FLASH) { + digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH); + delay(100); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); + delay(100); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH); + delay(100); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); + delay(100); + } else if (light_command == LIGHT_STATES::LIGHT_HIGH) { + digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH); + delay(750); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); + delay(100); + } else if (light_command == LIGHT_STATES::LIGHT_OFF) { + digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH); + delay(2000); // wait for a second + digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW); + delay(2000); + } + + modbus_data[MODBUS_REGISTERS::LED_CONTROL] = LIGHT_STATES::NO_CHANGE; +} + +void get_co2_data() { + int voltage = ((analogRead(HARDWARE::C02_SENSOR) / 8192.0) * 3300); + + if (voltage < 400 || voltage > 2000) { + modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = 9999; + } else { + modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = map(voltage, 400, 2000, 0, 5000); + } + + // Serial.println(modbus_data[MODBUS_REGISTERS::CO2_READING_PPM]); + +} + void poll_modbus() { poll_state = slave.poll(modbus_data, num_modbus_registers); communication_good = !slave.getTimeOutState(); diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py index 4a6ae46..1270442 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -91,7 +91,7 @@ class SensorCore(QtCore.QThread): self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback) self.camera_msg = CameraStatuses() - self.bogie_msg = BogieStatuses() + self.bogie_msg = None # BogieStatuses() self.FrSky_msg = FrSkyStatus() self.GPS_msg = GPSInfo() self.jetson_msg = JetsonInfo() @@ -148,7 +148,7 @@ class SensorCore(QtCore.QThread): self.frsky_stylesheet_change_ready__signal.emit(COLOR_GREEN) def __jetson_callback(self, data): - self.jetson_cpu_update_ready__signal.emit("TX2 CPU\n" + str(data.jetson_CPU) + "%") + self.jetson_cpu_update_ready__signal.emit("TX2 CPU\n" + str(data.jetson_CPU) + " %") if data.jetson_CPU > 85: self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_ORANGE) @@ -157,7 +157,7 @@ class SensorCore(QtCore.QThread): else: self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_GREEN) - self.jetson_ram_update_ready__signal.emit("TX2 RAM\n" + str(data.jetson_RAM) + "%") + self.jetson_ram_update_ready__signal.emit("TX2 RAM\n" + str(data.jetson_RAM) + " %") if data.jetson_RAM > 79: self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_ORANGE) @@ -166,7 +166,7 @@ class SensorCore(QtCore.QThread): else: self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_GREEN) - self.jetson_gpu_temp_update_ready__signal.emit("TX2 TEMP\n" + str(data.jetson_GPU_temp) + "°C") + self.jetson_gpu_temp_update_ready__signal.emit("TX2 TEMP\n" + str(data.jetson_GPU_temp) + " °C") if data.jetson_GPU_temp > 64: self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_ORANGE) @@ -175,7 +175,7 @@ class SensorCore(QtCore.QThread): else: self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_GREEN) - self.jetson_emmc_update_ready__signal.emit("TX2 EMMC\n" + str(data.jetson_EMMC) + "%") + self.jetson_emmc_update_ready__signal.emit("TX2 EMMC\n" + str(data.jetson_EMMC) + " %") if data.jetson_EMMC > 79: self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_ORANGE) @@ -207,7 +207,7 @@ class SensorCore(QtCore.QThread): else: self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED) - self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + "V") + self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V") def __display_time(self): time = QtCore.QTime.currentTime() diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py index 1aaa182..7282a44 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py @@ -13,7 +13,7 @@ import json ##################################### # Global Variables ##################################### -THREAD_HERTZ = 2 +THREAD_HERTZ = 5 ACCESS_POINT_IP = "192.168.1.20" # The channel only has to be set on the access point. The staion will adjust. ACCESS_POINT_USER = "ubnt" diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 2cff728..32a95bb 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -264,7 +264,11 @@ background-color:darkgreen; border: 0.5px solid black; - <html><head/><body><p align="center"><span style=" font-weight:600;">TX2 EMMC</span></p></body></html> + TX2 EMMC +-- % + + + Qt::PlainText Qt::AlignCenter @@ -797,7 +801,11 @@ Disconnected background-color:darkgreen; border: 0.5px solid black; - <html><head/><body><p align="center"><span style=" font-weight:600;">TX2 CPU</span></p></body></html> + TX2 CPU +-- % + + + Qt::PlainText Qt::AlignCenter @@ -817,7 +825,11 @@ Disconnected background-color:darkgreen; border: 0.5px solid black; - <html><head/><body><p align="center"><span style=" font-size:9pt; font-weight:600;">TX2 TEMP</span></p></body></html> + TX2 Temp + -- °C + + + Qt::PlainText Qt::AlignCenter @@ -963,7 +975,11 @@ Connected background-color:darkgreen; border: 0.5px solid black; - <html><head/><body><p align="center"><span style=" font-weight:600;">TX2 RAM</span></p></body></html> + TX2 RAM +-- % + + + Qt::PlainText Qt::AlignCenter @@ -1047,7 +1063,7 @@ Connected Connection Quality -0.0% +-- % Qt::AlignCenter @@ -1088,8 +1104,8 @@ Connected QFrame::NoFrame - RX Rate: 240.0 Mbps -TX Rate: 300 Mbps + RX Rate: --- Mbps +TX Rate: --- Mbps Qt::AlignCenter @@ -1131,7 +1147,7 @@ TX Rate: 300 Mbps Successful Transmit -0.0% +-- % Qt::AlignCenter @@ -1172,8 +1188,8 @@ TX Rate: 300 Mbps QFrame::NoFrame - TX Latency: 10000 ms -RX Latency: 10000 ms + TX Latency: ----- ms +RX Latency: ----- ms Qt::AlignCenter diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py index 8ce35da..4b94fd5 100755 --- a/software/ros_packages/rover_control/src/drive_control/drive_control.py +++ b/software/ros_packages/rover_control/src/drive_control/drive_control.py @@ -119,7 +119,7 @@ class DriveControl(object): self.get_drive_status() except Exception, error: - print "Error occurred:", error + pass if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT: print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting." diff --git a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py index 4100c4a..3266206 100755 --- a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py +++ b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py @@ -70,6 +70,8 @@ REGISTER_STATE_MAPPING = { "DRIVE_VS_ARM": "SE_SWITCH" } +IRIS_LAST_SEEN_TIMEOUT = 1 # seconds + ##################################### # IrisController Class Definition @@ -98,6 +100,8 @@ class IrisController(object): self.iris_connected = False + self.iris_last_seen_time = time() + self.run() def __setup_minimalmodbus_for_485(self): @@ -114,11 +118,16 @@ class IrisController(object): self.broadcast_drive_if_current_mode() self.broadcast_arm_if_current_mode() self.broadcast_iris_status() + self.iris_last_seen_time = time() except Exception, error: print "Error occurred:", error return + if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT: + print "Iris not seen for", IRIS_LAST_SEEN_TIMEOUT, "seconds. Exiting." + return # Exit so respawn can take over + time_diff = time() - start_time sleep(max(self.wait_time - time_diff, 0)) diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py index 37cf6ac..292278e 100755 --- a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py +++ b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py @@ -10,6 +10,8 @@ from time import time, sleep import serial.rs485 import minimalmodbus +from std_msgs.msg import UInt8, UInt16 + # Custom Imports from rover_control.msg import TowerPanTiltControlMessage @@ -23,18 +25,21 @@ DEFAULT_BAUD = 115200 DEFAULT_INVERT = False -DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control" +DEFAULT_TOWER_LIGHT_CONTROL_TOPIC = "tower/light/control" +DEFAULT_TOWER_CO2_STATUS_TOPIC = "tower/status/co2" +DEFAULT_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control" -NODE_ID = 2 +TOWER_NODE_ID = 1 +PAN_TILT_NODE_ID = 2 -COMMUNICATIONS_TIMEOUT = 0.01 # Seconds +COMMUNICATIONS_TIMEOUT = 0.02 # Seconds -RX_DELAY = 0.01 -TX_DELAY = 0.01 +RX_DELAY = 0.02 +TX_DELAY = 0.02 DEFAULT_HERTZ = 20 -MODBUS_REGISTERS = { +PAN_TILT_MODBUS_REGISTERS = { "CENTER_ALL": 0, "PAN_ADJUST_POSITIVE": 1, @@ -43,15 +48,32 @@ MODBUS_REGISTERS = { "TILT_ADJUST_NEGATIVE": 4 } +TOWER_MODBUS_REGISTERS = { + "LED_CONTROL": 0, + "CO2_READING_PPM": 1 +} + PAN_TILT_CONTROL_DEFAULT_MESSAGE = [ 0, # No centering 0, # No pan positive adjustment 0, # No pan negative adjustment 0, # No tilt positive adjustment - 0 # No tilt negative adjustement + 0 # No tilt negative adjustement ] -PAN_TILT_LAST_SEEN_TIMEOUT = 2 # seconds +TOWER_LIGHT_STATES = { + "NO_CHANGE": 0, + "LIGHT_OFF": 1, + "LIGHT_FLASH": 2, + "LIGHT_MED": 3, + "LIGHT_HIGH": 4 +} + +TOWER_CONTROL_DEFAULT_MESSAGE = [ + TOWER_LIGHT_STATES["LIGHT_OFF"] # Light off +] + +NODE_LAST_SEEN_TIMEOUT = 2 # seconds ##################################### @@ -64,23 +86,40 @@ class TowerPanTiltControl(object): self.port = rospy.get_param("~port", DEFAULT_PORT) self.baud = rospy.get_param("~baud", DEFAULT_BAUD) - self.node_id = rospy.get_param("~node_id", NODE_ID) + self.tower_node_id = rospy.get_param("~tower_node_id", TOWER_NODE_ID) + self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID) - self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic", DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC) + self.tower_light_control_subscriber_topic = rospy.get_param("~tower_light_control_topic", + DEFAULT_TOWER_LIGHT_CONTROL_TOPIC) + self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic", + DEFAULT_PAN_TILT_CONTROL_TOPIC) + + self.tower_co2_publisher_topic = rospy.get_param("~tower_co2_status_topic", + DEFAULT_TOWER_CO2_STATUS_TOPIC) self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) self.pan_tilt_node = None + self.tower_node = None - self.connect_to_pan_tilt() + self.connect_to_pan_tilt_and_tower() - self.pan_tilt_control_subscriber = \ - rospy.Subscriber(self.pan_tilt_control_subscriber_topic, TowerPanTiltControlMessage, self.pan_tilt_control_callback) + self.pan_tilt_control_subscriber = rospy.Subscriber(self.pan_tilt_control_subscriber_topic, + TowerPanTiltControlMessage, + self.pan_tilt_control_callback) + + self.tower_light_control_subscriber = rospy.Subscriber(self.tower_light_control_subscriber_topic, UInt8, + self.tower_light_control_callback) + + self.tower_co2_publisher = rospy.Publisher(self.tower_co2_publisher_topic, UInt16, queue_size=1) self.pan_tilt_control_message = None - self.new_control_message = False + self.tower_light_control_message = None - self.pan_tilt_last_seen = time() + self.new_pan_tilt_control_message = False + self.new_tower_light_control_message = False + + self.modbus_nodes_seen_time = time() self.run() @@ -90,63 +129,99 @@ class TowerPanTiltControl(object): delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY) + self.tower_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) + self.tower_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, + delay_before_rx=RX_DELAY, + delay_before_tx=TX_DELAY) + def run(self): - self.send_startup_centering_command() + self.send_startup_centering_and_lights_off_command() while not rospy.is_shutdown(): start_time = time() try: self.send_pan_tilt_control_message() - self.pan_tilt_last_seen = time() + self.modbus_nodes_seen_time = time() except Exception, error: - print "Error occurred:", error + pass + # print "Error occurred:", error - if (time() - self.pan_tilt_last_seen) > PAN_TILT_LAST_SEEN_TIMEOUT: - print "Tower pan/tilt not seen for", PAN_TILT_LAST_SEEN_TIMEOUT, "seconds. Exiting." + try: + self.send_tower_control_message() + self.broadcast_co2_reading_message() + self.modbus_nodes_seen_time = time() + + except Exception, error: + pass + # print "Error occurred:", error + + if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: + print "Tower pan/tilt not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting." return # Exit so respawn can take over time_diff = time() - start_time sleep(max(self.wait_time - time_diff, 0)) - def connect_to_pan_tilt(self): - self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.node_id)) + def connect_to_pan_tilt_and_tower(self): + self.tower_node = minimalmodbus.Instrument(self.port, int(self.tower_node_id)) + self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id)) self.__setup_minimalmodbus_for_485() - def send_startup_centering_command(self): - registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) - registers[MODBUS_REGISTERS["CENTER_ALL"]] = 1 - self.pan_tilt_node.write_registers(0, registers) + def send_startup_centering_and_lights_off_command(self): + try: + registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) + registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1 + self.pan_tilt_node.write_registers(0, registers) + + self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"]) + except: + pass def send_pan_tilt_control_message(self): - if self.new_control_message: + if self.new_pan_tilt_control_message: pan_tilt_control_message = self.pan_tilt_control_message # type: TowerPanTiltControlMessage registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) - registers[MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center) + registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center) if pan_tilt_control_message.relative_pan_adjustment >= 0: - registers[MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment + registers[ + PAN_TILT_MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment else: - registers[MODBUS_REGISTERS["PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment + registers[PAN_TILT_MODBUS_REGISTERS[ + "PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment if pan_tilt_control_message.relative_tilt_adjustment >= 0: - registers[MODBUS_REGISTERS["TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment + registers[PAN_TILT_MODBUS_REGISTERS[ + "TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment else: - registers[MODBUS_REGISTERS["TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment + registers[PAN_TILT_MODBUS_REGISTERS[ + "TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment self.pan_tilt_node.write_registers(0, registers) - self.new_control_message = False + self.new_pan_tilt_control_message = False else: self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE) + def broadcast_co2_reading_message(self): + self.tower_co2_publisher.publish(UInt16(data=self.tower_node.read_register(1))) + + def send_tower_control_message(self): + if self.new_tower_light_control_message: + self.tower_node.write_register(0, self.tower_light_control_message.data) + self.new_tower_light_control_message = False + def pan_tilt_control_callback(self, pan_tilt_control): self.pan_tilt_control_message = pan_tilt_control - self.new_control_message = True + self.new_pan_tilt_control_message = True + def tower_light_control_callback(self, light_control): + self.tower_light_control_message = light_control + self.new_tower_light_control_message = True if __name__ == "__main__": - TowerPanTiltControl() \ No newline at end of file + TowerPanTiltControl() diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index e434d87..25274f9 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -13,6 +13,6 @@ - + diff --git a/software/ros_packages/rover_main/launch/rover/odometry2.launch b/software/ros_packages/rover_main/launch/rover/odometry2.launch index 9de8629..024e8f2 100644 --- a/software/ros_packages/rover_main/launch/rover/odometry2.launch +++ b/software/ros_packages/rover_main/launch/rover/odometry2.launch @@ -1,68 +1,71 @@ - - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - - - + + - - - - + + + + + + + + - - + + + + - [true, true, false, - false, false, false, - false , false, false, - false, false, false, - false, false, false] + + - [false, false, false, - true , true , true, - false, false, false, - true , true , true , - true , true , true ] + + + + + - - + + + + + - + + - - + - + + - + - - + - + + - - - + + + + + + diff --git a/software/ros_packages/rover_odometry/src/odometry.py b/software/ros_packages/rover_odometry/src/odometry.py index 982b947..a33978c 100755 --- a/software/ros_packages/rover_odometry/src/odometry.py +++ b/software/ros_packages/rover_odometry/src/odometry.py @@ -75,9 +75,11 @@ class Odometry(object): imu = temp.get('imu', None) if gps: + print gps self.broadcast_gps(gps) if imu: + # print imu self.broadcast_imu(imu) def broadcast_gps(self, gps):