Vlx readjustment

src/asterix/param/asterix.in.yml

@@ -49,6 +49,14 @@ asterix:
       strategy_mode: NORMAL
 
 
+  localisation_node:
+    ros__parameters:
+      vlx_lat_x: 80.0
+      vlx_lat_y: 130.5
+      vlx_face_x: 100.5
+      vlx_face_y: 110.0
+
+
   lcd_driver:
     ros__parameters:
       use_sim_time: !Var use_sim_time

src/modules/localisation/localisation/localisation_node.py

@@ -1,54 +1,71 @@
 #!/usr/bin/env python3
 
 
-"""Robot localisation node."""
+""" Robot localisation node """
 
 
 import math
 import numpy as np
-
+import time
+import copy
 import rclpy
-from geometry_msgs.msg import TransformStamped
+
+from geometry_msgs.msg import TransformStamped, PoseStamped, Quaternion
 from rcl_interfaces.msg import SetParametersResult
 from visualization_msgs.msg import MarkerArray
 from tf2_ros import StaticTransformBroadcaster
-from transformix_msgs.srv import TransformixParametersTransformStamped
+from .vlx.vlx_readjustment import VlxReadjustment
+from localisation.utils import euler_to_quaternion, is_simulation
+from nav_msgs.msg import Odometry
+from tf2_kdl import transform_to_kdl, do_transform_frame
 
 
 class Localisation(rclpy.node.Node):
-    """Robot localisation node."""
+    """ Robot localisation node """
 
     def __init__(self):
+        """ Init Localisation node """
         super().__init__("localisation_node")
         self.robot = self.get_namespace().strip("/")
         self.side = self.declare_parameter("side", "blue")
         self.add_on_set_parameters_callback(self._on_set_parameters)
-        self._x, self._y, self._theta = self.fetchStartPosition()
+        self.robot_pose = PoseStamped()
+        (
+            self.robot_pose.pose.position.x,
+            self.robot_pose.pose.position.y,
+            theta,
+        ) = self.fetchStartPosition()
+        self.robot_pose.pose.orientation = euler_to_quaternion(theta)
         self._tf_brodcaster = StaticTransformBroadcaster(self)
         self._tf = TransformStamped()
         self._tf.header.frame_id = "map"
         self._tf.child_frame_id = "odom"
         self.update_transform()
-        self.get_initial_tf_srv = self.create_service(
-            TransformixParametersTransformStamped,
-            "get_odom_map_tf",
-            self.get_initial_tf_srv_callback,
-        )
         self.subscription_ = self.create_subscription(
             MarkerArray,
             "/allies_positions_markers",
             self.allies_subscription_callback,
             10,
         )
+        self.subscription_ = self.create_subscription(
+            Odometry,
+            "odom",
+            self.odom_callback,
+            10,
+        )
         self.tf_publisher_ = self.create_publisher(
             TransformStamped, "adjust_odometry", 10
         )
+        self.odom_map_relative_publisher_ = self.create_publisher(
+            PoseStamped, "odom_map_relative", 10
+        )
         self.last_odom_update = 0
         self.get_logger().info(f"Default side is {self.side.value}")
+        self.vlx = VlxReadjustment(self)
         self.get_logger().info("Localisation node is ready")
 
     def _on_set_parameters(self, params):
-        """Handle Parameter events especially for side."""
+        """ Handle Parameter events especially for side """
         result = SetParametersResult()
         try:
             for param in params:
@@ -57,15 +74,20 @@ def _on_set_parameters(self, params):
                     self.side = param
                 else:
                     setattr(self, param.name, param)
-            self._x, self._y, self._theta = self.fetchStartPosition()
+            (
+                self.robot_pose.pose.position.x,
+                self.robot_pose.pose.position.y,
+                theta,
+            ) = self.fetchStartPosition()
+            self.robot_pose.pose.orientation = euler_to_quaternion(theta)
+            self.update_transform()
             result.successful = True
         except BaseException as e:
             result.reason = e
         return result
 
     def fetchStartPosition(self):
-        """Fetch start position for side and robot."""
-        # TODO : Calibrate the start position using VL53L1X
+        """ Fetch start position for side and robot """
         if self.robot == "asterix":
             if self.side.value == "blue":
                 return (0.29, 1.33, 0)
@@ -76,76 +98,85 @@ def fetchStartPosition(self):
                 return (0.29, 1.33, 0)
             elif self.side.value == "yellow":
                 return (3 - 0.29, 1.33, np.pi)
-        # Make it crash in case of undefined parameters
         return None
 
