From 1708f332ae900384b9e84889b7ab3979aaae9f5f Mon Sep 17 00:00:00 2001
From: tianb03 <tianbo@tianbot.com>
Date: Wed, 15 Feb 2023 15:51:24 +0800
Subject: [PATCH] mini and tt coordinate swarm

---
 launch/demo/demo_mc_tf_tt_circle_mini.launch | 32 ++++++++
 launch/demo/demo_mc_tf_tt_follow_mini.launch | 23 ++++++
 nodes/virtual_targets.py                     | 86 ++++++++++++++++++++
 3 files changed, 141 insertions(+)
 create mode 100644 launch/demo/demo_mc_tf_tt_circle_mini.launch
 create mode 100644 launch/demo/demo_mc_tf_tt_follow_mini.launch
 create mode 100755 nodes/virtual_targets.py

diff --git a/launch/demo/demo_mc_tf_tt_circle_mini.launch b/launch/demo/demo_mc_tf_tt_circle_mini.launch
new file mode 100644
index 0000000..5fe5ad8
--- /dev/null
+++ b/launch/demo/demo_mc_tf_tt_circle_mini.launch
@@ -0,0 +1,32 @@
+<launch>
+  <arg name="robot_name_1" default="tbmn_1" />
+  <arg name="drone_name_1" default="rmtt_0" />
+  <arg name="drone_name_2" default="rmtt_2" />  
+
+  <!-- There is no need to launch tianbot mini -->
+
+  <!-- There is no need to publish static tf from world to map, odom, etc. -->
+
+  <!-- There is no odom published from tianbot mini  -->
+
+  <!-- publish static transform to link vrpn link to robot base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="robot_1_link" args="0 0 0 0 0 0 $(arg robot_name_1) $(arg robot_name_1)/base_link 20" />
+  <node pkg="tf" type="static_transform_publisher" name="drone_1_link" args="0 0 0 0 0 0 $(arg drone_name_1) $(arg drone_name_1)/base_link 20" />
+  <node pkg="tf" type="static_transform_publisher" name="drone_2_link" args="0 0 0 0 0 0 $(arg drone_name_2) $(arg drone_name_2)/base_link 20" />
+
+  <node pkg="abc_swarm" type="virtual_targets.py" name="virtual_targets" />
+
+  <node pkg="abc_swarm" type="tianbot_mini_tf.py" name="drone_1_follower" output="screen">
+    <param name="target_frame" value="rmtt_0_target" />
+    <param name="follower_robot_name" value="$(arg drone_name_1)" />
+    <param name="set_distance" value="0.3" />
+  </node>
+  <node pkg="abc_swarm" type="tianbot_mini_tf.py" name="drone_2_follower" output="screen">
+    <param name="target_frame" value="rmtt_2_target" />
+    <param name="follower_robot_name" value="$(arg drone_name_2)" />
+    <param name="set_distance" value="0.3" />
+  </node>
+
+  <!-- RVIZ可视化调试工具 -->
+  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find abc_swarm)/rviz/demo_formation.rviz" /> -->
+</launch>
diff --git a/launch/demo/demo_mc_tf_tt_follow_mini.launch b/launch/demo/demo_mc_tf_tt_follow_mini.launch
new file mode 100644
index 0000000..fc0c42e
--- /dev/null
+++ b/launch/demo/demo_mc_tf_tt_follow_mini.launch
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="robot_name_1" default="tbmn_1" />
+  <arg name="drone_name_1" default="rmtt_0" /> 
+
+  <!-- There is no need to launch tianbot mini -->
+
+  <!-- There is no need to publish static tf from world to map, odom, etc. -->
+
+  <!-- There is no odom published from tianbot mini  -->
+
+  <!-- publish static transform to link vrpn link to robot base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="robot_1_link" args="0 0 0 0 0 0 $(arg robot_name_1) $(arg robot_name_1)/base_link 20" />
+  <node pkg="tf" type="static_transform_publisher" name="drone_1_link" args="0 0 0 0 0 0 $(arg drone_name_1) $(arg drone_name_1)/base_link 20" />
+
+  <node pkg="abc_swarm" type="tianbot_mini_tf.py" name="tianbot_mini_tf" output="screen">
+    <param name="target_frame" value="$(arg robot_name_1)" />
+    <param name="follower_robot_name" value="$(arg drone_name_1)" />
+    <param name="set_distance" value="0.3" />
+  </node>
+
+  <!-- RVIZ可视化调试工具 -->
+  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find abc_swarm)/rviz/demo_formation.rviz" /> -->
+</launch>
diff --git a/nodes/virtual_targets.py b/nodes/virtual_targets.py
new file mode 100755
index 0000000..9d6b487
--- /dev/null
+++ b/nodes/virtual_targets.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+import math
+import rospy
+import tf
+from tf import transformations
+from tf import broadcaster
+import numpy as np
+
+def broadcaster():
+    rospy.init_node('virtual_target', anonymous=True)
+    
+    # read params
+    center_frame = rospy.get_param('~center_frame', 'tbmn_1')
+    radius = rospy.get_param('~radius', 0.5)
+    target_frame = rospy.get_param('~target_frame', 'virtual_leader')
+    linear_speed = rospy.get_param('~speed', 0.1)
+
+    listener = tf.TransformListener()
+    listener.waitForTransform(center_frame, "world", rospy.Time(0), rospy.Duration(3.0));
+    try:
+        (trans, rot) = listener.lookupTransform("world", center_frame, rospy.Time())
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+        pass
+
+    br = tf.TransformBroadcaster()
+    x = trans[0]
+    y = trans[1]
+    theta = 0
+
+    r = radius
+    v = linear_speed
+    a = v / r
+
+    aligned_center_frame = center_frame + '_aligned'
+
+    br.sendTransform((x, y, 0),
+              tf.transformations.quaternion_from_euler(0, 0, 0),
+              rospy.Time.now(),
+              aligned_center_frame,
+              "world")
+    rospy.sleep(5)
+    
+    start = rospy.Time.now()
+    rate = rospy.Rate(20) 
+    while not rospy.is_shutdown():
+        try:
+            (trans, rot) = listener.lookupTransform("world", center_frame, rospy.Time())
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+            continue
+
+        x = trans[0]
+        y = trans[1]
+        theta = 0
+        br.sendTransform((x, y, 0),
+                tf.transformations.quaternion_from_euler(0, 0, 0),
+                rospy.Time.now(),
+                aligned_center_frame,
+                "world")        
+        
+        t = (rospy.Time.now() - start).to_sec()
+        theta = a * t
+        x = r * math.sin(theta)
+        y = - r * math.cos(theta)
+        br.sendTransform((x, y, 0),
+                     tf.transformations.quaternion_from_euler(0, 0, theta),
+                     rospy.Time.now(),
+                     "rmtt_0_target",
+                     aligned_center_frame)
+        
+        theta = a * t + np.pi
+        x = r * math.sin(theta)
+        y = - r * math.cos(theta)
+        br.sendTransform((x, y, 0),
+                tf.transformations.quaternion_from_euler(0, 0, theta),
+                rospy.Time.now(),
+                "rmtt_2_target",
+                aligned_center_frame)
+
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        broadcaster()
+    except rospy.ROSInterruptException:
+        pass
\ No newline at end of file