From d3a3a561e222df5d90880c7d8d677453ddc2a217 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sat, 7 Jul 2018 13:53:28 -0700 Subject: [PATCH] Initial odometry and localization working, though position grows without bound. It's a start though. --- .../UDEV_rules/99-rover-usb-serial.rules | 6 +- .../firmware/motor_driver/motor_driver.ino | 2 +- .../rover_main/launch/rover.launch | 3 + .../rover_main/launch/rover/odometry.launch | 71 +++++++++++++++++++ .../rover_odometry/src/odometry.py | 61 +++++++++------- 5 files changed, 115 insertions(+), 28 deletions(-) create mode 100644 software/ros_packages/rover_main/launch/rover/odometry.launch diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index b7dce53..1c19eb2 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -14,14 +14,14 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_0_3" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_1_0" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_1_1" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS_1_2" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyOdometry" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyTowerAndPanTilt" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_1_3" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_2_3" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART" ##### Mappings from OMSI / EXPO driving with individual adapters diff --git a/software/firmware/motor_driver/motor_driver.ino b/software/firmware/motor_driver/motor_driver.ino index 44a2933..6639da1 100644 --- a/software/firmware/motor_driver/motor_driver.ino +++ b/software/firmware/motor_driver/motor_driver.ino @@ -34,7 +34,7 @@ enum MODBUS_REGISTERS { }; ////////// Global Variables ////////// -const uint8_t node_id = 1; +const uint8_t node_id = 2; const uint8_t mobus_serial_port_number = 3; uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0}; diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index 4352782..e434d87 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -12,4 +12,7 @@ + + + diff --git a/software/ros_packages/rover_main/launch/rover/odometry.launch b/software/ros_packages/rover_main/launch/rover/odometry.launch new file mode 100644 index 0000000..b11160e --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/odometry.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [true, true, true, + 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 ab73a15..982b947 100755 --- a/software/ros_packages/rover_odometry/src/odometry.py +++ b/software/ros_packages/rover_odometry/src/odometry.py @@ -4,28 +4,26 @@ ##################################### # Python native imports import rospy - +import serial from time import time, sleep import json -import serial.rs485 - from nmea_msgs.msg import Sentence - +from sensor_msgs.msg import Imu ##################################### # Global Variables ##################################### NODE_NAME = "tower_odometry" -# DEFAULT_PORT = "/dev/rover/ttyOdometry" -DEFAULT_PORT = "/dev/ttyUSB0" +DEFAULT_PORT = "/dev/rover/ttyOdometry" +# DEFAULT_PORT = "/dev/ttyUSB0" DEFAULT_BAUD = 115200 # DEFAULT_GPS_SENTENCE_TOPIC = "gps/sentence" -DEFAULT_GPS_SENTENCE_TOPIC = "/nmea_sentence" -DEFAULT_DRIVE_CONTROL_STATUS_TOPIC = "drive_status/rear" +DEFAULT_GPS_SENTENCE_TOPIC = "gps/sentence" +DEFAULT_IMU_TOPIC = "imu/data" DEFAULT_HERTZ = 100 @@ -41,6 +39,7 @@ class Odometry(object): self.baud = rospy.get_param("~baud", DEFAULT_BAUD) self.gps_sentence_topic = rospy.get_param("~gps_sentence_topic", DEFAULT_GPS_SENTENCE_TOPIC) + self.imu_data_topic = rospy.get_param("~imu_data_topic", DEFAULT_IMU_TOPIC) self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) @@ -48,10 +47,10 @@ class Odometry(object): self.odom_serial.setRTS(0) self.sentence_publisher = rospy.Publisher(self.gps_sentence_topic, Sentence, queue_size=1) + self.imu_data_publisher = rospy.Publisher(self.imu_data_topic, Imu, queue_size=1) self.run() - def run(self): while not rospy.is_shutdown(): start_time = time() @@ -62,37 +61,51 @@ class Odometry(object): except Exception, error: print "Error occurred:", error - time_diff = time() - start_time sleep(max(self.wait_time - time_diff, 0)) def process_messages(self): if self.odom_serial.inWaiting(): - line = self.odom_serial.readline() # type:s - # print line + line = self.odom_serial.readline() temp = json.loads(line) + gps = temp.get('gps', None) + imu = temp.get('imu', None) if gps: self.broadcast_gps(gps) - imu = temp.get('imu', None) - if imu: - print imu - - - # if not line.startswith("ox"): - # print line, - # if line[0] == '$': - # print line, + self.broadcast_imu(imu) def broadcast_gps(self, gps): - # print gps - self.sentence_publisher.publish(Sentence(sentence=gps)) + message = Sentence() + message.header.frame_id = "gps" + message.header.stamp = rospy.get_rostime() + message.sentence = gps + self.sentence_publisher.publish(message) + def broadcast_imu(self, imu): + message = Imu() + message.header.frame_id = "imu" + message.header.stamp = rospy.get_rostime() + + message.orientation.x = imu["ox"] + message.orientation.y = imu["oy"] + message.orientation.z = imu["oz"] + message.orientation.w = imu["ow"] + + message.angular_velocity.x = imu["avx"] + message.angular_velocity.y = imu["avy"] + message.angular_velocity.z = imu["avz"] + + message.linear_acceleration.x = imu["lax"] + message.linear_acceleration.y = imu["lay"] + message.linear_acceleration.z = imu["laz"] + + self.imu_data_publisher.publish(message) if __name__ == "__main__": - Odometry() \ No newline at end of file + Odometry()