{'fraction_non_alphanumeric': 0.02749032030975009, 'fraction_numerical': 0.0031326997536078845, 'mean_word_length': 4.959932871827145, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
ChoiRbot: A ROS 2 Toolbox for Cooperative Robotics

In this paper, we introduce ChoiRbot, a toolbox for distributed cooperative robotics based on the novel Robot Operating System (ROS) 2. ChoiRbot provides a fully-functional toolset to execute complex distributed multi-robot tasks, either in simulation or experimentally, with a particular focus on networks of heterogeneous robots without a central coordinator. Thanks to its modular structure, ChoiRbot allows for a highly straight implementation of optimization-based distributed control schemes, such as distributed optimal control, model predictive control, task assignment, in which local computation and communication with neighboring robots are alternated. To this end, the toolbox provides functionalities for the solution of distributed optimization problems. The package can be also used to implement distributed feedback laws that do not need optimization features but do require the exchange of information among robots. The potential of the toolbox is illustrated with simulations and experiments on distributed robotics scenarios with mobile ground robots. The ChoiRbot toolbox is available at https://github.com/OPT4SMART/choirbot.


I. INTRODUCTION
There is an increasing interest towards several applications in cooperative robotics, such as task assignment/allocation, model predictive control, formation control. In peer-to-peer robotic networks it is often desirable to solve these problems without a coordinating unit. Robots in the network, relying on their limited knowledge of the problem data, have to exploit local computation and communication capabilities to solve the complex task, which often requires also the solution of optimization problems. Since its introduction, the Robot Operating System (ROS), [1], has gained popularity among robotics researchers as an open source framework for the development of robotics applications. Nowadays, ROS 2 is extending ROS capabilities, paving the way to real-time control systems and large-scale distributed architectures, [2]. While several theoretical frameworks have been proposed to solve optimization problems over networks of cooperating robots, see, e.g. the survey [3] and references therein, few architectures have been proposed to simulate and run experiments on teams of heterogeneous robots communicating according to arbitrary graphs and aiming at the cooperative solution of complex tasks.

A. Related Work
The ROS framework has been used as a building brick for a plethora of robotics applications, and several frameworks have been proposed to simulate and implement control and planning tasks. Authors in [4] propose a constraint-based task specification for robot controllers, defining a task specification language and the related controller. The framework in [5] allows users to create task plans for collaborative robots. Papers [6] and [7] instead propose architectures to simulate and control UAVs, while [8] allows users to write ROS code on a browser and run it on remote robots. On this purpose, it is worth mentioning the Robotarium, [9], a proprietary platform that allows to test and run control algorithms on robotic teams. These frameworks often leverage optimization routines to perform specific tasks. To name a few, the framework in [10] deals with trajectory optimization in dynamic environments, the architecture in [11] approaches search-and-rescue tasks by means of particle swarm optimization while [12] is suited for task allocation scenarios. Finally, in the recent years, robotics researchers started to develop robotic architectures based on the novel ROS 2 framework. Authors in [13] and [14] consider a framework for collaborative robotics, while the paper in [15] discusses an architecture for self-driving cars. The aforementioned frameworks are typically optimized for a specific task, and most of them focus on singlerobot systems. Moreover, the communication in multi-robot networks is often neglected or simulated by means of the resource-demanding all-to-all communication.

B. Contributions
In this paper, we introduce a novel ROS 2 toolbox for cooperative robotics named CHOIRBOT. This toolbox, written in Python, exploits the new functionalities of ROS 2 and provides a comprehensive set of libraries to facilitate multi-robot simulations and experiments. The main focus of CHOIRBOT is on peer-to-peer network of robots, where each robot has its own processor and is able to communicate with the neighboring units according to a given, user-defined graph. Importantly, CHOIRBOT does not require a central coordinating unit and, as such, allows for fully distributed control schemes. In order to maximize extendability and ensure a broad applicability, the toolbox is designed according to a modular structure. In such a way, we demonstrate that several applications of interest are easy to implement using CHOIRBOT, and we discuss in detail a few use cases that have been tested either in simulation or experimentally, namely dynamic task assignment, formation control and containment in leader-follower networks. A distinctive feature of CHOIRBOT is the ability to run general-purpose distributed optimization algorithms. To this end, the DISROPT package [16] has been fully integrated, allowing both for the semantic modeling and solution of optimization problems (locally at a robot) and for the execution of distributed optimization algorithms (cooperatively across all the robots).
The paper is organized as follows. In Section II, we provide a high-level description of the software architecture, while in Section III, additional software details are provided. Section IV discusses two complex scenarios implemented in CHOIRBOT. A basic use case together with implementation details is discussed in V. Finally, simulation and experimental results are provided in Section VI.

II. ARCHITECTURE DESCRIPTION
In this section, we describe the high-level architecture of CHOIRBOT. As already mentioned, the toolbox is modular and its blocks are intended to be combined as needed. We first focus on an overall description of the software and then we describe each block separately.

A. Overview of the Software
CHOIRBOT is written in Python and is based on the Robot Operating System (ROS) 2. An important feature of the new ROS 2 architecture is its ability to run programs in a totally distributed fashion, i.e. without a server acting as message broker [2]. This fits perfectly with the goal of CHOIRBOT of providing a platform for cooperative robotics over peer-to-peer networks without a central coordinating unit.
The toolbox is structured in a three-layer architecture. Specifically, there is a Team Guidance layer, a RoboPlanning layer and a RoboControl layer. The Team Guidance layer is responsible for taking highlevel decisions and for managing the robot lifecycle. The Team Guidance layer uses communication with neighbors in order to perform its tasks. The Roboplanning and Robocontrol layers are responsible for lower-level control actions as driven by the upper layer. Since our main goal is to ease the design and implementation of optimization-based distributed control schemes, the central focus of CHOIRBOT is the Team Guidance layer. In particular, the toolbox provides boilerplate code for distributed computation, i.e. communication among robots over a graph, coordination and optimization algorithms. Apart from a few specific scenarios, we do not provide comprehensive RoboPlanning and RoboControl features, which are robot specific and can be already implemented with existing tools.
In CHOIRBOT, each robotic agent in the network executes three ROS 2 processes, one for each layer. Each process is also associated to a separate ROS 2 node. To guarantee flexibility and code reusability, layers are implemented as Python classes. In the next we provide details about the three layers. A graphical illustration of the software architecture is represented in Figure 1.

B. Team Guidance Layer
The Team Guidance layer is the main entry point of the package. Here we provide an introductory description, while a more detailed analysis of this part of software is delayed to Section III.
The Team Guidance layer is modeled with a Guidance class, whose purpose is only to retrieve the basic information regarding the robotic agent (passed as ROS 2 parameters), to create an instance of the Communicator class to be described in Section III-A and to subscribe to the topic where the robot pose is published. In the current version of CHOIRBOT, the robot position can be either communicated by a Vicon motion tracking system or by a simulator (such as Gazebo). This basic version of the Guidance class is abstract in that it does not implement any guidance logic. However, the package currently provides two possible usable extensions. The first one (implemented as the OptimizationGuidance class) also provides optimization-related functionalities and is the starting point for any optimization-based distributed control scheme such as task assignment / allocation, optimal control, model predictive control. The toolbox can also be used to implement simpler distributed feedback laws that do not require the solution of optimization problems. To this end, we also provide a second extension of the Guidance class (implemented as the DistributedControlGuidance class), where robots repeatedly exchange their current position with their neighbors and compute a control input based on their position and the received positions. The actual form of the control input depends on the specific scenario and is left as an unimplemented method of the class. Currently implemented algorithms are rendezvous, containment, formation control.

C. RoboPlanning, RoboControl and RoboIntegration
Within the CHOIRBOT toolbox, the trajectory planning layer and the control layer do not communicate with the neighboring robotic agents. Instead, each robot has its own planning/control stack, which depends on the dynamics of the single robot and on the chosen control strategy. The base class for trajectory planners is Planner. We provide a point-to-point planner (in 2D or 3D), to be used in conjunction with controllers that are able to steer the robot towards a specified point in the space (e.g. for mobile ground robots). We also provide trajectory planners for aerial robots, which generate quad-rotor trajectories either to reach a desired constant speed or to reach a target point. As for controllers, the base class is Controller. For mobile ground robots, we provide two unicycle controllers that can be used either to reach a target point or to reach a desired velocity. For quadrotors, we provide a controller to stabilize the trajectories generated by the planners.
The CHOIRBOT toolbox is well integrated with simulation environments. In this regard, the Gazebo simulator [17] can be used. In case the user does not want to use external tools, we also provide a dynamics integration layer, named RoboIntegration, which can e.g. be used in conjunction with Rviz for visualization. Currently, there are numerical integrators for point-masses, unicycle robots and quadrotors. New integrators with custom dynamics can be written by extending the Integrator class.
III. EXPLORING THE TEAM GUIDANCE LAYER As already mentioned, almost all functionalities of CHOIRBOT involve the Team Guidance layer. In this section, we explore in more detail this part of software. We begin by analyzing the first important feature of the Team Guidance class, i.e. graph-based communication. Then, we analyze the two essential usages of the Team Guidance layer, represented by the OptimizationGuidance and the DistributedControlGuidance classes.

A. Graph-based Communication
At the core of every distributed control scheme is the graph-based communication among the robotic agents. This feature is provided in CHOIRBOT at the team guidance layer by the Communicator class, which can handle both synchronous/asynchronous and undirected/directed communication among the robots. To achieve this, the Communicator class only requires specification of the in-and out-neighbors of the robots and takes care of managing the necessary ROS 2 topics and subscriptions. To maintain the class interface of the class semantically clear, the method names correspond to the specific actions that can be performed: send, receive, asynchronous_receive, neighbors_exchange (i.e. simultaneous send/receive). We point out that this class has been designed in order to maintain compatibility with the DISROPT package [16], therefore all the distributed optimization algorithms implemented in DISROPT can be run by using the Communicator class provided by CHOIRBOT.
An important feature is that it is not necessary to declare the message types to be exchanged among the robots, which is typically needed for all ROS applications. Instead, we fixed the type of message to std_msgs/ByteMultiArray and the Communicator class handles (de)serialization of the exchanged message to (from) a byte sequence through the Python dill package. This allows the user to exchange nearly each type of message (vector, matrices, text, dictionaries, images, etc.) and even to change it at runtime without declaring what type of message must be sent.

B. Distributed Optimization
A consistent part of the toolbox is devoted to providing optimization-related functions. The main entry point is the OptimizationGuidance class. Since numerical optimization algorithms may require a certain number of iterations to converge, these computations are delegated to a separate thread running in the OptimizationThread class. This allows the ROS 2 guidance process to continue elaborating callbacks even though an optimization is in progress. This separate thread class, which is started by the OptimizationGuidance class, allows the user to start/stop the optimization processes on demand. At the end of an optimization process, the method optimization_ended is called. This method is supposed to be overridden by the user with the specific guidance logic, e.g. by retrieving the optimization results and by using them as needed. The actual body of the optimization algorithm is left unimplemented so as to allow the user to implement the desired method. Currently, we provide implementations for distributed task assignment and distributed MPC scenarios, which are analyzed in detail in Section IV.
As already mentioned, the toolbox supports running both local optimization algorithms at a robot and distributed optimization algorithms involving the whole network. To this end, CHOIRBOT integrates with the DISROPT package [16], which is used to model and to solve both local and distributed optimization problems with several algorithms.

C. Distributed Feedback Control
The toolbox is designed to support optimization-based distributed control algorithms, however simpler distributed feedback laws can also be implemented by using a subset of the toolbox features. The DistributedControlGuidance class implements a general communication and control structure allowing for the implementation of such feedback laws. To give an idea, let us report an excerpt of the main routine of the class, which is executed with a user-chosen frequency: In words, the class first exchanges the current position with the neighbors, then computes a velocity profile according to the exchanged data and to the considered feedback law, and finally publishes the control input on a topic. Despite its simplicity, this basic structure can be used for several distributed control algorithms such as containment and formation control. Simulation results can be found in Section VI.
IV. IMPLEMENTED COMPLEX SCENARIOS In this section, we discuss in detail two complex cooperative robotic scenarios that have been implemented in CHOIRBOT, i.e. distributed dynamic task assignment and distributed model predictive control. In both scenarios we employ the OptimizationGuidance class to leverage optimization capabilities.

A. Distributed Dynamic Task Assignment
Problem formulation: In this scenario, we assume there is a set of tasks that must be performed by a team of robots. In order to choose the final assignment, robots must self-coordinate by using their communication capabilities. The assignment problem can be formulated as an integer optimization problem with unimodularity properties, which is eventually reformulated into a distributed linear program [18]. Formally, assume there are N robots (indexed by i) and N tasks (indexed by k). A scalar c ik represents the cost incurred by robot i when servicing task k. The goal is to find the optimal assignment, i.e. to assign each robot i to exactly one task j such that the total incurred cost is minimized. We focus on the challenging scenario in which tasks are not known a-priori but arrive dynamically. While robots perform the previously assigned tasks, a new task can arrive and robots re-execute the distributed optimization algorithm to compute the new optimal assignment. Implementation: In CHOIRBOT, there is a number of classes to handle the dynamic task assignment scenario. We assume there is a cloud accepting task requests and forwarding them to the robots such as, e.g. in online order and delivery services. This mechanism is implemented with the TaskTable class, which is also responsible for maintaining an up-to-date task list and to mark tasks as completed. There is a TaskGuidance class that communicates with TaskTable, triggers the distributed optimization algorithm in the separate computation thread whenever a new task request is received and maintains a local task queue. While the queue is not empty, tasks are executed in order by calling the TaskExecutor class, which implements the specific task execution logic (e.g. to move to a given point and to perform loading/unloading operations). The separate computation thread is implemented with the TaskOptimizationThread class, in which methods from the TaskOptimizer class are called to handle the distributed optimization process. The TaskOptimizer class is responsible for formulating the actual linear programming task assignment problem and to run the distributed simplex algorithm [18] in DISROPT. In this scenario, communication among neighbors occurs within the TaskOptimizer class during the distributed optimization process.
Remarkably, from the point of view of the final user, all is needed is to implement the task execution logic in TaskExecutor and to integrate TaskTable with the task request service of the specific application scenario. The whole dynamic task assignment mechanism is taken care of by CHOIRBOT. In Section VI-A, experimental results on this scenario are provided.

B. Distributed Model Predictive Control
Let us now describe the second complex scenario implemented in CHOIRBOT.
Problem formulation: In this scenario, we assume there are N robots with linear dynamics of the type where, for all i ∈ {1, . . . , N }, x i (t) ∈ R n i and u i (t) ∈ R m i are the i-th robot state/input. We assume the robots must satisfy local state/input constraints x i (t) ∈ X i and u i (t) ∈ U i . Each robot is associated to an output z i (t) = C i x i (t) + D i u i (t), with z i ∈ R r i . The robot outputs are assumed to be coupled with a constraint N i=1 z i (t) ∈ S. Finally, we assume each robot i is equipped with a local cost function i (x i , u i ) that must be minimized. The conceptual idea of Model Predictive Control (MPC) is to solve a finite-horizon optimal control problem at each time step t, with trajectories spanning the time horizon [t, t + T ]. The first control input is applied and the process is repeated. In distributed model predictive control schemes, each robot solves a local version of the overall optimal control problem and leverages communication with neighbors to achieve an overall feasible solution.
Implementation: We implemented the classical distributed MPC algorithm in [19]. The departing point is the MPCGuidance class, which implements the actual steps of the distributed MPC algorithm. The problem data (i.e. system matrices A i , B i , C i , D i , local constraints X i , U i , coupling constraints S, prediction horizon T ) are provided by the user as class parameters. This class interacts with an MPCOptimizationThread class, which calls the optimize method from the MPCOptimizer class to solve the local optimal control problem at each control iteration. The MPCOptimizer class is responsible for formulating the local optimal control problem and for solving it by using the problem modeling and solving capabilities available in DISROPT. Note that, differently from the dynamic task assignment, in this scenario the solution of optimization problems is completely local. Here, communication among neighbors occurs within the MPCGuidance class, as required by the MPC algorithm (see [19] for details).

V. BASIC USAGE EXAMPLE
In this section, we consider a toy example that allows us to show the implementation of a basic distributed cooperative robotics scenario in CHOIRBOT. Specifically, we consider containment in leaderfollower networks as described in [20]. The mathematical formulation is as follows. Robots communicate according to an undirected connected graph G = (V, E), where V = {1, . . . , N } is the set of vertices and E ⊆ V × V is the set of edges. We denote by N i the set of neighbors of each robot i. Robots are partitioned in two groups, namely leaders and followers. The goal for the followers is to converge to the convex hull of the leaders' positions. To this end, the robots implement the dynamicṡ

A. Main Software Components
This scenario can be easily implemented in the proposed architecture by means of two components, namely a ContainmentGuidance class and a SingleIntegrator class. The first one is an extension of the DistributedControlGuidance class discussed in Section III-C and inherits the distributed feedback control structure. Thus, in the ContainmentGuidance class one can simply override the evaluate_velocity method in order to implement the control law in (1) as follows: for pos_ii in neigh_data.values(): u += self.containment_gain * (pos_ii -self.current_pose.position) return u The SingleIntegrator class instead extends the functionalities of the base class Integrator. In particular, it is sufficnet to override the (initially empty) integrate method to implement the classical forward Euler method for single-integrator dynamics: self.current_pos += self.samp_time * self.u

B. Visualizing the Evolution
Since we are not using an external simulation tool, we need a means to visualize the results graphically. To achieve this, it is possible to rely on the ROS 2 toolbox RVIZ, in which we associate each robot with a circle moving on the (x, y) plane. To achieve this, there is a class provided by the toolbox named RvizSpawner, which receives the pose published by the SingleIntegrator and forwards it to the RVIZ visualizer with a user-defined frequency.

C. Running the Simulation
The final simulation can be run by writing a ROS 2 launch file with all the required nodes, i.e. ContainmentGuidance, SingleIntegrator, RvizSpawner. Note that these three nodes must be executed by each of the robotic agents in the network. The launcher file is also responsible for declaring the initial positions of the robots and the communication graph, specified as a binary adjacency matrix.
In order to differentiate the ROS 2 nodes of each robot, robot-specific parameters must be passed as ROS 2 parameters at the time of spawing. For instance, the ContainmentGuidance classes are spawned in the launch file as follows A simulation with e.g. N = 6 robots can be run by executing the following command ros2 launch choirbot_examples containment.launch.py -n 6 VI. SIMULATIONS AND EXPERIMENTS In this section, we provide simulation and experimental results for three different scenarios that can be handled by CHOIRBOT. We begin by showing experimental results of the dynamic task assignment scenario described in Section IV-A. Then, we show how to the proposed package can be easily interfaced with Gazebo or Rviz and show simulation results for a team of mobile wheeled robots.

A. Dynamic Task Assignment on Turtlebot3 Mobile Robots
In this experiment, we consider a team of four Turtlebot 3 Burger mobile robots that have to accomplish a set of task scattered in the environment. A task is considered accomplished if the designed robot reaches its position on the {x, y} plane. As in real applications, problem data are not completely known a-priori, we consider a dynamic assignment problem where new data arrive during the execution. Thus, robots have to re-optimize and adjust their local planning whenever new information is available. Inspired by the approach in [21], we assume that a new task is revealed as soon as one has been completed. The pose of each robot is retrieved by communicating with a Vicon Motion capture system. In order to interface CHOIRBOT with the Vicon system, we developed an ad-hoc ROS 2 package, which is provided in the toolbox repository. As for the control layer to steer robots to the desired positions, we implemented a dedicated controller node executing the control law in [22].
In Figure 2, we report a snapshot from the experiment in which the robots are servicing a set of tasks, while Figure 3 reports a Gantt chart of the actual task execution. Finally, in Figure 4 we show the evolution of the distances of robots to the currently assigned tasks. A video of the experiment is available as supplementary material to the manuscript.

B. Formation Control for Unicycle-Like Robots
In this scenario, the goal is to drive robots to a translationally independent formation in the {x, y} plane that satisfies a set of given constraints. These constraints are specified by the communication graph edge set E and a set {d ij } (i,j)∈E of desired distances between two communicating robots i and j, with the requirement d ij = d ji for all (i, j) ∈ E. We refer the reader to, e.g. [23] for a detailed description. Let the robots apply the distributed control law The left side of each rectangle indicates the beginning of a task, while the right indicates that the task has been serviced by a robot. As described above, when a task is serviced (e.g. task 0) a new one begins (e.g.  Fig. 4. Evolution of the robot-to-task distances during the dynamic task assignment experiment. Each chart is associated to a different robot and shows the distance of the robot to the task assigned at that specific time. Discontinuities in the graph correspond to changes in the assignments. Eventually, all the agents complete the last assigned task. We extended the DistributedControlGuidance and override the evaluate_velocity method in order to implement the control law in (2). However, this is not directly implementable on unicycle-like robots. Thus, we developed a specific controller, named UnicycleVelocityController, that maps the input provided by (2) to a suitable set of inputs for wheeled robots. This is performed following the approach described in [9]. The interfacing with Gazebo is straightforward. Robot poses are retrieved directly by the odometry topic maintained by Gazebo, while robot inputs are sent on suitable topics read by Gazebo plugins. In Figure 5 a snapshot from a Gazebo simulation in which six Turtlebot 3 Burger robots have to draw an hexagon. A video of the simulation is available as supplementary material to the manuscript.

C. Containment
Finally, let us show simulation results for the containment example outlined in Section V. We consider a scenarios with three followers and three leaders spanning a triangle. The evolution of the robot positions is reported in Figure 6. VII. CONCLUSIONS In this paper, we presented CHOIRBOT, a novel toolbox for distributed cooperative robotics written in Python and based on the ROS 2 platform. The toolbox is designed with a three-layer structure and provides a comprehensive set of functionalities for communication and distributed optimization. Thanks to this toolbox, complex optimization-based cooperative robotic scenarios can be implemented in a straightforward manner, thus allowing programmers to focus on the most important part of the production code. Several scenarios have been described and simulations and experimental results have been presented.