ROS2 Package for Autonomous Mobile Robot (AMR) Service on Qualcomm Robotics Platform
QRB ROS AMR Service is a ROS2 package suite designed to manage AMR behaviors on Qualcomm robotics platform. Key features include:
- P2P navigation requests.
- Path following.
- Mapping services.
- Automatic return to charging station when battery is low.
qrb_ros_amr
: ROS2 package implementing action/service clients & servers, publishers and subscribers for navigation, mapping and AMR status feedback.
qrb_amr_manager
: C++ library providing APIs for AMR state management and charging logic.
- Low Power Manager: Returns to charging station when battery < 2.2V.
- Manager: Library interface.
- State Machine: Manages AMR state and actions.
`Follow Path Service & Follow Path Manager: ROS2 package for path following navigation.
`Robot Base & Robot Base Controller: ROS2 package for AMR base control.
2D LiDAR SLAM ROS & 2D LiDAR SLAM
: Provides mapping and localization.
Nav2
: Provides P2P navigation.
Interface | Name | Type | Description |
---|---|---|---|
Publisher | amr_status | qrb_ros_amr_msgs::msg::AMRStatus | Publishes AMR status |
Subscriber | test | std_msgs::msg::Int16 | Receives debugging information |
Subscriber | debug_exception | std_msgs::msg::Int16 | Receives simulated exception info |
Service Server | sub_cmd | qrb_ros_amr_msgs::srv::SubCmd | Handles sub-command requests |
Service Server | amr_mapping | qrb_ros_amr_msgs::srv::Mapping | Handles mapping requests |
Service Server | api | qrb_ros_amr_msgs::srv::API | Handles API requests |
Action Server | cmd | qrb_ros_amr_msgs::action::Cmd | Handles navigation commands |
Name | Type | Description | Default Value |
---|---|---|---|
status_change_id | uint8 | AMR changed status ID | - |
amr_exception | bool | AMR exception information | - |
error_code | uint8 | Error code when AMR is in exception | - |
current_pose | geometry_msgs/PoseStamped | Current position of AMR | - |
current_state | uint8 | Current state of AMR | - |
battery_vol | float64 | Current battery voltage | - |
vel | geometry_msgs/Vector3 | Current velocity of AMR | - |
mileage | float64 | Reserved | - |
header | std_msgs/Header | Message header | - |
Note
Other modules can interact with these interfaces for AMR monitoring.
Name | Type | Description | Default Value |
---|---|---|---|
data | int16 | Debugging ID | - |
Note
Other modules can send debugging information to simulate AMR events in developer mode.
Name | Type | Description | Default Value |
---|---|---|---|
data | int16 | Debugging exception ID | - |
Note
Other modules can send debugging exceptions to simulate AMR exception events in developer mode.
Name | Type | Description | Default Value |
---|---|---|---|
subcommand | uint8 | Sub-command | - |
result | bool | Result of sub-command | - |
Note
Other modules can send sub-commands to cancel, pause, or resume navigation.
Name | Type | Description | Default Value |
---|---|---|---|
cmd | uint8 | Mapping command | - |
result | bool | Result of command | - |
Note
Other modules can send mapping commands to load map, start mapping, or stop mapping.
Name | Type | Description | Default Value |
---|---|---|---|
api_id | uint8 | API ID | - |
developer_mode | bool | Enable/disable developer mode | - |
result | bool | Result of API handling | - |
Note
Other modules can send API requests to initialize AMR, release AMR, or control developer mode.
Name | Type | Description | Default Value |
---|---|---|---|
command | uint8 | Command ID (navigation, charging, etc.) | - |
goal | geometry_msgs/PoseStamped | Goal for point-to-point navigation | - |
path | nav_msgs/Path | Reserved | - |
goal_id | uint32 | Target waypoint ID for follow path | - |
passing_waypoint_ids | uint32[] | Waypoints to pass through on follow path | - |
result | bool | Result of command | - |
current_pose | geometry_msgs/PoseStamped | Current position of AMR | - |
passing_waypoint_id | uint32 | Passing waypoint ID during follow path | - |
distance_to_goal | float32 | Distance from current position to goal | - |
Note
Other modules can send command requests to start point-to-point navigation, follow path, or return to charging station.
Function | Parameters | Description |
---|---|---|
void init_amr() | - | Initialize AMR service. |
void release_amr() | - | Release AMR service. |
bool check_potential_state(int cmd) | cmd: Navigation command (P2PNav, FollowPath, Return Charging) | Check if AMR state meets navigation requirements. |
bool check_potential_subcmd_state(uint8_t subcmd) | subcmd: Sub-command (Cancel, Pause, Resume) | Check if AMR state meets sub-command requirements. |
void process_cmd(int cmd, void * buffer, size_t len) | cmd: Navigation command, buffer: Goal, len: Length of goal | Start P2P or charging station navigation. |
void process_cmd(int cmd, uint32_t goal_id, std::vector & ids) | cmd: Follow path command, goal_id: Target waypoint, ids: Passing waypoints | Start follow path navigation. |
bool start_mapping() | - | Start mapping. |
bool stop_mapping() | - | Stop mapping. |
bool load_map() | - | Load map. |
void process_sub_cmd(int msg) | msg: Sub-command | Cancel, pause, or resume navigation (including follow path and charging). |
void notify_battery_changed(float battery_vol) | battery_vol: Battery voltage | Update battery voltage. |
void notify_charging_state_changed(uint8_t state) | state: Charging state | Update charging state. |
void register_start_charging_callback(start_charging_func_t cb) | cb: Start charging callback | Register callback for starting charging. |
void register_publish_twist_callback(publish_twist_func_t cb) | cb: Move AMR callback | Register callback for moving AMR. |
void register_start_p2p_nav_callback(start_p2p_func_t cb) | cb: P2P navigation completion callback | Register callback for P2P navigation completion. |
void register_start_waypoint_follow_path_callback(start_waypoint_follow_path_func_t cb) | cb: Follow path completion callback | Register callback for follow path completion. |
register_navigate_to_charging_callback(navigate_to_charging_func_t cb) | cb: Charging navigation completion callback | Register callback for charging navigation completion. |
void register_sub_cmd_callback(sub_cmd_func_t cb) | cb: Sub-command callback | Register callback for sub-command. |
void register_change_mode_callback(change_mode_func_t cb) | cb: Control mode callback | Register callback to set control mode (application, charging, remote controller). |
void register_slam_command_callback(slam_command_func_t cb) | cb: SLAM command callback | Register callback for SLAM commands (start/stop mapping, save/load map, localization, relocalization). |
Note
These APIs enable AMR service integration for non-ROS applications. For ROS packages, please use the qrb_ros_amr_service APIs..
Development Hardware | Qualcomm Dragonwing™ RB3 Gen2 | Qualcomm Dragonwing™ IQ-9075 EVK |
---|---|---|
Hardware Overview |
Important
PREREQUISITES: The following steps need to be run on Qualcomm Ubuntu and ROS Jazzy.
Reference Install Ubuntu on Qualcomm IoT Platforms and Install ROS Jazzy to setup environment.
For Qualcomm Linux, please check out the Qualcomm Intelligent Robotics Product SDK documents.
Install dependencies:
sudo apt install ros-jazzy-nav2-msgs
sudo apt install ros-jazzy-nav-2d-msgs
Clone and build:
source /opt/ros/jazzy/setup.bash
git clone https://github.com/qualcomm-qrb-ros/qrb_ros_amr_service.git
git clone https://github.com/qualcomm-qrb-ros/qrb_ros_interfaces.git
colcon build
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch qrb_ros_amr qrb_ros_amr_bringup.launch.py
Sample output:
[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2025-09-17-10-58-59-978240-ubuntu-3342
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [qrb_ros_amr-1]: process started with pid [3345]
[qrb_ros_amr-1] [amr_manager]: AMRManager
[qrb_ros_amr-1] [amr_manager]: init
[qrb_ros_amr-1] [INFO] [1758106740.351087121] [mapping_service_server]: MappingServiceServer
[qrb_ros_amr-1] [amr_state_machine]: register_start_p2p_nav_callback
[qrb_ros_amr-1] [amr_state_machine]: register_start_follow_path_callback
[qrb_ros_amr-1] [amr_state_machine]: register_start_waypoint_follow_path_callback
[qrb_ros_amr-1] [amr_state_machine]: register_sub_cmd_callback
[qrb_ros_amr-1] [amr_state_machine]: register_navigate_to_charging_callback
[qrb_ros_amr-1] [INFO] [1758106740.403569396] [cartographer_service_client]: Creating
[qrb_ros_amr-1] [INFO] [1758106740.403678772] [cartographer_service_client]: init_client
[qrb_ros_amr-1] [amr_state_machine]: register_slam_command_callback
[qrb_ros_amr-1] [low_power_manager]: register_change_mode_callback
[qrb_ros_amr-1] [INFO] [1758106740.417741601] [amr_status_transporter]: AMRStatusTransporter
[qrb_ros_amr-1] [amr_state_machine]: register_start_charging_callback
[qrb_ros_amr-1] [amr_state_machine]: register_notify_exception_callback
[qrb_ros_amr-1] [amr_state_machine]: register_send_amr_state_changed_callback
[qrb_ros_amr-1] [amr_state_machine]: register_publish_twist_callback
[qrb_ros_amr-1] [INFO] [1758106740.434623101] [developer_mode_subscriber]: DeveloperModeSubscriber
[qrb_ros_amr-1] [amr_state_machine]: register_node_manager_callback
[qrb_ros_amr-1] [INFO] [1758106740.456975064] [amr_controller]: Init amr node
...
We love community contributions! Get started by reading our CONTRIBUTING.md.
Feel free to create an issue for bug report, feature requests or any discussion💡.
Thanks to all our contributors who have helped make this project better!
quic-zhanlin |
xiaowz-robotics |
Does it support other AMR?
No, it only supports Qualcomm AMR.
Project is licensed under the BSD-3-Clause License. See LICENSE for the full license text.