oscillation_ctrl/src/offb_node.cpp

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.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 <mavros_msgs/Thrust.h>
#include <mavros_msgs/VFR_HUD.h>
#include <oscillation_ctrl/WaypointTrack.h>
#include <tf2/LinearMath/Quaternion.h>
#include <iostream>


/********* CALLBACK FUNCTIONS **********************/
// Initiate variables
mavros_msgs::State current_state;
geometry_msgs::PoseStamped desPose;

// Callback function which will save the current state of the autopilot. 
// Allows to check connection, arming, and Offboard tags*/
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// Cb to recieve pose information
// Initiate variables
geometry_msgs::PoseStamped pose;
geometry_msgs::Quaternion q_init;
geometry_msgs::PoseStamped mavPose;
bool pose_read = false;
double current_altitude;

void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
	mavPose = *msg;
	current_altitude = mavPose.pose.position.z;
	while(pose_read == false){
		q_init = mavPose.pose.orientation;
		if(q_init.x == 0.0 && q_init.w == 0.0){
			ROS_INFO("Waiting...");
		} else {
			pose_read = true;
            // pose.pose.orientation.x = q_init.x;
            // pose.pose.orientation.y = q_init.y;
            // pose.pose.orientation.z = q_init.z;
            // pose.pose.orientation.w = q_init.w;
		}
	}

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    
    /********************** SUBSCRIBERS **********************/
    // Get current state
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);

    // Pose subscriber
    ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
	("mavros/local_position/pose",10,mavPose_cb);

    // Waypoint Subscriber
    /*
    ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
	("/reference/waypoints",10,waypoints_cb);
    */
    ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
	("status/waypoint_tracker");
    
    /********************** PUBLISHERS **********************/
    // Initiate publisher to publish commanded local position
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);

    // Publish desired attitude
    ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
        ("mavros/setpoint_attitude/thrust", 10);

    // Publish attitude commands
    ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
	("/mavros/setpoint_attitude/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");

    //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.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
    pose.pose.position.x = wpoint_srv.response.xd.x;
    pose.pose.position.y = wpoint_srv.response.xd.y;
    pose.pose.position.z = wpoint_srv.response.xd.z;

    /*/ Populate pose msg
    pose.pose.orientation.x = q_init.x;
    pose.pose.orientation.y = q_init.y;
    pose.pose.orientation.z = q_init.z;
    pose.pose.orientation.w = q_init.w;
    */

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    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){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(3.0))){
                if(arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
	    // check if waypoints have changed desired waypoints
        waypoint_cl.call(wpoint_srv);
        pose.pose.position.x = wpoint_srv.response.xd.x;
        pose.pose.position.y = wpoint_srv.response.xd.y;
        pose.pose.position.z = wpoint_srv.response.xd.z;

        // User info
        ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z);
        ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z);
        ROS_INFO("---------------------------");
		
		local_pos_pub.publish(pose);

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

    return 0;
}