Skip to content

Commit

Permalink
Add example that uses VehicleCommand service
Browse files Browse the repository at this point in the history
Signed-off-by: Beniamino Pozzan <[email protected]>
  • Loading branch information
beniaminopozzan committed Mar 10, 2024
1 parent 5e46ae5 commit 2d56cb6
Show file tree
Hide file tree
Showing 3 changed files with 296 additions and 1 deletion.
5 changes: 5 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,11 @@ add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
ament_target_dependencies(offboard_control rclcpp px4_msgs)
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})

# examples/offboard/offboard_control_srv
add_executable(offboard_control_srv src/examples/offboard/offboard_control_srv.cpp)
ament_target_dependencies(offboard_control_srv rclcpp px4_msgs)
install(TARGETS offboard_control_srv DESTINATION lib/${PROJECT_NAME})


############
# Install ##
Expand Down
3 changes: 2 additions & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,8 @@
<version>0.1.0</version>
<description>Package to interface ROS 2 with PX4</description>

<maintainer email="[email protected]">Nuno Marques</maintainer>
<maintainer email="[email protected]">Beniamino Pozzan</maintainer>
<author email="[email protected]">Beniamino Pozzan</author>
<author email="[email protected]">Nuno Marques</author>

<license>BSD 3-Clause</license>
Expand Down
289 changes: 289 additions & 0 deletions src/examples/offboard/offboard_control_srv.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,289 @@
/****************************************************************************
*
* Copyright 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @brief Offboard control example
* @file offboard_control.cpp
* @addtogroup examples *
* @author Beniamino Pozzan <[email protected]>
* @author Mickey Cowden <[email protected]>
* @author Nuno Marques <[email protected]>
*/

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/srv/vehicle_command.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <iostream>
#include <string>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

class OffboardControl : public rclcpp::Node
{
public:
OffboardControl(std::string px4_namespace) :
Node("offboard_control_srv"),
state_{State::init},
service_result_{0},
service_done_{false},
offboard_control_mode_publisher_{this->create_publisher<OffboardControlMode>(px4_namespace+"in/offboard_control_mode", 10)},
trajectory_setpoint_publisher_{this->create_publisher<TrajectorySetpoint>(px4_namespace+"in/trajectory_setpoint", 10)},
vehicle_command_client_{this->create_client<px4_msgs::srv::VehicleCommand>(px4_namespace+"vehicle_command")}
{
RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services");
RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service");
while (!vehicle_command_client_->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}

timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this));
}

void switch_to_offboard_mode();
void arm();
void disarm();

private:
enum class State{
init,
offboard_requested,
wait_for_stable_offboard_mode,
arm_requested,
armed
} state_;
uint8_t service_result_;
bool service_done_;
rclcpp::TimerBase::SharedPtr timer_;

rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedPtr vehicle_command_client_;


void publish_offboard_control_mode();
void publish_trajectory_setpoint();
void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
void response_callback(rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future);
void timer_callback(void);
};

/**
* @brief Send a command to switch to offboard mode
*/
void OffboardControl::switch_to_offboard_mode(){
RCLCPP_INFO(this->get_logger(), "requesting switch to Offboard mode");
request_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
}

/**
* @brief Send a command to Arm the vehicle
*/
void OffboardControl::arm()
{
RCLCPP_INFO(this->get_logger(), "requesting arm");
request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
}

/**
* @brief Send a command to Disarm the vehicle
*/
void OffboardControl::disarm()
{
RCLCPP_INFO(this->get_logger(), "requesting disarm");
request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
}

/**
* @brief Publish the offboard control mode.
* For this example, only position and altitude controls are active.
*/
void OffboardControl::publish_offboard_control_mode()
{
OffboardControlMode msg{};
msg.position = true;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = false;
msg.body_rate = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
}

/**
* @brief Publish a trajectory setpoint
* For this example, it sends a trajectory setpoint to make the
* vehicle hover at 5 meters with a yaw angle of 180 degrees.
*/
void OffboardControl::publish_trajectory_setpoint()
{
TrajectorySetpoint msg{};
msg.position = {0.0, 0.0, -5.0};
msg.yaw = -3.14; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher_->publish(msg);
}

/**
* @brief Publish vehicle commands
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
* @param param1 Command parameter 1
* @param param2 Command parameter 2
*/
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");
}

void OffboardControl::timer_callback(void){
static uint8_t num_of_steps = 0;

// offboard_control_mode needs to be paired with trajectory_setpoint
publish_offboard_control_mode();
publish_trajectory_setpoint();

switch (state_)
{
case State::init :
switch_to_offboard_mode();
state_ = State::offboard_requested;
break;
case State::offboard_requested :
if(service_done_){
if (service_result_==0){
RCLCPP_INFO(this->get_logger(), "Entered offboard mode");
state_ = State::wait_for_stable_offboard_mode;
}
else{
RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting");
rclcpp::shutdown();
}
}
break;
case State::wait_for_stable_offboard_mode :
if (++num_of_steps>10){
arm();
state_ = State::arm_requested;
}
break;
case State::arm_requested :
if(service_done_){
if (service_result_==0){
RCLCPP_INFO(this->get_logger(), "vehicle is armed");
state_ = State::armed;
}
else{
RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting");
rclcpp::shutdown();
}
}
break;
default:
break;
}
}

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;
switch (service_result_)
{
case reply.VEHICLE_CMD_RESULT_ACCEPTED:
RCLCPP_INFO(this->get_logger(), "command accepted");
break;
case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
RCLCPP_WARN(this->get_logger(), "command temporarily rejected");
break;
case reply.VEHICLE_CMD_RESULT_DENIED:
RCLCPP_WARN(this->get_logger(), "command denied");
break;
case reply.VEHICLE_CMD_RESULT_UNSUPPORTED:
RCLCPP_WARN(this->get_logger(), "command unsupported");
break;
case reply.VEHICLE_CMD_RESULT_FAILED:
RCLCPP_WARN(this->get_logger(), "command failed");
break;
case reply.VEHICLE_CMD_RESULT_IN_PROGRESS:
RCLCPP_WARN(this->get_logger(), "command in progress");
break;
case reply.VEHICLE_CMD_RESULT_CANCELLED:
RCLCPP_WARN(this->get_logger(), "command cancelled");
break;
default:
RCLCPP_WARN(this->get_logger(), "command reply unknown");
break;
}
service_done_ = true;
} else {
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}

int main(int argc, char *argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<OffboardControl>("/fmu/"));

rclcpp::shutdown();
return 0;
}

0 comments on commit 2d56cb6

Please sign in to comment.