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

Got farther with mapping, heading indication, and firmware with cal for IMU. Need to figure out which direction IMU is in for ROS to work right.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-27 21:25:24 -07:00
parent 63454f22d1
commit 9d3969f9e4
9 changed files with 241 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
software/firmware/tower/tower.ino
View File

@@ -2,6 +2,8 @@
#include <ModbusRtu.h>
#include <Adafruit_BNO055_t3.h>
#include <ArduinoJson.h>
#include <utility/imumaths.h>
#include <EEPROM.h>
/*
Imu/data (Imu)
@@ -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) {

73
software/reference/design_reference/UI Design/rover_icon.svg Normal file
View File

@@ -0,0 +1,73 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="1000"
height="1000"
viewBox="0 0 1000 1000"
id="svg2"
version="1.1"
inkscape:version="0.91 r13725"
sodipodi:docname="rover_icon.svg"
inkscape:export-filename="/home/caperren/Github/Rover_2017_2018/software/ros_packages/ground_station/src/Resources/Images/rover.png"
inkscape:export-xdpi="400"
inkscape:export-ydpi="400">
<defs
id="defs4" />
<sodipodi:namedview
id="base"
pagecolor="#323232"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0"
inkscape:pageshadow="2"
inkscape:zoom="0.35"
inkscape:cx="338.99045"
inkscape:cy="775.33545"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
inkscape:window-width="2556"
inkscape:window-height="1401"
inkscape:window-x="1080"
inkscape:window-y="18"
inkscape:window-maximized="0"
units="px" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(0,-52.362161)">
<path
style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:26;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="M 499.63597,74.698266 319.16703,356.39733 c 37.73561,-64.04828 106.4946,-103.40171 180.8327,-103.49752 74.34377,0.10172 143.10405,39.46639 180.8327,103.52539 L 499.63487,74.698266 Z"
id="rect4196"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccccc" />
<circle
style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:30;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="path4157-6"
cx="500"
cy="552.36218"
r="229.86818" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 2.4 KiB

7
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
View File

@@ -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)

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

@@ -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

50
software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py
View File

@@ -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,20 +99,19 @@ 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
if should_update:
self.current_heading_shown_rotation_angle = int(self.shown_heading)
self.current_heading_shown_rotation_angle = int(self.current_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)
@@ -98,6 +119,7 @@ class SpeedAndHeadingIndication(QtCore.QThread):
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)

BIN
software/ros_packages/ground_station/src/Resources/Images/rover.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 251 KiB

After

Width:  |  Height:  |  Size: 220 KiB

28
software/ros_packages/rover_camera/src/rover_camera.cpp
View File

@@ -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;

3
software/ros_packages/rover_main/launch/rover/cameras.launch
View File

@@ -3,6 +3,7 @@
<!-- Start Undercarriage Camera -->
<node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
<param name="device_path" value="/dev/rover/camera_undercarriage"/>
<param name="upside_down" value="true"/>
</node>
<!--&lt;!&ndash; Start Main Navigation Camera &ndash;&gt;-->
@@ -17,7 +18,7 @@
<!--&lt;!&ndash; Start End Effector Camera &ndash;&gt;-->
<node name="end_effector" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 4" respawn="true" output="screen" >
<param name="is_rtsp_camera" value="True" />
<param name="is_rtsp_camera" value="true" />
<param name="device_path" value="rtsp://192.168.1.11" />
</node>
</group>

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

@@ -153,7 +153,8 @@
<rosparam param="topics">
[
{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},
]
</rosparam>
</node>