oscillation_ctrl/src/path_follow.cpp

/**
 * @file path_follow.cpp
 * @brief Offboard path trajectory tracking
*/

#include <ros/ros.h>
#include <std_msgs/Bool.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;
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 = q_init.x;
	// 		buff_pose.pose.orientation.y = q_init.y;
	// 		buff_pose.pose.orientation.z = q_init.z;
	// 		buff_pose.pose.orientation.w = q_init.w;
	// 		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_ONCE("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;

    // Boolean to set vehicle into oscillation damp mode   
    bool oscillation_damp = false;

    // Keep track of time since requests
    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_ONCE("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 
		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 != "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");
			}
		} 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");
		}

		// Check if we want to use oscillation controller
		if (ros::param::has("/status/use_ctrl")){
			ros::param::get("/status/use_ctrl", oscillation_damp);
			if(oscillation_damp == true){
				ROS_INFO("ATTITUDE CTRL");
				att_targ.header.stamp = ros::Time::now();
				// Publish attitude commands
				att_targ_pub.publish(att_targ);
			} else {
				ROS_INFO("POSITION CTRL");
				//ROS_INFO("POSITION CTRL");
				// Publish position setpoints
				local_pos_pub.publish(buff_pose);
			}
			ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
			ROS_INFO("---------------------------");	
		} else {
			ROS_WARN("CONTROL PARAM NOT FOUND. PLEASE SET '/status/use_ctrl'");
		}

		ros::spinOnce();
		rate_pub.sleep();
    }

    return 0;
}