ROS2 API Reference

This document describes all ROS 2 interfaces exposed by GR-Navigation, including topics, services, and actions.

Important Notes

When the navigation system runs in a custom namespace, the namespace is prefixed to every node, topic, service, and action. Message structures remain unchanged.

e.g.

Without a namespace	With the `robot1` namespace
/slam/set_mode	/robot1/slam/set_mode
`ros2 service call /slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"`	`ros2 service call /robot1/slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"`
/slam/mode_status	/robot1/slam/mode_status
ros2 topic echo /slam/mode_status	ros2 topic echo /robot1/slam/mode_status

1. Switching Between Mapping and Localization

1.1 /slam/set_mode (Service)

Switch between mapping and localization modes.

Service Type: fourier_msgs/srv/SetMode

Example Call:

Bash# Switch to mapping mode
ros2 service call /slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"

# Switch to localization mode
ros2 service call /slam/set_mode fourier_msgs/srv/SetMode "{mode: 'localization'}"

Python:

Pythonfrom fourier_msgs.srv import SetMode
import rclpy
from rclpy.node import Node

class ModeClient(Node):
 def __init__(self):
 super().__init__('mode_client')
 self.client = self.create_client(SetMode, '/slam/set_mode')
 self.client.wait_for_service()

 def switch_mode(self, mode: str):
 request = SetMode.Request()
 request.mode = mode
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

# Example
rclpy.init()
client = ModeClient()
result = client.switch_mode('localization')
print(f"Success: {result.success}, Message: {result.message}")

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/set_mode.hpp>

class ModeClient : public rclcpp::Node {
public:
 ModeClient() : Node("mode_client") {
 client_ = create_client<fourier_msgs::srv::SetMode>("/slam/set_mode");
 client_->wait_for_service();
 }

 bool switch_mode(const std::string& mode) {
 auto request = std::make_shared<fourier_msgs::srv::SetMode::Request>();
 request->mode = mode;
 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 RCLCPP_INFO(get_logger(), "Result: %s", result->message.c_str());
 return result->success;
 }
 return false;
 }

private:
 rclcpp::Client<fourier_msgs::srv::SetMode>::SharedPtr client_;
};

// Example
int main(int argc, char** argv) {
 rclcpp::init(argc, argv);
 auto client = std::make_shared<ModeClient>();
 client->switch_mode("localization");
 rclcpp::shutdown();
 return 0;
}

1.2 /slam/mode_status (Topic)

Publish the current operating mode.

Message Type: std_msgs/msg/String

Message Values:

"mapping" - Mapping mode
"localization" - Localization mode

Subscription Example:

Bashros2 topic echo /slam/mode_status

2. Mapping Mode APIs

2.1 /clear_map (Topic)

Clear the current map and restart mapping.

Message Type: std_msgs/msg/String

Behavior:

In localization mode, switch to mapping mode and clear the cached map.
In mapping mode, restart mapping and clear all map data.

Publishing Example:

Bashros2 topic pub /clear_map std_msgs/msg/String "{data: ''}" --once

Python publisher:

Pythonfrom std_msgs.msg import String
import rclpy
from rclpy.node import Node

class ClearMapPublisher(Node):
 def __init__(self):
 super().__init__('clear_map_publisher')
 self.publisher = self.create_publisher(String, '/clear_map', 10)

 def clear(self):
 msg = String()
 msg.data = ''
 self.publisher.publish(msg)
 self.get_logger().info('Clear map command sent')

rclpy.init()
node = ClearMapPublisher()
node.clear()

C++ publisher:

C++#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

class ClearMapPublisher : public rclcpp::Node {
public:
 ClearMapPublisher() : Node("clear_map_publisher") {
 publisher_ = create_publisher<std_msgs::msg::String>("/clear_map", 10);
 }

 void clear() {
 auto msg = std_msgs::msg::String();
 msg.data = "";
 publisher_->publish(msg);
 RCLCPP_INFO(get_logger(), "Clear map command sent");
 }

private:
 rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

2.2 /map (Topic)

Publish the optimized 2D map.

Message Type: nav_msgs/msg/OccupancyGrid

Data Source:

Mapping mode: the mapping and localization module publishes the live 2D occupancy grid.
Localization mode: the map service publishes the loaded 2D occupancy grid.

Configuration Parameters:

Parameter	Value	Description
Resolution	0.05 m	Each grid cell represents 5 cm.
Frame ID	`map`	Map coordinate frame.

Subscription Example:

Bashros2 topic echo /map

2.3 /cloud_registered_gravity (Topic)

Publish gravity-aligned 3D point-cloud map data.

Message Type: sensor_msgs/msg/PointCloud2

Description: Gravity-aligned point-cloud data produced by the mapping and localization module.

Subscription Example:

Bashros2 topic echo /cloud_registered_gravity

2.4 /optimize_map (Topic)

Publish the optimized 2D map.

Message Type: nav_msgs/msg/OccupancyGrid

Description: A denoised 2D occupancy grid with isolated pixels and noise removed. This topic is published only in localization mode.

Subscription Example:

Bashros2 topic echo /optimize_map

2.5 /slam/save_map (Service)

Save the 3D point-cloud map.

Service Type: fourier_msgs/srv/SaveMap

Saved Data:

3D map: global.pcd, stored as a PCL binary-compressed point cloud.

Example Call:

Bash# Save to the default location: ./data/my_map/
ros2 service call /slam/save_map fourier_msgs/srv/SaveMap "{map_id: 'my_map'}"

# Save to an absolute path
ros2 service call /slam/save_map fourier_msgs/srv/SaveMap "{map_id: '/home/user/maps/office'}"

Python:

Pythonfrom fourier_msgs.srv import SaveMap
import rclpy
from rclpy.node import Node

class MapSaver(Node):
 def __init__(self):
 super().__init__('map_saver')
 self.client = self.create_client(SaveMap, '/slam/save_map')
 self.client.wait_for_service()

 def save(self, map_id: str):
 request = SaveMap.Request()
 request.map_id = map_id
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
        return result.response == 0  # 0 indicates success

rclpy.init()
saver = MapSaver()
success = saver.save('/home/user/maps/my_map')
print(f"Save {'succeeded' if success else 'failed'}")

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/save_map.hpp>

class MapSaver : public rclcpp::Node {
public:
 MapSaver() : Node("map_saver") {
 client_ = create_client<fourier_msgs::srv::SaveMap>("/slam/save_map");
 client_->wait_for_service();
 }

 bool save(const std::string& map_id) {
 auto request = std::make_shared<fourier_msgs::srv::SaveMap::Request>();
 request->map_id = map_id;
 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 return future.get()->response == 0;
 }
 return false;
 }

private:
 rclcpp::Client<fourier_msgs::srv::SaveMap>::SharedPtr client_;
};

2.6 Saving a 2D Map

Save the 2D occupancy grid with the map-saving utility.

Command-line tool: map_saver_cli

Saved Data:

2D map: map.pgm and map.yaml in the standard 2D occupancy-grid format.

Example Call:

Bash# Specify the topic and free-space threshold
map_saver_cli --free 0.196 -t /map -f /path/to/save/map

Parameters:

Option	Description
`-f` / `--output`	Output path without a file extension.
`-t` / `--topic`	Map topic name; defaults to `/map`.
`--free`	Free-space threshold from 0.0 to 1.0; defaults to 0.25.
`--occupied`	Occupied-space threshold from 0.0 to 1.0; defaults to 0.65.

3. Localization Mode APIs

3.1 /initialpose (Topic)

Set the robot initial pose

Message Type: geometry_msgs/msg/PoseWithCovarianceStamped

Publishing Example:

Bash# Set position to (1.0, 2.0, 0.0) and yaw to 90 degrees as a quaternion
ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
 "{header: {frame_id: 'map'}, pose: {pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.707, w: 0.707}}}}" --once

Python publisher:

Pythonfrom geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
from rclpy.node import Node
import math

class InitialPosePublisher(Node):
 def __init__(self):
 super().__init__('initialpose_publisher')
 self.publisher = self.create_publisher(
 PoseWithCovarianceStamped, '/initialpose', 10)

 def set_pose(self, x: float, y: float, yaw: float):
 msg = PoseWithCovarianceStamped()
 msg.header.frame_id = 'map'
 msg.header.stamp = self.get_clock().now().to_msg()
 msg.pose.pose.position.x = x
 msg.pose.pose.position.y = y
 msg.pose.pose.position.z = 0.0
        # Convert yaw to a quaternion
 msg.pose.pose.orientation.x = 0.0
 msg.pose.pose.orientation.y = 0.0
 msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
 msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
 self.publisher.publish(msg)

rclpy.init()
node = InitialPosePublisher()
node.set_pose(1.0, 2.0, 1.57)  # x=1, y=2, yaw=90 degrees

C++ publisher:

C++#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <cmath>

class InitialPosePublisher : public rclcpp::Node {
public:
 InitialPosePublisher() : Node("initialpose_publisher") {
 publisher_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
 "/initialpose", 10);
 }

 void set_pose(double x, double y, double yaw) {
 auto msg = geometry_msgs::msg::PoseWithCovarianceStamped();
 msg.header.frame_id = "map";
 msg.header.stamp = now();
 msg.pose.pose.position.x = x;
 msg.pose.pose.position.y = y;
 msg.pose.pose.position.z = 0.0;
        // Convert yaw to a quaternion
 msg.pose.pose.orientation.x = 0.0;
 msg.pose.pose.orientation.y = 0.0;
 msg.pose.pose.orientation.z = std::sin(yaw / 2.0);
 msg.pose.pose.orientation.w = std::cos(yaw / 2.0);
 publisher_->publish(msg);
 }

private:
 rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
};

3.2 /slam/load_map (Service)

Load a map and switch to localization mode

Service Type: fourier_msgs/srv/LoadMap

Example Call:

Bashros2 service call /slam/load_map fourier_msgs/srv/LoadMap \
 "{map_path: '/home/user/maps/office', x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}"

Python:

Pythonfrom fourier_msgs.srv import LoadMap
import rclpy
from rclpy.node import Node

class MapLoader(Node):
 def __init__(self):
 super().__init__('map_loader')
 self.client = self.create_client(LoadMap, '/slam/load_map')
 self.client.wait_for_service()

 def load(self, path: str, x=0.0, y=0.0, z=0.0, yaw=0.0):
 request = LoadMap.Request()
 request.map_path = path
 request.x = x
 request.y = y
 request.z = z
 request.yaw = yaw
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
        return result.result == 0  # 0 indicates success

rclpy.init()
loader = MapLoader()
success = loader.load('/home/user/maps/office', x=1.0, y=2.0, yaw=1.57)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/load_map.hpp>

class MapLoader : public rclcpp::Node {
public:
 MapLoader() : Node("map_loader") {
 client_ = create_client<fourier_msgs::srv::LoadMap>("/slam/load_map");
 client_->wait_for_service();
 }

 bool load(const std::string& path, double x=0.0, double y=0.0,
 double z=0.0, double yaw=0.0) {
 auto request = std::make_shared<fourier_msgs::srv::LoadMap::Request>();
 request->map_path = path;
 request->x = x;
 request->y = y;
 request->z = z;
 request->yaw = yaw;
 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 return future.get()->result == 0;
 }
 return false;
 }

private:
 rclcpp::Client<fourier_msgs::srv::LoadMap>::SharedPtr client_;
};

3.3 /robot_pose (Topic)

Publish the full 3D robot pose (6 DoF).

Message Type: geometry_msgs/msg/PoseStamped

Description: The localization system publishes the robot's complete 3D pose in the map frame, including x, y, and z position and quaternion orientation. Unlike the planar pose commonly used for 2D navigation, this topic provides all six degrees of freedom.

Publication rate: Configurable; 20 Hz by default through the pose_pub_period parameter.

Frame ID: map

Subscription Example:

Bashros2 topic echo /robot_pose

Python subscriber:

Pythonfrom geometry_msgs.msg import PoseStamped
import rclpy
from rclpy.node import Node

class PoseSubscriber(Node):
 def __init__(self):
 super().__init__('pose_subscriber')
 self.subscription = self.create_subscription(
 PoseStamped, '/robot_pose', self.pose_callback, 10)

 def pose_callback(self, msg):
 pos = msg.pose.position
 ori = msg.pose.orientation
 print(f"Position: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}")
 print(f"Orientation: x={ori.x:.3f}, y={ori.y:.3f}, z={ori.z:.3f}, w={ori.w:.3f}")

rclpy.init()
node = PoseSubscriber()
rclpy.spin(node)

C++ subscriber:

C++#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

class PoseSubscriber : public rclcpp::Node {
public:
 PoseSubscriber() : Node("pose_subscriber") {
 subscription_ = create_subscription<geometry_msgs::msg::PoseStamped>(
 "/robot_pose", 10,
 [this](geometry_msgs::msg::PoseStamped::SharedPtr msg) {
 RCLCPP_INFO(get_logger(), "Position: [%.3f, %.3f, %.3f]",
 msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
 });
 }

private:
 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
};

3.4 /odom (Topic)

Publish robot odometry data

Message Type: nav_msgs/msg/Odometry

Description: Contains the robot position, orientation, and velocity.

Subscription Example:

Bashros2 topic echo /odom

3.5 /odom_status_code (Topic)

Publish the localization status code

Message Type: std_msgs/msg/Int8

Description: Publishes the localization system state in real time for monitoring and diagnostics.

Status codes:

Code	Name	Description
0	IDLE	Idle.
1	INITIALIZING	Initialization in progress.
2	GOOD	Localization is operating normally.
3	FOLLOWING_DR	Localization has degraded to dead reckoning.
4	FAIL	Localization failed.

Subscription Example:

Bashros2 topic echo /odom_status_code

Python subscriber:

Pythonfrom std_msgs.msg import Int8
import rclpy
from rclpy.node import Node

class OdomStatusSubscriber(Node):
    # Status-code constants
 IDLE = 0
 INITIALIZING = 1
 GOOD = 2
 FOLLOWING_DR = 3
 FAIL = 4

 STATUS_NAMES = {
 0: "IDLE",
 1: "INITIALIZING",
 2: "GOOD",
 3: "FOLLOWING_DR",
 4: "FAIL"
 }

 def __init__(self):
 super().__init__('odom_status_subscriber')
 self.subscription = self.create_subscription(
 Int8, '/odom_status_code', self.status_callback, 10)

 def status_callback(self, msg):
 status_name = self.STATUS_NAMES.get(msg.data, "UNKNOWN")
 print(f"Localization status: {status_name} (code: {msg.data})")

 if msg.data == self.GOOD:
            print("Localization is operating normally")
 elif msg.data == self.FOLLOWING_DR:
            print("Warning: localization degraded; using dead reckoning")
 elif msg.data == self.FAIL:
            print("Error: localization failed")

rclpy.init()
node = OdomStatusSubscriber()
rclpy.spin(node)

C++ subscriber:

C++#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>

class OdomStatusSubscriber : public rclcpp::Node {
public:
    // Status-code constants
 static constexpr int8_t IDLE = 0;
 static constexpr int8_t INITIALIZING = 1;
 static constexpr int8_t GOOD = 2;
 static constexpr int8_t FOLLOWING_DR = 3;
 static constexpr int8_t FAIL = 4;

 OdomStatusSubscriber() : Node("odom_status_subscriber") {
 subscription_ = create_subscription<std_msgs::msg::Int8>(
 "/odom_status_code", 10,
 [this](std_msgs::msg::Int8::SharedPtr msg) {
 std::string status_name = getStatusName(msg->data);
                RCLCPP_INFO(get_logger(), "Localization status: %s (code: %d)",
 status_name.c_str(), msg->data);

 if (msg->data == GOOD) {
                    RCLCPP_INFO(get_logger(), "Localization is operating normally");
 } else if (msg->data == FOLLOWING_DR) {
                    RCLCPP_WARN(get_logger(), "Localization degraded; using dead reckoning");
 } else if (msg->data == FAIL) {
                    RCLCPP_ERROR(get_logger(), "Localization failed");
 }
 });
 }

private:
 std::string getStatusName(int8_t code) {
 switch(code) {
 case IDLE: return "IDLE";
 case INITIALIZING: return "INITIALIZING";
 case GOOD: return "GOOD";
 case FOLLOWING_DR: return "FOLLOWING_DR";
 case FAIL: return "FAIL";
 default: return "UNKNOWN";
 }
 }

 rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr subscription_;
};

3.6 /odom_status_score (Topic)

Publish the localization confidence score

Message Type: std_msgs/msg/Int8

Description: Publishes a confidence score for evaluating localization quality. The value is valid when the localization state is GOOD, FOLLOWING_DR, or FAIL; it is zero in other states.

Data interpretation:

When /odom_status_code is 2, 3, or 4, data is the localization confidence from 0 to 100.
When /odom_status_code is 0 or 1, data is 0.

Subscription Example:

Bashros2 topic echo /odom_status_score

Python subscriber (used with the status code):

Pythonfrom std_msgs.msg import Int8, Float32
import rclpy
from rclpy.node import Node

class OdomMonitor(Node):
 def __init__(self):
 super().__init__('odom_monitor')
 self.status_code = 0
 self.status_score = 0.0

 self.code_sub = self.create_subscription(
 Int8, '/odom_status_code', self.code_callback, 10)
 self.score_sub = self.create_subscription(
 Float32, '/odom_status_score', self.score_callback, 10)

 def code_callback(self, msg):
 self.status_code = msg.data
 self.print_status()

 def score_callback(self, msg):
 self.status_score = msg.data
 self.print_status()

 def print_status(self):
 if self.status_code == 2: # GOOD
 print(f"Localization is healthy - confidence: {self.status_score:.1f}/100")
 if self.status_score < 50:
                print("Warning: localization confidence is low")
 else:
 print(f"Localization status is abnormal (code: {self.status_code}), score: {self.status_score}")

rclpy.init()
node = OdomMonitor()
rclpy.spin(node)

C++ subscriber (used with the status code):

C++#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/float32.hpp>

class OdomMonitor : public rclcpp::Node {
public:
 OdomMonitor() : Node("odom_monitor"), status_code_(0), status_score_(0.0) {
 code_sub_ = create_subscription<std_msgs::msg::Int8>(
 "/odom_status_code", 10,
 [this](std_msgs::msg::Int8::SharedPtr msg) {
 status_code_ = msg->data;
 print_status();
 });

 score_sub_ = create_subscription<std_msgs::msg::Int8>(
 "/odom_status_score", 10,
 [this](std_msgs::msg::Int8::SharedPtr msg) {
 status_score_ = msg->data;
 print_status();
 });
 }

private:
 void print_status() {
 if (status_code_ == 2) { // GOOD
            RCLCPP_INFO(get_logger(), "Localization is healthy - confidence: %.1f/100",
 status_score_);
 if (status_score_ < 50) {
                RCLCPP_WARN(get_logger(), "Localization confidence is low");
 }
 } else {
            RCLCPP_INFO(get_logger(), "Localization state is abnormal (code: %d), score: %.1f",
 status_code_, status_score_);
 }
 }

 rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr code_sub_;
 rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr score_sub_;
 int8_t status_code_;
 float status_score_;
};

3.7 Relocalization

Re-estimate the robot pose in the loaded map when localization is lost or uncertain. The system exposes global and local relocalization services and a status topic.

3.7.1 /slam/global_relocalization (Service)

Trigger global relocalization

Service Type: std_srvs/srv/Empty

Example Call:

Bashros2 service call /slam/global_relocalization std_srvs/srv/Empty

3.7.2 /slam/trigger_local_relocalization (Service)

Trigger local relocalization within a polygon in the map frame. An empty polygon_vertices array triggers global relocalization.

Service Type: fourier_msgs/srv/VPRLocalRelocalization

Example Call:

Bashros2 service call /slam/trigger_local_relocalization \
 fourier_msgs/srv/VPRLocalRelocalization \
 "{polygon_vertices: [{x: 1.0, y: 1.0, z: 0.0}, {x: 4.0, y: 1.0, z: 0.0}, {x: 4.0, y: 4.0, z: 0.0}, {x: 1.0, y: 4.0, z: 0.0}]}"

3.7.6 /reloc_status (Topic)

Indicates whether relocalization is in progress; true means the process has not finished.

Message Type: std_msgs/msg/Bool

4.1 navigate_to_pose (Action)

Navigate to a specified goal

Action Type: nav2_msgs/action/NavigateToPose

Goal Request

Field	Type	Description
pose	geometry_msgs/PoseStamped	Target pose, including position and orientation.
behavior_tree	string	Optional behavior-tree file path; leave empty to use the default configuration.

Result

Field	Type	Description
result	std_msgs/Empty	Empty result payload on success.
error_code	uint16	Error code
error_msg	string	Human-readable error description.

Error code:

Error code	Name	Description
0	NONE	Navigation succeeded without an error.
9001	UNKNOWN	Unknown error.
9002	FAILED_TO_LOAD_BEHAVIOR_TREE	Failed to load the behavior tree.
9003	TF_ERROR	Transform error.
9004	GOAL_CHECKER_ERROR	Goal-checker error.
9005	PREEMPTED	The action was preempted.
9006	NO_VALID_PATH	No valid path was found.

Live Feedback

Field	Type	Description
current_pose	geometry_msgs/PoseStamped	Current robot pose.
navigation_time	builtin_interfaces/Duration	Elapsed navigation time.
estimated_time_remaining	builtin_interfaces/Duration	Estimated time remaining.
number_of_recoveries	int16	Number of recovery attempts.
distance_remaining	float32	Remaining distance in meters.

Example Call

Command line:

Bash# Navigate to position (1.0, 2.0) with a yaw of 45 degrees
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
 "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.383, w: 0.924}}}}"

# Include feedback
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
 "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.383, w: 0.924}}}}" --feedback

Python:

Pythonfrom geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
import math
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

class NavigateClient(Node):
 def __init__(self):
 super().__init__('navigate_client')
 self.client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

 def send_goal(self, x: float, y: float, yaw: float):
 goal = NavigateToPose.Goal()
 goal.pose = PoseStamped()
 goal.pose.header.frame_id = 'map'
 goal.pose.header.stamp = self.get_clock().now().to_msg()
 goal.pose.pose.position.x = x
 goal.pose.pose.position.y = y
 goal.pose.pose.orientation.z = math.sin(yaw / 2.0)
 goal.pose.pose.orientation.w = math.cos(yaw / 2.0)

 self.client.wait_for_server()
 return self.client.send_goal_async(goal)

rclpy.init()
node = NavigateClient()
node.send_goal(1.0, 2.0, 0.785)
rclpy.spin(node)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <cmath>

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class NavigationClient : public rclcpp::Node {
public:
 NavigationClient() : Node("navigation_client") {
 client_ = rclcpp_action::create_client<NavigateToPose>(
 this, "/navigate_to_pose");
 client_->wait_for_action_server();
 }

 void navigate_to(double x, double y, double yaw) {
        // Build the goal
 auto goal = NavigateToPose::Goal();
 goal.pose.header.frame_id = "map";
 goal.pose.header.stamp = now();
 goal.pose.pose.position.x = x;
 goal.pose.pose.position.y = y;
 goal.pose.pose.position.z = 0.0;
 goal.pose.pose.orientation.x = 0.0;
 goal.pose.pose.orientation.y = 0.0;
 goal.pose.pose.orientation.z = std::sin(yaw / 2.0);
 goal.pose.pose.orientation.w = std::cos(yaw / 2.0);

        // Configure callbacks
 auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();

        // Feedback callback
 send_goal_options.feedback_callback =
 [this](GoalHandleNavigateToPose::SharedPtr,
 const std::shared_ptr<const NavigateToPose::Feedback> feedback) {
                RCLCPP_INFO(get_logger(), "Distance remaining: %.2f m, recoveries: %d",
 feedback->distance_remaining, feedback->number_of_recoveries);
 };

        // Result callback
 send_goal_options.result_callback =
 [this](const GoalHandleNavigateToPose::WrappedResult& result) {
 switch (result.code) {
 case rclcpp_action::ResultCode::SUCCEEDED:
                        RCLCPP_INFO(get_logger(), "Navigation succeeded");
 break;
 case rclcpp_action::ResultCode::ABORTED:
                        RCLCPP_ERROR(get_logger(), "Navigation aborted");
 break;
 case rclcpp_action::ResultCode::CANCELED:
                        RCLCPP_WARN(get_logger(), "Navigation canceled");
 break;
 default:
                        RCLCPP_ERROR(get_logger(), "Unknown result");
 break;
 }
 };

        // Send the goal
 client_->async_send_goal(goal, send_goal_options);
 }

 void cancel_navigation() {
 client_->async_cancel_all_goals();
 }

private:
 rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
};

int main(int argc, char** argv) {
 rclcpp::init(argc, argv);
 auto client = std::make_shared<NavigationClient>();
    client->navigate_to(1.0, 2.0, 0.785);  // x=1, y=2, yaw=45 degrees
 rclcpp::spin(client);
 rclcpp::shutdown();
 return 0;
}

4.2 /plan (Topic)

Publish the global plan

Message Type: nav_msgs/msg/Path

Description: Global path generated by the navigation system.

Subscription Example:

Bashros2 topic echo /plan

4.3 /cmd_vel (Topic)

Robot velocity command

Message Type: geometry_msgs/msg/Twist

Subscription Example:

Bashros2 topic echo /cmd_vel

4.4 cancel_current_action (Service)

Cancel the navigation action currently in progress

Service Type: fourier_msgs/srv/CancelCurrentAction

Example Call:

Bashros2 service call /cancel_current_action fourier_msgs/srv/CancelCurrentAction

Python:

Pythonfrom fourier_msgs.srv import CancelCurrentAction
import rclpy
from rclpy.node import Node

class ActionCanceller(Node):
 def __init__(self):
 super().__init__('action_canceller')
 self.client = self.create_client(CancelCurrentAction, '/cancel_current_action')
 self.client.wait_for_service()

 def cancel(self):
 request = CancelCurrentAction.Request()
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 return result.success

rclpy.init()
canceller = ActionCanceller()
if canceller.cancel():
 print("Action cancelled successfully")

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/cancel_current_action.hpp>

class ActionCanceller : public rclcpp::Node {
public:
 ActionCanceller() : Node("action_canceller") {
 client_ = create_client<fourier_msgs::srv::CancelCurrentAction>(
 "/cancel_current_action");
 client_->wait_for_service();
 }

 bool cancel() {
 auto request = std::make_shared<fourier_msgs::srv::CancelCurrentAction::Request>();
 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 return future.get()->success;
 }
 return false;
 }

private:
 rclcpp::Client<fourier_msgs::srv::CancelCurrentAction>::SharedPtr client_;
};

4.5 get_current_action (Service)

Get information about the action currently in progress

Service Type: fourier_msgs/srv/GetCurrentAction

Example Call:

Bashros2 service call /get_current_action fourier_msgs/srv/GetCurrentAction

Python:

Pythonfrom fourier_msgs.srv import GetCurrentAction
import rclpy
from rclpy.node import Node

class ActionMonitor(Node):
 def __init__(self):
 super().__init__('action_monitor')
 self.client = self.create_client(GetCurrentAction, '/get_current_action')
 self.client.wait_for_service()

 def get_status(self):
 request = GetCurrentAction.Request()
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 if result.success:
 print(f"Action: {result.action_name}")
 print(f"Status: {result.status_description}")
 return result

rclpy.init()
monitor = ActionMonitor()
monitor.get_status()

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/get_current_action.hpp>

class ActionMonitor : public rclcpp::Node {
public:
 ActionMonitor() : Node("action_monitor") {
 client_ = create_client<fourier_msgs::srv::GetCurrentAction>(
 "/get_current_action");
 client_->wait_for_service();
 }

 void get_status() {
 auto request = std::make_shared<fourier_msgs::srv::GetCurrentAction::Request>();
 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Action: %s, Status: %s",
 result->action_name.c_str(),
 result->status_description.c_str());
 }
 }
 }

private:
 rclcpp::Client<fourier_msgs::srv::GetCurrentAction>::SharedPtr client_;
};

4.6 action_status (Topic)

Publish the current action execution status

Message Type: fourier_msgs/msg/ActionStatus

Publication rate: 1 Hz

Subscription Example:

Bashros2 topic echo /action_status

5. Sensor Data APIs

5.1 /scan (Topic)

Publish 2D laser-scan data.

Message Type: sensor_msgs/msg/LaserScan

Description: A 2D scan generated from 3D LiDAR data for use by the 2D navigation stack.

Subscription Example:

Bashros2 topic echo /scan

5.2 /segmented_groundless_points (Topic)

Publish a ground-removed 3D point cloud.

Message Type: sensor_msgs/msg/PointCloud2

Description: Point-cloud data after ground segmentation, used for obstacle detection.

Subscription Example:

Bashros2 topic echo /segmented_groundless_points

5.3 /rslidar_points (Topic)

Publish raw RoboSense LiDAR point-cloud data.

Message Type: sensor_msgs/msg/PointCloud2

Subscription Example:

Bashros2 topic echo /rslidar_points

5.4 /imu (Topic)

Publish IMU sensor data.

Message Type: sensor_msgs/msg/Imu

Description: Contains acceleration and angular-velocity measurements used by mapping and localization.

Subscription Example:

Bashros2 topic echo /imu

6. Camera APIs

The camera subsystem provides color images, depth images, and point-cloud data. Camera data is used for environmental perception, obstacle detection, and navigation.

6.1 Camera Configuration

The system supports multiple cameras. Camera parameters are configured in the launch file.

Configuration notes:

Multiple cameras are supported.
Camera names follow the pattern camera_01, camera_02, camera_03, and so on.
Each camera has an independent USB connection, frame, and topic namespace.
Camera numbering starts at 01 and increments sequentially.

Topic naming:

Camera 1 topics use the /camera_01/ prefix.
Camera 2 topics use the /camera_02/ prefix.
Examples: /camera_01/color/image_raw and /camera_02/depth/points.

6.2 Camera Driver APIs (CAMERA_SDK_ROS2)

6.2.1 /camera_01/color/image_raw (Topic)

Publish color camera images

Message Type: sensor_msgs/msg/Image

Description: Publishes uncompressed color images, usually encoded as RGB.

Subscription Example:

Bashros2 topic echo /camera_01/color/image_raw

