Odometry readjustement

src/assurancetourix/assurancetourix/include/assurancetourix.hpp

@@ -114,6 +114,10 @@ class Assurancetourix : public rclcpp::Node {
   std::string base_frame, header_frame_id, topic_for_gradient_layer, side, allies_positions_topic;
 
   rclcpp::Client<transformix_msgs::srv::TransformixParametersTransformStamped>::SharedPtr transformClient;
+
+#ifdef SIMULATION
+  rclcpp::Time get_sim_time(std::shared_ptr<webots::Robot> wb_robot);
+#endif /* SIMULATION */
 };
 
 #endif /* ONBOARD_VISION_NODE_HPP */

src/assurancetourix/assurancetourix/src/assurancetourix.cpp

@@ -135,10 +135,17 @@ void Assurancetourix::simulation_marker_callback() {
     x = wb_supervisor->getFromDef(robot)->getPosition()[0];
     y = 2 - wb_supervisor->getFromDef(robot)->getPosition()[2];
 
-    webots_marker.header.stamp = this->get_clock()->now();
+    webots_marker.header.stamp = get_sim_time(wb_supervisor);
+    tf2::Quaternion q;
+    q.setRPY(0, 0, theta);
+
+    webots_marker.pose.orientation.x = q.x();
+    webots_marker.pose.orientation.y = q.y();
+    webots_marker.pose.orientation.z = q.z();
+    webots_marker.pose.orientation.w = q.w();
+
     webots_marker.pose.position.x = x;
     webots_marker.pose.position.y = y;
-    webots_marker.pose.orientation.z = theta;
 
     webots_marker.text = robot;
     webots_marker.id = id;
@@ -147,7 +154,10 @@ void Assurancetourix::simulation_marker_callback() {
     id++;
   }
   id = 6;
-  webots_marker.pose.orientation.z = 0;
+  webots_marker.pose.orientation.x = 0.0;
+  webots_marker.pose.orientation.y = 0.0;
+  webots_marker.pose.orientation.z = 0.0;
+  webots_marker.pose.orientation.w = 1.0;
   webots_marker.header.stamp = this->get_clock()->now();
   if (comeback) {
     comeback_x += 0.1;
@@ -477,6 +487,14 @@ void Assurancetourix::_anotate_image(Mat img) {
   }
 }
 
+#ifdef SIMULATION
+rclcpp::Time Assurancetourix::get_sim_time(std::shared_ptr<webots::Robot> wb_robot) {
+  double seconds = 0;
+  double nanosec = modf(wb_robot->getTime(), &seconds) * 1e9;
+  return rclcpp::Time((uint32_t)seconds, (uint32_t)nanosec);
+}
+#endif /* SIMULATION */
+
 Assurancetourix::~Assurancetourix() {
 #ifdef MIPI_CAMERA
   arducam::arducam_close_camera(camera_instance);

src/modules/drive/CMakeLists.txt

@@ -20,6 +20,8 @@ find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(actuators_srvs REQUIRED)
@@ -39,6 +41,8 @@ set(dependencies
   "sensor_msgs"
   "tf2"
   "tf2_msgs"
+  "tf2_ros"
+  "tf2_geometry_msgs"
   "std_srvs"
   "diagnostic_msgs"
   "actuators_srvs"

src/modules/drive/include/drive.hpp

@@ -10,6 +10,7 @@
 #include <webots/PositionSensor.hpp>
 #endif
 #include "std_srvs/srv/set_bool.hpp"
+#include "std_srvs/srv/trigger.hpp"
 #include "actuators_srvs/srv/slider.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -18,6 +19,12 @@
 #include <math.h>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 
@@ -113,6 +120,11 @@ class Drive : public rclcpp::Node {
   diagnostic_msgs::msg::DiagnosticArray diagnostics_array_;
   diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_;
 
+  /* Odometry re-adjustement */
+  geometry_msgs::msg::TransformStamped _map_to_odom_tf;
+  std::vector<geometry_msgs::msg::TransformStamped> _previous_tf;
+  rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr _adjust_odometry_sub;
+
   double _accel;
   double _wheel_separation;
   double _wheel_radius;
@@ -165,6 +177,15 @@ class Drive : public rclcpp::Node {
                              const std_srvs::srv::SetBool::Response::SharedPtr response);
   void handle_set_slider_position(const std::shared_ptr<rmw_request_id_t> request_header, const actuators_srvs::srv::Slider::Request::SharedPtr request,
                                   const actuators_srvs::srv::Slider::Response::SharedPtr response);
+  void adjust_odometry_callback(const geometry_msgs::msg::TransformStamped::SharedPtr tf_stamped_msg);
+  void extract_pose_from_transform(geometry_msgs::msg::TransformStamped &transform_in, geometry_msgs::msg::PoseStamped &pose_out);
+  void set_transform_from_pose(geometry_msgs::msg::PoseStamped &pose_in, geometry_msgs::msg::TransformStamped &transform_out);
+  void get_pose_in_another_frame(geometry_msgs::msg::PoseStamped &pose_in, geometry_msgs::msg::PoseStamped &pose_out, geometry_msgs::msg::TransformStamped &transform);
+  tf2::Quaternion get_tf2_quaternion(geometry_msgs::msg::Quaternion &q);
+  tf2::Vector3 get_tf2_vector(geometry_msgs::msg::Point &p);
+  tf2::Vector3 get_tf2_vector(geometry_msgs::msg::Vector3 &p);
+  void set_pose_from_tf_t_q(tf2::Vector3 &t, tf2::Quaternion &q, geometry_msgs::msg::PoseStamped &pose_out);
+  double dummy_tree_digits_precision(double a);
 
 #ifdef SIMULATION
   void sim_step();

src/modules/drive/src/drive.cpp

@@ -60,6 +60,9 @@ Drive::Drive() : Node("drive_node") {
   _set_slider_postion = this->create_service<actuators_srvs::srv::Slider>(
       "slider_position", std::bind(&Drive::handle_set_slider_position, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
+  _adjust_odometry_sub = this->create_subscription<geometry_msgs::msg::TransformStamped>(
+      "adjust_odometry", qos, std::bind(&Drive::adjust_odometry_callback, this, std::placeholders::_1));
+
   diagnostics_timer_ = this->create_wall_timer(1s, std::bind(&Drive::update_diagnostic, this));
 
 #ifdef USE_TIMER
@@ -129,6 +132,20 @@ void Drive::init_variables() {
 #else
   time_since_last_sync_ = get_sim_time();
 #endif /* SIMULATION */
+
+  tf2_ros::Buffer tfBuffer(std::make_shared<rclcpp::Clock>(), tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME));
+  tf2_ros::TransformListener tfListener(tfBuffer);
+  std::string errorMsg;
+  if(tfBuffer.canTransform(odom_.header.frame_id, "map", std::chrono::system_clock::now(), std::chrono::milliseconds(10000)))
+  {
+    _map_to_odom_tf = tfBuffer.lookupTransform(odom_.header.frame_id, "map", std::chrono::system_clock::now(), std::chrono::milliseconds(10000));
+  }
+
+  geometry_msgs::msg::TransformStamped init_vector;
+  init_vector.header.stamp = this->get_clock()->now();
+  init_vector.header.frame_id = odom_.header.frame_id;
+  init_vector.child_frame_id = odom_.child_frame_id;
+  for (int i = 0; i<30; i++) _previous_tf.push_back(init_vector);
 }
 
 int8_t Drive::compute_velocity_cmd(double velocity) {
@@ -256,6 +273,9 @@ void Drive::update_tf() {
   tf2_msgs::msg::TFMessage odom_tf_msg;
   odom_tf_msg.transforms.push_back(odom_tf);
   tf_pub_->publish(odom_tf_msg);
+
+  _previous_tf.insert(_previous_tf.begin(), odom_tf);
+  if (_previous_tf.size() > 30) _previous_tf.pop_back();
 }
 
 void Drive::update_joint_states() {
@@ -315,6 +335,126 @@ rclcpp::Time Drive::get_sim_time() {
 void Drive::sim_step() { this->wb_robot->step(timestep); }
 #endif /* SIMULATION */
 
+void Drive::adjust_odometry_callback(const geometry_msgs::msg::TransformStamped::SharedPtr tf_stamped_msg){
+  rclcpp::Time stamp_msg = tf_stamped_msg->header.stamp;
+  int right_stamp_index = 0;
+  while (stamp_msg < (rclcpp::Time)_previous_tf.at(right_stamp_index).header.stamp){
+    right_stamp_index++;
+    if (right_stamp_index == _previous_tf.size()) {
+      RCLCPP_WARN(this->get_logger(), "Not enough tf stored to readjust odometry");
+      return;
+    }
+  }
+
+  if (right_stamp_index != 0){
+    if (stamp_msg - (rclcpp::Time)_previous_tf[right_stamp_index].header.stamp
+      > (rclcpp::Time)_previous_tf[right_stamp_index - 1].header.stamp - stamp_msg) right_stamp_index--;
+  }
+
+  geometry_msgs::msg::TransformStamped msg_received_relative_to_odom_tf,
+                                       nearest_tf = _previous_tf[right_stamp_index];
+
+  geometry_msgs::msg::PoseStamped msg_received_relative_to_map, msg_received_relative_to_odom,
+                                  base_link_relative_to_odom_estim, base_link_relative_to_near,
+                                  base_link_relative_to_odom_corrected;
+
+ /* Getting msg_received_relative_to_odom */
+  extract_pose_from_transform(*tf_stamped_msg, msg_received_relative_to_map);
+  get_pose_in_another_frame(msg_received_relative_to_map, msg_received_relative_to_odom, _map_to_odom_tf);
+
+  /* Getting base_link_relative_to_near */
+  extract_pose_from_transform(_previous_tf[0], base_link_relative_to_odom_estim);
+  get_pose_in_another_frame(base_link_relative_to_odom_estim, base_link_relative_to_near, nearest_tf);
+
+  /* Getting base_link_relative_to_odom_corrected */
+  set_transform_from_pose(msg_received_relative_to_odom, msg_received_relative_to_odom_tf);
+  get_pose_in_another_frame(base_link_relative_to_near, base_link_relative_to_odom_corrected, msg_received_relative_to_odom_tf);
+
+  tf2::Quaternion q(
+    base_link_relative_to_odom_corrected.pose.orientation.x,
+    base_link_relative_to_odom_corrected.pose.orientation.y,
+    base_link_relative_to_odom_corrected.pose.orientation.z,
+    base_link_relative_to_odom_corrected.pose.orientation.w
+  );
+  tf2::Matrix3x3 m(q);
+  double roll, pitch, yaw;
+  m.getRPY(roll, pitch, yaw);
+  odom_pose_.thetha = yaw;
+  odom_pose_.x = base_link_relative_to_odom_corrected.pose.position.x;
+  odom_pose_.y = base_link_relative_to_odom_corrected.pose.position.y;
+}
+
+void Drive::extract_pose_from_transform(geometry_msgs::msg::TransformStamped &transform_in, geometry_msgs::msg::PoseStamped &pose_out){
+  pose_out.header = transform_in.header;
+  pose_out.pose.position.x = transform_in.transform.translation.x;
+  pose_out.pose.position.y = transform_in.transform.translation.y;
+  pose_out.pose.position.z = transform_in.transform.translation.z;
+  pose_out.pose.orientation = transform_in.transform.rotation;
+}
+
+void Drive::set_transform_from_pose(geometry_msgs::msg::PoseStamped &pose_in, geometry_msgs::msg::TransformStamped &transform_out){
+  transform_out.header.frame_id = odom_.header.frame_id;
+  transform_out.child_frame_id = odom_.child_frame_id;
+  transform_out.transform.translation.x = pose_in.pose.position.x;
+  transform_out.transform.translation.y = pose_in.pose.position.y;
+  transform_out.transform.translation.z = pose_in.pose.position.z;
+  transform_out.transform.rotation = pose_in.pose.orientation;
+}
+
+void Drive::get_pose_in_another_frame(geometry_msgs::msg::PoseStamped &pose_in, geometry_msgs::msg::PoseStamped &pose_out, geometry_msgs::msg::TransformStamped &transform){
+  pose_out.header = pose_in.header;
+  pose_out.header.frame_id = transform.child_frame_id;
+
+  tf2::Vector3 t_in_new_frame,
+               t_pose_in = get_tf2_vector(pose_in.pose.position),
+               t_transform = get_tf2_vector(transform.transform.translation);
+  tf2::Quaternion q_in_new_frame,
+                  q_pose_in = get_tf2_quaternion(pose_in.pose.orientation),
+                  q_transform = get_tf2_quaternion(transform.transform.rotation);
+
+  if (pose_in.header.frame_id == transform.header.frame_id){
+    t_in_new_frame = t_pose_in - t_transform;
+  } else {
+    t_in_new_frame = t_pose_in + t_transform;
+  }
+
+  if (pose_in.header.frame_id == odom_.child_frame_id || pose_in.header.frame_id == transform.header.frame_id){
+    q_in_new_frame = q_transform * q_pose_in.inverse();
+  } else {
+    q_in_new_frame = q_pose_in * q_transform.inverse();
+  }
+
+
+  set_pose_from_tf_t_q(t_in_new_frame, q_in_new_frame, pose_out);
+}
+
+tf2::Quaternion Drive::get_tf2_quaternion(geometry_msgs::msg::Quaternion &q){
+  return tf2::Quaternion(q.x,q.y,q.z,q.w);
+}
+
+tf2::Vector3 Drive::get_tf2_vector(geometry_msgs::msg::Point &p){
+  return tf2::Vector3(p.x, p.y, p.z);
+}
+
+tf2::Vector3 Drive::get_tf2_vector(geometry_msgs::msg::Vector3 &p){
+  return tf2::Vector3(p.x, p.y, p.z);
+}
+
+void Drive::set_pose_from_tf_t_q(tf2::Vector3 &t, tf2::Quaternion &q, geometry_msgs::msg::PoseStamped &pose_out){
+  pose_out.pose.position.x = dummy_tree_digits_precision(t.x());
+  pose_out.pose.position.y = dummy_tree_digits_precision(t.y());
+  pose_out.pose.position.z = 0;
+  pose_out.pose.orientation.x = q.x();
+  pose_out.pose.orientation.y = q.y();
+  pose_out.pose.orientation.z = q.z();
+  pose_out.pose.orientation.w = q.w();
+}
+
+double Drive::dummy_tree_digits_precision(double a){
+  int dummy = (int)(a*1000);
+  return (double)(dummy)/1000;
+}
+
 Drive::~Drive() {
 #ifndef SIMULATION
   this->i2c_mutex.lock();

src/modules/localisation/localisation/localisation_node.py

@@ -4,14 +4,15 @@
 """Robot localisation node."""
 
 
+import math
 import numpy as np
 
 import rclpy
 from geometry_msgs.msg import TransformStamped
 from rcl_interfaces.msg import SetParametersResult
+from visualization_msgs.msg import MarkerArray
 from tf2_ros import StaticTransformBroadcaster
 
-
 class Localisation(rclpy.node.Node):
     """Robot localisation node."""
 
@@ -23,11 +24,16 @@ def __init__(self):
         self._x, self._y, self._theta = self.fetchStartPosition()
         self._tf_brodcaster = StaticTransformBroadcaster(self)
         self._tf = TransformStamped()
-        self._tf.header.frame_id = "map"
-        self._tf.child_frame_id = "odom"
-        self.create_timer(1, self.update_transform)
-        self.get_logger().info(f"Default side is {self.side.value}")
-        self.get_logger().info("Localisation node is ready")
+        self._tf.header.frame_id = 'map'
+        self._tf.child_frame_id = 'odom'
+        self.update_transform()
+        self.subscription_ = self.create_subscription(
+            MarkerArray, '/allies_positions_markers', self.allies_subscription_callback, 10)
+        self.subscription_  # prevent unused variable warning
+        self.tf_publisher_ = self.create_publisher(TransformStamped, 'adjust_odometry', 10)
+        self.last_odom_update = 0
+        self.get_logger().info(f'Default side is {self.side.value}')
+        self.get_logger().info('Localisation node is ready')
 
     def _on_set_parameters(self, params):
         """Handle Parameter events especially for side."""
@@ -77,6 +83,20 @@ def euler_to_quaternion(self, yaw, pitch=0, roll=0):
         ) * np.sin(pitch / 2) * np.sin(yaw / 2)
         return [qx, qy, qz, qw]
 
+    def quaternion_to_euler(self, x, y, z, w):
+        """Conversion between quaternions and euler angles."""
+        t0 = +2.0 * (w * x + y * z)
+        t1 = +1.0 - 2.0 * (x * x + y * y)
+        X = math.atan2(t0, t1)
+        t2 = +2.0 * (w * y - z * x)
+        t2 = +1.0 if t2 > +1.0 else t2
+        t2 = -1.0 if t2 < -1.0 else t2
+        Y = math.asin(t2)
+        t3 = +2.0 * (w * z + x * y)
+        t4 = +1.0 - 2.0 * (y * y + z * z)
+        Z = math.atan2(t3, t4)
+        return (X, Y, Z)
+
     def update_transform(self):
         """Update and publish transform callback."""
         self._tf.header.stamp = self.get_clock().now().to_msg()
@@ -89,6 +109,22 @@ def update_transform(self):
         self._tf.transform.rotation.w = float(qw)
         self._tf_brodcaster.sendTransform(self._tf)
 
+    def allies_subscription_callback(self, msg):
+        """Identity the robot marker in assurancetourix marker_array detection
+           publish the transformation for drive to readjust odometry"""
+        for allie_marker in msg.markers:
+            if allie_marker.text.lower() == self.robot and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 5:
+                self._x = allie_marker.pose.position.x
+                self._y = allie_marker.pose.position.y
+                q = allie_marker.pose.orientation
+                self._theta = self.quaternion_to_euler(q.x, q.y, q.z, q.w)[2]
+                self._tf.header.stamp = allie_marker.header.stamp
+                self._tf.transform.translation.x = allie_marker.pose.position.x
+                self._tf.transform.translation.y = allie_marker.pose.position.y
+                self._tf.transform.translation.z = float(0)
+                self._tf.transform.rotation = q
+                self.tf_publisher_.publish(self._tf)
+                self.last_odom_update = self.get_clock().now().to_msg().sec
 
 def main(args=None):
     """Entrypoint."""