ezrassor_swarm_control
The ezrassor_swarm_control package provides a high level swarm control and management system that is responsible for the tasking, deployment, and guidance of each individual EZ-RASSOR in a swarm. By leveraging scheduling and path planning algorithms, as well as the ROS actionlib API, this system enables rovers to cooperate efficiently and intelligently in mining regolith from the lunar surface. Our swarm controller can be easily ran as just two ROS nodes which act as a higher level intelligence unit for a network of rovers. The diagram below visualizes the system's architecture:
The most simple way to simulate this swarm control functionality is to run the swarm_simulation.launch file. An example command is provided below. For more information, see the ezrassor_launcher section of this wiki.
roslaunch ezrassor_launcher swarm_simulation.launch \
robot_count:=5 \
spawn_x_coords:="0 3 6 9 12" \
spawn_y_coords:="0 0 0 0 0"
This node functions as the central planning and control unit for the high level swarm control system. It operates by continuously communicating with each rover via the get_rover_status service and analyzing metrics such as battery levels, rover positioning, and distances to dig sites or charging stations to make intelligent decisions. A scheduling algorithm ensures that empty or sparsely populated dig sites are prioritized for mining and that rover's are directed to charging stations when low on battery. In order to prevent rovers from navigating in treacherous terrain, this node also utilizes an A* path planner which runs on the simulated environment's elevation map. Leveraging NASA's Lunar Surface Model, we've also created a world called nasa_moon_dem to simulate the lunar environment. Once a path is generated, it is sent to a rover's waypoint_client, which will direct the rover along its path.
The following is a list of topic outputs published by this node, with each topic's message type shown in brackets:
OUTPUTS node -> /ezrassor<i for i in [1, n]>/waypoint_client [ezrassor_swarm_control/msg/Path] where i is each number in range [1, N], with N being the number of rovers
This node implements a simple action client for an EZ-RASSOR, serving as a communication layer between the swarm controller and the rover. Note that the swarm controller forms a one to many relationship with the swarm, with each rover having it's own waypoint_client. After planning, the swarm control node will send a rover's path to their respective waypoint client. The client is then responsible for sending each waypoint in the high-level path to its rover for autonomous navigation. The waypoint_client node also leverages callback functions to analyze the waypoint action client's feedback topic and re-plan paths when necessary. Utilizing this distributed Local Repair A* approach allows the system to to efficiency guide rovers and re-plan to prevent collisions or rover navigation error on the fly.
The following is a list of topic inputs and outputs, with each topic's type shown in brackets. Note that the waypoint goal, feedback, and result topics are defined in waypoint.action and the publisher and subscribers are created implicitly by the action clients and servers.
INPUTS node -> /ezrassor<i>/waypoint_client [ezrassor_swarm_control/msg/Path] node -> /ezrassor<i>/waypoint/feedback [ezrassor_swarm_control/msg/waypointFeedback] node -> /ezrassor<i>/waypoint/result [ezrassor_swarm_control/msg/waypointResult] where i is a rover number in range [1, N], with N being the number of rovers OUTPUTS node -> /ezrassor<i>/waypoint/goal [ezrassor_swarm_control/msg/waypointGoal] where i is a rover number in range [1, N], with N being the number of rovers
As described above, we implement an action client-server architecture to create a robust communication layer between the swarm controller and each EZ-RASSOR in a swarm. In order for the swarm controller, the client, to communicate with a server, or a rover, one must first define a set of messages and topics on they communicate. This defines the Goal, Feedback, and Result messages with which clients and servers communicate. This action specification is provided below:
GOAL # Goal is a point in space target [geometry_msgs/Point] RESULT # Result is the rover's final pose, battery, and whether the request was preempted pose [geometry_msgs/Pose] battery [std_msgs/Int8] preempted [std_msgs/Int8] FEEDBACK # Feedback message includes the rover's current pose and battery during autonomous navigation pose [geometry_msgs/Pose] battery [std_msgs/Int8]
To enable the swarm controller to send the waypoint_client node entire paths created by the A* path planner, we create a custom ROS message. This Path message is simply an array comprised of 3D Point messages. The message definition is also provided here:
path geometry_msgs/Point[]
This service, implemented as a callable function in swarm_utils.py, enables any node to request a rover to reply with its current battery level and pose. The function takes an integer in range [1, N], where N is the number of rovers in the swarm, and returns that rover's status in the form of the service's reply message. This reply message, comprised of a rover's battery level and position, is defined in GetRoverStatus.srv. The fields in this reply message are also described below:
pose [geometry_msgs/Pose] battery [std_msgs/Int8]
A example of how to utilize this service is shown below:
from swarm_utils import get_rover_status # Send status request to rover number 1 rover_status = get_rover_status(1) # Parse reply message battery, pose = rover_status.battery, rover_status.pose
This service allows any node in the EZ-RASSOR platform to preemptively stop a rover from continuing along a path sent to its waypoint_client. The service is also implemented as a callable function in swarm_utils.py that takes in the rover number that should be halted. Once called, the service will set the status of the rover's current waypoint action to preempted and prepare the waypoint client to receive a new path. Note that this function returns an empty response message. A usage example is provided below:
from swarm_utils import preempt_rover_path # Preempt rover number one's path preempt_rover_path(1)
This launch file launches the central swarm control node. All of the parameters which can be supplied to this launch file are listed below. Note that in order for the swarm control node to function accurately, one most supply this launch file with the file name of the elevation map used by the chosen Gazebo world.
robot_count- The number of EZ-RASSORs in the swarm. Defaults to
2. digsite_y_coord- The y coordinate of the target digsite. Defaults to
10. digsite_x_coord- The x coordinate of the target digsite. Defaults to
10. lander_coords- The x and y coordinate of the charging station on the lunar surface. Defaults to
5 5. world- The world file name that the Gazebo simulation loads. Defaults to
nasa_moon_dem, which is a Gazebo world that simulates the true lunar surface. elevation_map- The file name of the elevation map which the chosen Gazebo world utilizes. Defaults to
nasa_moon_dem.jpg, which is a small section of NASA's Lunar Surface Model
This launch file will recursively spawn an individual rover into Gazebo and launch its waypoint action client, as well as the rovers onboard autonomous navigation nodes, for each rover in a swarm. All possible parameters are listed below:
robot_count- The number of EZ-RASSORs in the swarm. Defaults to
2. spawn_x_coords- The x spawn coordinate of each rover in the swarm. Defaults to
3 6. spawn_y_coords- The y spawn coordinate of each rover in the swarm.. Defaults to
0 0. spawn_z_coords- The z spawn coordinate of each rover in the swarm.. Defaults to
default. enable_real_odometry- The z spawn coordinate of each rover in the swarm.. Defaults to
default. swarm_control- Whether to enable the nodes on each individual rover neccessary to be controlled by the swarm controller. Defaults to
default. world- The world file name that the Gazebo simulation loads. Defaults to
nasa_moon_dem, which is a Gazebo world that simulates the true lunar surface. elevation_map- The file name of the elevation map which the chosen Gazebo world utilizes. Defaults to
nasa_moon_dem.jpg, which is a small section of NASA's Lunar Surface Model