forked from cesar.alejandro/oscillation_ctrl
215 lines
6.8 KiB
C++
215 lines
6.8 KiB
C++
/**
|
|
* @file path_follow.cpp
|
|
* @brief Offboard path trajectory tracking
|
|
*/
|
|
|
|
#include <ros/ros.h>
|
|
#include <sensor_msgs/NavSatFix.h>
|
|
#include <oscillation_ctrl/RefSignal.h>
|
|
#include <oscillation_ctrl/EulerAngles.h>
|
|
#include <oscillation_ctrl/WaypointTrack.h>
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
#include <geometry_msgs/Quaternion.h>
|
|
#include <mavros_msgs/CommandBool.h>
|
|
#include <mavros_msgs/SetMode.h>
|
|
#include <mavros_msgs/State.h>
|
|
#include <mavros_msgs/CommandTOL.h>
|
|
#include <mavros_msgs/AttitudeTarget.h>
|
|
#include <tf2/LinearMath/Quaternion.h>
|
|
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
#include <iostream>
|
|
#include <cmath> // for std::abs
|
|
|
|
/********* CALLBACK FUNCTIONS **********************/
|
|
|
|
// Callback function which will save the current state of the autopilot.
|
|
// Allows to check connection, arming, and Offboard tags*/
|
|
mavros_msgs::State current_state;
|
|
bool land = false;
|
|
void state_cb(const mavros_msgs::State::ConstPtr& msg){
|
|
current_state = *msg;
|
|
}
|
|
|
|
// Cb to determine attitude target
|
|
mavros_msgs::AttitudeTarget att_targ;
|
|
void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
|
|
att_targ = *msg;
|
|
}
|
|
|
|
// Cb to recieve pose information
|
|
geometry_msgs::PoseStamped buff_pose;
|
|
geometry_msgs::Quaternion q_init;
|
|
geometry_msgs::PoseStamped mavPose;
|
|
bool gps_read = false;
|
|
double current_altitude;
|
|
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
|
mavPose = *msg;
|
|
current_altitude = mavPose.pose.position.z;
|
|
while(gps_read == false){
|
|
q_init = mavPose.pose.orientation;
|
|
if(q_init.x == 0.0 && q_init.w == 0.0){
|
|
ROS_INFO("Waiting...");
|
|
} else {
|
|
buff_pose.pose.orientation.x = 0.0;
|
|
buff_pose.pose.orientation.y = 0.0;
|
|
buff_pose.pose.orientation.z = 0.0;
|
|
buff_pose.pose.orientation.w = 1.0;
|
|
gps_read = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "path_follow");
|
|
ros::NodeHandle nh;
|
|
|
|
/********************** SUBSCRIBERS **********************/
|
|
// Get current state
|
|
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
|
|
("mavros/state", 10, state_cb);
|
|
|
|
// Get attitude target from klausen control
|
|
ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
|
|
("command/att_target",10,att_targ_cb);
|
|
|
|
// Pose subscriber
|
|
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
|
("mavros/local_position/pose",10,mavPose_cb);
|
|
|
|
/********************** PUBLISHERS **********************/
|
|
// Initiate publisher to publish commanded local position
|
|
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
|
("mavros/setpoint_position/local", 10);
|
|
|
|
// Publish attitude and thrust commands
|
|
ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
|
|
("mavros/setpoint_raw/attitude",10);
|
|
|
|
// Service Clients
|
|
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
|
("mavros/cmd/arming");
|
|
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
|
("mavros/set_mode");
|
|
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
|
("mavros/cmd/takeoff");
|
|
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
|
("status/waypoint_tracker");
|
|
|
|
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
|
|
ros::Rate rate_wait(20.0);
|
|
|
|
// wait for FCU connection
|
|
while(ros::ok() && !current_state.connected){
|
|
ros::spinOnce();
|
|
ROS_INFO("Waiting for FCU connection");
|
|
rate_wait.sleep();
|
|
}
|
|
|
|
if (current_state.connected){
|
|
ROS_INFO("Connected to FCU");
|
|
} else {
|
|
ROS_INFO("Never Connected");
|
|
}
|
|
|
|
/*********** Initiate variables ************************/
|
|
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
|
|
ros::Rate rate_pub(25.0);
|
|
|
|
// Desired mode is offboard
|
|
mavros_msgs::SetMode offb_set_mode;
|
|
offb_set_mode.request.custom_mode = "OFFBOARD";
|
|
|
|
// Arm UAV
|
|
mavros_msgs::CommandBool arm_cmd;
|
|
arm_cmd.request.value = true;
|
|
|
|
// Take off command
|
|
bool takeoff = false, att_cmds = false;
|
|
bool oscillation_damp = true;
|
|
|
|
// Keep track of time since requests
|
|
ros::Time tkoff_req = ros::Time::now();
|
|
ros::Time last_request = ros::Time::now();
|
|
|
|
//send a few setpoints before starting
|
|
for(int i = 100; ros::ok() && i > 0; --i){
|
|
local_pos_pub.publish(buff_pose);
|
|
ros::spinOnce();
|
|
ROS_INFO("Publishing position setpoints");
|
|
rate_wait.sleep();
|
|
}
|
|
|
|
// Retrieve desired waypoints
|
|
oscillation_ctrl::WaypointTrack wpoint_srv;
|
|
wpoint_srv.request.query = false;
|
|
if (waypoint_cl.call(wpoint_srv)){
|
|
ROS_INFO("Waypoints received");
|
|
// populate desired waypoints - this is only for original hover as
|
|
// a change of waypoints is handled by ref_signal.py
|
|
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
|
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
|
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
|
} else {
|
|
ROS_INFO("NO waypoints received");
|
|
}
|
|
double alt_des = wpoint_srv.response.xd.z; // Desired height
|
|
while(ros::ok()){
|
|
if(current_state.mode == "AUTO.LAND"){
|
|
land = true;
|
|
while(land == true){
|
|
ROS_INFO("Des Altitude: LANDING");
|
|
}
|
|
} else {
|
|
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
|
|
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
|
|
} else {
|
|
ROS_INFO("Could not enter offboard mode");
|
|
}
|
|
//last_request = ros::Time::now();
|
|
} else {
|
|
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
|
|
if( arming_client.call(arm_cmd) && arm_cmd.response.success){
|
|
ROS_INFO("Vehicle armed");
|
|
}
|
|
last_request = ros::Time::now();
|
|
}
|
|
}
|
|
if(current_state.mode == "OFFBOARD" && current_state.armed){
|
|
ROS_INFO_ONCE("Spiri is taking off");
|
|
if(!takeoff){
|
|
tkoff_req = ros::Time::now();
|
|
takeoff = true;
|
|
}
|
|
}
|
|
// Check if we want to use oscillation controller
|
|
//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
|
|
if (ros::param::has("/status/use_ctrl")){
|
|
ros::param::get("/status/use_ctrl", oscillation_damp);
|
|
if(oscillation_damp){
|
|
ROS_INFO("USING ATTITUDE CTRL");
|
|
att_targ.header.stamp = ros::Time::now();
|
|
att_targ_pub.publish(att_targ);
|
|
} else {
|
|
// Check if waypoints have changed
|
|
if (waypoint_cl.call(wpoint_srv))
|
|
{
|
|
// populate desired waypoints
|
|
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
|
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
|
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
|
}
|
|
local_pos_pub.publish(buff_pose);
|
|
ROS_INFO("USING POSITION CTRL");
|
|
}
|
|
}
|
|
ROS_INFO("Des Altitude: %.2f", alt_des);
|
|
ROS_INFO("Cur Altitude: %.2f", current_altitude);
|
|
ROS_INFO("---------------------------");
|
|
ros::spinOnce();
|
|
rate_pub.sleep();
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
} |