#!/usr/bin/env python3
# Correction du TP 6 (partie 3, ROS), etape 2 -- le robot S suit le robot M.
# Usage : python3 suiveur.py [MENEUR] [SUIVEUR] [DISTANCE]   (1 2 0.6)
# En simulation avec la verite terrain (ground_truth_tf:=true, le defaut) :
# les arbres TF des robots sont relies par le repere commun « world ».
import math
import sys

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rclpy.time import Time
from geometry_msgs.msg import Twist
from tf2_ros import Buffer, TransformListener, TransformException

V_MAX = 0.15
W_MAX = 1.0


class Suiveur(Node):
    def __init__(self, leader, follower, distance):
        super().__init__('ratbot_lab_suiveur')
        self.leader = f'ratbot_{leader}/base_footprint'
        self.follower = f'ratbot_{follower}/base_footprint'
        self.distance = distance
        self.pub = self.create_publisher(Twist, f'/ratbot_{follower}/base/cmd_vel', 10)
        self.tf = Buffer()
        self.listener = TransformListener(self.tf, self)
        self.last_log = 0.0
        self.create_timer(0.05, self.tick)

    def tick(self):
        cmd = Twist()
        try:
            # position du meneur dans le repere du suiveur
            t = self.tf.lookup_transform(self.follower, self.leader, Time())
        except TransformException:
            self.pub.publish(cmd)            # pas de position : on s'arrete
            return
        x = t.transform.translation.x
        y = t.transform.translation.y
        dist = math.hypot(x, y)
        angle = math.atan2(y, x)
        # tourner vers le meneur ; n'avancer que s'il est a peu pres devant
        cmd.angular.z = max(-W_MAX, min(W_MAX, 1.5 * angle))
        if abs(angle) < math.radians(45):
            cmd.linear.x = max(0.0, min(V_MAX, 0.8 * (dist - self.distance)))
        self.pub.publish(cmd)
        now = self.get_clock().now().nanoseconds / 1e9
        if now - self.last_log >= 2.0:
            self.last_log = now
            self.get_logger().info(f'meneur a {dist:.2f} m, {math.degrees(angle):.0f} deg')


def main():
    leader = int(sys.argv[1]) if len(sys.argv) > 1 else 1
    follower = int(sys.argv[2]) if len(sys.argv) > 2 else 2
    distance = float(sys.argv[3]) if len(sys.argv) > 3 else 0.6
    rclpy.init()
    node = Suiveur(leader, follower, distance)
    try:
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        if rclpy.ok():
            node.pub.publish(Twist())       # arret du suiveur
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()

# Reponses attendues aux questions :
#  1. /ratbot_1/... et /ratbot_2/... : memes noms relatifs, deux espaces de
#     noms. Les noeuds aussi : /ratbot_1/twist_mux, /ratbot_2/twist_mux...
#  2. Un seul topic /tf, mais des reperes prefixes : ratbot_1/odom,
#     ratbot_1/base_footprint, ratbot_2/odom... Sans prefixe, les deux robots
#     publieraient « base_footprint » et l'arbre serait incoherent.
#  3. world, commun, relie les odom des robots (verite terrain). Sans lui
#     (ground_truth_tf:=false), deux arbres separes : lookup_transform
#     echoue, aucun robot ne sait ou est l'autre.
#  4. Chaque odometrie part de zero a la place de depart du robot : les deux
#     reperes odom ne coincident pas. Il faut un repere commun.
#  5. Le suiveur n'avance que si l'ecart depasse la distance voulue, il ne
#     recule jamais : si le meneur vient vers lui, l'ecart diminue (mesure :
#     jusqu'a 0,11 m en simulation). Un correcteur qui recule, ou un arret
#     d'urgence sous 0,3 m, l'evite.
#  6. Sur le vrai parc, pas de verite terrain : il faut que les robots se
#     localisent dans la meme carte (AMCL, repere map commun), et publient
#     leur position ; ou un systeme exterieur (cameras au plafond, balises).