diff --git a/src/localisation/launch/wheel_encoder.xml b/src/localisation/launch/wheel_encoder.xml
index f45a60e..a29235d 100644
--- a/src/localisation/launch/wheel_encoder.xml
+++ b/src/localisation/launch/wheel_encoder.xml
@@ -1,6 +1,6 @@
-
-
+
+
diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py
index e06daf3..5eb166a 100755
--- a/src/localisation/src/wheel_encoder.py
+++ b/src/localisation/src/wheel_encoder.py
@@ -2,6 +2,7 @@
# Generates odometry from wheel poses
+import math
import sys
import tf.transformations as transform
from sensor_msgs.msg import JointState, Imu
@@ -17,9 +18,13 @@ class WheelEncoder:
def __init__(self):
self.name = rospy.get_param('rover_name', default='small_scout_1')
+ rospy.loginfo("Subscribing to /{}/imu".format(self.name))
+ rospy.loginfo("Subscribing to /{}/joint_states".format(self.name))
+ rospy.loginfo("Publishing to /{}/odom/wheel".format(self.name))
+
rospy.Subscriber("/{}/imu".format(self.name), Imu, self.imuCallback)
rospy.Subscriber("/{}/joint_states".format(self.name), JointState, self.jointStatesCallback)
- self.odom_pub = rospy.Publisher("/{}/odom".format(self.name), Odometry, queue_size=50)
+ self.odom_pub = rospy.Publisher("/{}/odom/wheel".format(self.name), Odometry, queue_size=50)
self.odom_broadcaster = tf.TransformBroadcaster()
self.last_time = rospy.Time.now()
self.x = 0
@@ -29,9 +34,12 @@ def __init__(self):
self.previous_front_right_wheel_angle = 0
self.previous_back_left_wheel_angle = 0
self.previous_back_right_wheel_angle = 0
- self.wheel_radius = 0.27 # meters
- self.track_width = 1.75 # meters
+ #self.wheel_radius = 0.27 # meters
+ self.wheel_radius = 0.17 # meters
+ # self.track_width = 1.75 # meters
+ self.track_width = 1 # meters
self.sample_rate = 1
+ self.msg_min_delta = 0.5 # seconds <--- Changing this from 0.1 spoils the accuracy. That should not be the case!
self.message_count = 0
def imuCallback(self, data):
@@ -41,7 +49,7 @@ def imuCallback(self, data):
data.orientation.z,
data.orientation.w)
roll, pitch, yaw = transform.euler_from_quaternion(q) # in [-pi, pi]
- self.theta = yaw
+ self.theta = yaw + 0.78 # rotate 45 degrees
def jointStatesCallback(self, data):
@@ -51,15 +59,20 @@ def jointStatesCallback(self, data):
if self.message_count % self.sample_rate != 0:
return
-
- back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ]
- back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ]
- front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ]
- front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ]
+
+ # Divide by 180 to get radians
+ back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ]*math.pi/180
+ back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ]*math.pi/180
+ front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ]*math.pi/180
+ front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ]*math.pi/180
current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec()
-
+
+ # Check that sufficient time has passed to avoid duplicate time stamps
+ if dt < self.msg_min_delta: # Specify the minimum time between messages for function execution (to prevent timestamp warning killing the repl)
+ return
+
### Calculate angular velocity ###
# Angular displacement in radians - could use all four wheels, start with just two
@@ -92,10 +105,16 @@ def jointStatesCallback(self, data):
v_left = omega_left * self.wheel_radius
v_right = omega_right * self.wheel_radius
- v_rx = ( v_right + v_left ) /2
+ rospy.loginfo("Wheel Radius: " + str(self.wheel_radius))
+
+ v_rx = ( v_right + v_left ) / 2.0
v_ry = 0
+
+ rospy.loginfo("Velocity: " + str(v_rx) + " m/s")
+ rospy.loginfo("Yaw: " + str(self.theta))
+
# We increase the track width by a factor of 4 to match empirical tests
- v_rtheta = ( v_right - v_left ) / self.track_width
+ v_rtheta = ( v_right - v_left ) / 2*self.track_width
# Velocities and pose in the odom frame
# The velocities expressed in the robot base frame can be transformed into the odom frame.
@@ -103,43 +122,57 @@ def jointStatesCallback(self, data):
# We are operating in the x-y plane, we ignore altitude - which will result in errors as the robot climbs up and down.
# Integrate the velocities over time (multiply by the time differential dt and sum)
- v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt
- v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt
- #v_wx = v_rx*cos(self.theta+v_rtheta/2)
- #v_wy = v_rx*sin(self.theta+v_rtheta/2)
+ #v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt
+ #v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt
+
+ # We know our heading from the imu - no need to calculate it from wheel velocities
+ v_wx = v_rx*cos(self.theta)
+ v_wy = v_rx*sin(self.theta)
- v_wtheta = v_rtheta * dt
+ rospy.loginfo("X displacement: " + str(v_wx) + " m")
+ rospy.loginfo("Y displacement: " + str(v_wy) + " m")
+ rospy.loginfo("Delta_t: " + str(dt) + " s")
+
+ rospy.loginfo("Back Left Wheel Angle: " + str(back_left_wheel_angle))
+
+ #v_wtheta = v_rtheta * dt
- self.x += v_wx
- self.y += v_wy
+ self.x += v_wx*2*math.pi # I don't understand why this 2pi factor is needed
+ self.y += v_wy*2*math.pi
+
+ rospy.loginfo("X coord: " + str(self.x))
+ rospy.loginfo("Y coord: " + str(self.y))
# Replaced with IMU theta
#self.theta += v_wtheta
# Composing your odometry message
-
+
# The odometry message contains pose and twist information. The pose represents your current robot pose in the the odom frame (x, y, theta).
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.theta)
+ # Check if the timestamp will be different
+
+
# first, we'll publish the transform over tf
- self.odom_broadcaster.sendTransform(
- (self.x, self.y, 0.),
- odom_quat,
- current_time,
- "{}_tf/base_footprint".format(self.name),
- "odom"
- )
-
+ #self.odom_broadcaster.sendTransform(
+ # (self.x, self.y, 0.),
+ # odom_quat,
+ # current_time,
+ # "{}_/base_footprint".format(self.name),
+ # "odom"
+ #)
+
# And link up the map
- self.odom_broadcaster.sendTransform(
- (0, 0, 0),
- tf.transformations.quaternion_from_euler(0, 0, 0),
- current_time,
- "odom",
- "map"
- )
+ #self.odom_broadcaster.sendTransform(
+ # (0, 0, 0),
+ # tf.transformations.quaternion_from_euler(0, 0, 0),
+ # current_time,
+ # "odom",
+ # "map"
+ #)
# next, we'll publish the odometry message over ROS
odom = Odometry()
@@ -150,7 +183,7 @@ def jointStatesCallback(self, data):
odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*odom_quat))
# set the velocity
- odom.child_frame_id = "{}_tf/base_footprint".format(self.name)
+ odom.child_frame_id = "{}_base_footprint".format(self.name)
odom.twist.twist = Twist(Vector3(v_rx, v_ry, 0), Vector3(0, 0, v_rtheta))
# publish the message
@@ -163,15 +196,17 @@ def jointStatesCallback(self, data):
self.previous_front_left_wheel_angle = front_left_wheel_angle
self.previous_front_right_wheel_angle = front_right_wheel_angle
self.previous_back_left_wheel_angle = back_left_wheel_angle
- self.previous_back_right_wheel_angle = back_right_wheel_angle
+ self.previous_back_right_wheel_angle = back_right_wheel_angle
def shutdownHandler():
- print("Wheel encoder shutting down.")
+ rospy.loginfo("Wheel encoder shutting down.")
if __name__ == '__main__':
rospy.init_node('wheel_encoder', anonymous=True)
+ rospy.loginfo("Wheel encoder node started")
+
# Register shutdown handler (includes ctrl-c handling)
rospy.on_shutdown( shutdownHandler )
diff --git a/src/navigation/src/bug_nav.py b/src/navigation/src/bug_nav.py
index 4fdbf1b..77b23f5 100755
--- a/src/navigation/src/bug_nav.py
+++ b/src/navigation/src/bug_nav.py
@@ -234,7 +234,7 @@ def at( self ):
def init_listener():
- rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, estimated_location_handler)
+ rospy.Subscriber('/small_scout_1/odom', Odometry, estimated_location_handler)
rospy.Subscriber('/small_scout_1/laser/scan', LaserScan, lidar_handler)
rospy.Subscriber("/small_scout_1/imu", Imu, imu_handler)
rospy.Subscriber('/small_scout_1/obstacle', Obstacles, obstacle_handler)
@@ -666,8 +666,8 @@ def bug_algorithm(tx, ty, bug_type):
global stats_printed
global total_time_start
- print "Waiting for location data on '/small_scout_1/odom/filtered...'"
- rospy.wait_for_message('/small_scout_1/odom/filtered', Odometry,)
+ print "Waiting for location data on '/small_scout_1/odom...'"
+ rospy.wait_for_message('/small_scout_1/odom', Odometry,)
print "... received."
print("Waiting for break service...")
diff --git a/src/object_detection/src/Cubesat_Detection.py b/src/object_detection/src/Cubesat_Detection.py
index 8961d61..b500213 100755
--- a/src/object_detection/src/Cubesat_Detection.py
+++ b/src/object_detection/src/Cubesat_Detection.py
@@ -35,7 +35,7 @@ def __init__(self):
self.bridge = CvBridge()
self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback)
- # self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback)
+ # self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback)
self.left_camera_subscriber = rospy.Subscriber('/small_scout_1/camera/left/image_raw', Image, self.cam_callback)
self.cubesat_detection_image_left_publisher = rospy.Publisher('/small_scout_1/cubesat_detections/image/left', Image, queue_size=100)
diff --git a/src/object_detection/src/Leg_Detection.py b/src/object_detection/src/Leg_Detection.py
index d29d73a..bdec539 100644
--- a/src/object_detection/src/Leg_Detection.py
+++ b/src/object_detection/src/Leg_Detection.py
@@ -34,7 +34,7 @@ def __init__(self):
self.bridge = CvBridge()
self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback)
- self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback)
+ self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback)
self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image)
self.leg_detection_image_left_publisher = rospy.Publisher('/small_scout_1/leg_detections/image/left', Image, queue_size=100)
diff --git a/src/object_detection/src/Logo_Detection.py b/src/object_detection/src/Logo_Detection.py
index fa28721..fbc2f90 100755
--- a/src/object_detection/src/Logo_Detection.py
+++ b/src/object_detection/src/Logo_Detection.py
@@ -33,7 +33,7 @@ def __init__(self):
self.bridge = CvBridge()
self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback)
- self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback)
+ self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback)
self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image)
self.logo_detection_image_left_publisher = rospy.Publisher('/small_scout_1/logo_detections/image/left', Image, queue_size=100)
diff --git a/src/object_detection/src/Volatile_Detection.py b/src/object_detection/src/Volatile_Detection.py
index 8f115ff..c8f8939 100755
--- a/src/object_detection/src/Volatile_Detection.py
+++ b/src/object_detection/src/Volatile_Detection.py
@@ -32,7 +32,7 @@ def __init__(self):
self.bridge = CvBridge()
self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback)
- self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback)
+ self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback)
self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image)
self.volatile_detection_image_left_publisher = rospy.Publisher('/small_scout_1/volatile_detections/image/left', Image, queue_size=100)
diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch
index 4b1c044..c4adaa2 100644
--- a/src/scoot/launch/scoot.launch
+++ b/src/scoot/launch/scoot.launch
@@ -18,11 +18,9 @@
-
@@ -48,26 +46,24 @@
-
-
diff --git a/src/scoot/launch/scoot.xml b/src/scoot/launch/scoot.xml
index 8f231b9..d10ed22 100644
--- a/src/scoot/launch/scoot.xml
+++ b/src/scoot/launch/scoot.xml
@@ -33,8 +33,15 @@
-
+
+
+
+
+
+
+
+
diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py
index 0ae287b..5ef5376 100755
--- a/src/scoot/src/Driver.py
+++ b/src/scoot/src/Driver.py
@@ -106,7 +106,7 @@ def __init__(self):
# Subscribers
rospy.Subscriber('/' + self.rover_name + '/obstacle', Obstacles, self._obstacle)
rospy.Subscriber('/' + self.rover_name + '/detections', Detection, self._vision)
- rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom)
+ rospy.Subscriber('/' + self.rover_name + '/odom', Odometry, self._odom)
# Services
self.control = rospy.Service('control', Core, self._control)
diff --git a/src/scoot/src/FakeLocalization.py b/src/scoot/src/FakeLocalization.py
index 33a018f..9191f86 100755
--- a/src/scoot/src/FakeLocalization.py
+++ b/src/scoot/src/FakeLocalization.py
@@ -7,7 +7,7 @@
def publish_fake_odom():
rover_name = rospy.get_param('rover_name', default='small_scout_1')
- pub = rospy.Publisher('/' + rover_name + '/odometry/filtered', Odometry, queue_size=10)
+ pub = rospy.Publisher('/' + rover_name + '/odom/fake', Odometry, queue_size=10)
rospy.init_node('odom', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py
index 0fb7d06..e68ffb3 100755
--- a/src/scoot/src/Scoot.py
+++ b/src/scoot/src/Scoot.py
@@ -259,7 +259,7 @@ def start(self, **kwargs):
rospy.loginfo("Done waiting for rover specific services")
# Subscribe to topics.
- rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom)
+ rospy.Subscriber('/' + self.rover_name + '/odom', Odometry, self._odom)
rospy.Subscriber('/' + self.rover_name + '/system_monitor', msg.SystemMonitorMsg, self._health)
rospy.Subscriber('/' + self.rover_name + '/joint_states', JointState, self._joint_states)
rospy.Subscriber('/srcp2/score', msg.ScoreMsg, self._score)
diff --git a/src/scripts/src/deltaDelta.py b/src/scripts/src/deltaDelta.py
index 22395ca..b0333b3 100755
--- a/src/scripts/src/deltaDelta.py
+++ b/src/scripts/src/deltaDelta.py
@@ -26,9 +26,9 @@ def run():
global wheel
global fake
global ekf
- rospy.Subscriber('/small_scout_1/odom/', Odometry, _wheel)
- rospy.Subscriber('/small_scout_1/odom/filtered', Odometry, _fake)
- rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, _ekf)
+ rospy.Subscriber('/small_scout_1/odom/wheel', Odometry, _wheel)
+ rospy.Subscriber('/small_scout_1/odom/fake', Odometry, _fake)
+ rospy.Subscriber('/small_scout_1/odom', Odometry, _ekf)
rospy.init_node("odom_dif_test")
rospy.sleep(0.5)
delta_f_e = dist(fake, ekf)
@@ -36,9 +36,9 @@ def run():
delta_f_w = dist(fake, wheel)
while not rospy.is_shutdown():
rospy.sleep(0.3)
- print "delta_f_e:", delta_f_e - dist(fake, ekf)
- print "delta_w_e:", delta_w_e - dist(wheel, ekf)
- print "delta_f_w:", delta_f_w - dist(fake, wheel)
+ print("delta_f_e:", delta_f_e - dist(fake, ekf))
+ print("delta_w_e:", delta_w_e - dist(wheel, ekf))
+ print("delta_f_w:", delta_f_w - dist(fake, wheel))
if __name__ == '__main__':
run()