6.2.2 /camera_01/color/camera_info (Topic)

Publish color camera calibration information

Message Type: sensor_msgs/msg/CameraInfo

Description: Contains camera intrinsic parameters, distortion coefficients, and calibration information.

Subscription Example:

Bashros2 topic echo /camera_01/color/camera_info

6.2.3 /camera_01/depth/image_raw (Topic)

Publish depth camera images

Message Type: sensor_msgs/msg/Image

Description: Publishes depth images whose pixel values represent distance, usually in millimeters or meters depending on the configured encoding.

Subscription Example:

Bashros2 topic echo /camera_01/depth/image_raw

6.2.4 /camera_01/depth/camera_info (Topic)

Publish depth camera calibration information

Message Type: sensor_msgs/msg/CameraInfo

Description: Contains camera intrinsic parameters, distortion coefficients, and calibration information.

Subscription Example:

Bashros2 topic echo /camera_01/depth/camera_info

6.2.5 /camera_01/depth/points (Topic)

Publish depth point-cloud data

Message Type: sensor_msgs/msg/PointCloud2

Description: Organized 3D point-cloud data in the camera optical frame, typically camera_01_color_optical_frame.

Subscription Example:

Bashros2 topic echo /camera_01/depth/points

6.3 Camera Preprocessing APIs (camera_preprocessing)

The camera preprocessing node synchronizes sensor data, interpolates the robot pose, and publishes fused and filtered outputs.

6.3.1 /camera_01/fused_data (Topic)

Publish fused camera data containing an RGB image, point cloud, and robot pose.

Message Type: fourier_msgs/msg/UnifiedCameraData

Message Values:

header: Message header containing the camera timestamp and coordinate frame.
camera_name: Camera identifier used to distinguish multiple cameras.
rgb_image: RGB image data. image data. image data. image data. image data.
point_cloud: Raw point cloud in the camera frame.
robot_pose: Robot pose interpolated at the camera timestamp.
camera_transform: Transform from camera_link to base_link.

Behavior:

Synchronizes the RGB image and point cloud.
Interpolates the robot pose at the sensor timestamp.
Packages all synchronized data in one message.
Supports downstream perception, mapping, and navigation modules.

Subscription Example:

Bashros2 topic echo /camera_01/fused_data

Python subscriber:

Pythonfrom fourier_msgs.msg import UnifiedCameraData
import rclpy
from rclpy.node import Node

class CameraDataSubscriber(Node):
 def __init__(self):
 super().__init__('camera_data_subscriber')
 self.subscription = self.create_subscription(
 UnifiedCameraData,
 '/camera_01/fused_data',
 self.callback,
 10)

 def callback(self, msg):
 print(f"Camera: {msg.camera_name}")
 print(f"Image size: {msg.rgb_image.width}x{msg.rgb_image.height}")
 print(f"Point cloud points: {msg.point_cloud.width * msg.point_cloud.height}")
 print(f"Robot pose: x={msg.robot_pose.position.x:.3f}, "
 f"y={msg.robot_pose.position.y:.3f}, "
 f"z={msg.robot_pose.position.z:.3f}")

rclpy.init()
node = CameraDataSubscriber()
rclpy.spin(node)

C++ subscriber:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/msg/unified_camera_data.hpp>

class CameraDataSubscriber : public rclcpp::Node {
public:
 CameraDataSubscriber() : Node("camera_data_subscriber") {
 subscription_ = create_subscription<fourier_msgs::msg::UnifiedCameraData>(
 "/camera_01/fused_data", 10,
 [this](fourier_msgs::msg::UnifiedCameraData::SharedPtr msg) {
 RCLCPP_INFO(get_logger(), "Camera: %s", msg->camera_name.c_str());
 RCLCPP_INFO(get_logger(), "Image size: %dx%d",
 msg->rgb_image.width, msg->rgb_image.height);
 RCLCPP_INFO(get_logger(), "Point cloud points: %d",
 msg->point_cloud.width * msg->point_cloud.height);
 RCLCPP_INFO(get_logger(), "Robot pose: [%.3f, %.3f, %.3f]",
 msg->robot_pose.position.x,
 msg->robot_pose.position.y,
 msg->robot_pose.position.z);
 });
 }

private:
 rclcpp::Subscription<fourier_msgs::msg::UnifiedCameraData>::SharedPtr subscription_;
};

6.3.2 /camera_01/filtered_pointcloud (Topic)

Publish filtered point-cloud data

Message Type: sensor_msgs/msg/PointCloud2

Behavior:

Applies the configured point-cloud filters:
- Voxel downsampling (VoxelGridFilter): reduces point-cloud density.
- Region filtering (RegionFilter): keeps points inside the configured 3D region.
- Ground filtering (GroundFilter): removes ground points.
- Frame transformation (TransformFilter): transforms the point cloud to the target frame.
Publishes the filtered point cloud for obstacle detection and navigation.

Coordinate frame: base_link, or the frame configured for the transform filter.

Subscription Example:

Bashros2 topic echo /camera_01/filtered_pointcloud

Python subscriber:

Pythonfrom sensor_msgs.msg import PointCloud2
import rclpy
from rclpy.node import Node

class FilteredCloudSubscriber(Node):
 def __init__(self):
 super().__init__('filtered_cloud_subscriber')
 self.subscription = self.create_subscription(
 PointCloud2,
 '/camera_01/filtered_pointcloud',
 self.callback,
 10)

 def callback(self, msg):
 point_count = msg.width * msg.height
 print(f"Filtered point cloud: {point_count} points")
 print(f"Frame ID: {msg.header.frame_id}")
 print(f"Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec}")

rclpy.init()
node = FilteredCloudSubscriber()
rclpy.spin(node)

C++ subscriber:

C++#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

class FilteredCloudSubscriber : public rclcpp::Node {
public:
 FilteredCloudSubscriber() : Node("filtered_cloud_subscriber") {
 subscription_ = create_subscription<sensor_msgs::msg::PointCloud2>(
 "/camera_01/filtered_pointcloud", 10,
 [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) {
 int point_count = msg->width * msg->height;
 RCLCPP_INFO(get_logger(), "Filtered point cloud: %d points", point_count);
 RCLCPP_INFO(get_logger(), "Frame ID: %s", msg->header.frame_id.c_str());
 });
 }

private:
 rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
};

7. Health Monitoring APIs

The health-monitoring interfaces aggregate component status and report active warnings and errors.

7.1 /Humanoid_nav/health (Topic)

Publish aggregated system health status

Message Type: fourier_msgs/msg/HealthInfo

Publication rate: 10 Hz by default (configurable).

Subscription Example:

Bashros2 topic echo /Humanoid_nav/health

Each error includes a severity and error code defined by BaseErrorInfo.

7.2 Python Subscription Example

Pythonfrom fourier_msgs.msg import HealthInfo
import rclpy
from rclpy.node import Node

class HealthSubscriber(Node):
 def __init__(self):
 super().__init__('health_subscriber')
 self.subscription = self.create_subscription(
 HealthInfo, '/Humanoid_nav/health', self.health_callback, 10)

 def health_callback(self, msg):
 if msg.has_fatal:
            print("Fatal error")
 elif msg.has_error:
            print("An error is active")
 elif msg.has_warning:
            print("A warning is active")
 else:
            print("System healthy")

 for error in msg.errors:
 print(f" [{hex(error.error_code)}] {error.message}")

rclpy.init()
node = HealthSubscriber()
rclpy.spin(node)

7.3 C++ Subscription Example

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/msg/health_info.hpp>

class HealthSubscriber : public rclcpp::Node {
public:
 HealthSubscriber() : Node("health_subscriber") {
 subscription_ = create_subscription<fourier_msgs::msg::HealthInfo>(
 "/Humanoid_nav/health", 10,
 [this](fourier_msgs::msg::HealthInfo::SharedPtr msg) {
 if (msg->has_fatal) {
                    RCLCPP_FATAL(get_logger(), "Fatal error");
 } else if (msg->has_error) {
                    RCLCPP_ERROR(get_logger(), "An error is active");
 } else if (msg->has_warning) {
                    RCLCPP_WARN(get_logger(), "A warning is active");
 }

 for (const auto& error : msg->errors) {
 RCLCPP_INFO(get_logger(), "[0x%08X] %s",
 error.error_code, error.message.c_str());
 }
 });
 }

private:
 rclcpp::Subscription<fourier_msgs::msg::HealthInfo>::SharedPtr subscription_;
};

8. Event Notification APIs

The event interfaces aggregate and publish navigation-related system events.

8.1 /Humanoid_nav/events (Topic)

Publish aggregated system events

Message Type: fourier_msgs/msg/EventsInfo

Publication rate: 10 Hz by default (configurable).

Subscription Example:

Bashros2 topic echo /Humanoid_nav/events

See BaseEventInfo for the complete event-type list and trigger behavior.

8.1.1 `goal_in_forbidden_area` (Navigation Goal in a Forbidden Area)

During navigate_to_pose goal validation, Map Manager checks whether the requested goal lies inside a forbidden-area polygon. If it does, navigation is rejected and this event is published.

Field	Value
`event_type`	`goal_in_forbidden_area`
`source`	`navigation`
`message`	`Goal pose (x, y) lies inside forbidden area id=N on floor '1F'.`

Difference from enter_forbidden_area: goal_in_forbidden_area validates the requested goal when navigation starts. enter_forbidden_area monitors the robot's current position while it is moving and is published by monitor.

Recommended handling: Ask the user to choose another goal, or update the forbidden areas through Map Manager before starting navigation again.

8.2 Python Subscription Example

Pythonfrom fourier_msgs.msg import EventsInfo
import rclpy
from rclpy.node import Node

class EventsSubscriber(Node):
 def __init__(self):
 super().__init__('events_subscriber')
 self.subscription = self.create_subscription(
 EventsInfo, '/Humanoid_nav/events', self.events_callback, 10)

 def events_callback(self, msg):
 for event in msg.events:
 print(f"[{event.event_type}] {event.message} (from: {event.source})")

            # Handle specific event types
 if event.event_type == "obstacle_blocked":
                print("Warning: path is blocked; replanning may be required")
 elif event.event_type == "near_obstacle":
                print("Warning: a nearby obstacle was detected")
 elif event.event_type == "out_of_map":
                print("Warning: robot left the map boundary or entered an unknown area")
 elif event.event_type == "enter_forbidden_area":
                print("Warning: robot entered a forbidden area")
 elif event.event_type == "goal_in_forbidden_area":
                print("Error: navigation goal is inside a forbidden area; choose another goal")
 elif event.event_type == "enter_dangerous_area":
                print("Warning: robot entered a speed-restricted area")
 elif event.event_type == "map_loop_closure":
                print("Info: loop closure detected; map optimized")

rclpy.init()
node = EventsSubscriber()
rclpy.spin(node)

8.3 C++ Subscription Example

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/msg/events_info.hpp>

class EventsSubscriber : public rclcpp::Node {
public:
 EventsSubscriber() : Node("events_subscriber") {
 subscription_ = create_subscription<fourier_msgs::msg::EventsInfo>(
 "/Humanoid_nav/events", 10,
 [this](fourier_msgs::msg::EventsInfo::SharedPtr msg) {
 for (const auto& event : msg->events) {
 RCLCPP_INFO(get_logger(), "[%s] %s (from: %s)",
 event.event_type.c_str(),
 event.message.c_str(),
 event.source.c_str());

 if (event.event_type == "obstacle_blocked") {
                        RCLCPP_WARN(get_logger(), "Path is blocked; replanning may be required");
 } else if (event.event_type == "near_obstacle") {
                        RCLCPP_WARN(get_logger(), "A nearby obstacle was detected");
 } else if (event.event_type == "out_of_map") {
                        RCLCPP_WARN(get_logger(), "Robot left the map boundary or entered an unknown area");
 } else if (event.event_type == "enter_forbidden_area") {
                        RCLCPP_WARN(get_logger(), "Robot entered a forbidden area");
 } else if (event.event_type == "goal_in_forbidden_area") {
                        RCLCPP_ERROR(get_logger(), "Navigation goal is inside a forbidden area; choose another goal");
 } else if (event.event_type == "enter_dangerous_area") {
                        RCLCPP_WARN(get_logger(), "Robot entered a speed-restricted area");
 }
 }
 });
 }

private:
 rclcpp::Subscription<fourier_msgs::msg::EventsInfo>::SharedPtr subscription_;
};

9. Map Management APIs

Map Manager manages composite maps and floors, together with POIs, virtual walls, forbidden areas, speed-restricted areas, rooms, and semantic objects.

9.0 Namespace supports

Map Manager supports ROS 2 namespaces for multi-robot deployments.

Service paths: Services use root paths by default. When a namespace is enabled, it is prefixed to every service path.

Namespace	Service path
No namespace (default)	`/add_floor`, `/list_floors`
`namespace:=robot1`	`/robot1/add_floor`、`/robot1/list_floors`

Command-line prefix (MM):

Bash# Single-robot mode without a namespace: services use root paths
MM=

# Multi-robot mode with a namespace such as robot1
MM=/robot1

# Use ${MM} for all subsequent service calls
ros2 service call ${MM}/load_composite_map map_manager/srv/LoadCompositeMap ...

Configuration notes:ros2 service list | grep -E "add_floor|list_floors" path

Topic paths:

Topic without a namespace	Topic under `robot1`
`/costmap_filter_info`	`/robot1/costmap_filter_info`
`/keepout_filter_mask`	`/robot1/keepout_filter_mask`

Launch examples:

Bash# Single robot (default)
ros2 launch map_manager map_manager.launch.py

# Multiple robots with namespaces
ros2 launch map_manager map_manager.launch.py namespace:=robot1 use_namespace:=true

9.1 Message Type Definitions

9.1.1 Floor Message

Message Type: map_manager/msg/Floor

Field	Type	Description
floor_id	string	Unique floor identifier
name	string	Floor name
level	int32	Numeric floor level, for example 1, 2, or -1.
min_height	float64	Minimum floor height (m)
max_height	float64	Maximum floor height (m)
reference_height	float64	(m)
origin_offset	geometry_msgs/Pose
map_path	string	Map file path
status	uint8	status (0=UNKNOWN, 1=ACTIVE, 2=INACTIVE)

9.1.2 CompositeMapInfo Message

Message Type: map_manager/msg/CompositeMapInfo

Field	Type	Description
map_id	string	Unique map identifier.
name	string	Map name
version	string
created_at	builtin_interfaces/Time	create
modified_at	builtin_interfaces/Time
origin	geometry_msgs/Pose	map
floors	map_manager/msg/Floor[]	List of floors
transitions	map_manager/msg/FloorTransition[]	List of floor transition points
root_path	string	map

9.1.3 POI Message

Message Type: map_manager/msg/POI

Field	Type	Description
id	string	POI Unique identifier
name	string	POI Name
type	string	POI Type (door, room, charging_station )
floor_id	string	Floor identifier; leave empty when the interface allows all floors.
pose	geometry_msgs/Pose2D	2D (x, y, theta)
height	float64	Floor reference height (m)
properties	string	(JSON )

9.1.4 VirtualWall Message

Message Type: map_manager/msg/VirtualWall

Field	Type	Description
id	int32	virtual wall ID
floor_id	string	floor ID
start	geometry_msgs/Point
end	geometry_msgs/Point

