added wheel's odometry with ekf fusion example

Author: rasheeddo

arduino/hall_odom_2wheels_PCINT/hall_odom_2wheels_PCINT.ino

@@ -3,9 +3,13 @@
 byte last_CH1_state, last_CH2_state, last_CH3_state, last_CH4_state;
 unsigned long counter_1, counter_2, counter_3, counter_4, current_time;
 unsigned long last_time_fall_1, last_time_fall_2, last_time_fall_3, last_time_fall_4;
+unsigned long last_stamped_L, last_stamped_R;
 const byte ledPin = 13;
 volatile byte led_state = LOW;
 int magnets = 8;
+float R_wheel = 0.15;
+float circular_dist = 0.9424777961;  // this came from 2*pi*R_wheel
+float move_in_one_trig = 0.1178125;
 /////////////////////////////////
 /////////// Left wheel //////////
 /////////////////////////////////
@@ -19,6 +23,10 @@ volatile float prev_rpm_L = 0.0;
 float time_passed_L;
 float first_count_time_L;
 float hall_timeout_L;
+float out_rpm_L;
+int counter_L = 0;
+float VL = 0.0;
+float out_VL;
 
 //////////////////////////////////
 /////////// Right wheel //////////
@@ -33,13 +41,15 @@ volatile float prev_rpm_R = 0.0;
 float time_passed_R;
 float first_count_time_R;
 float hall_timeout_R;
-float out_rpm_L;
 float out_rpm_R;
+int counter_R = 0;
+float VR = 0.0;
+float out_VR;
 
 ////////////////////////////////////////////
 /////////////// JSON object ////////////////
 ////////////////////////////////////////////
