diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index 4ef8dc8..b1ecf62 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -2,6 +2,8 @@ #include #include #include +#include +#include /* Imu/data (Imu) @@ -99,7 +101,7 @@ const char baud115200[] = "PUBX,41,1,3,3,115200,0"; void setup() { // Debugging Serial.begin(9600); -// while (!Serial); + // while (!Serial); // Raw pin setup setup_hardware(); @@ -113,16 +115,7 @@ void setup() { GPS_IMU_STREAMING_PORT.begin(115200); GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN); - // IMU Setup - Serial.println("Setting up IMU"); - if (!bno.begin()) { - /* There was a problem detecting the BNO055 ... check your connections */ - Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); - while (1); - } - Serial.println("IMU Online. Setting to external crystal."); - bno.setExtCrystalUse(true); - Serial.println("IMU Configured."); + setup_imu(); // GPS Setup GPS_SERIAL_PORT.begin(9600); @@ -186,18 +179,73 @@ void send_imu_stream_line(JsonObject &root) { imu_object["avy"] = angular_vel.y(); imu_object["avz"] = angular_vel.z(); - // - // /* Display calibration status for each sensor. */ - // uint8_t system, gyro, accel, mag = 0; - // bno.getCalibration(&system, &gyro, &accel, &mag); - // Serial.print("CALIBRATION: Sys="); - // Serial.print(system, DEC); - // Serial.print(" Gyro="); - // Serial.print(gyro, DEC); - // Serial.print(" Accel="); - // Serial.print(accel, DEC); - // Serial.print(" Mag="); - // Serial.print(mag, DEC); +} + +void setup_imu(){ + + // IMU Setup + Serial.println("Setting up IMU"); + if (!bno.begin()) { + /* There was a problem detecting the BNO055 ... check your connections */ + Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); + while (1); + } + Serial.println("IMU Online. Setting to external crystal."); + + int eeAddress = 0; + long bnoID; + bool foundCalib = false; + + EEPROM.get(eeAddress, bnoID); + + adafruit_bno055_offsets_t calibrationData; + sensor_t sensor; + + /* + * Look for the sensor's unique ID at the beginning oF EEPROM. + * This isn't foolproof, but it's better than nothing. + */ + bno.getSensor(&sensor); + eeAddress += sizeof(long); + EEPROM.get(eeAddress, calibrationData); + bno.setSensorOffsets(calibrationData); + bno.setAxisConfig(Adafruit_BNO055::REMAP_CONFIG_P6); + bno.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P6); + bno.setMode(Adafruit_BNO055::OPERATION_MODE_IMUPLUS); + delay(300); + + bno.setExtCrystalUse(true); + Serial.println("IMU Configured"); + // while(1){ + // display_cal_status(); + // } +} + +void display_cal_status(void) +{ + /* Get the four calibration values (0..3) */ + /* Any sensor data reporting 0 should be ignored, */ + /* 3 means 'fully calibrated" */ + uint8_t system, gyro, accel, mag; + system = gyro = accel = mag = 0; + bno.getCalibration(&system, &gyro, &accel, &mag); + + /* The data should be ignored until the system calibration is > 0 */ + Serial.print("\t"); + if (!system) + { + Serial.print("! "); + } + + /* Display the individual values */ + Serial.print("Sys:"); + Serial.print(system, DEC); + Serial.print(" G:"); + Serial.print(gyro, DEC); + Serial.print(" A:"); + Serial.print(accel, DEC); + Serial.print(" M:"); + Serial.println(mag, DEC); } void process_gps_and_send_if_ready(JsonObject &root) { diff --git a/software/reference/design_reference/UI Design/rover_icon.svg b/software/reference/design_reference/UI Design/rover_icon.svg new file mode 100644 index 0000000..a77dc59 --- /dev/null +++ b/software/reference/design_reference/UI Design/rover_icon.svg @@ -0,0 +1,73 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py index 3f77309..53c0951 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -453,7 +453,7 @@ class OverlayImage(object): return self.display_image def load_rover_icon(self): - self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((50, 50)) + self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((60, 60)) def _draw_coordinate_text(self, latitude, longitude): location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude) @@ -475,7 +475,7 @@ class OverlayImage(object): y -= 25 rotated = self.indicator.copy() - rotated = rotated.rotate(angle, resample=PIL.Image.BICUBIC) + rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC) # rotated.save("rotated.png") self.big_image.paste(rotated, (x, y), rotated) if self.write_once: @@ -483,7 +483,8 @@ class OverlayImage(object): self.write_once = False def update(self, latitude, longitude): - + # self.left_x -= 50 + # self.upper_y -= 50 self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y)) self._draw_coordinate_text(latitude, longitude) diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py index dc1ebd4..ce1ccff 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -10,6 +10,10 @@ import numpy import logging import rospy +from tf import transformations +from scipy.interpolate import interp1d +import math +from sensor_msgs.msg import Imu # Custom Imports import RoverMap @@ -21,6 +25,7 @@ from sensor_msgs.msg import NavSatFix # put some stuff here later so you can remember GPS_POSITION_TOPIC = "/rover_odometry/fix" +IMU_DATA_TOPIC = "/rover_odometry/imu/data" class RoverMapCoordinator(QtCore.QThread): @@ -58,6 +63,17 @@ class RoverMapCoordinator(QtCore.QThread): self.latitude = None self.last_heading = 0 + self.imu_data = None + self.new_imu_data = False + + self.yaw = None + self.pitch = None + self.roll = None + + self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180]) + + self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received) + def run(self): self.logger.debug("Starting Map Coordinator Thread") self.pixmap_ready_signal.emit() # This gets us the loading map @@ -66,6 +82,10 @@ class RoverMapCoordinator(QtCore.QThread): self._map_setup() self.setup_map_flag = False else: + if self.new_imu_data: + self.calculate_euler_from_imu() + self.new_imu_data = False + self._get_map_image() self.msleep(30) @@ -158,3 +178,17 @@ class RoverMapCoordinator(QtCore.QThread): def gps_position_updated_callback(self, data): self.latitude = data.latitude self.longitude = data.longitude + + def on_imu_data_received(self, data): + self.imu_data = data + self.new_imu_data = True + + def calculate_euler_from_imu(self): + quat = ( + self.imu_data.orientation.x, + self.imu_data.orientation.y, + self.imu_data.orientation.z, + self.imu_data.orientation.w, + ) + self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat) + self.last_heading = self.euler_interpolator(self.yaw) % 360 diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py index 53fb305..a01861a 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -9,6 +9,11 @@ from time import time import PIL.Image from PIL.ImageQt import ImageQt from random import random +import rospy +from tf import transformations +from scipy.interpolate import interp1d +import math +from sensor_msgs.msg import Imu ##################################### # Global Variables @@ -17,6 +22,8 @@ THREAD_HERTZ = 20 ROTATION_SPEED_MODIFIER = 2.5 +IMU_DATA_TOPIC = "/rover_odometry/imu/data" + ##################################### # Controller Class Definition @@ -61,12 +68,27 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.last_current_heading_shown = 0 self.rotation_direction = 1 + self.imu_data = None + self.new_imu_data = False + + self.yaw = None + self.pitch = None + self.roll = None + + self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180]) + + self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received) + def run(self): self.on_heading_changed__slot(self.current_heading) while self.run_thread_flag: start_time = time() + if self.new_imu_data: + self.calculate_euler_from_imu() + self.new_imu_data = False + if self.current_heading_changed: self.update_heading_movement() self.current_heading_changed = False @@ -77,27 +99,27 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.msleep(max(int(self.wait_time - time_diff), 0)) + def calculate_euler_from_imu(self): + quat = ( + self.imu_data.orientation.x, + self.imu_data.orientation.y, + self.imu_data.orientation.z, + self.imu_data.orientation.w, + ) + self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat) + self.current_heading = self.euler_interpolator(self.yaw) % 360 + def rotate_compass_if_needed(self): - heading_difference = abs(int(self.shown_heading) - self.current_heading) - should_update = False - if heading_difference > ROTATION_SPEED_MODIFIER: - self.shown_heading += self.rotation_direction * ROTATION_SPEED_MODIFIER - self.shown_heading %= 360 - should_update = True - elif heading_difference != 0: - self.shown_heading = self.current_heading - should_update = True + self.current_heading_shown_rotation_angle = int(self.current_heading) - if should_update: - self.current_heading_shown_rotation_angle = int(self.shown_heading) + if self.current_heading_shown_rotation_angle != self.last_current_heading_shown: + new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC) + self.last_current_heading_shown = self.current_heading_shown_rotation_angle - if self.current_heading_shown_rotation_angle != self.last_current_heading_shown: - new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC) - self.last_current_heading_shown = self.current_heading_shown_rotation_angle - - self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image)) - self.show_compass_image__signal.emit() + self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image)) + self.show_compass_image__signal.emit() + self.heading_text_update_ready__signal.emit(str(self.current_heading_shown_rotation_angle) + "°") def update_heading_movement(self): current_minus_shown = (self.current_heading - self.shown_heading) % 360 @@ -126,6 +148,10 @@ class SpeedAndHeadingIndication(QtCore.QThread): def on_new_compass_image_ready__slot(self): self.heading_compass_label.setPixmap(self.compass_pixmap) + def on_imu_data_received(self, data): + self.imu_data = data + self.new_imu_data = True + def connect_signals_and_slots(self): self.show_compass_image__signal.connect(self.on_new_compass_image_ready__slot) self.heading_text_update_ready__signal.connect(self.heading_text_label.setText) diff --git a/software/ros_packages/ground_station/src/Resources/Images/rover.png b/software/ros_packages/ground_station/src/Resources/Images/rover.png index fa76398..72121ed 100644 Binary files a/software/ros_packages/ground_station/src/Resources/Images/rover.png and b/software/ros_packages/ground_station/src/Resources/Images/rover.png differ diff --git a/software/ros_packages/rover_camera/src/rover_camera.cpp b/software/ros_packages/rover_camera/src/rover_camera.cpp index 625c4c4..baca27e 100644 --- a/software/ros_packages/rover_camera/src/rover_camera.cpp +++ b/software/ros_packages/rover_camera/src/rover_camera.cpp @@ -26,6 +26,8 @@ public: node_handle->param("device_path", capture_device_path, std::string("/dev/video0")); node_handle->param("fps", fps, 30); + node_handle->param("upside_down", upside_down, false); + node_handle->param("large_image_width", large_image_width, 1280); node_handle->param("large_image_height", large_image_height, 720); node_handle->param("medium_image_width", medium_image_width, 640); @@ -97,30 +99,22 @@ public: } if(!image_large.empty()){ - bool large_ready_to_broadcast = false; - bool medium_ready_to_broadcast = false; - bool small_ready_to_broadcast = false; + if(upside_down){ + cv::flip(image_large, image_large, -1); + } if(broadcast_large_image){ large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg(); - large_ready_to_broadcast = true; - } - - if(broadcast_medium_image){ + large_image_publisher.publish(large_image_message); + }else if(broadcast_medium_image){ cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height)); medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg(); - medium_ready_to_broadcast = true; - } - - if(broadcast_small_image){ + medium_image_publisher.publish(medium_image_message); + }else if(broadcast_small_image){ cv::resize(image_large, image_small, cv::Size(small_image_width, small_image_height)); small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg(); - small_ready_to_broadcast = true; + small_image_publisher.publish(small_image_message); } - - if(large_ready_to_broadcast){ large_image_publisher.publish(large_image_message); }; - if(medium_ready_to_broadcast){ medium_image_publisher.publish(medium_image_message); }; - if(small_ready_to_broadcast){ small_image_publisher.publish(small_image_message); }; } ros::spinOnce(); @@ -152,6 +146,8 @@ private: ros::Rate *loop_rate; + bool upside_down; + int large_image_width; int large_image_height; int medium_image_width; diff --git a/software/ros_packages/rover_main/launch/rover/cameras.launch b/software/ros_packages/rover_main/launch/rover/cameras.launch index a5f99ad..52aae29 100644 --- a/software/ros_packages/rover_main/launch/rover/cameras.launch +++ b/software/ros_packages/rover_main/launch/rover/cameras.launch @@ -3,6 +3,7 @@ + @@ -17,7 +18,7 @@ - + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 511bd96..bba4c13 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -153,7 +153,8 @@ [ {name: "/rover_status/battery_status", compress: false, rate: 1.0}, - {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0} + {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}, + {name: "/rover_odometry/imu/data", compress: false, rate: 10.0}, ]