├── src └── my_bearing_formation │ ├── my_bearing_formation │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-38.pyc │ │ └── agent_i.cpython-38.pyc │ ├── visualizer.py │ └── agent_i.py │ ├── resource │ ├── my_bearing_formation │ └── rviz_config.rviz │ ├── setup.cfg │ ├── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py │ ├── package.xml │ ├── setup.py │ └── launch │ └── bearing_formation.launch.py ├── plot_csv.py └── README.md /src/my_bearing_formation/my_bearing_formation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/my_bearing_formation/resource/my_bearing_formation: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/my_bearing_formation/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/my_bearing_formation 3 | [install] 4 | install_scripts=$base/lib/my_bearing_formation 5 | -------------------------------------------------------------------------------- /src/my_bearing_formation/my_bearing_formation/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LorenzoBalandi/Robot-Formation-Control/HEAD/src/my_bearing_formation/my_bearing_formation/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /src/my_bearing_formation/my_bearing_formation/__pycache__/agent_i.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LorenzoBalandi/Robot-Formation-Control/HEAD/src/my_bearing_formation/my_bearing_formation/__pycache__/agent_i.cpython-38.pyc -------------------------------------------------------------------------------- /src/my_bearing_formation/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /src/my_bearing_formation/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /src/my_bearing_formation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | my_bearing_formation 5 | 0.0.0 6 | ROS package for DAS project 1 7 | lorenzo 8 | TODO: License declaration 9 | 10 | rclpy 11 | std_msgs 12 | ros2launch 13 | geometry_msgs 14 | visualization_msgs 15 | 16 | ament_copyright 17 | ament_flake8 18 | ament_pep257 19 | python3-pytest 20 | 21 | 22 | ament_python 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/my_bearing_formation/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /src/my_bearing_formation/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from glob import glob 3 | 4 | package_name = 'my_bearing_formation' 5 | scripts = ['agent_i','visualizer'] 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ('share/' + package_name, glob('launch/*.launch.py')), 16 | ('share/' + package_name, glob('resource/*.rviz')) 17 | ], 18 | install_requires=['setuptools'], 19 | zip_safe=True, 20 | maintainer='lorenzo', 21 | maintainer_email='lorenzo.balandi@studio.unibo.it', 22 | description='ROS package for DAS project 1', 23 | license='TODO: License declaration', 24 | tests_require=['pytest'], 25 | entry_points={ 26 | 'console_scripts': [ 27 | '{1} = {0}.{1}:main'.format(package_name, script) for script in scripts 28 | ], 29 | }, 30 | ) 31 | 32 | -------------------------------------------------------------------------------- /plot_csv.py: -------------------------------------------------------------------------------- 1 | 2 | # 11/05/2022 3 | # IN-LP 4 | # 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | import signal 8 | import os 9 | signal.signal(signal.SIGINT, signal.SIG_DFL) 10 | 11 | 12 | _, _, files = next(os.walk("./_csv_file")) 13 | NN = len(files) 14 | 15 | xx_csv = {} 16 | Tlist = [] 17 | 18 | for ii in range(NN): 19 | xx_csv[ii] = np.genfromtxt("_csv_file/agent_{}.csv".format(ii), delimiter=',').T 20 | Tlist.append(xx_csv[ii].shape[1]) 21 | 22 | n_x = xx_csv[ii].shape[0] 23 | print(n_x) 24 | Tmax = min(Tlist) 25 | 26 | xx = np.zeros((NN*n_x,Tmax)) 27 | 28 | for ii in range(NN): 29 | for jj in range(n_x): 30 | index_ii = ii*n_x+jj 31 | xx[index_ii,:] = xx_csv[ii][jj][:Tmax] # useful to remove last samples 32 | 33 | plt.figure() 34 | for x in xx: 35 | plt.plot(range(Tmax), x) 36 | 37 | block_var = False if n_x < 3 else True 38 | plt.show(block=block_var) 39 | 40 | 41 | if 1 and n_x == 2: # animation 42 | plt.figure() 43 | dt = 4 # sub-sampling of the plot horizon 44 | #dt = 3 45 | for tt in range(0,Tmax,dt): 46 | xx_tt = xx[:,tt].T 47 | for ii in range(NN): 48 | index_ii = ii*n_x + np.arange(n_x) 49 | xx_ii = xx_tt[index_ii] 50 | plt.plot(xx_ii[0],xx_ii[1], marker='o', markersize=15, fillstyle='none', color = 'tab:red') 51 | 52 | 53 | axes_lim = (np.min(xx)-1,np.max(xx)+1) 54 | #axes_lim = (-5,5) 55 | plt.xlim(axes_lim); plt.ylim(axes_lim) 56 | plt.plot(xx[0:n_x*NN:n_x,:].T,xx[1:n_x*NN:n_x,:].T, alpha=0.3) # uncomment to show trajectories 57 | 58 | plt.axis('equal') 59 | plt.text(0,3,"iter: {}".format(tt)) # write the iteration 60 | plt.text(0,2.5,"time: {}s".format(tt*5e-2)) # write the time (iteration*sampling time defined in the launch file) 61 | plt.grid() 62 | #plt.show(block=True) 63 | plt.show(block=False) 64 | #plt.pause(0.1) 65 | plt.pause(0.0001) 66 | if tt < Tmax - dt - 1: 67 | plt.clf() 68 | plt.show() 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot-Formation-Control 2 | Formation control for a multi-agent system in a bidimensional space. Various types of formations available. 3 | The implemented control laws are described in this [paper](https://arxiv.org/pdf/1506.05636.pdf). The approach is a **bearing-based** one. 4 | This project was developed as course project for the course *Distributed Autonomous Systems* at University of Bologna. 5 | 6 | The main files of the project are the *launch file*, which launchs all the agents as ROS nodes, and the *agent_i.py*, which is is executed by each agent. 7 | The agents are divided into leaders and followers, the system is distributed because each agent uses only information from its neighbors. The communication is described by a fixed graph. 8 | 9 | To define the **formation type**, modify the following line in the launch file: 10 | ``` 11 | formation_type = 'square' # <---- modify this to change formation type 12 | ``` 13 | 14 | Formation types: 15 | - `square` <--- square formation, fixed 16 | - `moving_square` <--- square formation, leaders move with constant velocity (piecewise constant) 17 | - `letters` <--- the agents forms, one, after the other, the letters of the word BALA 18 | - `moving_square_circle` <--- square formation, leaders move following circular pattern (TIME-VARYING LEADERS VELOCITY CASE) 19 | 20 | To compile and execute: 21 | ``` 22 | source /opt/ros/foxy/setup.bash 23 | . install/setup.bash 24 | colcon build && ros2 launch my_bearing_formation bearing_formation.launch.py 25 | ``` 26 | 27 | **Visualization**: each simulation will generate a *csv file* for each agent, by executing the *plot_cvs.py* file these files are read and the trajectories are plotted using matplotlib. 28 | For alle the formation types except letters, also Rviz visualizaion is available during the simulation execution. 29 | 30 | Some **videos**: 31 | 32 | https://user-images.githubusercontent.com/100198704/180421279-d233fd80-b753-4803-a8ed-bf9a0f110884.mp4 33 | 34 | 35 | 36 | 37 | https://user-images.githubusercontent.com/100198704/180422646-84dbcf8c-f15a-4a90-98b6-c3eb75822721.mp4 38 | 39 | 40 | 41 | https://user-images.githubusercontent.com/100198704/180422681-af51afea-3818-4b78-9bce-656de6f01a3e.mp4 42 | 43 | **Further improvements**: 44 | - each agent computes the bearings relative to neighbors only 45 | - the uniqueness of the formation is verified in a reliable way also in the letters case 46 | - control with integral action 47 | -------------------------------------------------------------------------------- /src/my_bearing_formation/my_bearing_formation/visualizer.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from visualization_msgs.msg import Marker 4 | from geometry_msgs.msg import Pose 5 | from std_msgs.msg import Float32MultiArray as msg_float 6 | 7 | class Visualizer(Node): 8 | 9 | def __init__(self): 10 | super().__init__('visualizer', 11 | allow_undeclared_parameters=True, 12 | automatically_declare_parameters_from_overrides=True) 13 | 14 | # Get parameters from launcher 15 | self.agent_id = self.get_parameter('agent_id').value 16 | self.communication_time = self.get_parameter('communication_time').value 17 | 18 | 19 | ####################################################################################### 20 | # Let's subscribe to the topic we want to visualize 21 | 22 | self.subscription = self.create_subscription( 23 | msg_float, 24 | '/topic_{}'.format(self.agent_id), 25 | self.listener_callback, 26 | 10) 27 | 28 | ####################################################################################### 29 | 30 | # Create the publisher that will communicate with Rviz 31 | self.timer = self.create_timer(self.communication_time, self.publish_data) 32 | self.publisher = self.create_publisher( 33 | Marker, 34 | '/visualization_topic', 35 | 1) 36 | 37 | # Initialize the current_pose method (in this example you can also use list or np.array) 38 | self.current_pose = Pose() 39 | 40 | 41 | def listener_callback(self, msg): 42 | # store (and rearrange) the received message 43 | self.current_pose.position.x = msg.data[1] 44 | self.current_pose.position.y = msg.data[2] 45 | # self.current_pose.position.z = msg.data[3] 46 | 47 | def publish_data(self): 48 | if self.current_pose.position is not None: 49 | # Set the type of message to send to Rviz -> Marker 50 | # (see http://docs.ros.org/en/noetic/api/visualization_msgs/html/index-msg.html) 51 | marker = Marker() 52 | 53 | # Select the name of the reference frame, without it markers will be not visualized 54 | marker.header.frame_id = 'my_frame' 55 | marker.header.stamp = self.get_clock().now().to_msg() 56 | 57 | # Select the type of marker 58 | marker.type = Marker.SPHERE 59 | 60 | # set the pose of the marker (orientation is omitted in this example) 61 | marker.pose.position.x = self.current_pose.position.x 62 | marker.pose.position.y = self.current_pose.position.y 63 | marker.pose.position.z = self.current_pose.position.z 64 | 65 | # Select the marker action (ADD, DELATE) 66 | marker.action = Marker.ADD 67 | 68 | # Select the namespace of the marker 69 | marker.ns = 'agents' 70 | 71 | # Let the marker be unique by setting its id 72 | marker.id = self.agent_id 73 | 74 | # Specify the scale of the marker 75 | scale = 0.2 76 | marker.scale.x = scale 77 | marker.scale.y = scale 78 | marker.scale.z = scale 79 | 80 | # Specify the color of the marker as RGBA 81 | color = [0.0, 0.5, 0.5, 1.0] 82 | if self.agent_id < 2: # if leaders 83 | color = [1.0, 0.0, 0.0, 1.0] 84 | 85 | marker.color.r = color[0] 86 | marker.color.g = color[1] 87 | marker.color.b = color[2] 88 | marker.color.a = color[3] 89 | 90 | # Let's publish the marker 91 | self.publisher.publish(marker) 92 | 93 | def main(): 94 | rclpy.init() 95 | 96 | visualizer = Visualizer() 97 | 98 | try: 99 | rclpy.spin(visualizer) 100 | except KeyboardInterrupt: 101 | print("----- Visualizer stopped cleanly -----") 102 | finally: 103 | rclpy.shutdown() 104 | 105 | if __name__ == '__main__': 106 | main() -------------------------------------------------------------------------------- /src/my_bearing_formation/resource/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | Splitter Ratio: 0.5 11 | Tree Height: 582 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz_default_plugins/Grid 31 | Color: 211; 215; 207 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Class: rviz_default_plugins/Marker 47 | Enabled: true 48 | Name: Marker 49 | Namespaces: 50 | agents: true 51 | Topic: 52 | Depth: 5 53 | Durability Policy: Volatile 54 | History Policy: Keep Last 55 | Reliability Policy: Reliable 56 | Value: /visualization_topic 57 | Value: true 58 | Enabled: true 59 | Global Options: 60 | Background Color: 243; 243; 243 61 | Fixed Frame: my_frame 62 | Frame Rate: 30 63 | Name: root 64 | Tools: 65 | - Class: rviz_default_plugins/Interact 66 | Hide Inactive Objects: true 67 | - Class: rviz_default_plugins/MoveCamera 68 | - Class: rviz_default_plugins/Select 69 | - Class: rviz_default_plugins/FocusCamera 70 | - Class: rviz_default_plugins/Measure 71 | Line color: 128; 128; 0 72 | - Class: rviz_default_plugins/SetInitialPose 73 | Topic: 74 | Depth: 5 75 | Durability Policy: Volatile 76 | History Policy: Keep Last 77 | Reliability Policy: Reliable 78 | Value: /initialpose 79 | - Class: rviz_default_plugins/SetGoal 80 | Topic: 81 | Depth: 5 82 | Durability Policy: Volatile 83 | History Policy: Keep Last 84 | Reliability Policy: Reliable 85 | Value: /goal_pose 86 | - Class: rviz_default_plugins/PublishPoint 87 | Single click: true 88 | Topic: 89 | Depth: 5 90 | Durability Policy: Volatile 91 | History Policy: Keep Last 92 | Reliability Policy: Reliable 93 | Value: /clicked_point 94 | Transformation: 95 | Current: 96 | Class: rviz_default_plugins/TF 97 | Value: true 98 | Views: 99 | Current: 100 | Class: rviz_default_plugins/Orbit 101 | Distance: 15.285283088684082 102 | Enable Stereo Rendering: 103 | Stereo Eye Separation: 0.05999999865889549 104 | Stereo Focal Distance: 1 105 | Swap Stereo Eyes: false 106 | Value: false 107 | Focal Point: 108 | X: 0 109 | Y: 0 110 | Z: 0 111 | Focal Shape Fixed Size: true 112 | Focal Shape Size: 0.05000000074505806 113 | Invert Z Axis: false 114 | Name: Current View 115 | Near Clip Distance: 0.009999999776482582 116 | Pitch: 0.20539824664592743 117 | Target Frame: 118 | Value: Orbit (rviz) 119 | Yaw: 0.0053977956995368 120 | Saved: ~ 121 | Window Geometry: 122 | Displays: 123 | collapsed: true 124 | Height: 838 125 | Hide Left Dock: true 126 | Hide Right Dock: true 127 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002d1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002d1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003f3000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 128 | Selection: 129 | collapsed: false 130 | Tool Properties: 131 | collapsed: false 132 | Views: 133 | collapsed: true 134 | Width: 1011 135 | X: 478 136 | Y: 82 137 | -------------------------------------------------------------------------------- /src/my_bearing_formation/launch/bearing_formation.launch.py: -------------------------------------------------------------------------------- 1 | # LB, FS - DAS 2022 Project 2 | # LAUNCH FILE 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | import numpy as np 7 | import networkx as nx 8 | import os 9 | from ament_index_python.packages import get_package_share_directory 10 | 11 | ''' 12 | From this launch file you can select the formation of the agents. 13 | Formation type: 14 | square <--- square formation, fixed 15 | moving_square <--- square formation, leaders move with constant velocity (piecewise constant) 16 | letters <--- the agents forms, one, after the other, the letters of the word BALA 17 | moving_square_circle <--- square formation, leaders move following circular pattern (TIME-VARYING LEADERS VELOCITY CASE) 18 | 19 | The formation type will influence: 20 | -the number of agents required to do it 21 | -the number of iterations (time) required to see it well 22 | -the control gains 23 | -the initial conditions of the leaders 24 | -initial conditions of the follower (they are random in all cases, but in letter formation is more sparse) 25 | The number of leaders is always 2. 26 | ''' 27 | 28 | formation_type = 'square' # <---- modify this to change formation type 29 | RVIZ = True 30 | 31 | def compute_adj(NN,p_ER): 32 | 33 | I_NN = np.eye(NN) # np.identity 34 | Adj = np.random.binomial(1,p_ER, (NN,NN)) # matrix in which each element is 0 or 1 with p_ER probability 35 | #print(Adj) 36 | Adj = np.logical_or(Adj,Adj.T) # get a simmetric matrix 37 | #print(Adj) 38 | Adj = np.multiply(Adj, np.logical_not(I_NN)).astype(int) # remove self loops by multiplying by a matrix with 0 on diags 39 | 40 | print("Adjacency:\n", Adj) 41 | 42 | return Adj 43 | 44 | def compute_adj_square(): 45 | return np.array([ 46 | [0, 1, 0, 1], 47 | [1, 0, 1, 1], 48 | [0, 1, 0, 1], 49 | [1, 1, 1, 0] 50 | ]) 51 | 52 | def generate_launch_description(): 53 | 54 | COMM_TIME = 5e-2 # communication time period 55 | 56 | global formation_type 57 | global RVIZ 58 | 59 | if formation_type == 'square': 60 | MAXITERS = 750 61 | NN = 4 62 | Kp = 110 63 | Kv = 110 64 | x_init_leaders = np.array([0.0, 0.0, 0.0, 1.0]) # define initial positions for the leaders (fixed positions) 65 | Adj = compute_adj_square() 66 | 67 | elif formation_type == 'moving_square': 68 | MAXITERS = 1500 69 | NN = 4 70 | Kp = 110 71 | Kv = 110 72 | x_init_leaders = np.array([0.0, 0.0, 0.0, 1.0]) 73 | Adj = compute_adj_square() 74 | 75 | elif formation_type == 'letters': 76 | RVIZ = False # not supported 77 | MAXITERS = 2000 # 500 iters for each letter 78 | NN = 10 79 | Kp = 50 80 | Kv = 35 81 | x_init_leaders = np.array([0.0, 0.0, 0.0, 10.0]) 82 | Adj = compute_adj(NN,0.7) 83 | 84 | elif formation_type == 'moving_square_circle': 85 | MAXITERS = 10000 86 | NN = 4 87 | Kp = 25 88 | Kv = 25 89 | x_init_leaders = np.array([0.0, 0.0, 0.0, 1.0]) 90 | Adj = compute_adj_square() 91 | 92 | else: 93 | print("Insert a valid formation type!") 94 | exit() # terminate the program 95 | 96 | # first Nl agents are leaders, other agents are Nf followers 97 | Nl = 2 # number of leaders 98 | Nf = NN-Nl # number of followers 99 | n_x = 2 # dimension of x_i 100 | # n_v = n_x # dimension of v_i (same as x_i) 101 | # n_u = n_x # dimension of u_i (same as x_i) 102 | 103 | # define initial positions for the followers (random positions) 104 | x_init_followers = np.random.rand(n_x*Nf)*5 105 | # initial velocities are assumed to be 0 for all agents 106 | x_init = np.hstack((x_init_leaders,x_init_followers)) 107 | 108 | launch_description = [] # Append here your nodes 109 | 110 | if RVIZ == True: 111 | rviz_config_dir = get_package_share_directory('my_bearing_formation') 112 | rviz_config_file = os.path.join(rviz_config_dir, 'rviz_config.rviz') 113 | 114 | launch_description.append( 115 | Node( 116 | package='rviz2', 117 | node_executable='rviz2', 118 | arguments=['-d', rviz_config_file], 119 | # output='screen', 120 | # prefix='xterm -title "rviz2" -hold -e' 121 | )) 122 | 123 | for ii in range(NN): 124 | Adj_ii = Adj[:, ii].tolist() # extract column ii 125 | N_ii = np.nonzero(Adj[:, ii])[0].tolist() # indices of neighbors of agent ii 126 | ii_index = ii*n_x + np.arange(n_x) 127 | 128 | if ii < Nl: # if agent is a leader 129 | agent_type = "leader" 130 | x_init_ii = x_init[ii_index].flatten().tolist() 131 | else: 132 | agent_type = "follower" 133 | x_init_ii = x_init[ii_index].flatten().tolist() 134 | 135 | launch_description.append( 136 | Node( 137 | package='my_bearing_formation', 138 | node_namespace ='agent_{}'.format(ii), 139 | node_executable='agent_i', 140 | parameters=[{ 141 | 'agent_id': ii, 142 | 'max_iters': MAXITERS, 143 | 'communication_time': COMM_TIME, 144 | 'neigh': N_ii, 145 | 'Adj_ii' : Adj_ii, 146 | 'x_init': x_init_ii, 147 | 'agent_type' : agent_type, # "leader" or "follower" 148 | 'Kp' : Kp, # position control gain 149 | 'Kv' : Kv, # velocity control gain 150 | 'NN' : NN, # total number of agents 151 | 'formation_type' : formation_type # formation type (required for bearings computation) 152 | }], 153 | output='screen', 154 | prefix='xterm -title "agent_{}" -hold -e'.format(ii) 155 | )) 156 | if RVIZ == True: 157 | launch_description.append( 158 | Node( 159 | package='my_bearing_formation', 160 | node_namespace='agent_{}'.format(ii), 161 | node_executable='visualizer', 162 | parameters=[{ 163 | 'agent_id': ii, 164 | 'communication_time': COMM_TIME, 165 | }], 166 | )) 167 | 168 | return LaunchDescription(launch_description) -------------------------------------------------------------------------------- /src/my_bearing_formation/my_bearing_formation/agent_i.py: -------------------------------------------------------------------------------- 1 | # LB, FS, June 2022 2 | # DAS Project 1 Task 2 3 | # AGENT_I 4 | 5 | from time import sleep 6 | import numpy as np 7 | import rclpy 8 | from rclpy.node import Node 9 | from std_msgs.msg import Float32MultiArray as msg_float 10 | 11 | ''' 12 | Possibili improvements: 13 | - calcolare P_star solo per i neighbours, ora ogni agente lo calcola rispetto a tutti 14 | gli altri agenti ma poi è usato solo quello dei neighbors 15 | - verificare l'unicità della formazione controllando la non singolarità di Bff (vedi paper) 16 | ''' 17 | 18 | radius = 2 19 | angular_vel = 0.001 20 | vel = 0.005 # 21 | time = 0 # variable used to describe the circle that leaders follow 22 | 23 | ''' 24 | Sincronizzazione risolta 25 | ''' 26 | 27 | ############################### FUNCTIONS ############################### 28 | def formation_update(tt, x_init, agent_type, dt, x_i, neigh, data, P_star, i, Kp, Kv, formation_type): 29 | # solo i follower devono eseguire questo (cambiare in agent_i) 30 | """ 31 | dt = discretization step 32 | x_i = state of agent i (n_x positions, n_x velocities) 33 | neigh = list of neihbors 34 | data = state of neighbors 35 | a_i = i-th row of the adjacency matrix 36 | """ 37 | global time # time comes from the global scope 38 | 39 | # LEADERS 40 | if agent_type == "leader": 41 | for j in neigh: 42 | data[j].pop(0) # we have to pop from the list to avoid synchronization problems 43 | if formation_type == 'moving_square': 44 | if tt > 750: 45 | x_i[0] = -vel*time # x position of leader 46 | x_i[2] = -vel # x velocity of leader 47 | time += 1 48 | elif formation_type == 'moving_square_circle': 49 | if tt > 750 and tt < 8000: 50 | # define the circle to be followed by leaders 51 | x_i[0] = radius*np.cos(angular_vel*time) + x_init[0] - radius # x pos 52 | x_i[1] = radius*np.sin(angular_vel*time) + x_init[1] # y pos 53 | x_i[2] = -radius*angular_vel*np.sin(angular_vel*time) # x vel 54 | x_i[3] = radius*angular_vel*np.cos(angular_vel*time) # y vel 55 | x_i[4] = -radius*(angular_vel**2)*np.cos(angular_vel*time) # x acc 56 | x_i[5] = -radius*(angular_vel**2)*np.sin(angular_vel*time) # y acc 57 | time += 1 58 | 59 | return x_i 60 | 61 | # FOLLOWERS (leaders won't arrive here because they encounter a return before) 62 | n_x = len(x_i)//3 # number of velocities and positions 63 | p_i = x_i[0:n_x] 64 | v_i = x_i[n_x:2*n_x] 65 | a_i = x_i[2*n_x:] 66 | pdot_i = np.zeros(p_i.shape) 67 | vdot_i = np.zeros(v_i.shape) # derivative of the velocity 68 | 69 | if formation_type != 'moving_square_circle': 70 | for j in neigh: 71 | #print("data[j] " , data[j]) # [[0,0,0,0,0]] 72 | #print("\tlen data of neigh {}: {} ".format(j,len(data[j]))) 73 | #print("\tdata of neigh {} : {} ".format(j,data[j]) ) 74 | x_j = np.array(data[j].pop(0)[1:]) # warning: pop remove the elements, the length will change! 75 | #print("data j after pop: " , data[j]) 76 | p_j = x_j[0:n_x] 77 | v_j = x_j[n_x:2*n_x] 78 | #print("pj: " , p_j) 79 | #print("vj: " , v_j) 80 | 81 | # check if the diagonals are to be considered 82 | vdot_i += - P_star[i*n_x:i*n_x+n_x, j*n_x:j*n_x+n_x] @ (Kp*(p_i-p_j)+Kv*(v_i-v_j)) # from the paper 83 | # xdot_i += - a_i[j]*(x_i - x_j) # from slides of laplacian consensus 84 | 85 | pdot_i = v_i 86 | # Forward Euler 87 | p_i += dt*pdot_i 88 | v_i += dt*vdot_i 89 | # x_i += dt*xdot_i 90 | x_i = np.hstack((p_i,v_i,a_i)) 91 | 92 | else: 93 | KKi = np.zeros((n_x,n_x)) 94 | for j in neigh: 95 | x_j = np.array(data[j].pop(0)[1:]) # warning: pop remove the elements, the length will change! 96 | p_j = x_j[0:n_x] 97 | v_j = x_j[n_x:2*n_x] 98 | a_j = x_j[2*n_x:] 99 | KKi += P_star[i*n_x:i*n_x+n_x, j*n_x:j*n_x+n_x] 100 | vdot_i += - P_star[i*n_x:i*n_x+n_x, j*n_x:j*n_x+n_x] @ (Kp*(p_i-p_j)+Kv*(v_i-v_j) - a_j) # from the paper 101 | print(np.linalg.det(KKi)) 102 | 103 | vdot_i = np.linalg.inv(KKi) @ vdot_i 104 | pdot_i = v_i 105 | # Forward Euler 106 | p_i += dt*pdot_i 107 | v_i += dt*vdot_i 108 | a_i = vdot_i 109 | # x_i += dt*xdot_i 110 | x_i = np.hstack((p_i,v_i,a_i)) 111 | 112 | 113 | return x_i 114 | 115 | 116 | def writer(file_name, string): 117 | """ 118 | inner function for logging 119 | """ 120 | file = open(file_name, "a") # "a" is for append 121 | file.write(string) 122 | file.close() 123 | 124 | 125 | def define_formation(NN,shape,n_x): 126 | """ 127 | define the points that represent the letter to show, 128 | needed only in case of 'letters' formation! 129 | """ 130 | xx = np.zeros((NN,n_x)) # NN rows, n_x columns 131 | 132 | if shape == 'A': # letter A 133 | xx[0] = np.array([0,0]) 134 | xx[1] = np.array([0,10]) 135 | xx[2] = np.array([0,2.5]) 136 | xx[3] = np.array([0,5]) 137 | xx[4] = np.array([0,7.5]) 138 | xx[5] = np.array([2.5,5]) 139 | xx[6] = np.array([2.5,7.5]) 140 | xx[7] = np.array([5,5]) 141 | xx[8] = np.array([7.5,2.5]) 142 | xx[9] = np.array([10,0]) 143 | 144 | elif shape == 'B': # letter B 145 | xx[0] = np.array([0,0]) 146 | xx[1] = np.array([0,10]) 147 | xx[2] = np.array([5,2.5]) 148 | xx[3] = np.array([1.5,3.75]) 149 | xx[4] = np.array([0,5]) 150 | xx[5] = np.array([2.5,6.25]) 151 | xx[6] = np.array([5,7.5]) 152 | xx[7] = np.array([1.25,8.75]) 153 | xx[8] = np.array([2.5,1.25]) 154 | xx[9] = np.array([0,7.5]) 155 | 156 | elif shape == 'L': # letter L 157 | xx[0] = np.array([0,0]) 158 | xx[1] = np.array([0,10]) 159 | xx[2] = np.array([0,3.32]) 160 | xx[3] = np.array([0,4.98]) 161 | xx[4] = np.array([0,6.64]) 162 | xx[5] = np.array([0,8.3]) 163 | xx[6] = np.array([0,1.66]) 164 | xx[7] = np.array([1.66,0]) 165 | xx[8] = np.array([3.32,0]) 166 | xx[9] = np.array([5,0]) 167 | else: 168 | print("Error: unknown shape!") 169 | return xx 170 | 171 | def compute_P_star(NN,n_x,formation_type,letter): 172 | ''' 173 | NN: total number of agents 174 | n_x: dimension of state of each agent 175 | formation_type: type of formation to compute 176 | letter: the letter to show (ignored if not 'letters' formation type) 177 | ''' 178 | # compute matrix of desired bearings 179 | g_star = np.zeros((NN,NN,n_x)) 180 | P_star = np.zeros((NN*n_x,NN*n_x)) # orthogonal projection matrix of the desired bearings 181 | xx = np.zeros((NN,n_x)) # NN rows, n_x columns 182 | # desired positions (formation) 183 | if formation_type != 'letters': 184 | xx[0] = np.array([0.0, 0.0]) 185 | xx[1] = np.array([0.0, 1.0]) 186 | xx[2] = np.array([1.0, 1.0]) 187 | xx[3] = np.array([1.0, 0.0]) 188 | else: 189 | xx = define_formation(NN,letter,n_x) 190 | 191 | for ii in range(NN): 192 | for jj in range(NN): 193 | if ii != jj: # bearings of an agent wrt itself have no meaning 194 | g_star[ii,jj,:] = xx[jj]-xx[ii] 195 | norm = np.linalg.norm(xx[jj]-xx[ii]) 196 | if norm == 0: 197 | print("ERROR: NORM IS 0") 198 | if norm != 0: # check if the norm is 0 199 | g_star[ii,jj,:] = g_star[ii,jj,:]/norm 200 | #P_gij = np.eye(n_x) - g_star[ii,jj]@g_star[ii,jj].T 201 | P_gij = np.eye(n_x) - np.outer(g_star[ii,jj],g_star[ii,jj].T) 202 | #print(np.shape(P_gij)) 203 | P_star[ii*n_x:ii*n_x+n_x, jj*n_x:jj*n_x+n_x] = P_gij # compose the matrix by adding sub-matrices 204 | 205 | #print("g_star:") 206 | #for i in range(n_x): 207 | #print(g_star[:,:,i]) 208 | 209 | #print("P_star:") 210 | #print(np.shape(P_star)) 211 | #print(P_star) 212 | 213 | return P_star 214 | 215 | 216 | ############################### AGENT NODE ############################### 217 | 218 | class Agent(Node): # inherited from class Node 219 | def __init__(self): 220 | # take constructor of class Node and modify it 221 | super().__init__('agent', 222 | allow_undeclared_parameters=True, 223 | automatically_declare_parameters_from_overrides=True) 224 | 225 | # Get parameters from launcher 226 | self.agent_id = self.get_parameter('agent_id').value 227 | self.neigh = self.get_parameter('neigh').value 228 | self.agent_type = self.get_parameter('agent_type').value # "leader" or "follower" 229 | print("I'm a ", self.agent_type) 230 | 231 | Adj_ii = self.get_parameter('Adj_ii').value 232 | self.Adj_ii = np.array(Adj_ii) # it returns an n_x by 1 array 233 | 234 | # construct the state: x = [positions, velocities] 235 | #x_i = self.get_parameter('x_init').value 236 | self.x_init = self.get_parameter('x_init').value 237 | self.n_x = len(self.x_init) 238 | self.x_i = np.array(self.x_init) # it returns an n_x by 1 array 239 | v_init = np.zeros(self.n_x) # initial velocities are 0 240 | a_init = np.zeros(self.n_x) # initial accelrations are 0 241 | self.x_i = np.hstack((self.x_i, v_init, a_init)) # add velocities and acc to state, now the state has dim 3*n_x 242 | print("Initial state: " , self.x_i) 243 | 244 | self.max_iters = self.get_parameter('max_iters').value 245 | self.communication_time = self.get_parameter('communication_time').value 246 | self.NN = self.get_parameter('NN').value # each agent must know the total number of agents 247 | self.Kp = self.get_parameter('Kp').value 248 | self.Kv = self.get_parameter('Kv').value 249 | self.formation_type = self.get_parameter('formation_type').value 250 | print("\nFormation type:") 251 | print(self.formation_type) 252 | self.tt = 0 253 | 254 | # Desired bearings 255 | # g_star is a matrix with element i,j computed as in the paper, elements on diagonal are 0 256 | self.P_star = compute_P_star(self.NN,self.n_x,self.formation_type,'B') 257 | 258 | # create logging file 259 | self.file_name = "_csv_file/agent_{}.csv".format(self.agent_id) 260 | file = open(self.file_name, "w+") # 'w+' needs to create file and open in writing mode if doesn't exist 261 | file.close() 262 | 263 | # initialize subscription dictionary 264 | self.subscriptions_list = {} # curly brackets --> dictionary 265 | 266 | # create a subscription to each neighbor 267 | for j in self.neigh: 268 | topic_name = '/topic_{}'.format(j) 269 | self.subscriptions_list[j] = self.create_subscription( 270 | msg_float, 271 | topic_name, 272 | lambda msg, node = j: self.listener_callback(msg, node), 273 | 10) 274 | 275 | # create the publisher 276 | self.publisher_ = self.create_publisher( 277 | msg_float, 278 | '/topic_{}'.format(self.agent_id), 279 | 10) 280 | 281 | self.timer = self.create_timer(self.communication_time, self.timer_callback) 282 | 283 | # initialize a dictionary with the list of received messages from each neighbor j [a queue] 284 | self.received_data = { j: [] for j in self.neigh } 285 | 286 | print("Setup of agent {} complete".format(self.agent_id)) 287 | 288 | 289 | def listener_callback(self, msg, node): 290 | self.received_data[node].append(list(msg.data)) 291 | 292 | def timer_callback(self): 293 | # Initialize a message of type float 294 | msg = msg_float() 295 | 296 | if self.tt == 0: # Let the publisher start at the first iteration 297 | msg.data = [float(self.tt)] 298 | 299 | # for element in self.x_i: 300 | # msg.data.append(float(element)) 301 | [msg.data.append(float(element)) for element in self.x_i] 302 | 303 | self.publisher_.publish(msg) 304 | self.tt += 1 305 | 306 | # log files 307 | # 1) visualize on the terminal 308 | string_for_logger = [round(i,4) for i in msg.data.tolist()[1:]] 309 | print("Iter = {} \t Value = {}".format(int(msg.data[0]), string_for_logger)) 310 | 311 | # 2) save on file 312 | data_for_csv = msg.data.tolist().copy() 313 | data_for_csv = [str(round(element,4)) for element in data_for_csv[1:self.n_x+1]] # pick only the position 314 | data_for_csv = ','.join(data_for_csv) 315 | writer(self.file_name,data_for_csv+'\n') 316 | 317 | else: 318 | # Check if lists are nonempty 319 | all_received = all(self.received_data[j] for j in self.neigh) # check if all neighbors' have been received 320 | 321 | sync = False 322 | # Have all messages at time t-1 arrived? 323 | if all_received: 324 | sync = all(self.tt-1 == self.received_data[j][0][0] for j in self.neigh) # True if all True 325 | 326 | if sync: 327 | DeltaT = self.communication_time/10 328 | 329 | # In case of letters formation we have to recompute the bearings when we change the letter to show 330 | if self.formation_type == 'letters': 331 | # Compute desired bearings 332 | if (self.tt == 500): 333 | self.P_star = compute_P_star(self.NN,self.n_x,self.formation_type,'A') 334 | elif (self.tt == 1000): 335 | self.P_star = compute_P_star(self.NN,self.n_x,self.formation_type,'L') 336 | elif (self.tt == 1500): 337 | self.P_star = compute_P_star(self.NN,self.n_x,self.formation_type,'A') 338 | 339 | self.x_i = formation_update(self.tt, self.x_init, self.agent_type, DeltaT, self.x_i, self.neigh, 340 | self.received_data, self.P_star, self.agent_id, self.Kp, self.Kv, self.formation_type) 341 | 342 | # publish the updated message 343 | msg.data = [float(self.tt)] 344 | [msg.data.append(float(element)) for element in self.x_i] 345 | self.publisher_.publish(msg) 346 | 347 | # save data on csv file 348 | data_for_csv = msg.data.tolist().copy() 349 | data_for_csv = [str(round(element,4)) for element in data_for_csv[1:self.n_x+1]] # pick only the position 350 | data_for_csv = ','.join(data_for_csv) 351 | writer(self.file_name,data_for_csv+'\n') 352 | 353 | string_for_logger = [round(i,4) for i in msg.data.tolist()[1:]] 354 | print("Iter = {} \t Value = {}".format(int(msg.data[0]), string_for_logger)) 355 | 356 | # Stop the node if tt exceeds MAXITERS 357 | if self.tt > self.max_iters: 358 | print("\nMAXITERS reached") 359 | sleep(3) # [seconds] 360 | self.destroy_node() 361 | 362 | # update iteration counter 363 | self.tt += 1 364 | 365 | ############################### MAIN ############################### 366 | 367 | def main(args=None): 368 | rclpy.init(args=args) 369 | 370 | agent = Agent() 371 | print("Agent {:d} -- Waiting for sync.".format(agent.agent_id)) 372 | sleep(0.5) 373 | print("GO!\n") 374 | 375 | try: 376 | rclpy.spin(agent) 377 | except KeyboardInterrupt: 378 | print("----- Node stopped cleanly -----") 379 | finally: 380 | rclpy.shutdown() 381 | 382 | if __name__ == '__main__': 383 | main() --------------------------------------------------------------------------------