9.1.5 ForbiddenArea Message

Message Type: map_manager/msg/ForbiddenArea

Field	Type	Description
id	int32	forbidden area ID
boundary	geometry_msgs/Polygon

9.1.6 DangerousArea Message

Message Type: map_manager/msg/DangerousArea

Field	Type	Description
id	int32	speed-restricted area ID
boundary	geometry_msgs/Polygon
speed_limit	float32	speed limit (m/s)

9.1.7 Room Message

Message Type: map_manager/msg/Room

Field	Type	Description
id	string	room ID
name	string	Human-readable room name.
type	string	Room type.
boundary	geometry_msgs/Point[]
floor_height	float64
ceiling_height	float64
connected_rooms	string[]	room ID list
objects	string[]	contains ID list

9.1.8 SemanticObject Message

Message Type: map_manager/msg/SemanticObject

Field	Type	Description
id	string	ID
name	string	Name
type	string	Type
category	string
pose	geometry_msgs/Pose	3D
dimensions	geometry_msgs/Vector3	(x, y, z)
is_static	bool	yesno

9.1.9 FloorTransition Message

Message Type: map_manager/msg/FloorTransition

Field	Type	Description
id	string	ID
name	string	Name
transition_type	uint8	Type (0=, 1=, 2=, 3=)
from_floor_id	string	floor ID
from_pose	geometry_msgs/Pose2D
to_floor_id	string	floor ID
to_pose	geometry_msgs/Pose2D
bidirectional	bool	yesno
cost	float64
available	bool	yesno

9.2 Composite Map Services

Note: The following examples use services without a namespace (/xxx). With a namespace, use /<namespace>/xxx. ${MM} represents the optional namespace prefix.

9.2.1 /load_composite_map (Service)

Load a composite map from a directory or .mmap archive.

Service Type: map_manager/srv/LoadCompositeMap

Request:

Field	Type	Description
map_path	string	Map directory or `.mmap` archive path.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
map_info	map_manager/msg/CompositeMapInfo	Loaded map information

Example Call:

Bash# Load a map in directory format
ros2 service call ${MM}/load_composite_map map_manager/srv/LoadCompositeMap \
 "{map_path: '/opt/fftai/Navigation/Map/office'}"

# Load a map from a compressed archive
ros2 service call ${MM}/load_composite_map map_manager/srv/LoadCompositeMap \
 "{map_path: '/opt/fftai/Navigation/Map/office.mmap'}"

Python:

Pythonfrom map_manager.srv import LoadCompositeMap
import rclpy
from rclpy.node import Node

class MapLoader(Node):
 def __init__(self):
 super().__init__('map_loader')
 self.client = self.create_client(LoadCompositeMap, '/load_composite_map')
 self.client.wait_for_service()

 def load(self, path: str):
 request = LoadCompositeMap.Request()
 request.map_path = path
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 if result.success:
 print(f"Loaded map: {result.map_info.name}")
 print(f"Floors: {[f.floor_id for f in result.map_info.floors]}")
 return result

rclpy.init()
loader = MapLoader()
loader.load('/opt/fftai/Navigation/Map/office')

9.2.2 /save_composite_map (Service)

Save a composite map in directory or compressed .mmap format.

Service Type: map_manager/srv/SaveCompositeMap

Request:

Field	Type	Default	Description
map_path	string	-	Destination path.
compress	bool	false	yesno .mmap

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
saved_path	string	Actual output path, including an added `.mmap` extension when applicable.

Example Call:

Bash# Save in directory format
ros2 service call ${MM}/save_composite_map map_manager/srv/SaveCompositeMap \
 "{map_path: '/opt/fftai/Navigation/Map/my_map', compress: false}"

# Save as a compressed archive
ros2 service call ${MM}/save_composite_map map_manager/srv/SaveCompositeMap \
 "{map_path: '/opt/fftai/Navigation/Map/my_map', compress: true}"

# Compress automatically when the path ends in .mmap
ros2 service call ${MM}/save_composite_map map_manager/srv/SaveCompositeMap \
 "{map_path: '/opt/fftai/Navigation/Map/my_map.mmap'}"

9.2.3 /clear_composite_map (Service)

Clear the currently loaded composite map.

Service Type: map_manager/srv/ClearCompositeMap

9.3 Floor Management Services

9.3.1 /list_floors (Service)

List all floors

Service Type: map_manager/srv/ListFloors

Response:

Field	Type	Description
floors	map_manager/msg/Floor[]	List of floors
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/list_floors map_manager/srv/ListFloors

Python:

Pythonfrom map_manager.srv import ListFloors
import rclpy
from rclpy.node import Node

class FloorLister(Node):
 def __init__(self):
 super().__init__('floor_lister')
 self.client = self.create_client(ListFloors, '/list_floors')
 self.client.wait_for_service()

 def list_floors(self):
 request = ListFloors.Request()
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Total floors: {len(result.floors)}")
 for floor in result.floors:
 status = ['UNKNOWN', 'ACTIVE', 'INACTIVE'][floor.status]
 print(f" {floor.floor_id}: {floor.name} (level {floor.level}, {status})")
 print(f" Height: {floor.min_height:.2f} ~ {floor.max_height:.2f} m")
 return result

rclpy.init()
lister = FloorLister()
lister.list_floors()

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/list_floors.hpp>

class FloorLister : public rclcpp::Node {
public:
 FloorLister() : Node("floor_lister") {
 client_ = create_client<map_manager::srv::ListFloors>("/list_floors");
 client_->wait_for_service();
 }

 void list_floors() {
 auto request = std::make_shared<map_manager::srv::ListFloors::Request>();
 auto future = client_->async_send_request(request);

 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();

 if (result->success) {
 RCLCPP_INFO(get_logger(), "Total floors: %zu", result->floors.size());
 for (const auto& floor : result->floors) {
 const char* status[] = {"UNKNOWN", "ACTIVE", "INACTIVE"};
 RCLCPP_INFO(get_logger(), " %s: %s (level %d, %s)",
 floor.floor_id.c_str(), floor.name.c_str(),
 floor.level, status[floor.status]);
 RCLCPP_INFO(get_logger(), " Height: %.2f ~ %.2f m",
 floor.min_height, floor.max_height);
 }
 }
 }
 }

private:
 rclcpp::Client<map_manager::srv::ListFloors>::SharedPtr client_;
};

9.3.2 /get_current_floor (Service)

Get the current floor

Service Type: map_manager/srv/GetCurrentFloor

Response:

Field	Type	Description
floor	map_manager/msg/Floor	Current floor information
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_current_floor map_manager/srv/GetCurrentFloor

Python:

Pythonfrom map_manager.srv import GetCurrentFloor
import rclpy
from rclpy.node import Node

class CurrentFloorGetter(Node):
 def __init__(self):
 super().__init__('current_floor_getter')
 self.client = self.create_client(GetCurrentFloor, '/get_current_floor')
 self.client.wait_for_service()

 def get_current_floor(self):
 request = GetCurrentFloor.Request()
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 floor = result.floor
 print(f"Current floor: {floor.floor_id} ({floor.name})")
 print(f" Level: {floor.level}")
 print(f" Height range: {floor.min_height:.2f} ~ {floor.max_height:.2f} m")
 return result.floor

rclpy.init()
getter = CurrentFloorGetter()
current = getter.get_current_floor()

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/get_current_floor.hpp>

class CurrentFloorGetter : public rclcpp::Node {
public:
 CurrentFloorGetter() : Node("current_floor_getter") {
 client_ = create_client<map_manager::srv::GetCurrentFloor>(
 "/get_current_floor");
 client_->wait_for_service();
 }

 map_manager::msg::Floor get_current_floor() {
 auto request = std::make_shared<map_manager::srv::GetCurrentFloor::Request>();
 auto future = client_->async_send_request(request);

 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Current floor: %s (%s), level %d",
 result->floor.floor_id.c_str(), result->floor.name.c_str(),
 result->floor.level);
 return result->floor;
 }
 }
 return map_manager::msg::Floor();
 }

private:
 rclcpp::Client<map_manager::srv::GetCurrentFloor>::SharedPtr client_;
};

9.3.3 /switch_floor (Service)

switchspecifiedfloor

Service Type: map_manager/srv/SwitchFloor

Request:

Field	Type	Description
floor_id	string	floor ID
initial_pose	geometry_msgs/Pose2D	switchinitial pose
use_transition	bool	yesno
transition_id	string	Floor-transition ID; required when `use_transition` is `true`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
current_floor	map_manager/msg/Floor	Floor information after the switch.

Example Call:

Bash# Switch floors directly
ros2 service call ${MM}/switch_floor map_manager/srv/SwitchFloor \
 "{floor_id: '2F', initial_pose: {x: 0.0, y: 0.0, theta: 0.0}, use_transition: false}"

# Switch through an elevator transition
ros2 service call ${MM}/switch_floor map_manager/srv/SwitchFloor \
 "{floor_id: '2F', use_transition: true, transition_id: 'elevator_1'}"

9.3.4 /add_floor (Service)

addfloor

Service Type: map_manager/srv/AddFloor

Request:

Field	Type	Description
floor	map_manager/msg/Floor	Floor information

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
floor_id	string	addfloor ID

Example Call:

Bashros2 service call ${MM}/add_floor map_manager/srv/AddFloor \
 "{floor: {floor_id: '2F', name: 'Second Floor', level: 2, min_height: 3.0, max_height: 6.0, reference_height: 3.0, status: 1}}"

Python:

Pythonfrom map_manager.srv import AddFloor
from map_manager.msg import Floor
import rclpy
from rclpy.node import Node

class FloorManager(Node):
 def __init__(self):
 super().__init__('floor_manager')
 self.client = self.create_client(AddFloor, '/add_floor')
 self.client.wait_for_service()

 def add_floor(self, floor_id: str, name: str, level: int,
 min_height: float = 0.0, max_height: float = 3.0):
 request = AddFloor.Request()
 request.floor = Floor()
 request.floor.floor_id = floor_id
 request.floor.name = name
 request.floor.level = level
 request.floor.min_height = min_height
 request.floor.max_height = max_height
 request.floor.reference_height = min_height
 request.floor.status = 1 # ACTIVE

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Floor added: {result.floor_id}")
 else:
 print(f"Failed: {result.message}")
 return result.success

rclpy.init()
manager = FloorManager()
manager.add_floor('2F', 'Second Floor', 2, 3.0, 6.0)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_floor.hpp>
#include <map_manager/msg/floor.hpp>

class FloorManager : public rclcpp::Node {
public:
 FloorManager() : Node("floor_manager") {
 client_ = create_client<map_manager::srv::AddFloor>("/add_floor");
 client_->wait_for_service();
 }

 bool add_floor(const std::string& floor_id, const std::string& name, int32_t level,
 double min_height = 0.0, double max_height = 3.0) {
 auto request = std::make_shared<map_manager::srv::AddFloor::Request>();
 request->floor.floor_id = floor_id;
 request->floor.name = name;
 request->floor.level = level;
 request->floor.min_height = min_height;
 request->floor.max_height = max_height;
 request->floor.reference_height = min_height;
 request->floor.status = 1; // ACTIVE

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Floor added: %s", result->floor_id.c_str());
 return true;
 }
 RCLCPP_ERROR(get_logger(), "Failed: %s", result->message.c_str());
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddFloor>::SharedPtr client_;
};

9.3.5 /remove_floor (Service)

Remove a floor

Service Type: map_manager/srv/RemoveFloor

Request:

Field	Type	Description
floor_id	string	floor ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_floor map_manager/srv/RemoveFloor \
 "{floor_id: '2F'}"

Python:

Pythonfrom map_manager.srv import RemoveFloor
import rclpy
from rclpy.node import Node

class FloorRemover(Node):
 def __init__(self):
 super().__init__('floor_remover')
 self.client = self.create_client(RemoveFloor, '/remove_floor')
 self.client.wait_for_service()

 def remove_floor(self, floor_id: str):
 request = RemoveFloor.Request()
 request.floor_id = floor_id
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 return result.success

rclpy.init()
remover = FloorRemover()
remover.remove_floor('2F')

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/remove_floor.hpp>

class FloorRemover : public rclcpp::Node {
public:
 FloorRemover() : Node("floor_remover") {
 client_ = create_client<map_manager::srv::RemoveFloor>("/remove_floor");
 client_->wait_for_service();
 }

 bool remove_floor(const std::string& floor_id) {
 auto request = std::make_shared<map_manager::srv::RemoveFloor::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 return future.get()->success;
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::RemoveFloor>::SharedPtr client_;
};

9.3.6 /save_floor (Service)

Save data for the current floor.

Service Type: map_manager/srv/SaveFloor

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select the current floor.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
saved_path	string	Actual output path.

Example Call:

Bash# Save the current floor
ros2 service call ${MM}/save_floor map_manager/srv/SaveFloor "{floor_id: ''}"

# Save a specified floor
ros2 service call ${MM}/save_floor map_manager/srv/SaveFloor "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import SaveFloor
import rclpy
from rclpy.node import Node

class FloorSaver(Node):
 def __init__(self):
 super().__init__('floor_saver')
 self.client = self.create_client(SaveFloor, '/save_floor')
 self.client.wait_for_service()

 def save_floor(self, floor_id: str = ''):
        """Save a floor; an empty floor_id selects the current floor."""
 request = SaveFloor.Request()
 request.floor_id = floor_id

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future, timeout_sec=120.0)
 result = future.result()

 if result.success:
 print(f"Floor saved to: {result.saved_path}")
 else:
 print(f"Failed: {result.message}")
 return result.success

rclpy.init()
saver = FloorSaver()
saver.save_floor('1F')  # Save floor 1F

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/save_floor.hpp>

class FloorSaver : public rclcpp::Node {
public:
 FloorSaver() : Node("floor_saver") {
 client_ = create_client<map_manager::srv::SaveFloor>("/save_floor");
 client_->wait_for_service();
 }

 bool save_floor(const std::string& floor_id = "") {
 auto request = std::make_shared<map_manager::srv::SaveFloor::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future,
 std::chrono::seconds(120)) == rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Floor saved to: %s",
 result->saved_path.c_str());
 return true;
 }
 RCLCPP_ERROR(get_logger(), "Failed: %s", result->message.c_str());
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::SaveFloor>::SharedPtr client_;
};

9.3.7 /load_floor (Service)

Load all data for a floor, including localization, navigation, semantic, and virtual layers. The floor must already exist.

Service Type: map_manager/srv/LoadFloor

Request:

Field	Type	Description
floor_id	string	Identifier of the floor to load.
input_path	string	Path to the floor data.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
floor_id	string	loadfloor ID

Example Call:

Bashros2 service call ${MM}/load_floor map_manager/srv/LoadFloor \
 "{floor_id: '1F', input_path: '/data/composite_map/floors/1F'}"

Python:

Pythonfrom map_manager.srv import LoadFloor
import rclpy
from rclpy.node import Node

class FloorLoader(Node):
 def __init__(self):
 super().__init__('floor_loader')
 self.client = self.create_client(LoadFloor, '/load_floor')
 self.client.wait_for_service()

 def load_floor(self, floor_id: str, input_path: str):
 request = LoadFloor.Request()
 request.floor_id = floor_id
 request.input_path = input_path
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future, timeout_sec=60.0)
 result = future.result()
 if result.success:
 print(f"Floor loaded: {result.floor_id}")
 else:
 print(f"Failed: {result.message}")
 return result.success

