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 pan/tilt and tower combo node done. Other misc changes.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-14 20:54:50 -07:00
parent de93764849
commit 1b7f5437b9
10 changed files with 271 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,9 +2,6 @@
#include <ModbusRtu.h> #include <ModbusRtu.h>
#include <Adafruit_BNO055_t3.h> #include <Adafruit_BNO055_t3.h>
#include <ArduinoJson.h> #include <ArduinoJson.h>
#include "FastLED.h"
#include <NMEAGPS.h>
/* /*
Imu/data (Imu) Imu/data (Imu)
@@ -44,7 +41,7 @@ enum HARDWARE {
IMU_SDA = 18, IMU_SDA = 18,
IMU_SCL = 19, IMU_SCL = 19,
WS2812_DATA = 11, WHITE_LED_CONTROL = 11,
C02_SENSOR = A7, C02_SENSOR = A7,
MISC_PIN = A8, MISC_PIN = A8,
@@ -52,7 +49,16 @@ enum HARDWARE {
}; };
enum MODBUS_REGISTERS { 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 #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); 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"; const char baud115200[] = "PUBX,41,1,3,3,115200,0";
void setup() { void setup() {
// Debugging // Debugging
Serial.begin(9600); Serial.begin(9600);
// while (!Serial);
// Raw pin setup // Raw pin setup
setup_hardware(); setup_hardware();
@@ -134,6 +139,8 @@ void loop() {
set_leds(); set_leds();
send_imu_stream_line(root); send_imu_stream_line(root);
process_gps_and_send_if_ready(root); process_gps_and_send_if_ready(root);
process_white_led_command();
get_co2_data();
// Print JSON and newline // Print JSON and newline
root.printTo(GPS_IMU_STREAMING_PORT); root.printTo(GPS_IMU_STREAMING_PORT);
@@ -144,13 +151,14 @@ void loop() {
void setup_hardware() { void setup_hardware() {
// Setup pins as inputs / outputs // Setup pins as inputs / outputs
pinMode(HARDWARE::WS2812_DATA, OUTPUT); pinMode(HARDWARE::WHITE_LED_CONTROL, OUTPUT);
pinMode(HARDWARE::C02_SENSOR, INPUT); pinMode(HARDWARE::C02_SENSOR, INPUT);
pinMode(HARDWARE::MISC_PIN, OUTPUT); pinMode(HARDWARE::MISC_PIN, OUTPUT);
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
// Set default pin states // Set default pin states
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
// Set teensy to increased analog resolution // 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() { void poll_modbus() {
poll_state = slave.poll(modbus_data, num_modbus_registers); poll_state = slave.poll(modbus_data, num_modbus_registers);
communication_good = !slave.getTimeOutState(); communication_good = !slave.getTimeOutState();

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

@@ -91,7 +91,7 @@ class SensorCore(QtCore.QThread):
self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback) self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback)
self.camera_msg = CameraStatuses() self.camera_msg = CameraStatuses()
self.bogie_msg = BogieStatuses() self.bogie_msg = None # BogieStatuses()
self.FrSky_msg = FrSkyStatus() self.FrSky_msg = FrSkyStatus()
self.GPS_msg = GPSInfo() self.GPS_msg = GPSInfo()
self.jetson_msg = JetsonInfo() self.jetson_msg = JetsonInfo()
@@ -148,7 +148,7 @@ class SensorCore(QtCore.QThread):
self.frsky_stylesheet_change_ready__signal.emit(COLOR_GREEN) self.frsky_stylesheet_change_ready__signal.emit(COLOR_GREEN)
def __jetson_callback(self, data): 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: if data.jetson_CPU > 85:
self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_ORANGE) self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
@@ -157,7 +157,7 @@ class SensorCore(QtCore.QThread):
else: else:
self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_GREEN) 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: if data.jetson_RAM > 79:
self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_ORANGE) self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
@@ -166,7 +166,7 @@ class SensorCore(QtCore.QThread):
else: else:
self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_GREEN) 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: if data.jetson_GPU_temp > 64:
self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_ORANGE) self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
@@ -175,7 +175,7 @@ class SensorCore(QtCore.QThread):
else: else:
self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_GREEN) 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: if data.jetson_EMMC > 79:
self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_ORANGE) self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
@@ -207,7 +207,7 @@ class SensorCore(QtCore.QThread):
else: else:
self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED) 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): def __display_time(self):
time = QtCore.QTime.currentTime() time = QtCore.QTime.currentTime()

2
software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py
View File

@@ -13,7 +13,7 @@ import json
##################################### #####################################
# Global Variables # 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_IP = "192.168.1.20" # The channel only has to be set on the access point. The staion will adjust.
ACCESS_POINT_USER = "ubnt" ACCESS_POINT_USER = "ubnt"

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

@@ -264,7 +264,11 @@
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string> <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property> </property>
<property name="text"> <property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 EMMC&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string> <string>TX2 EMMC
-- %</string>
</property>
<property name="textFormat">
<enum>Qt::PlainText</enum>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -797,7 +801,11 @@ Disconnected</string>
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string> <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property> </property>
<property name="text"> <property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 CPU&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string> <string>TX2 CPU
-- %</string>
</property>
<property name="textFormat">
<enum>Qt::PlainText</enum>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -817,7 +825,11 @@ Disconnected</string>
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string> <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property> </property>
<property name="text"> <property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-size:9pt; font-weight:600;&quot;&gt;TX2 TEMP&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string> <string>TX2 Temp
-- °C</string>
</property>
<property name="textFormat">
<enum>Qt::PlainText</enum>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -963,7 +975,11 @@ Connected</string>
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string> <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property> </property>
<property name="text"> <property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 RAM&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string> <string>TX2 RAM
-- %</string>
</property>
<property name="textFormat">
<enum>Qt::PlainText</enum>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -1047,7 +1063,7 @@ Connected</string>
</property> </property>
<property name="text"> <property name="text">
<string>Connection Quality <string>Connection Quality
0.0%</string> -- %</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -1088,8 +1104,8 @@ Connected</string>
<enum>QFrame::NoFrame</enum> <enum>QFrame::NoFrame</enum>
</property> </property>
<property name="text"> <property name="text">
<string>RX Rate: 240.0 Mbps <string>RX Rate: --- Mbps
TX Rate: 300 Mbps</string> TX Rate: --- Mbps</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -1131,7 +1147,7 @@ TX Rate: 300 Mbps</string>
</property> </property>
<property name="text"> <property name="text">
<string>Successful Transmit <string>Successful Transmit
0.0%</string> -- %</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@@ -1172,8 +1188,8 @@ TX Rate: 300 Mbps</string>
<enum>QFrame::NoFrame</enum> <enum>QFrame::NoFrame</enum>
</property> </property>
<property name="text"> <property name="text">
<string>TX Latency: 10000 ms <string>TX Latency: ----- ms
RX Latency: 10000 ms</string> RX Latency: ----- ms</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>

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

@@ -119,7 +119,7 @@ class DriveControl(object):
self.get_drive_status() self.get_drive_status()
except Exception, error: except Exception, error:
print "Error occurred:", error pass
if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT: if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT:
print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting." print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting."

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

@@ -70,6 +70,8 @@ REGISTER_STATE_MAPPING = {
"DRIVE_VS_ARM": "SE_SWITCH" "DRIVE_VS_ARM": "SE_SWITCH"
} }
IRIS_LAST_SEEN_TIMEOUT = 1 # seconds
##################################### #####################################
# IrisController Class Definition # IrisController Class Definition
@@ -98,6 +100,8 @@ class IrisController(object):
self.iris_connected = False self.iris_connected = False
self.iris_last_seen_time = time()
self.run() self.run()
def __setup_minimalmodbus_for_485(self): def __setup_minimalmodbus_for_485(self):
@@ -114,11 +118,16 @@ class IrisController(object):
self.broadcast_drive_if_current_mode() self.broadcast_drive_if_current_mode()
self.broadcast_arm_if_current_mode() self.broadcast_arm_if_current_mode()
self.broadcast_iris_status() self.broadcast_iris_status()
self.iris_last_seen_time = time()
except Exception, error: except Exception, error:
print "Error occurred:", error print "Error occurred:", error
return 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 time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0)) sleep(max(self.wait_time - time_diff, 0))

145
software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py
View File

@@ -10,6 +10,8 @@ from time import time, sleep
import serial.rs485 import serial.rs485
import minimalmodbus import minimalmodbus
from std_msgs.msg import UInt8, UInt16
# Custom Imports # Custom Imports
from rover_control.msg import TowerPanTiltControlMessage from rover_control.msg import TowerPanTiltControlMessage
@@ -23,18 +25,21 @@ DEFAULT_BAUD = 115200
DEFAULT_INVERT = False 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 RX_DELAY = 0.02
TX_DELAY = 0.01 TX_DELAY = 0.02
DEFAULT_HERTZ = 20 DEFAULT_HERTZ = 20
MODBUS_REGISTERS = { PAN_TILT_MODBUS_REGISTERS = {
"CENTER_ALL": 0, "CENTER_ALL": 0,
"PAN_ADJUST_POSITIVE": 1, "PAN_ADJUST_POSITIVE": 1,
@@ -43,15 +48,32 @@ MODBUS_REGISTERS = {
"TILT_ADJUST_NEGATIVE": 4 "TILT_ADJUST_NEGATIVE": 4
} }
TOWER_MODBUS_REGISTERS = {
"LED_CONTROL": 0,
"CO2_READING_PPM": 1
}
PAN_TILT_CONTROL_DEFAULT_MESSAGE = [ PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
0, # No centering 0, # No centering
0, # No pan positive adjustment 0, # No pan positive adjustment
0, # No pan negative adjustment 0, # No pan negative adjustment
0, # No tilt positive 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.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD) 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.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.pan_tilt_node = None 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 = \ self.pan_tilt_control_subscriber = rospy.Subscriber(self.pan_tilt_control_subscriber_topic,
rospy.Subscriber(self.pan_tilt_control_subscriber_topic, TowerPanTiltControlMessage, self.pan_tilt_control_callback) 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.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() self.run()
@@ -90,63 +129,99 @@ class TowerPanTiltControl(object):
delay_before_rx=RX_DELAY, delay_before_rx=RX_DELAY,
delay_before_tx=TX_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): def run(self):
self.send_startup_centering_command() self.send_startup_centering_and_lights_off_command()
while not rospy.is_shutdown(): while not rospy.is_shutdown():
start_time = time() start_time = time()
try: try:
self.send_pan_tilt_control_message() self.send_pan_tilt_control_message()
self.pan_tilt_last_seen = time() self.modbus_nodes_seen_time = time()
except Exception, error: except Exception, error:
print "Error occurred:", error pass
# print "Error occurred:", error
if (time() - self.pan_tilt_last_seen) > PAN_TILT_LAST_SEEN_TIMEOUT: try:
print "Tower pan/tilt not seen for", PAN_TILT_LAST_SEEN_TIMEOUT, "seconds. Exiting." 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 return # Exit so respawn can take over
time_diff = time() - start_time time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0)) sleep(max(self.wait_time - time_diff, 0))
def connect_to_pan_tilt(self): def connect_to_pan_tilt_and_tower(self):
self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.node_id)) 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() self.__setup_minimalmodbus_for_485()
def send_startup_centering_command(self): def send_startup_centering_and_lights_off_command(self):
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) try:
registers[MODBUS_REGISTERS["CENTER_ALL"]] = 1 registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
self.pan_tilt_node.write_registers(0, registers) 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): 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 pan_tilt_control_message = self.pan_tilt_control_message # type: TowerPanTiltControlMessage
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) 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: 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: 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: 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: 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.pan_tilt_node.write_registers(0, registers)
self.new_control_message = False self.new_pan_tilt_control_message = False
else: else:
self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE) 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): def pan_tilt_control_callback(self, pan_tilt_control):
self.pan_tilt_control_message = 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__": if __name__ == "__main__":
TowerPanTiltControl() TowerPanTiltControl()

2
software/ros_packages/rover_main/launch/rover.launch
View File

@@ -13,6 +13,6 @@
<include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/> <include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/>
<!-- ########## Start Odometry Nodes ########## --> <!-- ########## Start Odometry Nodes ########## -->
<include file="$(find rover_main)/launch/rover/odometry.launch"/> <include file="$(find rover_main)/launch/rover/odometry2.launch"/>
</launch> </launch>

103
software/ros_packages/rover_main/launch/rover/odometry2.launch
View File

@@ -1,68 +1,71 @@
<launch> <launch>
<!-- https://github.com/vikiboy/AGV_Localization/blob/master/robot_localization/launch/ekf.launch --> <!-- https://github.com/vikiboy/AGV_Localization/blob/master/robot_localization/launch/ekf.launch -->
<!-- https://answers.ros.org/question/241222/fusing-imu-gps-with-robot_location-package/ --> <!-- https://answers.ros.org/question/241222/fusing-imu-gps-with-robot_location-package/ -->
<!-- ########## Processes GPS and IMU Messages ########## -->
<node name="gps_and_imu" pkg="rover_odometry" type="odometry.py" respawn="true" output="screen" >
<param name="gps_sentence_topic" value="/nmea_sentence"/>
<param name="imu_data_topic" value="/imu/data"/>
<!--<remap from="nmea_sentence" to="gps/sentence"/>-->
<!--<remap from="imu/data" to="/imu/data"/>-->
</node>
<!-- ########## Converts GPS Sentences to GPS Fix data ########## -->
<node name="navsat_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen">
<remap from="nmea_sentence" to="gps/sentence"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link imu 20"/> <group ns="rover_odometry">
<node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps 20"/> <!-- ########## Processes GPS and IMU Messages ########## -->
<node name="gps_and_imu" pkg="rover_odometry" type="odometry.py" respawn="true" output="screen" >
<!--<param name="gps_sentence_topic" value="/nmea_sentence"/>-->
<!--<param name="imu_data_topic" value="/imu/data"/>-->
<!--<remap from="nmea_sentence" to="gps/sentence"/>-->
<!--<remap from="imu/data" to="/imu/data"/>-->
</node>
<!-- ########## Converts GPS Sentences to GPS Fix data ########## -->
<node name="navsat_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen">
<remap from="nmea_sentence" to="gps/sentence"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true"> <!--<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link imu 20"/>-->
<param name="output_frame" value="odom"/> <!--<node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps 20"/>-->
<param name="frequency" value="20"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/> <!--<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">-->
<param name="odom_frame" value="odom"/> <!--<param name="output_frame" value="odom"/>-->
<param name="base_link_frame" value="base_link"/> <!--<param name="frequency" value="20"/>-->
<param name="world_frame" value="odom"/> <!--<param name="odom_used" value="true"/>-->
<!--<param name="imu_used" value="true"/>-->
<!--<param name="vo_used" value="false"/>-->
<!--<param name="sensor_timeout" value="0.1"/>-->
<!--<param name="two_d_mode" value="false"/>-->
<param name="odom0" value="/odometry/gps"/> <!--<param name="map_frame" value="map"/>-->
<param name="imu0" value="/imu/data"/> <!--<param name="odom_frame" value="odom"/>-->
<!--<param name="base_link_frame" value="base_link"/>-->
<!--<param name="world_frame" value="odom"/>-->
<rosparam param="odom0_config">[true, true, false, <!--<param name="odom0" value="odometry/gps"/>-->
false, false, false, <!--<param name="imu0" value="imu/data"/>-->
false , false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, <!--<rosparam param="odom0_config">[true, true, false,-->
true , true , true, <!--false, false, false,-->
false, false, false, <!--false , false, false,-->
true , true , true , <!--false, false, false,-->
true , true , true ]</rosparam> <!--false, false, false]</rosparam>-->
<param name="odom0_differential" value="false"/> <!--<rosparam param="imu0_config">[false, false, false,-->
<param name="imu0_differential" value="false"/> <!--true , true , true,-->
<!--false, false, false,-->
<!--true , true , true ,-->
<!--true , true , true ]</rosparam>-->
<param name="imu0_remove_gravitational_acceleration" value="true"/> <!--<param name="odom0_differential" value="false"/>-->
<!--<param name="imu0_differential" value="false"/>-->
<param name="odom0_relative" value="false"/> <!--<param name="imu0_remove_gravitational_acceleration" value="true"/>-->
<param name="imu0_relative" value="false"/>
<param name="print_diagnostics" value="true"/> <!--<param name="odom0_relative" value="false"/>-->
<!--<param name="imu0_relative" value="false"/>-->
<!-- ======== ADVANCED PARAMETERS ======== --> <!--<param name="print_diagnostics" value="true"/>-->
<param name="odom0_queue_size" value="2"/> <!--&lt;!&ndash; ======== ADVANCED PARAMETERS ======== &ndash;&gt;-->
<param name="imu0_queue_size" value="10"/>
</node> <!--<param name="odom0_queue_size" value="2"/>-->
<!--<param name="imu0_queue_size" value="10"/>-->
<node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen"> <!--</node>-->
<param name="broadcast_utm_transform" value="true"/>
</node> <!--<node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen">-->
<!--<param name="broadcast_utm_transform" value="true"/>-->
<!--</node>-->
</group>
</launch> </launch>

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

@@ -75,9 +75,11 @@ class Odometry(object):
imu = temp.get('imu', None) imu = temp.get('imu', None)
if gps: if gps:
print gps
self.broadcast_gps(gps) self.broadcast_gps(gps)
if imu: if imu:
# print imu
self.broadcast_imu(imu) self.broadcast_imu(imu)
def broadcast_gps(self, gps): def broadcast_gps(self, gps):