Navigation & Localization
For a simple waypoint system, we would provide some form of goal state (pose, velocity, etc.) for the robot to reach. This process is considered simple for a few reasons. Firstly, it does not consider the robots current state (for example, the robot may be traveling very quickly and will not be able to stop in time). Secondly, it does not allow for any additional guiding of the robot (for example, it assumes that the robot will get from A to B as quickly as possible, do so safely, not hit anything along the way, etc.). Thirdly, it assumes that the robot is capable of reaching the desired state (for example, it cannot turn quick enough, it can't travel to a certain height, etc.). Regardless, a simple waypoint system is usually the first step to something smarter.
Continuing on, we will break down simple waypoint tracking in relation to navigating multirotor aircraft.
Firstly, it is assumed that the process of transmitting the current goal is being taken care of in a main() loop, or in a timer callback of some kind. Through this process, it is assumed that there is a "global" variable that is called to send out the current goal, and by changing this variable, the the next time the current goal is sent, it will automatically send out the changed version. This is in line with how the PX4 and Robin autopilots expect a "continuous stream" of command inputs before accepting the information. For a more in-depth example of sending a single waypoint, look towards the MAVROS Offboard Example or the mavros_guider sample package.
Before creating any sort of waypoint/goal state list, we must first decide on what we will be trying to control. For a multirotor UAV, it is probably desirable to control the entire pose (i.e. position and orientation), thus we will require a list of poses to use as our goals. For integration with ROS, this works out quite well, as the current goal that will be transmitted will most likely be of the type geometry_msgs/PoseStamped, and we can simply override the our output pose goal with a pose goal from a list of geometry_msgs/Pose goals.
Python*
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
global waypoint_list
global waypoint_counter
global output_goal
...
# Main
waypoint_list = []
waypoint_counter = 0
# In a loop
tmp_wp = Pose()
#tmp_wp = ... # Load/Generate waypoints somehow
waypoint_list.append(tmp_wp)
output_goal.pose = waypoint_list[waypoint_counter]
...
# In a function that checks the current state of the robot
# If current state matches goal state
waypoint_counter++
output_goal.pose = waypoint_list[waypoint_counter]C++
#import "geometry_msgs/PoseStamped"
#import "geometry_msgs/Pose"
#import <vector>
std::vector<geometry_msgs/Pose> waypoint_list;
uint waypoint_counter = 0;
geometry_msgs/PoseStamped output_goal;
...
# Main
# In a loop
geometry_msgs::Pose tmp_wp;
#tmp_wp = ... # Load/Generate waypoints somehow
waypoint_list.push_back(tmp_wp);
output_goal.pose = waypoint_list[waypoint_counter];
...
# In a function that checks the current state of the robot
# If current state matches goal state
waypoint_counter++;
output_goal.pose = waypoint_list[waypoint_counter];