Skip to content

Commit 2d56cb6

Browse files
beniaminopozzanbeniaminopozzan
committed
Add example that uses VehicleCommand service
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
1 parent 5e46ae5 commit 2d56cb6

File tree

3 files changed

+296
-1
lines changed

3 files changed

+296
-1
lines changed

‎CMakeLists.txt

+5
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,11 @@ add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
6060
ament_target_dependencies(offboard_control rclcpp px4_msgs)
6161
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})
6262

63+
# examples/offboard/offboard_control_srv
64+
add_executable(offboard_control_srv src/examples/offboard/offboard_control_srv.cpp)
65+
ament_target_dependencies(offboard_control_srv rclcpp px4_msgs)
66+
install(TARGETS offboard_control_srv DESTINATION lib/${PROJECT_NAME})
67+
6368

6469
############
6570
# Install ##

‎package.xml

+2-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,8 @@
55
<version>0.1.0</version>
66
<description>Package to interface ROS 2 with PX4</description>
77

8-
<maintainer email="nuno.marques@dronesolutions.io">Nuno Marques</maintainer>
8+
<maintainer email="beniamino.pozzan@gmail.com">Beniamino Pozzan</maintainer>
9+
<author email="beniamino.pozzan@gmail.com">Beniamino Pozzan</author>
910
<author email="nuno.marques@dronesolutions.io">Nuno Marques</author>
1011

1112
<license>BSD 3-Clause</license>

‎src/examples/offboard/offboard_control_srv.cpp

