Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// Example that demonstrates Aero Offboard Attitude control using MAVROS
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <unistd.h>
#include <mavros_msgs/CommandTOL.h>
#include <time.h>
#include <ros/duration.h>
// FIXME: We're not doing Error handling.
void arm(ros::NodeHandle& nh);
void takeoff(ros::NodeHandle& nh);
void attitudeControl(ros::NodeHandle& nh);
void land(ros::NodeHandle& nh);
mavros_msgs::State current_state;
void stateCB(const mavros_msgs::State::ConstPtr& msg);
int main(int argc, char** argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
// the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, stateCB);
// Wait for FCU connection
while (ros::ok() && !current_state.connected)
{
ros::spinOnce();
rate.sleep();
}
arm(nh);
takeoff(nh);
sleep(5); // let it takeoff
attitudeControl(nh);
land(nh);
return 0;
}
void stateCB(const mavros_msgs::State::ConstPtr& msg)
{
current_state = *msg;
}
void arm(ros::NodeHandle& nh)
{
// arming
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
mavros_msgs::CommandBool srv_arm;
srv_arm.request.value = true;
if (arming_client.call(srv_arm) && srv_arm.response.success)
ROS_INFO("Arm sent");
else
{
ROS_ERROR("Arming failed");
return;
}
}
void takeoff(ros::NodeHandle& nh)
{
ros::ServiceClient takeoff_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
mavros_msgs::CommandTOL srv_takeoff{};
srv_takeoff.request.altitude = 490.0;
srv_takeoff.request.latitude = 47.3977415;
srv_takeoff.request.longitude = 8.5455937;
if (takeoff_client.call(srv_takeoff) && srv_takeoff.response.success)
ROS_INFO("Takeoff sent");
else
{
ROS_ERROR("Takeoff failed");
ros::shutdown();
}
}
void land(ros::NodeHandle& nh)
{
// land
ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
mavros_msgs::CommandTOL srv_land{};
if (land_client.call(srv_land) && srv_land.response.success)
ROS_INFO("Land sent");
else
{
ROS_ERROR("Landing failed");
ros::shutdown();
}
}
void attitudeControl(ros::NodeHandle& nh)
{
// Attitude control
ros::Publisher set_pos_pub = nh.advertise<mavros_msgs::AttitudeTarget>("mavros/setpoint_raw/attitude", 10);
mavros_msgs::AttitudeTarget set_pos{};
set_pos.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ROLL_RATE | mavros_msgs::AttitudeTarget::IGNORE_PITCH_RATE |
mavros_msgs::AttitudeTarget::IGNORE_YAW_RATE | mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;
// Send a few setpoints before starting Offboard mode; otherwise switching to offboard mode will be rejecetd.
for (int i = 50; i > 0; i--)
{
set_pos_pub.publish(set_pos);
ros::Duration(0.01).sleep();
}
// Switching to Offboard mode
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
ROS_INFO("Offboard enabled");
else
{
ROS_ERROR("Unable to switch to offboard");
return;
}
if (set_pos_pub)
ROS_INFO("Thrust control begins");
set_pos.thrust = 0.8f;
for (int i = 0; i < 100; i++)
{
set_pos_pub.publish(set_pos);
ros::Duration(0.01).sleep();
}
set_pos.thrust = 0.1;
for (int i = 0; i < 100; i++)
{
set_pos_pub.publish(set_pos);
ros::Duration(0.01).sleep();
}
set_pos.thrust = 0.8;
for (int i = 0; i < 100; i++)
{
set_pos_pub.publish(set_pos);
ros::Duration(0.01).sleep();
}
set_pos.thrust = 0.1;
for (int i = 0; i < 100; i++)
{
set_pos_pub.publish(set_pos);
ros::Duration(0.01).sleep();
}
ROS_INFO("Done");
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment