ROS 2 API for navigation
The Origin One, with Autopilot Inference, is capable of navigating autonomously through an environment. It does so by making use of a localization map (and matching it LiDAR pointcloud or scan), or of RTK-GNSS, in addition to Aruco markers for setting an initial pose. Once localization is in place, then a behavior tree can be called to schedule the different services available for, firstly, planning a path using the localization map and something called a 'global costmap' (mainly for representing static obstacles obtained from the localization map and the 3D LiDAR as an grid-map where high grid values should be avoided by the planned) and, secondly, for executing that path by computing a reference velocity of the robot based on its current position of the robot and something called a 'local_costmap' (which is similar to the global costmap but then solely based on occupied areas detected by the current 3D LiDAR).
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. 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.
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 pbe 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. 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.
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 intial, 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.
Finally, the navigation stack makes use of a information about the coordinate system it is using. This coordinate system defines the global origin of the robot, i.e, the global frame, and is being published on the topic /coord_info. Within this topic you should see that the robot is using a so-called local tangent plane, which is a Carthesian definition of the operational area in (x, y, z) coordinates. The topic will also show where this global origin of this local tangent plane of the robot is located with respect to the Earth's frame, i.e., a latitude, longitude and altitude. Further, one may also view the localization map that is being used by the robot, which is published on /map_external. This map is defined in the same local tangent plane as is being specified by the coordinate system information, and therefore we also align the map-frame of the robot's transform with the global frame (or thus the local tangent plane).
Coordinate system information
The topic /autopilot/coord_info 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.
Localization map
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.
Localization topics
The topics related to the localization functionality of the robot are summarized in the table below. A more details account per topic is presented later on this page.
| 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. A more details account per topic is presented later on this page.
| 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. A more details account per service is presented later on this page.
| 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. A more details account per topic is presented later on this page.
| 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. A more details account per service is presented later on this page.
| 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.