rclpy.init()
loader = FloorLoader()
loader.load_floor('1F', '/data/composite_map/floors/1F')

9.3.8 /clear_floor (Service)

Clear all data associated with a specified floor without deleting the floor entry. Use remove_floor to delete the floor itself.

Service Type: map_manager/srv/ClearFloor

Request:

Field	Type	Description
floor_id	string	floor ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/clear_floor map_manager/srv/ClearFloor \
 "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import ClearFloor
import rclpy
from rclpy.node import Node

class FloorClearer(Node):
 def __init__(self):
 super().__init__('floor_clearer')
 self.client = self.create_client(ClearFloor, '/clear_floor')
 self.client.wait_for_service()

 def clear_floor(self, floor_id: str):
 request = ClearFloor.Request()
 request.floor_id = floor_id
 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 if result.success:
 print(f"Floor {floor_id} cleared")
 else:
 print(f"Failed: {result.message}")
 return result.success

rclpy.init()
clearer = FloorClearer()
clearer.clear_floor('1F')

9.4 POI Management Services

9.4.1 /add_poi (Service)

Add a POI

Service Type: map_manager/srv/AddPOI

Request:

Field	Type	Description
poi	map_manager/msg/POI	POI data

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/add_poi map_manager/srv/AddPOI \
 "{poi: {id: 'charging_1', name: 'Charging Station', type: 'charging_station', floor_id: '1F', pose: {x: 5.0, y: 3.0, theta: 1.57}}}"

Python:

Pythonfrom map_manager.srv import AddPOI
from map_manager.msg import POI
from geometry_msgs.msg import Pose2D
import rclpy
from rclpy.node import Node

class POIManager(Node):
 def __init__(self):
 super().__init__('poi_manager')
 self.client = self.create_client(AddPOI, '/add_poi')
 self.client.wait_for_service()

 def add_poi(self, poi_id: str, name: str, poi_type: str,
 floor_id: str, x: float, y: float, theta: float = 0.0):
 request = AddPOI.Request()
 request.poi = POI()
 request.poi.id = poi_id
 request.poi.name = name
 request.poi.type = poi_type
 request.poi.floor_id = floor_id
 request.poi.pose = Pose2D(x=x, y=y, theta=theta)

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()
 return result.success

# Example
rclpy.init()
manager = POIManager()
success = manager.add_poi('charging_1', 'Charging Station',
 'charging_station', '1F', 5.0, 3.0, 1.57)
print(f"POI added: {success}")

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_poi.hpp>
#include <map_manager/msg/poi.hpp>

class POIManager : public rclcpp::Node {
public:
 POIManager() : Node("poi_manager") {
 client_ = create_client<map_manager::srv::AddPOI>("/add_poi");
 client_->wait_for_service();
 }

 bool add_poi(const std::string& id, const std::string& name,
 const std::string& type, const std::string& floor_id,
 double x, double y, double theta = 0.0) {
 auto request = std::make_shared<map_manager::srv::AddPOI::Request>();
 request->poi.id = id;
 request->poi.name = name;
 request->poi.type = type;
 request->poi.floor_id = floor_id;
 request->poi.pose.x = x;
 request->poi.pose.y = y;
 request->poi.pose.theta = theta;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 RCLCPP_INFO(get_logger(), "%s", result->message.c_str());
 return result->success;
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddPOI>::SharedPtr client_;
};

9.4.2 /list_pois (Service)

List POIs

Service Type: map_manager/srv/ListPOIs

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
pois	map_manager/msg/POI[]	List of POIs
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bash# List POIs on all floors
ros2 service call ${MM}/list_pois map_manager/srv/ListPOIs "{floor_id: ''}"

# List POIs on floor 1F
ros2 service call ${MM}/list_pois map_manager/srv/ListPOIs "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import ListPOIs
import rclpy
from rclpy.node import Node

class POILister(Node):
 def __init__(self):
 super().__init__('poi_lister')
 self.client = self.create_client(ListPOIs, '/list_pois')
 self.client.wait_for_service()

 def list_pois(self, floor_id: str = ''):
        """List POIs; an empty floor_id selects all floors."""
 request = ListPOIs.Request()
 request.floor_id = floor_id

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Found {len(result.pois)} POIs")
 for poi in result.pois:
 print(f" - {poi.id}: {poi.name} ({poi.type}) at "
 f"({poi.pose.x:.2f}, {poi.pose.y:.2f}) on floor {poi.floor_id}")
 return result

# Example
rclpy.init()
lister = POILister()
lister.list_pois('1F')  # List POIs on floor 1F

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/list_pois.hpp>

class POILister : public rclcpp::Node {
public:
 POILister() : Node("poi_lister") {
 client_ = create_client<map_manager::srv::ListPOIs>("/list_pois");
 client_->wait_for_service();
 }

 void list_pois(const std::string& floor_id = "") {
 auto request = std::make_shared<map_manager::srv::ListPOIs::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();

 if (result->success) {
 RCLCPP_INFO(get_logger(), "Found %zu POIs", result->pois.size());
 for (const auto& poi : result->pois) {
 RCLCPP_INFO(get_logger(), " - %s: %s (%s) at (%.2f, %.2f) on floor %s",
 poi.id.c_str(), poi.name.c_str(), poi.type.c_str(),
 poi.pose.x, poi.pose.y, poi.floor_id.c_str());
 }
 }
 }
 }

private:
 rclcpp::Client<map_manager::srv::ListPOIs>::SharedPtr client_;
};

9.4.3 /get_poi (Service)

get POI

Service Type: map_manager/srv/GetPOI

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	string	POI ID

Response:

Field	Type	Description
poi	map_manager/msg/POI	POI data
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_poi map_manager/srv/GetPOI \
 "{floor_id: '1F', id: 'charging_1'}"

9.4.4 /update_poi (Service)

Update a POI

Service Type: map_manager/srv/UpdatePOI

Request:

Field	Type	Description
poi	map_manager/msg/POI	Updated POI data; must include `id` and `floor_id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/update_poi map_manager/srv/UpdatePOI \
 "{poi: {id: 'charging_1', name: 'Main Charger', type: 'charging_station', floor_id: '1F', pose: {x: 5.5, y: 3.0, theta: 1.57}}}"

9.4.5 /remove_poi (Service)

Remove a POI

Service Type: map_manager/srv/RemovePOI

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	string	POI ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_poi map_manager/srv/RemovePOI \
 "{floor_id: '1F', id: 'charging_1'}"

9.4.6 /remove_all_pois (Service)

Remove all POIs

Service Type: map_manager/srv/RemoveAllPOIs

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to remove POIs from all floors.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_all_pois map_manager/srv/RemoveAllPOIs \
 "{floor_id: '1F'}"

9.5 Virtual Wall Management Services

9.5.1 /add_virtual_wall (Service)

Add a virtual wall

Service Type: map_manager/srv/AddVirtualWall

Request:

Field	Type	Description
floor_id	string	floor ID
wall	map_manager/msg/VirtualWall	Virtual wall data

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
wall_id	int32	addvirtual wall ID

Example Call:

Bashros2 service call ${MM}/add_virtual_wall map_manager/srv/AddVirtualWall \
 "{floor_id: '1F', wall: {id: 1, start: {x: 0.0, y: 0.0, z: 0.0}, end: {x: 2.0, y: 0.0, z: 0.0}}}"

Python:

Pythonfrom map_manager.srv import AddVirtualWall
from map_manager.msg import VirtualWall
from geometry_msgs.msg import Point
import rclpy
from rclpy.node import Node

class VirtualWallManager(Node):
 def __init__(self):
 super().__init__('virtual_wall_manager')
 self.client = self.create_client(AddVirtualWall, '/add_virtual_wall')
 self.client.wait_for_service()

 def add_wall(self, floor_id: str, wall_id: int,
 start_x: float, start_y: float,
 end_x: float, end_y: float):
 request = AddVirtualWall.Request()
 request.floor_id = floor_id
 request.wall = VirtualWall()
 request.wall.id = wall_id
 request.wall.floor_id = floor_id
 request.wall.start = Point(x=start_x, y=start_y, z=0.0)
 request.wall.end = Point(x=end_x, y=end_y, z=0.0)

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Virtual wall added with ID: {result.wall_id}")
 else:
 print(f"Failed: {result.message}")
 return result.success

# Example
rclpy.init()
manager = VirtualWallManager()
# Add a virtual wall from (0, 0) to (5, 0)
manager.add_wall('1F', 1, 0.0, 0.0, 5.0, 0.0)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_virtual_wall.hpp>
#include <map_manager/msg/virtual_wall.hpp>

class VirtualWallManager : public rclcpp::Node {
public:
 VirtualWallManager() : Node("virtual_wall_manager") {
 client_ = create_client<map_manager::srv::AddVirtualWall>(
 "/add_virtual_wall");
 client_->wait_for_service();
 }

 bool add_wall(const std::string& floor_id, int32_t wall_id,
 double start_x, double start_y,
 double end_x, double end_y) {
 auto request = std::make_shared<map_manager::srv::AddVirtualWall::Request>();
 request->floor_id = floor_id;
 request->wall.id = wall_id;
 request->wall.floor_id = floor_id;
 request->wall.start.x = start_x;
 request->wall.start.y = start_y;
 request->wall.start.z = 0.0;
 request->wall.end.x = end_x;
 request->wall.end.y = end_y;
 request->wall.end.z = 0.0;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Virtual wall added with ID: %d",
 result->wall_id);
 return true;
 }
 RCLCPP_ERROR(get_logger(), "Failed: %s", result->message.c_str());
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddVirtualWall>::SharedPtr client_;
};

9.5.2 /list_virtual_walls (Service)

List virtual walls

Service Type: map_manager/srv/ListVirtualWalls

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
walls	map_manager/msg/VirtualWall[]	List of virtual walls
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bash# List virtual walls on all floors
ros2 service call ${MM}/list_virtual_walls map_manager/srv/ListVirtualWalls "{floor_id: ''}"

# List virtual walls on floor 1F
ros2 service call ${MM}/list_virtual_walls map_manager/srv/ListVirtualWalls "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import ListVirtualWalls
import rclpy
from rclpy.node import Node

class VirtualWallLister(Node):
 def __init__(self):
 super().__init__('virtual_wall_lister')
 self.client = self.create_client(ListVirtualWalls, '/list_virtual_walls')
 self.client.wait_for_service()

 def list_walls(self, floor_id: str = ''):
 request = ListVirtualWalls.Request()
 request.floor_id = floor_id

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Found {len(result.walls)} virtual walls")
 for wall in result.walls:
 print(f" Wall {wall.id}: ({wall.start.x:.2f}, {wall.start.y:.2f}) -> "
 f"({wall.end.x:.2f}, {wall.end.y:.2f})")
 return result

rclpy.init()
lister = VirtualWallLister()
lister.list_walls('1F')

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/list_virtual_walls.hpp>

class VirtualWallLister : public rclcpp::Node {
public:
 VirtualWallLister() : Node("virtual_wall_lister") {
 client_ = create_client<map_manager::srv::ListVirtualWalls>(
 "/list_virtual_walls");
 client_->wait_for_service();
 }

 void list_walls(const std::string& floor_id = "") {
 auto request = std::make_shared<map_manager::srv::ListVirtualWalls::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();

 if (result->success) {
 RCLCPP_INFO(get_logger(), "Found %zu virtual walls", result->walls.size());
 for (const auto& wall : result->walls) {
 RCLCPP_INFO(get_logger(),
 " Wall %d: (%.2f, %.2f) -> (%.2f, %.2f)",
 wall.id, wall.start.x, wall.start.y, wall.end.x, wall.end.y);
 }
 }
 }
 }

private:
 rclcpp::Client<map_manager::srv::ListVirtualWalls>::SharedPtr client_;
};

9.5.3 /get_virtual_wall (Service)

getvirtual wall

Service Type: map_manager/srv/GetVirtualWall

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	int32	virtual wall ID

Response:

Field	Type	Description
wall	map_manager/msg/VirtualWall	Virtual wall data
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_virtual_wall map_manager/srv/GetVirtualWall \
 "{floor_id: '1F', id: 1}"

9.5.4 /update_virtual_wall (Service)

Update a virtual wall

Service Type: map_manager/srv/UpdateVirtualWall

Request:

Field	Type	Description
wall	map_manager/msg/VirtualWall	Updated virtual-wall data; must include `id` and `floor_id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/update_virtual_wall map_manager/srv/UpdateVirtualWall \
 "{wall: {id: 1, floor_id: '1F', start: {x: 0.0, y: 0.0, z: 0.0}, end: {x: 3.0, y: 0.0, z: 0.0}}}"

9.5.5 /remove_virtual_wall (Service)

Remove a virtual wall.

Service Type: map_manager/srv/RemoveVirtualWall

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
wall_id	int32	virtual wall ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_virtual_wall map_manager/srv/RemoveVirtualWall \
 "{floor_id: '1F', wall_id: 1}"

9.5.6 /remove_virtual_walls (Service)

Remove all virtual walls from a specified floor.

Service Type: map_manager/srv/RemoveVirtualWalls

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to remove entries from all floors.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	Number of virtual walls removed.

Example Call:

Bashros2 service call ${MM}/remove_virtual_walls map_manager/srv/RemoveVirtualWalls \
 "{floor_id: '1F'}"

9.6 Forbidden Area Services

9.6.1 /add_forbidden_area (Service)

Add a forbidden area()

Service Type: map_manager/srv/AddForbiddenArea

Request:

Field	Type	Description
floor_id	string	floor ID
area	map_manager/msg/ForbiddenArea	Forbidden-area data()

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
area_id	int32	forbidden area ID
wall_ids	int32[]	virtual wall ID list

Example Call:

Bash# Add a 2 m x 2 m square forbidden area
ros2 service call ${MM}/add_forbidden_area map_manager/srv/AddForbiddenArea \
 "{floor_id: '1F', area: {id: 1, boundary: {points: [{x: 0.0, y: 0.0, z: 0.0}, {x: 2.0, y: 0.0, z: 0.0}, {x: 2.0, y: 2.0, z: 0.0}, {x: 0.0, y: 2.0, z: 0.0}]}}}"

Python:

Pythonfrom map_manager.srv import AddForbiddenArea
from map_manager.msg import ForbiddenArea
from geometry_msgs.msg import Polygon, Point32
import rclpy
from rclpy.node import Node

class ForbiddenAreaManager(Node):
 def __init__(self):
 super().__init__('forbidden_area_manager')
 self.client = self.create_client(AddForbiddenArea, '/add_forbidden_area')
 self.client.wait_for_service()

 def add_forbidden_area(self, floor_id: str, area_id: int, points: list):
 """
        Add a forbidden area
        points: Polygon vertices, for example [(x1, y1), (x2, y2), (x3, y3), ...]
 """
 request = AddForbiddenArea.Request()
 request.floor_id = floor_id
 request.area = ForbiddenArea()
 request.area.id = area_id
 request.area.boundary = Polygon()
 request.area.boundary.points = [
 Point32(x=float(p[0]), y=float(p[1]), z=0.0) for p in points
 ]

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Forbidden area added with ID: {result.area_id}")
 print(f"Generated virtual walls: {result.wall_ids}")
 else:
 print(f"Failed: {result.message}")
 return result.success

