[misc] added task scripts

robots/assemble_quadrotors/scripts/soft/all_yaw.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+import math
+from std_msgs.msg import Float64
+import numpy as np
+
+class AllYaw():
+    def __init__(self):
+
+        rospy.init_node("allyaw")
+
+        # subscribers & publishers
+        self.yaw1_pub = rospy.Publisher("/target_yaw1",Float64 , queue_size=1)
+        self.yaw2_pub = rospy.Publisher("/target_yaw2",Float64 , queue_size=1)
+
+    def turn(self, angle):
+
+        # prepare message
+        msg_yaw1 = Float64()
+        msg_yaw2 = Float64()
+
+        msg_yaw1.data = angle
+        msg_yaw2.data = angle
+
+        # publish
+        self.yaw1_pub.publish(msg_yaw1)
+        self.yaw2_pub.publish(msg_yaw2)
+
+if __name__=="__main__":
+    total_args = len(sys.argv)
+    try:
+        if(total_args > 1):
+            allyaw = AllYaw()
+            allyaw.turn(float(sys.argv[1]))
+        else:
+            print("no target yaw provided")
+    except rospy.ROSInterruptException: 
+        pass

robots/assemble_quadrotors/scripts/soft/bend_demo.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+import math
+from std_msgs.msg import Float64
+import numpy as np
+
+class BendDemo():
+    def __init__(self):
+
+        rospy.init_node("bend_demo")
+
+        # subscribers & publishers
+        self.yaw1_pub = rospy.Publisher("/target_yaw1",Float64 , queue_size=1)
+        self.yaw2_pub = rospy.Publisher("/target_yaw2",Float64 , queue_size=1)
+
+        self.pitch1_pub = rospy.Publisher("/target_pitch1",Float64 , queue_size=1)
+        self.pitch2_pub = rospy.Publisher("/target_pitch2",Float64 , queue_size=1)
+
+        # state variables
+
+        # maximum allowed pitch/yaw (will move inside this)
+        self.max_yaw = 0.1
+        self.max_pitch = 0.2
+
+        # step width and counts
+        self.angle_step = 10
+        self.step_count = 0
+
+
+    #main func
+    def main(self):
+        r = rospy.Rate(1.0) # 1hz -> angle_step[s] to reach max bend
+
+        while not rospy.is_shutdown():
+
+            # prepare msgs
+            yaw1_msg = Float64()
+            yaw2_msg = Float64()
+            pitch1_msg = Float64()
+            pitch2_msg = Float64()
+
+            # set values
+            yaw1_msg.data = (self.max_yaw/2)*math.sin((math.pi/2)*(1/self.angle_step)*self.step_count)
+            yaw2_msg.data = -1*(self.max_yaw/2)*math.sin((math.pi/2)*(1/self.angle_step)*self.step_count)
+
+            pitch1_msg.data = (self.max_pitch/2)*math.sin((math.pi/2)*(1/self.angle_step)*self.step_count)
+            pitch2_msg.data = -1*(self.max_pitch/2)*math.sin((math.pi/2)*(1/self.angle_step)*self.step_count)
+
+            # send msgs
+            self.yaw1_pub.publish(yaw1_msg)
+            self.yaw2_pub.publish(yaw2_msg)
+            self.pitch1_pub.publish(pitch1_msg)
+            self.pitch2_pub.publish(pitch2_msg)
+
+            r.sleep()
+            self.step_count+=1
+
+if __name__=="__main__":
+    try:
+        bend_demo = BendDemo()
+        bend_demo.main()
+    except rospy.ROSInterruptException: 
+        pass

