Ekf

src/localisation/launch/wheel_encoder.xml

@@ -1,6 +1,6 @@
 <launch>
   <arg name="name"/>
   <param name="rover_name" value="$(arg name)"/>
-  <node name="wheel_encoder" pkg="localisation" type="wheel_encoder.py" args="$(arg name)" respawn="true" >
-  </node>
+    <node name="wheel_encoder" pkg="localisation" type="wheel_encoder.py" args="$(arg name)" respawn="true" >
+    </node>
 </launch>

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,54 +105,74 @@ 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. 
         # To transform velocities into the odom frame we only need to rotate them according to the current robot orientation. 
         # 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 )
 

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...")

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)

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)

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)

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)

src/scoot/launch/scoot.launch

@@ -18,11 +18,9 @@
       <arg name="mode" value="$(arg mode)"/>
     </include>
 
-    <!--
     <include file="$(find localisation)/launch/wheel_encoder.xml">
       <arg name="name" value="$(arg name)"/>
     </include>
-    -->
 
     <!-- The stereo_image_proc node is a part of a ROS package that does stereo image processing -->
     <!-- Detailed documentation is found here: http://wiki.ros.org/stereo_image_proc -->
@@ -48,26 +46,24 @@
     </node>
 
     <node name="drive_controller" pkg="drive_controller" type="drive_controller.py"></node>
-
   </group>
 
-    <!--
   <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf_node_odom" clear_params="true">
     <param name="two_d_mode" value="true"/>
     <param name="publish_tf" value="true" />
     <param name="map_frame" value="map" />
     <param name="odom_frame" value="odom" />
 
-    <param name="base_link_frame" value="$(arg name)_tf/base_footprint"/>
+    <param name="base_link_frame" value="$(arg name)_base_footprint"/>
 
-    <param name="odom0" value="/small_scout_1/odom" />
+    <param name="odom0" value="$(arg name)/odom/wheel" />
     <rosparam param="odom0_config">[false, false, false,
                                     false, false, true,
                                     true, false, false,
                                     false, false, true,
                                     false, false, false]</rosparam>
 
-    <param name="imu0" value="/small_scout_1/imu" />
+    <param name="imu0" value="$(arg name)/imu" />
     <rosparam param="imu0_config">[false, false, false,
                                    false, false, true,
                                    false, false, false,
@@ -106,8 +102,7 @@
                                                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0,
                                                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1]</rosparam>
 
-    <remap from="/odometry/filtered" to="$(arg name)/odometry/filtered"/>
+    <remap from="/odometry/filtered" to="$(arg name)/odom"/>
   </node>
-   -->
 
 </launch>

src/scoot/launch/scoot.xml

@@ -33,8 +33,15 @@
     <param name="TURN_SPEED_FAST" value="2"/>
   </node>
 
-  <node name="odom" pkg="scoot" type="FakeLocalization.py" args="$(arg name)" respawn="true" >
+
+  <node name="fake_odom" pkg="scoot" type="FakeLocalization.py" args="$(arg name)" respawn="true" >
         <param name="publish_debug_topic" value="false" />
   </node>
 
+
+
+  <node name="odom" pkg="localisation" type="wheel_encoder.py" args="$(arg name)" respawn="true" output="log">
+        <param name="publish_debug_topic" value="false" />
+  </node>
+
 </launch>

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)

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

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)

src/scripts/src/deltaDelta.py

@@ -26,19 +26,19 @@ 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)
     delta_w_e = dist(wheel, ekf)
     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()