#include "class_model/PubData.h"

PubDataClass::PubDataClass() : node_handle_("~"){

    std::string ros_namespace;
    if (!node_handle_.hasParam("namespace"))
	{
	}else{
		node_handle_.getParam("namespace", ros_namespace);
	}

    PubData=node_handle_.advertise<mavros_msgs::Waypoint>(ros_namespace+"/DroneStatus",100);
    Compass=node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10,
                                   &PubDataClass::Compass_callback, this);

}

PubDataClass::~PubDataClass() { ros::shutdown(); }

void PubDataClass::publishData(velocity v){

    msg.param2 = v.x;
    msg.param3 = v.y;
    PubData.publish(msg);
    ROS_INFO("pub drone status");

}

void PubDataClass::Compass_callback(const std_msgs::Float64::ConstPtr &degree){
    msg.param1 = degree->data;
    
}