robots/assemble_quadrotors/scripts/soft/circle_demo.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+import math
+from aerial_robot_msgs.msg import FlightNav
+from geometry_msgs.msg import Point # /assemble_quadrotorsN/assemble/newCoG
+import numpy as np
+
+class CircleDemo():
+    def __init__(self):
+
+        rospy.init_node("circle_demo")
+        # state variables
+
+        # initial CoG location (use this as the center)
+        self.cog_loc_x_init = 0.0
+        self.cog_loc_y_init = 0.0
+        first = True
+
+        # circle radius[m]
+        self.circle_radius = 0.5
+
+        # step width and counts
+        self.circle_steps = 100
+
+        # subscribers & publishers
+        self.nav1_pub = rospy.Publisher("/assemble_quadrotors1/uav/nav",FlightNav , queue_size=1)
+        self.nav2_pub = rospy.Publisher("/assemble_quadrotors2/uav/nav",FlightNav , queue_size=1)
+
+        self.initial_cog_sub = rospy.Subscriber('/assemble_quadrotors1/assemble/newCoG',Point,self.initialCoGCallback)
+
+    def initialCoGCallback(self, msg):
+        if first is True:
+            self.cog_loc_x_init = msg.x
+            self.cog_loc_y_init = msg.y
+            first = False
+        else:
+            return
+
+    #main func
+    def main(self):
+        r = rospy.Rate(1.0) # 1hz -> angle_step[s] to reach max bend
+
+        i=0
+        rospy.sleep(1)
+        print("start")
+        while (not rospy.is_shutdown()) and (i < self.circle_steps):
+
+            if i==0:
+                # move to the initial position (x=r,y=0)
+                initial_position = FlightNav()
+                initial_position.target = 1
+                initial_position.control_frame = 0
+                initial_position.pos_xy_nav_mode=2
+                initial_position.target_pos_x = self.cog_loc_x_init + self.circle_radius
+                initial_position.target_pos_y = self.cog_loc_y_init
+
+                self.nav1_pub.publish(initial_position)
+                self.nav2_pub.publish(initial_position)
+
+                print("sending to initial location")
+                rospy.sleep(5)
+
+            print(f"step {i+1} of {self.circle_steps}")
+            theta = i * (2*math.pi)/self.circle_steps
+
+            dx = self.circle_radius * math.cos(theta)
+            dy = self.circle_radius * math.sin(theta)
+            # prepare msgs
+            updated_position = FlightNav()
+
+            # set values
+            updated_position.target = 1
+            updated_position.control_frame = 0
+            updated_position.pos_xy_nav_mode=2
+            updated_position.target_pos_x = self.cog_loc_x_init + dx
+            updated_position.target_pos_y = self.cog_loc_y_init + dy
+
+
+            # send msgs
+            self.nav1_pub.publish(updated_position)
+            self.nav2_pub.publish(updated_position)
+
+            r.sleep()
+            i+=1
+        print("task finished")
+
+if __name__=="__main__":
+    try:
+        circle_demo = CircleDemo()
+        circle_demo.main()
+    except rospy.ROSInterruptException: 
+        pass

robots/assemble_quadrotors/scripts/soft/cube_demo.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+import math
+from aerial_robot_msgs.msg import FlightNav
+from geometry_msgs.msg import Point # /assemble_quadrotorsN/assemble/newCoG
+import numpy as np
+
+class CubeDemo():
+    def __init__(self):
+
+        rospy.init_node("cube_demo")
+        # state variables
+
+        # initial CoG location (use this as the center)
+        self.cog_loc_x_init = 2.0
+        self.cog_loc_y_init = 1.0
+        self.cog_loc_z_init = 0.5
+        first = True
+
+        # cube length[m]
+        self.cube_length = 0.5
+
+        # offset list
+        self.cube_offset = [[self.cube_length, 0.0,              0.0],
+                            [self.cube_length, 0.0,              self.cube_length],
+                            [self.cube_length, self.cube_length, self.cube_length],
+                            [self.cube_length, self.cube_length, 0.0],
+                            [0.0,              self.cube_length, 0.0],
+                            [0.0,              self.cube_length, self.cube_length],
+                            [0.0,              0.0,              self.cube_length],
+                            [0.0,              0.0,              0.0]]
+
+        self.move_time = 5.0
+
+        # subscribers & publishers
+        self.nav1_pub = rospy.Publisher("/assemble_quadrotors1/uav/nav",FlightNav , queue_size=1)
+        self.nav2_pub = rospy.Publisher("/assemble_quadrotors2/uav/nav",FlightNav , queue_size=1)
+
+        self.initial_cog_sub = rospy.Subscriber('/assemble_quadrotors1/assemble/newCoG',Point,self.initialCoGCallback)
+
+    def initialCoGCallback(self, msg):
+        if first is True:
+            self.cog_loc_x_init = msg.x
+            self.cog_loc_y_init = msg.y
+            first = False
+        else:
+            return
+
+    #main func
+    def main(self):
+        r = rospy.Rate(1.0/self.move_time) # 0.2hz (1 every 5sec)
+
+        i=0
+        rospy.sleep(1)
+        print("start")
+        while (not rospy.is_shutdown()) and (i < 8):
+
+            print(f"step {i+1} of 8")
+
+            # prepare msgs
+            updated_position = FlightNav()
+
+            # set values
+            updated_position.target = 1
+            updated_position.control_frame = 0
+            updated_position.pos_xy_nav_mode=2
+            updated_position.pos_z_nav_mode=2
+            updated_position.target_pos_x = self.cog_loc_x_init + self.cube_offset[i][0]
+            updated_position.target_pos_y = self.cog_loc_y_init + self.cube_offset[i][1]
+            updated_position.target_pos_z = self.cog_loc_z_init + self.cube_offset[i][2]
+
+            # send msgs
+            self.nav1_pub.publish(updated_position)
+            self.nav2_pub.publish(updated_position)
+
+            r.sleep()
+            i+=1
+        print("task finished")
+
+if __name__=="__main__":
+    try:
+        cube_demo = CubeDemo()
+        cube_demo.main()
+    except rospy.ROSInterruptException: 
+        pass