-    def euler_to_quaternion(self, yaw, pitch=0, roll=0):
-        """Conversion between euler angles and quaternions."""
-        qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
-            roll / 2
-        ) * np.sin(pitch / 2) * np.sin(yaw / 2)
-        qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
-            roll / 2
-        ) * np.cos(pitch / 2) * np.sin(yaw / 2)
-        qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
-            roll / 2
-        ) * np.sin(pitch / 2) * np.cos(yaw / 2)
-        qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
-            roll / 2
-        ) * 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."""
+        """ Update and publish odom --> map transform """
         self._tf.header.stamp = self.get_clock().now().to_msg()
-        self._tf.transform.translation.x = float(self._x)
-        self._tf.transform.translation.y = float(self._y)
-        qx, qy, qz, qw = self.euler_to_quaternion(self._theta)
-        self._tf.transform.rotation.x = float(qx)
-        self._tf.transform.rotation.y = float(qy)
-        self._tf.transform.rotation.z = float(qz)
-        self._tf.transform.rotation.w = float(qw)
-        self._initial_tf = self._tf
+        self._tf.transform.translation.x = self.robot_pose.pose.position.x
+        self._tf.transform.translation.y = self.robot_pose.pose.position.y
+        self._tf.transform.rotation = self.robot_pose.pose.orientation
+        self._initial_tf = copy.deepcopy(self._tf)
         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:
+        """Identity the robot marker in assurancetourix marker_array detection,
+        send the marker to vlx_readjustment in order to try to refine the position
+        at a stamp given by the marker"""
+        for ally_marker in msg.markers:
             if (
-                allie_marker.text.lower() == self.robot
-                and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 5
+                ally_marker.text.lower() == self.robot
+                and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 0.75
             ):
-                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 get_initial_tf_srv_callback(self, request, response):
-        self.get_logger().info(f"incoming request for {self.robot} odom -> map tf")
-        response.transform_stamped = self._initial_tf
-        return response
+                if (
+                    is_simulation()
+                ):  # simulate marker delais (image analysis from assurancetourix)
+                    time.sleep(0.15)
+                if self.vlx.continuous_sampling == 0:
+                    self.create_and_send_tf(
+                        ally_marker.pose.position.x,
+                        ally_marker.pose.position.y,
+                        ally_marker.pose.orientation,
+                        ally_marker.header.stamp,
+                    )
+                else:
+                    self.vlx.try_to_readjust_with_vlx(
+                        ally_marker.pose.position.x,
+                        ally_marker.pose.position.y,
+                        ally_marker.pose.orientation,
+                        ally_marker.header.stamp,
+                    )
+                if self.vlx.continuous_sampling in [0, 1]:
+                    self.vlx.start_continuous_sampling_thread(0.65, 1)
+
+    def is_near_walls(self, pt):
+        """ Return true if the robot if less than 30cm from the wall """
+        return pt.x < 0.3 or pt.y < 0.3 or pt.x > 2.7 or pt.y > 1.7
+
+    def create_and_send_tf(self, x, y, q, stamp):
+        """ Create and send tf to drive for it to re-adjust odometry """
+        self._tf.header.stamp = stamp
+        self._tf.transform.translation.x = x
+        self._tf.transform.translation.y = y
+        self._tf.transform.translation.z = float(0)
+        self._tf.transform.rotation = q
+        self.last_odom_update = self.get_clock().now().to_msg().sec
+        self.tf_publisher_.publish(self._tf)
+
+    def odom_callback(self, msg):
+        """Odom callback, computes the new pose of the robot relative to map,
+        send this new pose on odom_map_relative topic and start vlx routine
+        if this pose is near walls"""
+        robot_tf = TransformStamped()
+        robot_tf.transform.translation.x = msg.pose.pose.position.x
+        robot_tf.transform.translation.y = msg.pose.pose.position.y
+        robot_tf.transform.rotation = msg.pose.pose.orientation
+        robot_frame = transform_to_kdl(robot_tf)
+        new_robot_pose_kdl = do_transform_frame(robot_frame, self._initial_tf)
+        self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x()
+        self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y()
+        q = new_robot_pose_kdl.M.GetQuaternion()
+        self.robot_pose.header.stamp = msg.header.stamp
+        self.robot_pose.header.frame_id = "map"
+        self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
+        self.odom_map_relative_publisher_.publish(self.robot_pose)
+        if self.is_near_walls(self.robot_pose.pose.position):
+            if self.vlx.continuous_sampling == 2:
+                self.vlx.near_wall_routine(self.robot_pose)
+            else:
+                self.vlx.start_near_wall_routine(self.robot_pose)
+        elif self.vlx.continuous_sampling == 2:
+            self.vlx.stop_near_wall_routine()
 
 
 def main(args=None):
@@ -156,5 +187,7 @@ def main(args=None):
         rclpy.spin(localisation)
     except KeyboardInterrupt:
         pass
+    localisation.vlx.stop_continuous_sampling_thread()
+    localisation.vlx.sensors.node.destroy_node()
     localisation.destroy_node()
     rclpy.shutdown()

src/modules/localisation/localisation/utils.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+
+"""Some utils functions"""
+
+import numpy as np
+import math
+
+from geometry_msgs.msg import Quaternion
+from platform import machine
+
+
+def euler_to_quaternion(yaw, pitch=0, roll=0):
+    """Conversion between euler angles and quaternions."""
+    qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
+        roll / 2
+    ) * np.sin(pitch / 2) * np.sin(yaw / 2)
+    qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
+        roll / 2
+    ) * np.cos(pitch / 2) * np.sin(yaw / 2)
+    qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
+        roll / 2
+    ) * np.sin(pitch / 2) * np.cos(yaw / 2)
+    qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
+        roll / 2
+    ) * np.sin(pitch / 2) * np.sin(yaw / 2)
+    return Quaternion(x=qx, y=qy, z=qz, w=qw)
+
+
+def quaternion_to_euler(q):
+    """Conversion between quaternions and euler angles.
+
+    :param q: The quaternion to transform to euler angles
+    :type q: geometry_msgs.msg.Quaternion
+    :return: euler angles equivalent
+    :rtype: tuple (x,y,z)
+    """
+    x, y, z, w = q.x, q.y, q.z, q.w
+    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 in_rectangle(rect, pose):
+    """Return true if the coord are inside the rect coordinates
+
+    :param rect: The list of rect coordinates.
+    :type rect: array [x_min, y_min, x_max_ y_max]
+    :param pose: the pose to test
+    :type coord: geometry_msgs.msg.PoseStamped
+    :return: true if the coord are inside the rect coordinates, else false
+    :rtype: bool
+    """
+    x = pose.pose.position.x
+    y = pose.pose.position.y
+    return rect[0] < x and rect[1] < y and rect[2] > x and rect[3] > y
+
+
+def is_simulation():
+    return True if machine() != "aarch64" else False