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

Initial odometry and localization working, though position grows without bound. It's a start though.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-07 13:53:28 -07:00
parent ddd4953495
commit d3a3a561e2
5 changed files with 115 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
software/environment/rover/UDEV_rules/99-rover-usb-serial.rules
View File

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

2
software/firmware/motor_driver/motor_driver.ino
View File

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

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

@@ -12,4 +12,7 @@
<include file="$(find rover_main)/launch/rover/topic_transport_senders.launch"/>
<include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/>
<!-- ########## Start Odometry Nodes ########## -->
<include file="$(find rover_main)/launch/rover/odometry.launch"/>
</launch>

71
software/ros_packages/rover_main/launch/rover/odometry.launch Normal file
View File

@@ -0,0 +1,71 @@
<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/ -->
<group ns="odometry">
<!-- ########## Processes GPS and IMU Messages ########## -->
<node name="gps_and_imu" pkg="rover_odometry" type="odometry.py" respawn="true" output="screen" />
<!-- ########## 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"/>
<remap from="fix" to="gps/fix"/>
</node>
<!-- ########## Performs a tranform on the GPS and IMU frames so odom can use it ########## -->
<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link imu 20"/>
<node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps 20"/>
<!-- ########## Main Odometry Localization Node ########## -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<remap from="/gpx/fix" to="gps/fix"/>
<param name="output_frame" value="odom"/>
<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"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="odom0" value="gps"/>
<param name="imu0" value="imu/data"/>
<rosparam param="odom0_config">[true, true, true,
false, false, false,
false , false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true , true , true,
false, false, false,
true , true , true ,
true , true , true ]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
</node>
<node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen">
<param name="broadcast_utm_transform" value="true"/>
</node>
</group>
</launch>

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

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