Skip to content

qualcomm-qrb-ros/qrb_ros_amr_service

QRB ROS AMR Service

Qualcomm QRB ROS

ROS2 Package for Autonomous Mobile Robot (AMR) Service on Qualcomm Robotics Platform

Qualcomm Ubuntu Jazzy


👋 Overview

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.
architecture

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.

🔎 Table of contents

⚓ APIs

🔹 qrb_ros_amr_service APIs

ROS interfaces

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

ROS message parameters

amr_status
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.

test
Name Type Description Default Value
data int16 Debugging ID -

Note

Other modules can send debugging information to simulate AMR events in developer mode.

debug_exception
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.

sub_cmd
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.

amr_mapping
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.

api
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.

cmd
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.

🔹 qrb_amr_manager APIs

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..

🎯 Supported targets

Development Hardware Qualcomm Dragonwing™ RB3 Gen2 Qualcomm Dragonwing™ IQ-9075 EVK
Hardware Overview

✨ Installation

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.

👨‍💻 Build from source

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

🚀 Usage

Start the amr service node

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
...

🤝 Contributing

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💡.

❤️ Contributors

Thanks to all our contributors who have helped make this project better!

quic-zhanlin
quic-zhanlin
xiaowz-robotics
xiaowz-robotics

❔ FAQs

Does it support other AMR?
No, it only supports Qualcomm AMR.

📜 License

Project is licensed under the BSD-3-Clause License. See LICENSE for the full license text.

About

No description, website, or topics provided.

Resources

License

Code of conduct

Contributing

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Contributors 4

  •  
  •  
  •  
  •