ROS 2 API for navigation
Introduction
The Origin One, with Autopilot Inference, is capable of navigating autonomously through an environment. To do so, the robot represents the geometry of the area in which it is operating as a local tangent plane, which is a Cartesian definition of the area in (x, y, z) coordinates. This local tangent plane is known to the robot as the, so called, "global frame". The mathematical origin of this global frame is a point in the real world, which is defined in the Earth's frame of latitude, longitude and altitude, with the Y-axis heading true North and the X-axis heading true East. This point in the real-world that is being used by the robot as the origin of its global frame is published on the topic /coord_system. Any map, path, polygon waypoint and estimated pose that is published by the robot will be defined within this frame. For example, the robot makes use of a localization map, which is a map of the area that was recorded using the mapping process. The localization map that the robots will exploit shall be published on /map_external. The local origin of this localization map, which is also captured by the message published on published on /map_external, has a position in the global frame. Note that the local origin of the map is only shifted in the position of the global frame and not shifted in orientation. This means that the X-axis of the localization map shall be pointing true East.
Global-poses
The alignment of the map in the Earth's frame is only important when operating (partially) outdoors, which is automatically achieved by creating a new map from an (accurate) outdoors position, i.e., where RTK-GNSS is available. In case you want to operate both indoors and outdoors and want to create a map from an indoor part of the environment using the Aruco marker, then you will need to position the Aruco marker with respect to the global frame. The point of origin of the global frame is defined by the /coord_system and oriented with the Earth's frame. From this global origin you will need to give the Aruco marker a new pose by measuring their distance in X (East) and Y (North) directions and also measuring the heading of the Aruco marker. See the Advanced topics for more information.
Map-polygon
The localization map, which is being published on /map_external, is accompanied by a polygon that is published on /map_layout_polygon. This polygon is in fact the boundary of the map as it was drawn by the user when creating a map. The robot uses this polygon to decide if it may use the localization map for estimating its position by matching the lidar pointcloud to it: whenever the robot is inside the polygon, then it will switch to a "map_supported_navigation" mode where it will exploit the localization map for estimating its position, while if the robot is outside the polygon, then it will switch to a "navigation" mode where it will NOT exploit the localization map for estimating its position.
The navigation stack integrates functionalities such as Localization, Planning and Control. For localization the robot makes use of the localization map (in combination with the lidar pointcloud) and of RTK-GNSS and Aruco markers. Planning a path is done on a so called "global costmap", while controlling the robot along a path is done on a so called "local costmap", which are both grid-maps with occupied and free spaces. Whether a grid-area of the "local costmap" is occupied or free is directly derived from the lidar pointcloud, while for the "global costmap" this is derived from the pre-recorded localization map in combination with the data of the lidar pointcloud (filtered with a moving window).
Localization
Prior to obtaining a localization the Autopilot Inference continuously checks for Aruco markers in its camera image that, when detected, will be published along with their relative pose on the topic /marker_pose. Also, the pointcloud that is obtained from the 3D LiDAR is filtered so that the body of the robot is removed from the pointcloud (and published on /points_filtered) while it is further transformed into a 2D laserscan (and published on /scan_filtered) for more efficient computations later on in software stack of the Autopilot Inference. Together with the RTK-GNSS fixes that are obtained from the sensing interfaces, this information is to to determine 3 individual estimates of the pose of the robot, which are /gnss/pose, /marker_based_pose and /amcl_pose, before they are merged into the final, fused estimated pose of the robot being /estimated_pose.
Initial-pose
The robot must have an initial pose before it may keep track of its position throughout the operation. Normally, it will obtain such an initial position from a known Aruco marker that was detected. If that is not that case, then one may set the initial pose of the robot by publishing on the topic /initial_pose.
Planning a path
The current pose of the robot is used when planning a path for the robot from its current position to some set destination. This planned path is being published on the topic /unsmoothed_path, although no path will be computed when the robot is requested to navigate along a specific path. In case on specific path is provided, than a path must be planned. To plan this path the planner makes use of the global costmap that is being published on /global_costmap. This global costmap combines prior information from the localization map with observed obstacles in the LiDAR data throughout the operation.
Moving-humans
It might be that the global costmap gets polluted by dynamic obstacles that move within the same operational area, such as human, and leave a 'trail' of obstacles in the costmap that are not there anymore in real. In such a situation you may clean the costmap by calling the service /clean_entirely_global_costmap to go to a state where it is only based on the localization map, after which new LiDAR data is being processed by the costmap again.
Controlling the robot along a path
Once the path is obtained, then the behavior tree will trigger the locomotion controller for moving the robot such that it will follow the path to the best of its capabilities, while avoiding obstacles it encounters that were not foreseen while planning the path. The local costmap, which is being published on /local_costmap, is used to detect ad-hoc obstacles and makes small adjustments to the incoming path so that small obstacles are directly avoided (similar as to the global costmap also this local costmap can be cleared to an initial, empty state by calling the service /clear_entirely_local_costmap). However, it may not adjust the incoming path too much, as that may violate the policy parameter that has bene published on /max_path_distance. In a similar way, the locomotion controller will also compute a forward speed of the robot, in addition to an angular speed, that do not violate the values published on /speed_limit.
Frame and map topics
The topics related to the global frame and to the localization map are summarized in the table below, while a more detailed account per topic is presented after the table.
| Topic | Format | Type | Function |
|---|---|---|---|
| autopilot/coord_system | autonomy_msgs/msg/CoordinateSystemInfo | publisher | The latitude, longitude and altitude of the mathematical origin of the global frame in the real world. |
| autopilot/information_manager/map_external | nav_msgs/msg/OccupancyGrid | publisher | Map, represented as an occupancy grid, used by the navigation stack for localization and path planning. |
| autopilot/information_manager/map_layout_polygon | geometry_msgs/msg/PolygonStamped | publisher | Polygon to indicate the boundary of the map so that, when crossed, the map is no longer used for localization. |
The topic /autopilot/coord_system has the following characteristics:
Message type: autonomy_msgs/msg/CoordinateSystemInfo
Frame: n/a
Frequency: depends on the publisher
Data: type, map_name, latitude, longitude, altitude
The content of the message that is published is a type, formatted as [uint8], to indicate the type of coordinate system as is defined in the table below. Further, there is map_name, formatted as [string], to indicate the name of the map in case the type of type local relative, i.e., 0, and also latitude, longitude and altitude, als formatted as [float64], that define the latitude, longitude and altitude of the global origin in case the `type is of a local tangent plane (1), i.e., it specifies the Earth's location of the local tangent plane's center.
The topic /autopilot/information_manager/map_external has the following characteristics:
Message type: nav_msgs/msg/OccupancyGrid
Frame: map
Frequency: latching
Data: header, info, data
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with info, formatted as nav_msgs/msg/MapMetaData, to indicate meta data of the map, such as its height, width and origin, and the data itself, formatted as int8[] to represent the occupancy probability per cell in the 2D map.
The topic /autopilot/information_manager/map_layout_polygon has the following characteristics:
Message type: geometry_msgs/msg/PolygonStamped | publisher
Frame: map
Frequency: latching
Data: header, polygon
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with polygon, formatted as geometry_msgs/msg/PolygonStamped, to indicate the list of points defining the polygon.
Localization topics
The topics related to the localization functionality of the robot are summarized in the table below, while a more detailed account per topic is presented after the table.
| Topic | Format | Type | Function |
|---|---|---|---|
| autopilot/marker_pose | artag_pose_msgs/msg/PoseWithIdArray | publisher | list of markers that are being detected with their marker ID and pose with respect to the camera frame of the robot. |
| autopilot/points_filtered | sensor_msgs/msg/PointCloud2 | publisher | Pointcloud of the 3D LiDAR in which the body of the robot was removed. |
| autopilot/scan_filtered | sensor_msgs/msg/LaserScan | publisher | Laserscan obtained by projecting the filtered pointcloud on a horizontal plane for all points that are within -20 cm and + 70 cm in height of the 3D LiDAR. |
| autopilot/gnss/pose | geometry_msgs/msg/PoseWithCovarianceStamped | publisher | Pose of the robot, (x, y, heading), by converting the RTK-GNSS fix into the Carthesian definition of the local tangent plane as is provided by the /coordinate_system_info. |
| autopilot/marker_based_pose | geometry_msgs/msg/PoseWithCovarianceStamped | publisher | Pose of the robot, (x, y, heading), by adding the relative position from the marker to the robot on the global position of the marker, i.e., the position of the marker within the local tangent plane as is provided by the /coordinate_system_info. |
| autopilot/amcl_pose | geometry_msgs/msg/PoseWithCovarianceStamped | publisher | Pose of the robot, (x, y, heading), by matching the /scan_filtered to the localization map and add that position within the map to position of the map within the global frame, i.e., the position of the marker within the local tangent plane as is provided by the /coordinate_system_info. |
| autopilot/estimated_pose | geometry_msgs/msg/PoseWithCovarianceStamped | publisher | Pose of the robot, (x, y, heading), by fusing all the above pose estimates along with the odometry information that is coming from the robot. The position is defined in the map frame that is added to the robot's transforms which refers to the global frame and thus to the global origin used by the robot. |
| initial_pose | geometry_msgs/msg/PoseWithCovarianceStamped | subscriber | Topic to publish the initial pose of the robot as a user in case you do want to deviate from the initial pose defined by an known Aruco marker or by the RTK-GNSS fix. |
Perception
The topic /autopilot/marker_pose has the following characteristics:
Message type: artag_pose_msgs/msg/PoseWithIdArray
Frame: optical
Frequency: depends on the publisher
Data: header, poses
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with poses, formatted as a list of artag_pose_msgs/msg/PoseWithId, which contains an id, formatted asint8[], to represent marker ID and a pose, formatted as geometry_msgs/msg/Pose, to represent the pose of the marker in the optical frame.
The topic /autopilot/points_filtered has the following characteristics:
Message type: sensor_msgs/msg/PointCloud2
Frame: lidar
Frequency: depends on the publisher
Data: o.a., header, height,width and data
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with a height and width, formatted as [uint32] to represent the size of the pointcloud in 2D, and data that is formatted as [uint8[]] is a list of integer values related to the reflection of a single laser beam of the LiDAR that can be found by matching the element number in this list of points to the height and width of the pointcloud;s size.
The topic /autopilot/scan_filtered has the following characteristics:
Message type: sensor_msgs/msg/LaserScan
Frame: lidar
Frequency: depends on the publisher
Data: a.o., header, angle_min, angle_max, angle_increment and ranges
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time. Further, angle_min, angle_max, angle_increment, formatted as [float32], can be used to compute all the angles in the horizontal plan of the 3D LiDAR at which a range has been measured, while ranges that is formated as a list of [float32] are the range values for each of those angles.
Robot poses
All topics /autopilot/gnss/pose, /autopilot/marker_based_pose, /autopilot/amcl_pose and /autopilot/estimated_pose have the following characteristics:
Message type: geometry_msgs/msg/PoseWithCovarianceStamped
Frame: map
Frequency: depends on the publisher
Data: header, pose
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, and a pose, formatted as geometry_msgs/msg/PoseWithCovariance to represent the 6D position of the robot along with the uncertainty on that pose in the form of a covariance matrix.
Initial pose
The topic /initial_pose has the following characteristics:
Message type: geometry_msgs/msg/PoseWithCovarianceStamped
Frame: map
Frequency: depends on the publisher
Data: header, pose
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, and a pose, formatted as geometry_msgs/msg/PoseWithCovariance to represent the 6D position of the robot along with the uncertainty on that pose in the form of a covariance matrix.
Path planning topics
The topics related to the path recording functionality of the robot are summarized in the table below, while a more detailed account per topic is presented after the table.
| Topic | Format | Type | Function |
|---|---|---|---|
| autopilot/unsmoothed_path | nav_msgs/msg/Path | publisher | Path of the robot that was planned. |
| autopilot/global_costmap | nav_msgs/msg/OccupancyGrid | publisher | Global costmap |
The topic /autopilot/unsmoothed_path has the following characteristics:
Message type: nav_msgs/msg/Path
Frame: map
Frequency: depends on the publisher
Data: header, poses
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with poses, formatted as a list of geometry_msgs/msg/PoseStamped, in which each element in the list represents the next 6D pose on the path where a pose is defined as a position (Carthesian) and an orientation (Quaternion).
The topic /autopilot/global_costmap has the following characteristics:
Message type: nav_msgs/msg/OccupancyGrid
Frame: map
Frequency: depends on the publisher
Data: header, info, data
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with info, formatted as nav_msgs/msg/MapMetaData, to indicate meta data of the map, such as its height, width and origin, and the data itself, formatted as int8[] to represent the cost values per cell in the 2D map for placing a point on the path in that cell. The total cost of a path is than the integral of all costs for every cell that the path is traversing (which is minimized by the planner).
Path planning services
The services related to the path recording functionality of the robot are summarized in the table below, while a more detailed account per service is presented after the table.
| Service | Format | Request variable | Function |
|---|---|---|---|
| /autopilot/clear_entirely_global_costmap | nav2_msgs/srv/ClearEntireCostmap | n/a | Service to call for a cleaning the global costmap to an initial state. |
The service /autopilot/clear_entirely_global_costmap has the following characteristics:
Message type: nav2_msgs/srv/ClearEntireCostmap
Request: n/a `
Response: n/a
Note that both the service-request as well as the service-response are empty.
Locomotion topics
The topics related to the path recording functionality of the robot are summarized in the table below, while a more detailed account per topic is presented after the table.
| Topic | Format | Type | Function |
|---|---|---|---|
| autopilot/speed_limit | nav2_msgs/msg/SpeedLimit | subscriber | Maximum forward speed for the robot that the locomotion controller must attain. |
| autopilot/max_distance | std_msgs/msg/Float64 | subscriber | Maximum distance to the path that the locomotion controller must attain. |
| autopilot/local_costmap | nav_msgs/msg/OccupancyGrid | publisher | Global costmap |
The topic /autopilot/speed_limit has the following characteristics:
Message type: nav2_msgs/msg/SpeedLimit
Frame: n/a
Frequency: n/a
Data: percentage, speed_limit
The content of the message that the locomotion controller subscribes to has a field speed_limit, formatted as a [float64], that is interpreted as an absolute limit on the speed unless the field percentage is set a True [Bool] in which case the speed_limit is interpreted as a percentage of some known maximum speed.
The topic /autopilot/max_distance has the following characteristics:
Message type: std_msgs/msg/Float64
Frame: n/a
Frequency: n/a
Data: data
The content of the message that the locomotion controller subscribes to has a field data, formatted as a [float64], that is interpreted as a maximum distance to the original path in [meters] that preferable should not be violated by the controller when computing the velocities of the robot.
The topic /autopilot/global_costmap has the following characteristics:
Message type: nav_msgs/msg/OccupancyGrid
Frame: map
Frequency: depends on the publisher
Data: header, info, data
The content of the message that is published is a header, formatted as std_msgs/Header to indicate the time, along with info, formatted as nav_msgs/msg/MapMetaData, to indicate meta data of the map, such as its height, width and origin, and the data itself, formatted as int8[] to represent the cost values per cell in the 2D map for placing a point on the path in that cell. The total cost of a path is than the integral of all costs for every cell that the path is traversing (which is minimized by the planner).
Locomotion services
The services related to the path recording functionality of the robot are summarized in the table below, while a more detailed account per service is presented after the table.
| Service | Format | Request variable | Function |
|---|---|---|---|
| /autopilot/clear_entirely_local_costmap | nav2_msgs/srv/ClearEntireCostmap | n/a | Service to call for a cleaning the local costmap to an initial state. |
The service /autopilot/clear_entirely_local_costmap has the following characteristics:
Message type: nav2_msgs/srv/ClearEntireCostmap
Request: n/a
Response: n/a
Note that both the service-request as well as the service-response are empty.