Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

C++ offboard example: change publishers QoS profile and timer frequency #179

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

C++ offboard example: change publishers QoS profile and timer frequency #179

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
119 changes: 88 additions & 31 deletions src/examples/offboard/offboard_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,42 +57,63 @@ using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

const rmw_qos_profile_t rmw_qos_profile_offboard_pub =
{
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
5,
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
RMW_QOS_POLICY_DURABILITY_VOLATILE,
RMW_QOS_DEADLINE_DEFAULT,
RMW_QOS_LIFESPAN_DEFAULT,
RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
false
};

class OffboardControl : public rclcpp::Node
{
public:
OffboardControl() : Node("offboard_control")
{

offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
vehicle_command_publisher_ = this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);
rmw_qos_profile_t qos_profile = rmw_qos_profile_offboard_pub;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", qos);
trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", qos);
vehicle_command_publisher_ = this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", qos);

offboard_setpoint_counter_ = 0;

auto timer_callback = [this]() -> void {

if (offboard_setpoint_counter_ == 10) {
// Change to Offboard mode after 10 setpoints
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);

if (offboard_setpoint_counter_ == 50) {
// Change to Offboard mode after 50 setpoints (1s)
this->engage_offBoard_mode();

// Arm the vehicle
this->arm();
}
if (offboard_setpoint_counter_ == 550){
// Land and cancel timer after (11s)
this->land();

this->timer_->cancel();
}
if (offboard_setpoint_counter_ < 550) {

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

// stop the counter after reaching 11
if (offboard_setpoint_counter_ < 11) {
offboard_setpoint_counter_++;
}
};
timer_ = this->create_wall_timer(100ms, timer_callback);
// position controller runs at 50Hz
timer_ = this->create_wall_timer(20ms, timer_callback);
}

void arm();
void disarm();
void land();
void engage_offBoard_mode();

private:
rclcpp::TimerBase::SharedPtr timer_;
Expand All @@ -107,15 +128,19 @@ class OffboardControl : public rclcpp::Node

void publish_offboard_control_mode();
void publish_trajectory_setpoint();
void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
void publish_vehicle_command(VehicleCommand msg);
};

/**
* @brief Send a command to Arm the vehicle
*/
void OffboardControl::arm()
{
publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
VehicleCommand msg{};

msg.param1 = 1;
msg.command = VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM;
publish_vehicle_command(msg);

RCLCPP_INFO(this->get_logger(), "Arm command send");
}
Expand All @@ -125,27 +150,68 @@ void OffboardControl::arm()
*/
void OffboardControl::disarm()
{
publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
VehicleCommand msg{};

msg.command = VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM;

publish_vehicle_command(msg);

RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

/**
* @brief Send a command to Land the vehicle
*/
void OffboardControl::land()
{
VehicleCommand msg{};

msg.command = VehicleCommand::VEHICLE_CMD_NAV_LAND;

publish_vehicle_command(msg);

RCLCPP_INFO(this->get_logger(), "Land command send");
}

/**
* @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 Engage the offboard control mode.
*/
void OffboardControl::engage_offBoard_mode()
{
VehicleCommand msg{};

msg.param1 = 1;
msg.param2 = 6;
msg.command = VehicleCommand::VEHICLE_CMD_DO_SET_MODE;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;

publish_vehicle_command(msg);

RCLCPP_INFO(this->get_logger(), "Offboard mode command sent");
}

/**
* @brief Publish a trajectory setpoint
* For this example, it sends a trajectory setpoint to make the
Expand All @@ -154,6 +220,7 @@ void OffboardControl::publish_offboard_control_mode()
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;
Expand All @@ -162,22 +229,12 @@ void OffboardControl::publish_trajectory_setpoint()

/**
* @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
* @param command Command message
*/
void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2)
void OffboardControl::publish_vehicle_command(VehicleCommand msg)
{
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;

vehicle_command_publisher_->publish(msg);
}

Expand Down