# Example
rclpy.init()
manager = ForbiddenAreaManager()
# Add a square forbidden area
manager.add_forbidden_area('1F', 1, [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)])
# Add a triangular forbidden area
manager.add_forbidden_area('1F', 2, [(5.0, 5.0), (7.0, 5.0), (6.0, 7.0)])

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_forbidden_area.hpp>
#include <map_manager/msg/forbidden_area.hpp>

class ForbiddenAreaManager : public rclcpp::Node {
public:
 ForbiddenAreaManager() : Node("forbidden_area_manager") {
 client_ = create_client<map_manager::srv::AddForbiddenArea>(
 "/add_forbidden_area");
 client_->wait_for_service();
 }

 bool add_forbidden_area(const std::string& floor_id, int32_t area_id,
 const std::vector<std::pair<double, double>>& points) {
 auto request = std::make_shared<map_manager::srv::AddForbiddenArea::Request>();
 request->floor_id = floor_id;
 request->area.id = area_id;

        // Build the polygon boundary
 for (const auto& p : points) {
 geometry_msgs::msg::Point32 point;
 point.x = p.first;
 point.y = p.second;
 point.z = 0.0;
 request->area.boundary.points.push_back(point);
 }

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Forbidden area added with ID: %d",
 result->area_id);
 RCLCPP_INFO(get_logger(), "Generated %zu virtual walls",
 result->wall_ids.size());
 return true;
 }
 RCLCPP_ERROR(get_logger(), "Failed: %s", result->message.c_str());
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddForbiddenArea>::SharedPtr client_;
};

// Example
int main(int argc, char** argv) {
 rclcpp::init(argc, argv);
 auto manager = std::make_shared<ForbiddenAreaManager>();
    // Add a square forbidden area
 manager->add_forbidden_area("1F", 1, {{0.0, 0.0}, {2.0, 0.0}, {2.0, 2.0}, {0.0, 2.0}});
 rclcpp::shutdown();
 return 0;
}

9.6.2 /list_forbidden_areas (Service)

List forbidden areas

Service Type: map_manager/srv/ListForbiddenAreas

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
areas	map_manager/msg/ForbiddenArea[]	List of forbidden areas
floor_ids	string[]	forbidden areafloor ID
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/list_forbidden_areas map_manager/srv/ListForbiddenAreas \
 "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import ListForbiddenAreas
import rclpy
from rclpy.node import Node

class ForbiddenAreaLister(Node):
 def __init__(self):
 super().__init__('forbidden_area_lister')
 self.client = self.create_client(ListForbiddenAreas, '/list_forbidden_areas')
 self.client.wait_for_service()

 def list_areas(self, floor_id: str = ''):
 request = ListForbiddenAreas.Request()
 request.floor_id = floor_id

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Found {len(result.areas)} forbidden areas")
 for i, area in enumerate(result.areas):
 floor = result.floor_ids[i] if i < len(result.floor_ids) else 'unknown'
 print(f" Area {area.id} on floor {floor}:")
 print(f" Vertices: {len(area.boundary.points)}")
 for j, point in enumerate(area.boundary.points):
 print(f" {j}: ({point.x:.2f}, {point.y:.2f})")
 return result

rclpy.init()
lister = ForbiddenAreaLister()
lister.list_areas('1F')

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/list_forbidden_areas.hpp>

class ForbiddenAreaLister : public rclcpp::Node {
public:
 ForbiddenAreaLister() : Node("forbidden_area_lister") {
 client_ = create_client<map_manager::srv::ListForbiddenAreas>(
 "/list_forbidden_areas");
 client_->wait_for_service();
 }

 void list_areas(const std::string& floor_id = "") {
 auto request = std::make_shared<map_manager::srv::ListForbiddenAreas::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();

 if (result->success) {
 RCLCPP_INFO(get_logger(), "Found %zu forbidden areas", result->areas.size());
 for (size_t i = 0; i < result->areas.size(); ++i) {
 const auto& area = result->areas[i];
 const std::string& floor = (i < result->floor_ids.size()) ?
 result->floor_ids[i] : "unknown";
 RCLCPP_INFO(get_logger(), " Area %d on floor %s: %zu vertices",
 area.id, floor.c_str(), area.boundary.points.size());
 }
 }
 }
 }

private:
 rclcpp::Client<map_manager::srv::ListForbiddenAreas>::SharedPtr client_;
};

9.6.3 /get_forbidden_area (Service)

getforbidden area

Service Type: map_manager/srv/GetForbiddenArea

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	int32	forbidden area ID

Response:

Field	Type	Description
area	map_manager/msg/ForbiddenArea	Forbidden-area data
area_floor_id	string	forbidden areafloor ID
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_forbidden_area map_manager/srv/GetForbiddenArea \
 "{floor_id: '1F', id: 1}"

9.6.4 /update_forbidden_area (Service)

Update a forbidden area

Service Type: map_manager/srv/UpdateForbiddenArea

Request:

Field	Type	Description
floor_id	string	Floor identifier (required).
area	map_manager/msg/ForbiddenArea	Updated forbidden-area data; must include `id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
wall_ids	int32[]	virtual wall ID list

Example Call:

Bashros2 service call ${MM}/update_forbidden_area map_manager/srv/UpdateForbiddenArea \
 "{floor_id: '1F', area: {id: 1, boundary: {points: [{x: 0.0, y: 0.0, z: 0.0}, {x: 3.0, y: 0.0, z: 0.0}, {x: 3.0, y: 3.0, z: 0.0}, {x: 0.0, y: 3.0, z: 0.0}]}}}"

9.6.5 /remove_forbidden_area (Service)

Remove a forbidden area

Service Type: map_manager/srv/RemoveForbiddenArea

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
area_id	int32	forbidden area ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
walls_removed	int32	Number of associated virtual walls removed.

Example Call:

Bashros2 service call ${MM}/remove_forbidden_area map_manager/srv/RemoveForbiddenArea \
 "{floor_id: '1F', area_id: 1}"

9.6.6 /remove_forbidden_areas (Service)

Remove forbidden areas in bulk. An empty floor_id selects all floors.

Service Type: map_manager/srv/RemoveForbiddenAreas

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to remove entries from all floors.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	removeforbidden areacount

Example Call:

Bashros2 service call ${MM}/remove_forbidden_areas map_manager/srv/RemoveForbiddenAreas \
 "{floor_id: '1F'}"

9.6.7 /remove_all_forbidden_areas (Service)

Remove every forbidden area on every floor. The request has no fields.

Service Type: map_manager/srv/RemoveAllForbiddenAreas

Request: No fields.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	removeforbidden areacount

Example Call:

Bashros2 service call ${MM}/remove_all_forbidden_areas map_manager/srv/RemoveAllForbiddenAreas "{}"

9.7 Speed-Restricted Area Services

9.7.1 /add_dangerous_area (Service)

Add a speed-restricted area

Service Type: map_manager/srv/AddDangerousArea

Request:

Field	Type	Description
floor_id	string	floor ID
area	map_manager/msg/DangerousArea	Speed-restricted-area data, including `speed_limit`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
area_id	int32	speed-restricted area ID

Example Call:

Bash# Add a 3 m x 3 m speed-restricted area with a 0.3 m/s limit
ros2 service call ${MM}/add_dangerous_area map_manager/srv/AddDangerousArea \
 "{floor_id: '1F', area: {id: 1, boundary: {points: [{x: 0.0, y: 0.0, z: 0.0}, {x: 3.0, y: 0.0, z: 0.0}, {x: 3.0, y: 3.0, z: 0.0}, {x: 0.0, y: 3.0, z: 0.0}]}, speed_limit: 0.3}}"

Python:

Pythonfrom map_manager.srv import AddDangerousArea
from map_manager.msg import DangerousArea
from geometry_msgs.msg import Polygon, Point32
import rclpy
from rclpy.node import Node

class DangerousAreaManager(Node):
 def __init__(self):
 super().__init__('dangerous_area_manager')
 self.client = self.create_client(AddDangerousArea, '/add_dangerous_area')
 self.client.wait_for_service()

 def add_dangerous_area(self, floor_id: str, area_id: int,
 points: list, speed_limit: float):
 """
        Add a speed-restricted area
        points: Polygon vertices, for example [(x1, y1), (x2, y2), ...]
        speed_limit: Speed limit in meters per second; 0.3 means 0.3 m/s
 """
 request = AddDangerousArea.Request()
 request.floor_id = floor_id
 request.area = DangerousArea()
 request.area.id = area_id
 request.area.boundary = Polygon()
 request.area.boundary.points = [
 Point32(x=float(p[0]), y=float(p[1]), z=0.0) for p in points
 ]
 request.area.speed_limit = speed_limit

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Dangerous area added with ID: {result.area_id}")
 print(f"Speed limit: {speed_limit} m/s")
 else:
 print(f"Failed: {result.message}")
 return result.success

# Example
rclpy.init()
manager = DangerousAreaManager()
# Add a speed-restricted area near an elevator with a 0.2 m/s limit
manager.add_dangerous_area('1F', 1,
 [(5.0, 5.0), (8.0, 5.0), (8.0, 8.0), (5.0, 8.0)], 0.2)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_dangerous_area.hpp>
#include <map_manager/msg/dangerous_area.hpp>

class DangerousAreaManager : public rclcpp::Node {
public:
 DangerousAreaManager() : Node("dangerous_area_manager") {
 client_ = create_client<map_manager::srv::AddDangerousArea>(
 "/add_dangerous_area");
 client_->wait_for_service();
 }

 bool add_dangerous_area(const std::string& floor_id, int32_t area_id,
 const std::vector<std::pair<double, double>>& points,
 float speed_limit) {
 auto request = std::make_shared<map_manager::srv::AddDangerousArea::Request>();
 request->floor_id = floor_id;
 request->area.id = area_id;
 request->area.speed_limit = speed_limit;

        // Build the polygon boundary
 for (const auto& p : points) {
 geometry_msgs::msg::Point32 point;
 point.x = p.first;
 point.y = p.second;
 point.z = 0.0;
 request->area.boundary.points.push_back(point);
 }

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Dangerous area added with ID: %d, "
 "speed limit: %.2f m/s", result->area_id, speed_limit);
 return true;
 }
 RCLCPP_ERROR(get_logger(), "Failed: %s", result->message.c_str());
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddDangerousArea>::SharedPtr client_;
};

// Example
int main(int argc, char** argv) {
 rclcpp::init(argc, argv);
 auto manager = std::make_shared<DangerousAreaManager>();
    // Add a speed-restricted area near the elevator
 manager->add_dangerous_area("1F", 1,
 {{5.0, 5.0}, {8.0, 5.0}, {8.0, 8.0}, {5.0, 8.0}}, 0.2f);
 rclcpp::shutdown();
 return 0;
}

9.7.2 /list_dangerous_areas (Service)

List speed-restricted areas

Service Type: map_manager/srv/ListDangerousAreas

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
areas	map_manager/msg/DangerousArea[]	List of speed-restricted areas
floor_ids	string[]	speed-restricted areafloor ID
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/list_dangerous_areas map_manager/srv/ListDangerousAreas \
 "{floor_id: '1F'}"

Python:

Pythonfrom map_manager.srv import ListDangerousAreas
import rclpy
from rclpy.node import Node

class DangerousAreaLister(Node):
 def __init__(self):
 super().__init__('dangerous_area_lister')
 self.client = self.create_client(ListDangerousAreas, '/list_dangerous_areas')
 self.client.wait_for_service()

 def list_areas(self, floor_id: str = ''):
 request = ListDangerousAreas.Request()
 request.floor_id = floor_id

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Found {len(result.areas)} dangerous areas")
 for i, area in enumerate(result.areas):
 floor = result.floor_ids[i] if i < len(result.floor_ids) else 'unknown'
 print(f" Area {area.id} on floor {floor}:")
 print(f" Speed limit: {area.speed_limit} m/s")
 print(f" Vertices: {len(area.boundary.points)}")
 return result

rclpy.init()
lister = DangerousAreaLister()
lister.list_areas('1F')

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/list_dangerous_areas.hpp>

class DangerousAreaLister : public rclcpp::Node {
public:
 DangerousAreaLister() : Node("dangerous_area_lister") {
 client_ = create_client<map_manager::srv::ListDangerousAreas>(
 "/list_dangerous_areas");
 client_->wait_for_service();
 }

 void list_areas(const std::string& floor_id = "") {
 auto request = std::make_shared<map_manager::srv::ListDangerousAreas::Request>();
 request->floor_id = floor_id;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();

 if (result->success) {
 RCLCPP_INFO(get_logger(), "Found %zu dangerous areas", result->areas.size());
 for (size_t i = 0; i < result->areas.size(); ++i) {
 const auto& area = result->areas[i];
 const std::string& floor = (i < result->floor_ids.size()) ?
 result->floor_ids[i] : "unknown";
 RCLCPP_INFO(get_logger(),
 " Area %d on floor %s: speed_limit=%.2f m/s, %zu vertices",
 area.id, floor.c_str(), area.speed_limit,
 area.boundary.points.size());
 }
 }
 }
 }

private:
 rclcpp::Client<map_manager::srv::ListDangerousAreas>::SharedPtr client_;
};

9.7.3 /get_dangerous_area (Service)

getspeed-restricted area

Service Type: map_manager/srv/GetDangerousArea

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	int32	speed-restricted area ID

Response:

Field	Type	Description
area	map_manager/msg/DangerousArea	Speed-restricted-area data
area_floor_id	string	speed-restricted areafloor ID
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_dangerous_area map_manager/srv/GetDangerousArea \
 "{floor_id: '1F', id: 1}"

9.7.4 /update_dangerous_area (Service)

Update a speed-restricted area

Service Type: map_manager/srv/UpdateDangerousArea

Request:

Field	Type	Description
floor_id	string	Floor identifier (required).
area	map_manager/msg/DangerousArea	Updated speed-restricted-area data; must include `id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/update_dangerous_area map_manager/srv/UpdateDangerousArea \
 "{floor_id: '1F', area: {id: 1, boundary: {points: [{x: 0.0, y: 0.0, z: 0.0}, {x: 4.0, y: 0.0, z: 0.0}, {x: 4.0, y: 4.0, z: 0.0}, {x: 0.0, y: 4.0, z: 0.0}]}, speed_limit: 0.5}}"

9.7.5 /remove_dangerous_area (Service)

Remove a speed-restricted area

Service Type: map_manager/srv/RemoveDangerousArea

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
area_id	int32	speed-restricted area ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_dangerous_area map_manager/srv/RemoveDangerousArea \
 "{floor_id: '1F', area_id: 1}"

9.7.6 /remove_all_dangerous_areas (Service)

Remove all speed-restricted areas

Service Type: map_manager/srv/RemoveAllDangerousAreas

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	removespeed-restricted areacount

Example Call:

Bashros2 service call ${MM}/remove_all_dangerous_areas map_manager/srv/RemoveAllDangerousAreas \
 "{floor_id: '1F'}"

9.8 Room Services

9.8.1 /add_room (Service)

Add a room

Service Type: map_manager/srv/AddRoom