-const size_t CAPACITY = JSON_OBJECT_SIZE(4);
+const size_t CAPACITY = JSON_OBJECT_SIZE(6);
 StaticJsonDocument<CAPACITY> doc;
 
 void setup() {
@@ -65,14 +75,18 @@ void loop() {
 
   if (fwd_dir_L == 0){
     out_rpm_L = -rpm_L;
+    out_VL = -VL;
   } else{
     out_rpm_L = rpm_L;
+    out_VL = VL;
   }
 
   if (fwd_dir_R == 0){
     out_rpm_R = -rpm_R;
+    out_VR = -VR;
   } else{
     out_rpm_R = rpm_R;
+    out_VR = VR;
   }
 
   digitalWrite(ledPin, led_state);
@@ -84,8 +98,10 @@ void loop() {
 
   doc["rpm_L"] = out_rpm_L;
   doc["rpm_R"] = out_rpm_R;
-  doc["count_L"] = count_L;
-  doc["count_R"] = count_R;
+  doc["counter_L"] = counter_L;
+  doc["counter_R"] = counter_R;
+  doc["VL"] = out_VL;
+  doc["VR"] = out_VR;
   serializeJson(doc, Serial);
   Serial.println();
 
@@ -98,12 +114,14 @@ void loop() {
     rpm_L = 0.0;
     prev_rpm_L = 0.0;
     count_L = 0;
+    VL = 0.0;
   }
 
   if (hall_timeout_R > 1.2){
     rpm_R = 0.0;
     prev_rpm_R = 0.0;
     count_R = 0;
+    VR = 0.0;
   }
 
 
@@ -130,7 +148,7 @@ ISR(PCINT0_vect){
      led_state = !led_state;
      last_time_fall_1 = current_time;
      ch1_fall = true;
-     
+     VL = move_in_one_trig/((current_time - last_stamped_L)/1000000.0);
      //////////////////////////
      /////// rpm_L cal //////////
      //////////////////////////
@@ -143,11 +161,14 @@ ISR(PCINT0_vect){
       // Calculate rpm_L according to how many magnets it passed...
       rpm_L = (float(count_L-1)/magnets)/(time_passed_L/60.0);
       prev_rpm_L = rpm_L;
-     } else if (count_L >= (magnets+1)){
-      count_L = 0;
+        if (count_L == (magnets+1)){
+          count_L = 0;
+        }
      } else{
       rpm_L = prev_rpm_L;
      }
+
+     last_stamped_L = micros();
 
   }
 
@@ -183,7 +204,8 @@ ISR(PCINT0_vect){
      //led_state = !led_state;
      last_time_fall_3 = current_time;
      ch3_fall = true;
-     
+
+     VR = move_in_one_trig/((current_time - last_stamped_R)/1000000.0);
      //////////////////////////
      /////// rpm_R cal //////////
      //////////////////////////
@@ -196,11 +218,14 @@ ISR(PCINT0_vect){
       // Calculate rpm_L according to how many magnets it passed...
       rpm_R = (float(count_R-1)/magnets)/(time_passed_R/60.0);
       prev_rpm_R = rpm_R;
-     } else if (count_R >= (magnets+1)){
-      count_R = 0;
+      if (count_R >= (magnets+1)){
+        count_R = 0;
+        }
      } else{
       rpm_R = prev_rpm_R;
      }
+
+     last_stamped_R = micros();
 
   }
 
@@ -229,9 +254,11 @@ ISR(PCINT0_vect){
   if (ch1_fall && ch2_fall){
     if (last_time_fall_1 < last_time_fall_2){
      fwd_dir_L = true;
+     counter_L += 1;
     }
     else{
       fwd_dir_L = false;
+      counter_L -= 1;
     }
     ch1_fall = false;
     ch2_fall = false;
@@ -245,9 +272,11 @@ ISR(PCINT0_vect){
   if (ch3_fall && ch4_fall){
     if (last_time_fall_3 < last_time_fall_4){
      fwd_dir_R = true;
+     counter_R += 1;
     }
     else{
       fwd_dir_R = false;
+      counter_R -= 1;
     }
     ch3_fall = false;
     ch4_fall = false;

docs/wheels_hall_sensor.md

@@ -52,4 +52,30 @@ The magnet ring of this new design uses simple 1mm double-sided tape to attach o
 
 Arduino's firmware and `jmoab-ros-wheels-rpm` are same.
 
-Please check on [this video](https://youtu.be/tCoUMoUfn2s), of how to operate it.
+Please check on [this video](https://youtu.be/tCoUMoUfn2s), of how to operate it.
+
+## Nodes
+
+Once you started `jmoab-ros-wheels-rpm.py`, it's publishing
+
+- `wheels_rpm` as array of [left_rpm, right_rpm]
+
+- `wheels_speed` as array of [VL, VR], linear velocity of the wheels
+
+- `odom` as Odometry topic, this is quite poor because our sensor is only 8 ticks per revolution.
+
+To improve the odom topic, we need [robot_pose_ekf package](http://wiki.ros.org/robot_pose_ekf) to fuse the sensor of wheel's odometry with IMU sensors.
+
+I changed [this](https://github.com/ros-planning/robot_pose_ekf/blob/fd6cef32b447e8b344a1111373e515aa2f8bfc50/robot_pose_ekf.launch#L5) `base_footprint` to `base_link`, and disable [this](https://github.com/ros-planning/robot_pose_ekf/blob/fd6cef32b447e8b344a1111373e515aa2f8bfc50/robot_pose_ekf.launch#L10) `vo_used` to false.
+
+You will need to run
+
+	roslaunch jmoab-ros jmoab-ros-ekf-odom-test.launch
+
+to start the nodes, and also
+
+	roslaunch robot_pose_ekf robot_pose_ekf.launch
+
+You will get `ekf_odom` topic for a fused odom. Please check on this [demo video](https://www.youtube.com/watch?v=SlqJzMmxYxU&ab_channel=stepbystep-robotics).
+
+The green arrow is `ekf_odom`, red arrow is original `odom`.

example/ekf_odom_generate.py

@@ -0,0 +1,63 @@
+#! /usr/bin/env python
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import numpy as np
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+
+br = tf2_ros.TransformBroadcaster()
+t = TransformStamped()
+ekf_odom_pub = rospy.Publisher("/ekf_odom", PoseWithCovarianceStamped, queue_size=10)
+ekf_odom_msg = PoseWithCovarianceStamped()
+
+def callback(data):
+
+	# print(data)
+
+	x = data.pose.pose.position.x
+	y = data.pose.pose.position.y
+	z = data.pose.pose.position.z
+
+	qw = data.pose.pose.orientation.w
+	qx = data.pose.pose.orientation.x
+	qy = data.pose.pose.orientation.y
+	qz = data.pose.pose.orientation.z
+
+	# construct tf
+	t.header.frame_id = "ekf_odom" 
+	t.header.stamp = rospy.Time.now()
+	t.child_frame_id = "base_link_ekf"	#"base_footprint"	#"base_link"
+	t.transform.translation.x = x
+	t.transform.translation.y = y
+	t.transform.translation.z = z
+
+	t.transform.rotation.x = qx
+	t.transform.rotation.y = qy
+	t.transform.rotation.z = qz
+	t.transform.rotation.w = qw
+	br.sendTransform(t)
+
+	ekf_odom_msg.header.stamp = rospy.Time.now()
+	ekf_odom_msg.header.frame_id = "ekf_odom"
+	ekf_odom_msg.pose.pose.position.x = x
+	ekf_odom_msg.pose.pose.position.y = y
+	ekf_odom_msg.pose.pose.position.z = z
+	ekf_odom_msg.pose.pose.orientation.x = qx
+	ekf_odom_msg.pose.pose.orientation.y = qy
+	ekf_odom_msg.pose.pose.orientation.z = qz
+	ekf_odom_msg.pose.pose.orientation.w = qw
+
+	ekf_odom_msg.pose.covariance = data.pose.covariance
+
+	ekf_odom_pub.publish(ekf_odom_msg)
+
+
+def listener():
+
+	rospy.init_node('generate_tf_for_ekf_odom', anonymous=True)
+	rospy.Subscriber("/robot_pose_ekf/odom_combined", PoseWithCovarianceStamped, callback)
+
+	rospy.spin()
+
+if __name__ == '__main__':
+	listener()

launch/jmoab-ros-ekf-odom-test.launch

@@ -0,0 +1,19 @@
+<launch>
+
+	<!-- We create a dummy map frame to let ekf_odom and odom have same references-->
+    <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="1.0 0.0 0.0 0.0 0.0 0.0 1.0 /map /odom 100" />
+    <node pkg="tf" type="static_transform_publisher" name="map_to_ekf_odom" args="1.0 0.0 0.0 0.0 0.0 0.0 1.0 /map /ekf_odom 100" />
+    <remap from="/jmoab_imu_raw" to="/imu_data" />
+
+	<node pkg="jmoab-ros" name="jmoab_ros_atcart_node" type="jmoab-ros-atcart.py" output="screen"/>
+    <node pkg="jmoab-ros" name="jmoab_ros_imu_node" type="jmoab-ros-imu.py" output="screen"/>
+
+    <!-- For hall sensor wheel odometry-->
+    <node pkg="jmoab-ros" name="jmoab_ros_wheels_rpm_node" type="jmoab-ros-wheels-rpm.py" args="/dev/ttyUSB0" output="screen"/>
+    <node pkg="jmoab-ros" name="ekf_odom_generate" type="ekf_odom_generate.py" output="screen"/>
+
+    <!-- For robot_pose_ekf-->
+    <!-- <include file="$(find robot_pose_ekf)/robot_pose_ekf.launch" /> -->
+
+
+</launch>

src/jmoab-ros-compass-simulation.py

@@ -255,7 +255,7 @@ def kalman_filter(self, measure, prev_state_est, prev_error_est):
 		################
 		## prediction ##
 		################
-		self.error_est = cur_error_est + 0.01 # 0.001 is process noise, could be removed
+		self.error_est = cur_error_est + 0.01 # 0.01 is process noise, could help when hdg_offset is fluctuating during time
 		# hdg_offset is not dynamic behaviour, so predicted state is constant as estimated state
 		self.state_predict = cur_state_est