+289
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,289 @@
1+
/****************************************************************************
2+
*
3+
* Copyright 2023 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* 1. Redistributions of source code must retain the above copyright notice, this
9+
* list of conditions and the following disclaimer.
10+
*
11+
* 2. Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* 3. Neither the name of the copyright holder nor the names of its contributors
16+
* may be used to endorse or promote products derived from this software without
17+
* specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29+
* POSSIBILITY OF SUCH DAMAGE.
30+
*
31+
****************************************************************************/
32+
33+
/**
34+
* @brief Offboard control example
35+
* @file offboard_control.cpp
36+
* @addtogroup examples *
37+
* @author Beniamino Pozzan <beniamino.pozzan@gmail.com>
38+
* @author Mickey Cowden <info@cowden.tech>
39+
* @author Nuno Marques <nuno.marques@dronesolutions.io>
40+
*/
41+
42+
#include <px4_msgs/msg/offboard_control_mode.hpp>
43+
#include <px4_msgs/msg/trajectory_setpoint.hpp>
44+
#include <px4_msgs/msg/vehicle_control_mode.hpp>
45+
#include <px4_msgs/srv/vehicle_command.hpp>
46+
#include <rclcpp/rclcpp.hpp>
47+
#include <stdint.h>
48+
49+
#include <chrono>
50+
#include <iostream>
51+
#include <string>
52+
53+
using namespace std::chrono;
54+
using namespace std::chrono_literals;
55+
using namespace px4_msgs::msg;
56+
57+
class OffboardControl : public rclcpp::Node
58+
{
59+
public:
60+
OffboardControl(std::string px4_namespace) :
61+
Node("offboard_control_srv"),
62+
state_{State::init},
63+
service_result_{0},
64+
service_done_{false},
65+
offboard_control_mode_publisher_{this->create_publisher<OffboardControlMode>(px4_namespace+"in/offboard_control_mode", 10)},
66+
trajectory_setpoint_publisher_{this->create_publisher<TrajectorySetpoint>(px4_namespace+"in/trajectory_setpoint", 10)},
67+
vehicle_command_client_{this->create_client<px4_msgs::srv::VehicleCommand>(px4_namespace+"vehicle_command")}
68+
{
69+
RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services");
70+
RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service");
71+
while (!vehicle_command_client_->wait_for_service(1s)) {
72+
if (!rclcpp::ok()) {
73+
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
74+
return;
75+
}
76+
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
77+
}
78+
79+
timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this));
80+
}
81+
82+
void switch_to_offboard_mode();
83+
void arm();
84+
void disarm();
85+
86+
private:
87+
enum class State{
88+
init,
89+
offboard_requested,
90+
wait_for_stable_offboard_mode,
91+
arm_requested,
92+
armed
93+
} state_;
94+
uint8_t service_result_;
95+
bool service_done_;
96+
rclcpp::TimerBase::SharedPtr timer_;
97+
98+
rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
99+
rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
100+
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedPtr vehicle_command_client_;
101+
102+
103+
void publish_offboard_control_mode();
104+
void publish_trajectory_setpoint();
105+
void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
106+
void response_callback(rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future);
107+
void timer_callback(void);
108+
};
109+
110+
/**
111+
* @brief Send a command to switch to offboard mode
112+
*/
113+
void OffboardControl::switch_to_offboard_mode(){
114+
RCLCPP_INFO(this->get_logger(), "requesting switch to Offboard mode");
115+
request_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
116+
}
117+
118+
/**
119+
* @brief Send a command to Arm the vehicle
120+
*/
121+
void OffboardControl::arm()
122+
{
123+
RCLCPP_INFO(this->get_logger(), "requesting arm");
124+
request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
125+
}
126+
127+
/**
128+
* @brief Send a command to Disarm the vehicle
129+
*/
130+
void OffboardControl::disarm()
131+
{
132+
RCLCPP_INFO(this->get_logger(), "requesting disarm");
133+
request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
134+
}
135+
136+
/**
137+
* @brief Publish the offboard control mode.
138+
* For this example, only position and altitude controls are active.
139+
*/
140+
void OffboardControl::publish_offboard_control_mode()
141+
{
142+
OffboardControlMode msg{};
143+
msg.position = true;
144+
msg.velocity = false;
145+
msg.acceleration = false;
146+
msg.attitude = false;
147+
msg.body_rate = false;
148+
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
149+
offboard_control_mode_publisher_->publish(msg);
150+
}
151+
152+
/**
153+
* @brief Publish a trajectory setpoint
154+
* For this example, it sends a trajectory setpoint to make the
155+
* vehicle hover at 5 meters with a yaw angle of 180 degrees.
156+
*/
157+
void OffboardControl::publish_trajectory_setpoint()
158+
{
159+
TrajectorySetpoint msg{};
160+
msg.position = {0.0, 0.0, -5.0};
161+
msg.yaw = -3.14; // [-PI:PI]
162+
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
163+
trajectory_setpoint_publisher_->publish(msg);
164+
}
165+
166+
/**
167+
* @brief Publish vehicle commands
168+
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
169+
* @param param1 Command parameter 1
170+
* @param param2 Command parameter 2
171+
*/
172+
void OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2)
173+
{
174+
auto request = std::make_shared<px4_msgs::srv::VehicleCommand::Request>();
175+
176+
VehicleCommand msg{};
177+
msg.param1 = param1;
178+
msg.param2 = param2;
179+
msg.command = command;
180+
msg.target_system = 1;
181+
msg.target_component = 1;
182+
msg.source_system = 1;
183+
msg.source_component = 1;
184+
msg.from_external = true;
185+
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
186+
request->request = msg;
187+
188+
service_done_ = false;
189+
auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this,
190+
std::placeholders::_1));
191+
RCLCPP_INFO(this->get_logger(), "Command send");
192+
}
193+
194+
void OffboardControl::timer_callback(void){
195+
static uint8_t num_of_steps = 0;
196+
197+
// offboard_control_mode needs to be paired with trajectory_setpoint
198+
publish_offboard_control_mode();
199+
publish_trajectory_setpoint();
200+
201+
switch (state_)
202+
{
203+
case State::init :
204+
switch_to_offboard_mode();
205+
state_ = State::offboard_requested;
206+
break;
207+
case State::offboard_requested :
208+
if(service_done_){
209+
if (service_result_==0){
210+
RCLCPP_INFO(this->get_logger(), "Entered offboard mode");
211+
state_ = State::wait_for_stable_offboard_mode;
212+
}
213+
else{
214+
RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting");
215+
rclcpp::shutdown();
216+
}
217+
}
218+
break;
219+
case State::wait_for_stable_offboard_mode :
220+
if (++num_of_steps>10){
221+
arm();
222+
state_ = State::arm_requested;
223+
}
224+
break;
225+
case State::arm_requested :
226+
if(service_done_){
227+
if (service_result_==0){
228+
RCLCPP_INFO(this->get_logger(), "vehicle is armed");
229+
state_ = State::armed;
230+
}
231+
else{
232+
RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting");
233+
rclcpp::shutdown();
234+
}
235+
}
236+
break;
237+
default:
238+
break;
239+
}
240+
}
241+
242+
void OffboardControl::response_callback(
243+
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future) {
244+
auto status = future.wait_for(1s);
245+
if (status == std::future_status::ready) {
246+
auto reply = future.get()->reply;
247+
service_result_ = reply.result;
248+
switch (service_result_)
249+
{
250+
case reply.VEHICLE_CMD_RESULT_ACCEPTED:
251+
RCLCPP_INFO(this->get_logger(), "command accepted");
252+
break;
253+
case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
254+
RCLCPP_WARN(this->get_logger(), "command temporarily rejected");
255+
break;
256+
case reply.VEHICLE_CMD_RESULT_DENIED:
257+
RCLCPP_WARN(this->get_logger(), "command denied");
258+
break;
259+
case reply.VEHICLE_CMD_RESULT_UNSUPPORTED:
260+
RCLCPP_WARN(this->get_logger(), "command unsupported");
261+
break;
262+
case reply.VEHICLE_CMD_RESULT_FAILED:
263+
RCLCPP_WARN(this->get_logger(), "command failed");
264+
break;
265+
case reply.VEHICLE_CMD_RESULT_IN_PROGRESS:
266+
RCLCPP_WARN(this->get_logger(), "command in progress");
267+
break;
268+
case reply.VEHICLE_CMD_RESULT_CANCELLED:
269+
RCLCPP_WARN(this->get_logger(), "command cancelled");
270+
break;
271+
default:
272+
RCLCPP_WARN(this->get_logger(), "command reply unknown");
273+
break;
274+
}
275+
service_done_ = true;
276+
} else {
277+
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
278+
}
279+
}
280+
281+
int main(int argc, char *argv[])
282+
{
283+
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
284+
rclcpp::init(argc, argv);
285+
rclcpp::spin(std::make_shared<OffboardControl>("/fmu/"));
286+
287+
rclcpp::shutdown();
288+
return 0;
289+
}

0 commit comments

Comments
 (0)