Request:

Field	Type	Description
floor_id	string	floor ID
room	map_manager/msg/Room	Room data; must include `room.id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
room_id	string	addroom ID

Example Call:

Bashros2 service call ${MM}/add_room map_manager/srv/AddRoom \
 "{floor_id: '1F', room: {id: 'room_101', name: 'Office 101', type: 'office', boundary: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 0.0, z: 0.0}, {x: 5.0, y: 4.0, z: 0.0}, {x: 0.0, y: 4.0, z: 0.0}], floor_height: 0.0, ceiling_height: 2.8}}"

Python:

Pythonfrom map_manager.srv import AddRoom
from map_manager.msg import Room
from geometry_msgs.msg import Point
import rclpy
from rclpy.node import Node

class RoomManager(Node):
 def __init__(self):
 super().__init__('room_manager')
 self.client = self.create_client(AddRoom, '/add_room')
 self.client.wait_for_service()

 def add_room(self, floor_id: str, room_id: str, name: str,
 room_type: str, boundary_points: list,
 floor_height: float = 0.0, ceiling_height: float = 2.8):
 """
        Add a room
        boundary_points: Room boundary points, for example [(x1, y1), (x2, y2), ...]
 """
 request = AddRoom.Request()
 request.floor_id = floor_id
 request.room = Room()
 request.room.id = room_id
 request.room.name = name
 request.room.type = room_type
 request.room.boundary = [
 Point(x=float(p[0]), y=float(p[1]), z=0.0) for p in boundary_points
 ]
 request.room.floor_height = floor_height
 request.room.ceiling_height = ceiling_height

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Room added: {result.room_id}")
 return result.success

rclpy.init()
manager = RoomManager()
# Add a 5 m x 4 m office
manager.add_room('1F', 'room_101', 'Office 101', 'office',
 [(0.0, 0.0), (5.0, 0.0), (5.0, 4.0), (0.0, 4.0)])

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_room.hpp>
#include <map_manager/msg/room.hpp>

class RoomManager : public rclcpp::Node {
public:
 RoomManager() : Node("room_manager") {
 client_ = create_client<map_manager::srv::AddRoom>("/add_room");
 client_->wait_for_service();
 }

 bool add_room(const std::string& floor_id, const std::string& room_id,
 const std::string& name, const std::string& type,
 const std::vector<std::pair<double, double>>& boundary_points,
 double floor_height = 0.0, double ceiling_height = 2.8) {
 auto request = std::make_shared<map_manager::srv::AddRoom::Request>();
 request->floor_id = floor_id;
 request->room.id = room_id;
 request->room.name = name;
 request->room.type = type;
 request->room.floor_height = floor_height;
 request->room.ceiling_height = ceiling_height;

 for (const auto& p : boundary_points) {
 geometry_msgs::msg::Point point;
 point.x = p.first;
 point.y = p.second;
 point.z = 0.0;
 request->room.boundary.push_back(point);
 }

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Room added: %s", result->room_id.c_str());
 return true;
 }
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddRoom>::SharedPtr client_;
};

9.8.2 /list_rooms (Service)

List rooms

Service Type: map_manager/srv/ListRooms

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
rooms	map_manager/msg/Room[]	List of rooms
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/list_rooms map_manager/srv/ListRooms \
 "{floor_id: '1F'}"

9.8.3 /get_room (Service)

Get a room

Service Type: map_manager/srv/GetRoom

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	string	room ID

Response:

Field	Type	Description
room	map_manager/msg/Room	Room data
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_room map_manager/srv/GetRoom \
 "{floor_id: '1F', id: 'room_101'}"

9.8.4 /update_room (Service)

Update a room

Service Type: map_manager/srv/UpdateRoom

Request:

Field	Type	Description
floor_id	string	floor ID
room	map_manager/msg/Room	Updated room data; must include `id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/update_room map_manager/srv/UpdateRoom \
 "{floor_id: '1F', room: {id: 'room_101', name: 'Meeting Room 101', type: 'meeting_room', boundary: [{x: 0.0, y: 0.0, z: 0.0}, {x: 6.0, y: 0.0, z: 0.0}, {x: 6.0, y: 5.0, z: 0.0}, {x: 0.0, y: 5.0, z: 0.0}]}}"

9.8.5 /remove_room (Service)

Remove a room

Service Type: map_manager/srv/RemoveRoom

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
room_id	string	room ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
objects_removed	int32	roomremovecount

Example Call:

Bashros2 service call ${MM}/remove_room map_manager/srv/RemoveRoom \
 "{floor_id: '1F', room_id: 'room_101'}"

9.8.6 /remove_all_rooms (Service)

Remove all rooms

Service Type: map_manager/srv/RemoveAllRooms

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	removeroomcount

Example Call:

Bashros2 service call ${MM}/remove_all_rooms map_manager/srv/RemoveAllRooms \
 "{floor_id: '1F'}"

9.9 Semantic Object Services

9.9.1 /add_semantic_object (Service)

Add a semantic object

Service Type: map_manager/srv/AddSemanticObject

Request:

