Skip to content
2 changes: 1 addition & 1 deletion en/middleware/uorb.md
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ Versioned messages include an additional field `uint32 MESSAGE_VERSION = x`, whe

Versioned and non-versioned messages are separated in the file system:

- Non-versioned topic message files and service message files remain in the [`msg/`](https://github.com/PX4/PX4-Autopilot/tree/main/msg) and [`srv/`](https://github.com/PX4/PX4-Autopilot/tree/main/srv) directories, respectively.
- Non-versioned topic message files and [server service](../ros2/user_guide.md#px4-ros-2-service-servers) message files remain in the [`msg/`](https://github.com/PX4/PX4-Autopilot/tree/main/msg) and [`srv/`](https://github.com/PX4/PX4-Autopilot/tree/main/srv) directories, respectively.
- The current (highest) version of message files are located in the `versioned` subfolders ([`msg/versioned`](https://github.com/PX4/PX4-Autopilot/tree/main/msg/versioned) and [`srv/versioned`](https://github.com/PX4/PX4-Autopilot/tree/main/srv/versioned)).
- Older versions of messages are stored in nested `msg/px4_msgs_old/` subfolders ([`msg/px4_msgs_old/msg/`](https://github.com/PX4/PX4-Autopilot/tree/main/msg/px4_msgs_old/msg) and [`msg/px4_msgs_old/srv/`](https://github.com/PX4/PX4-Autopilot/tree/main/msg/px4_msgs_old/srv)).
The files are also renamed with a suffix to indicate their version number.
Expand Down
11 changes: 11 additions & 0 deletions en/middleware/uxrce_dds.md
Original file line number Diff line number Diff line change
Expand Up @@ -490,6 +490,17 @@ Add a topic to the `subscriptions_multi` section to:
You can arbitrarily change the configuration.
For example, you could use different default namespaces or use a custom package to store the message definitions.

## DDS (ROS 2) Services

PX4 uXRCE-DDS middleware supports [ROS 2 services](https://docs.ros.org/en/jazzy/Concepts/Basic/About-Services.html).
These are remote procedure calls, from one node to another, which return a result.
They simplify communication between ROS 2 nodes and PX4 by encapsulating and associating the request and response topics, ensuring that replies are only returned to the specific requesting user.

A service server is the entity that will accept a remote procedure request, perform some computation on it, and return the result.
For example, the `/fmu/vehicle_command` service server defined in [`px4_msgs::srv::VehicleCommand`](https://github.com/PX4/px4_msgs/blob/main/srv/VehicleCommand.srv) can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.

For a list of services, details and examples see the [service documentation](../ros2/user_guide.md#px4-ros-2-service-servers) in the ROS 2 User Guide.

## Fast-RTPS to uXRCE-DDS Migration Guidelines

These guidelines explain how to migrate from using PX4 v1.13 [Fast-RTPS](../middleware/micrortps.md) middleware to PX4 v1.14 `uXRCE-DDS` middleware.
Expand Down
4 changes: 2 additions & 2 deletions en/ros2/px4_ros2_msg_translation_node.md
Original file line number Diff line number Diff line change
Expand Up @@ -318,15 +318,15 @@ The example describes the process of updating the `VehicleAttitude` message defi

For example, update `msg/versioned/VehicleAttitude.msg` from:

```.msg
```txt
uint32 MESSAGE_VERSION = 3
uint64 timestamp
...
```

to

```.msg
```txt
uint32 MESSAGE_VERSION = 4 # Increment
uint64 timestamp
float32 new_field # Make definition changes
Expand Down
219 changes: 164 additions & 55 deletions en/ros2/user_guide.md
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,7 @@ This section shows how to create a ROS 2 workspace hosted in your home directory
The [px4_ros_com](https://github.com/PX4/px4_ros_com) and [px4_msgs](https://github.com/PX4/px4_msgs) packages are cloned to a workspace folder, and then the `colcon` tool is used to build the workspace.
The example is run using `ros2 launch`.

You should use a version of the px4_msgs package with the _same_ message defintions as the PX4 firmware you have installed in the step above.
You should use a version of the px4_msgs package with the _same_ message definitions as the PX4 firmware you have installed in the step above.
Branches in the px4_msgs repo are named to correspond to the message definitions for different PX4 releases.
If for any reason you cannot ensure the same message definitions between your PX4 firmware and ROS 2 px4_msgs package, you will additionally need to [start the message translation node](#optional-starting-the-translation-node) as part of your setup process.

Expand Down Expand Up @@ -561,28 +561,28 @@ This creates a callback function for when the `SensorCombined` uORB messages are

```cpp
public:
explicit SensorCombinedListener() : Node("sensor_combined_listener")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);

subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>("/fmu/out/sensor_combined", qos,
[this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n";
std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl;
std::cout << "=============================" << std::endl;
std::cout << "ts: " << msg->timestamp << std::endl;
std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl;
std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl;
std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl;
std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl;
std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl;
std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl;
std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl;
std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl;
std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl;
});
}
explicit SensorCombinedListener() : Node("sensor_combined_listener")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);

subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>("/fmu/out/sensor_combined", qos,
[this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n";
std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl;
std::cout << "=============================" << std::endl;
std::cout << "ts: " << msg->timestamp << std::endl;
std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl;
std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl;
std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl;
std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl;
std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl;
std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl;
std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl;
std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl;
std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl;
});
}
```

::: info
Expand All @@ -595,7 +595,7 @@ The lines below create a publisher to the `SensorCombined` uORB topic, which can

````cpp
private:
rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
};
```s

Expand All @@ -604,13 +604,13 @@ The instantiation of the `SensorCombinedListener` class as a ROS node is done on
```cpp
int main(int argc, char *argv[])
{
std::cout << "Starting sensor_combined listener node..." << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<SensorCombinedListener>());
std::cout << "Starting sensor_combined listener node..." << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<SensorCombinedListener>());

rclcpp::shutdown();
return 0;
rclcpp::shutdown();
return 0;
}
````

Expand Down Expand Up @@ -643,27 +643,27 @@ The messages are sent based on a timed callback, which sends two messages per se

```cpp
public:
DebugVectAdvertiser() : Node("debug_vect_advertiser") {
publisher_ = this->create_publisher<px4_msgs::msg::DebugVect>("fmu/debug_vect/in", 10);
auto timer_callback =
[this]()->void {
auto debug_vect = px4_msgs::msg::DebugVect();
debug_vect.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();
std::string name = "test";
std::copy(name.begin(), name.end(), debug_vect.name.begin());
debug_vect.x = 1.0;
debug_vect.y = 2.0;
debug_vect.z = 3.0;
RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %llu x: %f y: %f z: %f \033[0m",
debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z);
this->publisher_->publish(debug_vect);
};
timer_ = this->create_wall_timer(500ms, timer_callback);
}
DebugVectAdvertiser() : Node("debug_vect_advertiser") {
publisher_ = this->create_publisher<px4_msgs::msg::DebugVect>("fmu/debug_vect/in", 10);
auto timer_callback =
[this]()->void {
auto debug_vect = px4_msgs::msg::DebugVect();
debug_vect.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();
std::string name = "test";
std::copy(name.begin(), name.end(), debug_vect.name.begin());
debug_vect.x = 1.0;
debug_vect.y = 2.0;
debug_vect.z = 3.0;
RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %llu x: %f y: %f z: %f \033[0m",
debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z);
this->publisher_->publish(debug_vect);
};
timer_ = this->create_wall_timer(500ms, timer_callback);
}

private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<px4_msgs::msg::DebugVect>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<px4_msgs::msg::DebugVect>::SharedPtr publisher_;
};
```

Expand All @@ -672,13 +672,13 @@ The instantiation of the `DebugVectAdvertiser` class as a ROS node is done on th
```cpp
int main(int argc, char *argv[])
{
std::cout << "Starting debug_vect advertiser node..." << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DebugVectAdvertiser>());
std::cout << "Starting debug_vect advertiser node..." << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DebugVectAdvertiser>());

rclcpp::shutdown();
return 0;
rclcpp::shutdown();
return 0;
}
```

Expand Down Expand Up @@ -751,6 +751,115 @@ will generate topics under the namespaces:

:::

## PX4 ROS 2 Service Servers

<Badge type="tip" text="PX4 v1.15" />

PX4 uXRCE-DDS middleware supports [ROS 2 services](https://docs.ros.org/en/jazzy/Concepts/Basic/About-Services.html).
Services are remote procedure calls, from one node to another, that return a result.

A service server is the entity that will accept a remote procedure request, perform some computation on it, and return the result.
They simplify communication between ROS 2 nodes and PX4 by encapsulating and associating the request and response topics, and ensuring that replies are only returned to the specific requesting user.
This is much easier that publishing the request, subscribing to the reply, and filtering out any unwanted responses.

The service servers that are built into the PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) module include:

- `/fmu/vehicle_command` (definition: [`px4_msgs::srv::VehicleCommand`](https://github.com/PX4/px4_msgs/blob/main/srv/VehicleCommand.srv).)
Copy link
Collaborator

@hamishwillee hamishwillee Feb 26, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I find the terminology a bit confusing.

  1. Am I correct that /fmu/vehicle_command is a service (and /fmu/out/ is a publication, and fmu/in is a subscription). How does this relate to "topic" and "message"/

2.You call px4_msgs::srv::VehicleCommand a service type - where does "Type" come from? I have called it a definition here, because that's what is linked. But might have to change back.

[Ultimately I'm asking if this line is OK!)

  1. VehicleCommand.srv is a service server message definition. Should we be auto-generating docs for this, as we do for normal uorb messages?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Am I correct that /fmu/vehicle_command is a service (and /fmu/out/ is a publication, and fmu/in is a subscription).

The way it is implemented by the uxrcedds_client in PX4 has the following conventions:

  • A topic whose name is in the form /fmu/out/* is published by PX4 and therefore is subscribed by the user ROS 2 node.
  • A topic whose name is in the form /fmu/in/* is subscribed by PX4 and therefore is published by the user ROS 2 node.
  • A service server is using instead the form /fmu/*.

Note however that this is pure naming convention.

How does this relate to "topic" and "message"/

So in ROS you have messages, for example the sensor_msgs::Imu or the PX4 version of it.
You exchange messages though topics where a node publishes a message on a topic and all subscriber of such topic will receive the message. The topic is nothing else but a identifier (the string /fmu/out/sensor_combined for example) used to match subscribers and publishers.

You have services, where a single server offers a service with a given name (/fmu/vehicle_command for example) and other nodes can create client to sumbit requests to it. The service will reply to such requests with responses. Requests and responses are just messages themself at the very end.

ROS abstracts a bit of that through the service files: a service file such as VehicleCommand.srv when compiled, is splitted in the request message and the reply message:

VehicleCommand.srv is

VehicleCommand request
---
VehicleCommandAck reply

everything above the --- constitutes the request and everything below the reply which will constitute completely independent messages.

You call px4_msgs::srv::VehicleCommand a service type - where does "Type" come from? I have called it a definition here, because that's what is linked. But might have to change back.

Type is probably the wrong word: interface is a better one: https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html#, the files that define interfaces are called interface definitions.

VehicleCommand.srv is a service server message definition. Should we be auto-generating docs for this, as we do for normal uorb messages?

the service definition is not used in the PX4 side, it is only used in the ROS 2 side.
PX4 internally do not uses services so there is no reason to create autodoc for it.
For completeness, the uxrcedds_client vehicle_command service server, when it receives a request from the ROS 2 node, converts it to a full uORB vehicle_command message and publishes inside PX4, the it waits for the vechicle_command_ack response uORB message, checks if it the right one and if it is then it packages in the reply and sends it to ROS 2.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you very much for this. I'm going to email myself with a link and think about it a bit more.


This service can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.

Details and specific examples are provided in the following sections.


### VehicleCommand service

This can be used to send commands to the vehicle, such as "take off", "land", change mode, and "orbit", and receive a response.

The service is available under `/fmu/vehicle_command`, which can be changed through custom namespaces just like the other PX4 topics.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"which can be changed through custom namespaces just like the other PX4 topics." - I don't know what that means. Can you link it.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

More info added in 890a999

The service type is defined in [`px4_msgs::srv::VehicleCommand`](https://github.com/PX4/px4_msgs/blob/main/srv/VehicleCommand.srv) as:

```txt
VehicleCommand request
---
VehicleCommandAck reply
```

Users can make service requests by sending [VehicleCommand](../msg_docs/VehicleCommand.md) messages, and receive a [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) message in response.
The service ensures that only the `VehicleCommandAck` reply generated for the specific request made by the user is sent back.

#### VehicleCommand Service Offboard Control Example

A complete _offboard control_ example using the VehicleCommand service is provided by the [offboard_control_srv](https://github.com/PX4/px4_ros_com/blob/main/src/examples/offboard/offboard_control_srv.cpp) node available in the `px4_ros_com` package.

The example closely follows the _offboard control_ example described in [ROS 2 Offboard Control Example](../ros2/offboard_control.md) but uses the `VehicleCommand` service to request mode changes, vehicle arming and vehicle disarming.

Like for any other ROS 2 service client, the ROS 2 application needs to declare a service client of type `px4_msgs::srv::VehicleCommand`:

```cpp
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedPtr vehicle_command_client_;
```

and then initialize it to the right ROS 2 service.
As the application assumes the standard PX4 namespace is used, this results in

```cpp
vehicle_command_client_{this->create_client<px4_msgs::srv::VehicleCommand>("/fmu/vehicle_command")}
```

After that, the client can be used to send any vehicle command request.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The code seems to also require that you first wait for the service:

while (!vehicle_command_client_->wait_for_service(1s)) {
			if (!rclcpp::ok()) {

Where does wait_for_service() come from?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is not a strict requirement but rather a good pracitice to wait until the service server is ready to accept requests before starting sending them.

You can perfectly not put it in the code: in such case when you send a request you might have to wait for the service to become available to serve it.

In the example it is used to check if PX4 is truly connected to the ROS 2 network. There the code waits untill connection before continuing.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't we say that? Note that I have no idea where this method is defined (?) I assume wait_for_service() is some kind of standard interface/API? Or is it our own interface that hides some other magic?

Assuming it is standard, you might do something like this:

Suggested change
After that, the client can be used to send any vehicle command request.
::: info
It is good practice to make sure the that the server server is ready to accept requests before sending them. You can use `wait_for_service()` to check if the server is ready.
:::
After that, the client can be used to send any vehicle command request.

For example, the `arm()` function is used to request the vehicle to arm:

```cpp
void OffboardControl::arm()
{
RCLCPP_INFO(this->get_logger(), "requesting arm");
request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
}
```

where `request_vehicle_command` handles formatting the request and sending it over in _asynchronous_ [mode](https://docs.ros.org/en/humble/How-To-Guides/Sync-Vs-Async.html#asynchronous-calls):

```cpp
void OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2)
{
auto request = std::make_shared<px4_msgs::srv::VehicleCommand::Request>();

VehicleCommand msg{};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
request->request = msg;

service_done_ = false;
auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this,
std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "Command send");
}
```

The response is finally captured asynchronously by the `response_callback` method which checks for the request result:

```cpp
void OffboardControl::response_callback(
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future) {
auto status = future.wait_for(1s);
if (status == std::future_status::ready) {
auto reply = future.get()->reply;
service_result_ = reply.result;
// make decision based on service_result_
service_done_ = true;
} else {
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
```

## ros2 CLI

The [ros2 CLI](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools.html) is a useful tool for working with ROS.
Expand Down