robots/assemble_quadrotors/scripts/soft/figure_8_demo.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+import math
+from aerial_robot_msgs.msg import FlightNav
+from geometry_msgs.msg import Point # /assemble_quadrotorsN/assemble/newCoG
+import numpy as np
+
+class Fig8Demo():
+    def __init__(self):
+
+        rospy.init_node("figure_8_demo")
+        # state variables
+
+        # initial CoG location (use this as the center)
+        self.cog_loc_x_init = 2.0
+        self.cog_loc_y_init = 1.0
+        first = True
+
+        # circle radius[m]
+        self.circle_radius = 0.5
+
+        # step width and counts
+        self.circle_steps = 100
+
+        # subscribers & publishers
+        self.nav1_pub = rospy.Publisher("/assemble_quadrotors1/uav/nav",FlightNav , queue_size=1)
+        self.nav2_pub = rospy.Publisher("/assemble_quadrotors2/uav/nav",FlightNav , queue_size=1)
+
+        self.initial_cog_sub = rospy.Subscriber('/assemble_quadrotors1/assemble/newCoG',Point,self.initialCoGCallback)
+
+    def initialCoGCallback(self, msg):
+        if first is True:
+            self.cog_loc_x_init = msg.x
+            self.cog_loc_y_init = msg.y
+            first = False
+        else:
+            return
+
+    #main func
+    def main(self):
+        r = rospy.Rate(1) # 1hz -> angle_step[s] to reach max bend
+
+        i=0
+        rospy.sleep(1)
+        print("start")
+        while (not rospy.is_shutdown()) and (i < 2*self.circle_steps):
+
+            if i<self.circle_steps:
+
+                print(f"step {i+1} of {self.circle_steps}")
+                theta = i*(2*math.pi)/self.circle_steps + math.pi
+
+                dx = self.circle_radius * math.cos(theta)
+                dy = self.circle_radius * math.sin(theta)
+                # prepare msgs
+                updated_position = FlightNav()
+
+                # set values
+                updated_position.target = 1
+                updated_position.control_frame = 0
+                updated_position.pos_xy_nav_mode=2
+                updated_position.target_pos_x = self.cog_loc_x_init + dx + self.circle_radius
+                updated_position.target_pos_y = self.cog_loc_y_init + dy
+
+
+                # send msgs
+                self.nav1_pub.publish(updated_position)
+                self.nav2_pub.publish(updated_position)
+
+            else:
+                print(f"step {i+1} of {self.circle_steps}")
+                theta = -(i-self.circle_steps)*(2*math.pi)/self.circle_steps
+
+                dx = self.circle_radius * math.cos(theta)
+                dy = self.circle_radius * math.sin(theta)
+                # prepare msgs
+                updated_position = FlightNav()
+
+                # set values
+                updated_position.target = 1
+                updated_position.control_frame = 0
+                updated_position.pos_xy_nav_mode=2
+                updated_position.target_pos_x = self.cog_loc_x_init + dx - self.circle_radius
+                updated_position.target_pos_y = self.cog_loc_y_init + dy
+
+
+                # send msgs
+                self.nav1_pub.publish(updated_position)
+                self.nav2_pub.publish(updated_position)
+            r.sleep()
+            i+=1
+        print("task finished")
+
+if __name__=="__main__":
+    try:
+        fig8_demo = Fig8Demo()
+        fig8_demo.main()
+    except rospy.ROSInterruptException: 
+        pass