Field	Type	Description
floor_id	string	floor ID
object	map_manager/msg/SemanticObject	Semantic-object data; must include `object.id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
object_id	string	add ID

Example Call:

Bashros2 service call ${MM}/add_semantic_object map_manager/srv/AddSemanticObject \
 "{floor_id: '1F', object: {id: 'desk_001', name: 'Office Desk', type: 'desk', category: 'furniture', pose: {position: {x: 2.0, y: 3.0, z: 0.0}, orientation: {w: 1.0}}, dimensions: {x: 1.4, y: 0.7, z: 0.75}, is_static: true}}"

Python:

Pythonfrom map_manager.srv import AddSemanticObject
from map_manager.msg import SemanticObject
from geometry_msgs.msg import Pose, Vector3
import rclpy
from rclpy.node import Node

class SemanticObjectManager(Node):
 def __init__(self):
 super().__init__('semantic_object_manager')
 self.client = self.create_client(AddSemanticObject, '/add_semantic_object')
 self.client.wait_for_service()

 def add_object(self, floor_id: str, obj_id: str, name: str,
 obj_type: str, category: str, x: float, y: float, z: float,
 length: float, width: float, height: float, is_static: bool = True):
 request = AddSemanticObject.Request()
 request.floor_id = floor_id
 request.object = SemanticObject()
 request.object.id = obj_id
 request.object.name = name
 request.object.type = obj_type
 request.object.category = category
 request.object.pose = Pose()
 request.object.pose.position.x = x
 request.object.pose.position.y = y
 request.object.pose.position.z = z
 request.object.pose.orientation.w = 1.0
 request.object.dimensions = Vector3(x=length, y=width, z=height)
 request.object.is_static = is_static

 future = self.client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 result = future.result()

 if result.success:
 print(f"Semantic object added: {result.object_id}")
 return result.success

rclpy.init()
manager = SemanticObjectManager()
# Add a 1.4 m x 0.7 m x 0.75 m desk
manager.add_object('1F', 'desk_001', 'Office Desk', 'desk', 'furniture',
 2.0, 3.0, 0.0, 1.4, 0.7, 0.75, True)

C++:

C++#include <rclcpp/rclcpp.hpp>
#include <map_manager/srv/add_semantic_object.hpp>
#include <map_manager/msg/semantic_object.hpp>

class SemanticObjectManager : public rclcpp::Node {
public:
 SemanticObjectManager() : Node("semantic_object_manager") {
 client_ = create_client<map_manager::srv::AddSemanticObject>(
 "/add_semantic_object");
 client_->wait_for_service();
 }

 bool add_object(const std::string& floor_id, const std::string& obj_id,
 const std::string& name, const std::string& type,
 const std::string& category, double x, double y, double z,
 double length, double width, double height, bool is_static = true) {
 auto request = std::make_shared<map_manager::srv::AddSemanticObject::Request>();
 request->floor_id = floor_id;
 request->object.id = obj_id;
 request->object.name = name;
 request->object.type = type;
 request->object.category = category;
 request->object.pose.position.x = x;
 request->object.pose.position.y = y;
 request->object.pose.position.z = z;
 request->object.pose.orientation.w = 1.0;
 request->object.dimensions.x = length;
 request->object.dimensions.y = width;
 request->object.dimensions.z = height;
 request->object.is_static = is_static;

 auto future = client_->async_send_request(request);
 if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
 rclcpp::FutureReturnCode::SUCCESS) {
 auto result = future.get();
 if (result->success) {
 RCLCPP_INFO(get_logger(), "Semantic object added: %s",
 result->object_id.c_str());
 return true;
 }
 }
 return false;
 }

private:
 rclcpp::Client<map_manager::srv::AddSemanticObject>::SharedPtr client_;
};

9.9.2 /list_semantic_objects (Service)

List semantic objects

Service Type: map_manager/srv/ListSemanticObjects

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
object_type	string	Object-type filter; leave empty to select all types.

Response:

Field	Type	Description
objects	map_manager/msg/SemanticObject[]	List of semantic objects
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/list_semantic_objects map_manager/srv/ListSemanticObjects \
 "{floor_id: '1F', object_type: 'desk'}"

9.9.3 /get_semantic_object (Service)

Get a semantic object

Service Type: map_manager/srv/GetSemanticObject

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
id	string	ID

Response:

Field	Type	Description
object	map_manager/msg/SemanticObject	Semantic object data
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/get_semantic_object map_manager/srv/GetSemanticObject \
 "{floor_id: '1F', id: 'desk_001'}"

9.9.4 /update_semantic_object (Service)

Update a semantic object

Service Type: map_manager/srv/UpdateSemanticObject

Request:

Field	Type	Description
floor_id	string	floor ID
object	map_manager/msg/SemanticObject	Updated semantic-object data; must include `id`.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/update_semantic_object map_manager/srv/UpdateSemanticObject \
 "{floor_id: '1F', object: {id: 'desk_001', name: 'Executive Desk', type: 'desk', category: 'furniture', pose: {position: {x: 3.0, y: 4.0, z: 0.0}, orientation: {w: 1.0}}, dimensions: {x: 1.6, y: 0.8, z: 0.75}, is_static: true}}"

9.9.5 /remove_semantic_object (Service)

Remove a semantic object

Service Type: map_manager/srv/RemoveSemanticObject

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
object_id	string	ID

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message

Example Call:

Bashros2 service call ${MM}/remove_semantic_object map_manager/srv/RemoveSemanticObject \
 "{floor_id: '1F', object_id: 'desk_001'}"

9.9.6 /remove_all_semantic_objects (Service)

Remove all semantic objects

Service Type: map_manager/srv/RemoveAllSemanticObjects

Request:

Field	Type	Description
floor_id	string	Floor identifier; leave empty to select all floors.
object_type	string	Object-type filter; leave empty to select all types.

Response:

Field	Type	Description
success	bool	Whether the operation succeeded
message	string	result message
count	int32	removecount

Example Call:

Bashros2 service call ${MM}/remove_all_semantic_objects map_manager/srv/RemoveAllSemanticObjects \
 "{floor_id: '1F', object_type: ''}"

9.10 Python Complete Example

Pythonfrom map_manager.srv import (
 LoadCompositeMap, SaveCompositeMap, ListFloors, SwitchFloor,
 AddPOI, ListPOIs, AddVirtualWall, AddForbiddenArea, AddDangerousArea
)
from map_manager.msg import POI, VirtualWall, ForbiddenArea, DangerousArea
from geometry_msgs.msg import Point, Pose2D, Polygon, Point32
import rclpy
from rclpy.node import Node

class MapManagerClient(Node):
 def __init__(self, namespace: str = ''):
 super().__init__('map_manager_client')

        # Build the service prefix from the namespace
        # namespace='' -> '' (service path: /load_composite_map)
        # namespace='robot1' -> '/robot1' (service path: /robot1/load_composite_map)
 prefix = f'/{namespace}' if namespace else ''

        # Create service clients
 self.load_client = self.create_client(LoadCompositeMap, f'{prefix}/load_composite_map')
 self.save_client = self.create_client(SaveCompositeMap, f'{prefix}/save_composite_map')
 self.list_floors_client = self.create_client(ListFloors, f'{prefix}/list_floors')
 self.switch_floor_client = self.create_client(SwitchFloor, f'{prefix}/switch_floor')
 self.add_poi_client = self.create_client(AddPOI, f'{prefix}/add_poi')
 self.list_pois_client = self.create_client(ListPOIs, f'{prefix}/list_pois')
 self.add_wall_client = self.create_client(AddVirtualWall, f'{prefix}/add_virtual_wall')
 self.add_forbidden_client = self.create_client(AddForbiddenArea, f'{prefix}/add_forbidden_area')
 self.add_dangerous_client = self.create_client(AddDangerousArea, f'{prefix}/add_dangerous_area')

 def load_map(self, path: str):
        """Load a composite map."""
 request = LoadCompositeMap.Request()
 request.map_path = path
 future = self.load_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def save_map(self, path: str, compress: bool = False):
        """Save a composite map."""
 request = SaveCompositeMap.Request()
 request.map_path = path
 request.compress = compress
 future = self.save_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def list_floors(self):
        """List all floors."""
 request = ListFloors.Request()
 future = self.list_floors_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def switch_floor(self, floor_id: str, x: float = 0.0, y: float = 0.0, theta: float = 0.0):
        """Switch floors."""
 request = SwitchFloor.Request()
 request.floor_id = floor_id
 request.initial_pose = Pose2D(x=x, y=y, theta=theta)
 request.use_transition = False
 future = self.switch_floor_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def add_poi(self, poi_id: str, name: str, poi_type: str, floor_id: str, x: float, y: float, theta: float):
        """Add a POI."""
 request = AddPOI.Request()
 request.poi = POI(
 id=poi_id, name=name, type=poi_type, floor_id=floor_id,
 pose=Pose2D(x=x, y=y, theta=theta)
 )
 future = self.add_poi_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def add_virtual_wall(self, floor_id: str, wall_id: int, start: tuple, end: tuple):
        """Add a virtual wall."""
 request = AddVirtualWall.Request()
 request.floor_id = floor_id
 request.wall = VirtualWall(
 id=wall_id,
 start=Point(x=start[0], y=start[1], z=0.0),
 end=Point(x=end[0], y=end[1], z=0.0)
 )
 future = self.add_wall_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def add_forbidden_area(self, floor_id: str, area_id: int, points: list):
        """Add a forbidden area."""
 request = AddForbiddenArea.Request()
 request.floor_id = floor_id
 polygon = Polygon()
 polygon.points = [Point32(x=p[0], y=p[1], z=0.0) for p in points]
 request.area = ForbiddenArea(id=area_id, boundary=polygon)
 future = self.add_forbidden_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()

 def add_dangerous_area(self, floor_id: str, area_id: int, points: list, speed_limit: float):
        """Add a speed-restricted area."""
 request = AddDangerousArea.Request()
 request.floor_id = floor_id
 polygon = Polygon()
 polygon.points = [Point32(x=p[0], y=p[1], z=0.0) for p in points]
 request.area = DangerousArea(id=area_id, boundary=polygon, speed_limit=speed_limit)
 future = self.add_dangerous_client.call_async(request)
 rclpy.spin_until_future_complete(self, future)
 return future.result()


# Example
def main():
 rclpy.init()
    # Single-robot mode (default)
 client = MapManagerClient()
    # Multi-robot mode with a namespace: client = MapManagerClient(namespace='robot1')

    # Load the map
 result = client.load_map('/opt/fftai/Navigation/Map/office')
 if result.success:
 print(f"Map loaded successfully: {result.map_info.name}")
 print(f"Floor count: {len(result.map_info.floors)}")

    # List floors
 floors = client.list_floors()
 for floor in floors.floors:
 print(f" floor: {floor.floor_id} ({floor.name})")

    # Switch floors
 client.switch_floor('2F', x=1.0, y=2.0, theta=0.0)

    # Add a POI
    client.add_poi('charging_1', 'Charging Station', 'charging_station', '1F', 5.0, 3.0, 1.57)

    # Add a virtual wall
 client.add_virtual_wall('1F', 1, (0.0, 0.0), (2.0, 0.0))

    # Add a forbidden area
 client.add_forbidden_area('1F', 1, [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)])

    # Add a speed-restricted area
 client.add_dangerous_area('1F', 1, [(5.0, 5.0), (8.0, 5.0), (8.0, 8.0), (5.0, 8.0)], 0.3)

    # Save the map
 client.save_map('/opt/fftai/Navigation/Map/office_updated', compress=True)

 rclpy.shutdown()

if __name__ == '__main__':
 main()

10. API Summary

Topic List

Topic	Message Type	Option	Description
/slam/mode_status	std_msgs/String	publish	currentstatus
/clear_map	std_msgs/String	subscribe	Clear maptrigger
/map	nav_msgs/OccupancyGrid	publish	2Dmap
/optimize_map	nav_msgs/OccupancyGrid	publish	2Dmap
/cloud_registered_gravity	sensor_msgs/PointCloud2	publish	3Dmap
/initialpose	geometry_msgs/PoseWithCovarianceStamped	subscribe	initial pose
/robot_pose	geometry_msgs/PoseStamped	publish	robot3D(6DOF)
/odom	nav_msgs/Odometry	publish	data
/odom_status_code	std_msgs/Int8	publish	status
/odom_status_score	std_msgs/Float32	publish	position
/plan	nav_msgs/Path	publish	global plan
/cmd_vel	geometry_msgs/Twist	publish
/action_status	fourier_msgs/ActionStatus	publish	Action execution status
/scan	sensor_msgs/LaserScan	publish	2D
/segmented_groundless_points	sensor_msgs/PointCloud2	publish
/imu	sensor_msgs/Imu	subscribe	IMUdata
/camera_01/color/image_raw	sensor_msgs/Image	publish
/camera_01/color/camera_info	sensor_msgs/CameraInfo	publish	information
/camera_01/depth/image_raw	sensor_msgs/Image	publish
/camera_01/depth/camera_info	sensor_msgs/CameraInfo	publish	information
/camera_01/depth/points	sensor_msgs/PointCloud2	publish
/camera_01/fused_data	fourier_msgs/UnifiedCameraData	publish	data
/camera_01/filtered_pointcloud	sensor_msgs/PointCloud2	publish
/Humanoid_nav/health	fourier_msgs/HealthInfo	publish	system health status
/Humanoid_nav/events	fourier_msgs/EventsInfo	publish	system event
/reloc_status	std_msgs/Bool	publish	Relocalizationstatus

Service List

Mapping and Localization Services

Service Name	Service Type	Description
/slam/set_mode	fourier_msgs/SetMode	switch/Localization mode
/slam/load_map	fourier_msgs/LoadMap	Load map
/slam/save_map	fourier_msgs/SaveMap	Save map
/slam/global_relocalization	std_srvs/Empty	Trigger global relocalization
/slam/trigger_local_relocalization	fourier_msgs/VPRLocalRelocalization	Local relocalization

Service Name	Service Type	Description
/cancel_current_action	fourier_msgs/CancelCurrentAction	Cancel the current action
/get_current_action	fourier_msgs/GetCurrentAction	Get the current action

Map Manager - Composite Map Services

Service Name	Service Type	Description
/load_composite_map	map_manager/LoadCompositeMap	Load a composite map
/save_composite_map	map_manager/SaveCompositeMap	Save a composite map
/clear_composite_map	map_manager/ClearCompositeMap	Clear the composite map

Map Manager - Floor Management Services

Service Name	Service Type	Description
/list_floors	map_manager/ListFloors	List all floors
/switch_floor	map_manager/SwitchFloor	Switch floors
/add_floor	map_manager/AddFloor	Add a floor
/remove_floor	map_manager/RemoveFloor	Remove a floor
/save_floor	map_manager/SaveFloor	Save floor data
/load_floor	map_manager/LoadFloor	Load floor data
/clear_floor	map_manager/ClearFloor	Clear floor data

Map Manager - POI Management Services

Service Name	Service Type	Description
/add_poi	map_manager/AddPOI	Add a POI
/get_poi	map_manager/GetPOI	Get a POI
/list_pois	map_manager/ListPOIs	List POIs
/update_poi	map_manager/UpdatePOI	Update a POI
/remove_poi	map_manager/RemovePOI	Remove a POI
/remove_all_pois	map_manager/RemoveAllPOIs	Remove all POIs

Map Manager - Virtual Wall Services

Service Name	Service Type	Description
/add_virtual_wall	map_manager/AddVirtualWall	Add a virtual wall
/get_virtual_wall	map_manager/GetVirtualWall	Get a virtual wall
/list_virtual_walls	map_manager/ListVirtualWalls	List virtual walls
/update_virtual_wall	map_manager/UpdateVirtualWall	Update a virtual wall
/remove_virtual_wall	map_manager/RemoveVirtualWall	Remove a virtual wall
/remove_virtual_walls	map_manager/RemoveVirtualWalls	Remove virtual walls in bulk

Map Manager - Forbidden Area Services

Service Name	Service Type	Description
/add_forbidden_area	map_manager/AddForbiddenArea	Add a forbidden area
/get_forbidden_area	map_manager/GetForbiddenArea	Get a forbidden area
/list_forbidden_areas	map_manager/ListForbiddenAreas	List forbidden areas
/update_forbidden_area	map_manager/UpdateForbiddenArea	Update a forbidden area
/remove_forbidden_area	map_manager/RemoveForbiddenArea	Remove one forbidden area
/remove_all_forbidden_areas	map_manager/RemoveAllForbiddenAreas	Remove all forbidden areas

Map Manager - Speed-Restricted Area Services

Service Name	Service Type	Description
/add_dangerous_area	map_manager/AddDangerousArea	Add a speed-restricted area
/get_dangerous_area	map_manager/GetDangerousArea	Get a speed-restricted area
/list_dangerous_areas	map_manager/ListDangerousAreas	List speed-restricted areas
/update_dangerous_area	map_manager/UpdateDangerousArea	Update a speed-restricted area
/remove_dangerous_area	map_manager/RemoveDangerousArea	Remove a speed-restricted area
/remove_all_dangerous_areas	map_manager/RemoveAllDangerousAreas	Remove all speed-restricted areas

Map Manager - Room Services

Service Name	Service Type	Description
/add_room	map_manager/AddRoom	Add a room
/get_room	map_manager/GetRoom	Get a room
/list_rooms	map_manager/ListRooms	List rooms
/update_room	map_manager/UpdateRoom	Update a room
/remove_room	map_manager/RemoveRoom	Remove a room
/remove_all_rooms	map_manager/RemoveAllRooms	Remove all rooms

Map Manager - Semantic Object Services

Service Name	Service Type	Description
/add_semantic_object	map_manager/AddSemanticObject	Add a semantic object
/get_semantic_object	map_manager/GetSemanticObject	Get a semantic object
/list_semantic_objects	map_manager/ListSemanticObjects	List semantic objects
/update_semantic_object	map_manager/UpdateSemanticObject	Update a semantic object
/remove_semantic_object	map_manager/RemoveSemanticObject	Remove a semantic object
/remove_all_semantic_objects	map_manager/RemoveAllSemanticObjects	Remove all semantic objects

Action List

Action	Action Type	Description
/navigate_to_pose	nav2_msgs/NavigateToPose	Navigate to one target pose
/navigate_through_poses	nav2_msgs/NavigateThroughPoses	Navigate through multiple poses
/follow_path	nav2_msgs/FollowPath	Follow a supplied path
/compute_path_to_pose	nav2_msgs/ComputePathToPose	Compute a path to a target pose

Message Type List

Map Manager Message

Message Type	Description
map_manager/msg/Floor	Floor information
map_manager/msg/CompositeMapInfo	Composite map information
map_manager/msg/FloorTransition	Floor transition point
map_manager/msg/POI	poi
map_manager/msg/VirtualWall	virtual wall
map_manager/msg/ForbiddenArea	forbidden area
map_manager/msg/DangerousArea	speed-restricted area
map_manager/msg/Room	room
map_manager/msg/SemanticObject	semantic object
map_manager/msg/MapLayer	map

Important Notes​

1. Switching Between Mapping and Localization​

1.1 /slam/set_mode (Service)​

1.2 /slam/mode_status (Topic)​

2. Mapping Mode APIs​

2.1 /clear_map (Topic)​

2.2 /map (Topic)​

2.3 /cloud_registered_gravity (Topic)​

2.4 /optimize_map (Topic)​

2.5 /slam/save_map (Service)​

2.6 Saving a 2D Map​

3. Localization Mode APIs​

3.1 /initialpose (Topic)​

3.2 /slam/load_map (Service)​

3.3 /robot_pose (Topic)​

3.4 /odom (Topic)​

3.5 /odom_status_code (Topic)​

3.6 /odom_status_score (Topic)​

3.7 Relocalization​

3.7.1 /slam/global_relocalization (Service)​

3.7.2 /slam/trigger_local_relocalization (Service)​

3.7.6 /reloc_status (Topic)​

4. Navigation APIs​

4.1 navigate_to_pose (Action)​

Goal Request​

Result​

Live Feedback​

Example Call​

4.2 /plan (Topic)​

4.3 /cmd_vel (Topic)​

4.4 cancel_current_action (Service)​

4.5 get_current_action (Service)​

4.6 action_status (Topic)​

5. Sensor Data APIs​

5.1 /scan (Topic)​

5.2 /segmented_groundless_points (Topic)​

5.3 /rslidar_points (Topic)​

5.4 /imu (Topic)​

6. Camera APIs​

6.1 Camera Configuration​

6.2 Camera Driver APIs (CAMERA_SDK_ROS2)​

6.2.1 /camera_01/color/image_raw (Topic)​

6.2.2 /camera_01/color/camera_info (Topic)​

6.2.3 /camera_01/depth/image_raw (Topic)​

6.2.4 /camera_01/depth/camera_info (Topic)​

6.2.5 /camera_01/depth/points (Topic)​

6.3 Camera Preprocessing APIs (camera_preprocessing)​

6.3.1 /camera_01/fused_data (Topic)​

6.3.2 /camera_01/filtered_pointcloud (Topic)​

7. Health Monitoring APIs​

7.1 /Humanoid_nav/health (Topic)​

7.2 Python Subscription Example​

7.3 C++ Subscription Example​

8. Event Notification APIs​

8.1 /Humanoid_nav/events (Topic)​

8.1.1 goal_in_forbidden_area (Navigation Goal in a Forbidden Area)​

8.2 Python Subscription Example​

8.3 C++ Subscription Example​

9. Map Management APIs​

9.0 Namespace supports​

9.1 Message Type Definitions​

9.1.1 Floor Message​

9.1.2 CompositeMapInfo Message​

9.1.3 POI Message​

9.1.4 VirtualWall Message​

9.1.5 ForbiddenArea Message​

9.1.6 DangerousArea Message​

9.1.7 Room Message​

9.1.8 SemanticObject Message​

9.1.9 FloorTransition Message​

9.2 Composite Map Services​

9.2.1 /load_composite_map (Service)​

9.2.2 /save_composite_map (Service)​

9.2.3 /clear_composite_map (Service)​

9.3 Floor Management Services​

9.3.1 /list_floors (Service)​

9.3.2 /get_current_floor (Service)​

9.3.3 /switch_floor (Service)​

9.3.4 /add_floor (Service)​

9.3.5 /remove_floor (Service)​

Important Notes

1. Switching Between Mapping and Localization

1.1 /slam/set_mode (Service)

1.2 /slam/mode_status (Topic)

2. Mapping Mode APIs

2.1 /clear_map (Topic)

2.2 /map (Topic)

2.3 /cloud_registered_gravity (Topic)

2.4 /optimize_map (Topic)

2.5 /slam/save_map (Service)

2.6 Saving a 2D Map

3. Localization Mode APIs

3.1 /initialpose (Topic)

3.2 /slam/load_map (Service)

3.3 /robot_pose (Topic)

3.4 /odom (Topic)

3.5 /odom_status_code (Topic)

3.6 /odom_status_score (Topic)

3.7 Relocalization

3.7.1 /slam/global_relocalization (Service)

3.7.2 /slam/trigger_local_relocalization (Service)

3.7.6 /reloc_status (Topic)

4. Navigation APIs

4.1 navigate_to_pose (Action)

Goal Request

Result

Live Feedback

Example Call

4.2 /plan (Topic)

4.3 /cmd_vel (Topic)

4.4 cancel_current_action (Service)

4.5 get_current_action (Service)

4.6 action_status (Topic)

5. Sensor Data APIs

5.1 /scan (Topic)

5.2 /segmented_groundless_points (Topic)

5.3 /rslidar_points (Topic)

5.4 /imu (Topic)

6. Camera APIs

6.1 Camera Configuration

6.2 Camera Driver APIs (CAMERA_SDK_ROS2)

6.2.1 /camera_01/color/image_raw (Topic)

6.2.2 /camera_01/color/camera_info (Topic)

6.2.3 /camera_01/depth/image_raw (Topic)

6.2.4 /camera_01/depth/camera_info (Topic)

6.2.5 /camera_01/depth/points (Topic)

6.3 Camera Preprocessing APIs (camera_preprocessing)

6.3.1 /camera_01/fused_data (Topic)

6.3.2 /camera_01/filtered_pointcloud (Topic)

7. Health Monitoring APIs

7.1 /Humanoid_nav/health (Topic)

7.2 Python Subscription Example

7.3 C++ Subscription Example

8. Event Notification APIs

8.1 /Humanoid_nav/events (Topic)

8.1.1 `goal_in_forbidden_area` (Navigation Goal in a Forbidden Area)

8.2 Python Subscription Example

8.3 C++ Subscription Example

9. Map Management APIs

9.0 Namespace supports

9.1 Message Type Definitions

9.1.1 Floor Message

9.1.2 CompositeMapInfo Message

9.1.3 POI Message

9.1.4 VirtualWall Message

9.1.5 ForbiddenArea Message

9.1.6 DangerousArea Message

9.1.7 Room Message

9.1.8 SemanticObject Message

9.1.9 FloorTransition Message

9.2 Composite Map Services

9.2.1 /load_composite_map (Service)

9.2.2 /save_composite_map (Service)

9.2.3 /clear_composite_map (Service)

9.3 Floor Management Services

9.3.1 /list_floors (Service)

9.3.2 /get_current_floor (Service)

9.3.3 /switch_floor (Service)

9.3.4 /add_floor (Service)

9.3.5 /remove_floor (Service)