<commit_before>//General

//custom

//ROS
#include <ros/ros.h>
#include "mavros_msgs/State.h"
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/CommandCode.h" // For CMD and NAV enums
#include "mavros_msgs/WaypointClear.h"
#include "mavros_msgs/CommandBool.h"
#include "mavros_msgs/CommandTOL.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/CommandLong.h"
#include "mavros_msgs/ParamSet.h"
#include "mavros_msgs/SetMode.h"
#include "mavros_msgs/WaypointList.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/ParamGet.h"


#include "mavlink_defines.h"	// For mavlink defines not accessible in other files

#include "gcs/path.h"
#include "gcs/uploadMission.h"
#include "gcs/clearMission.h"
#include "gcs/arm.h"
#include "gcs/disarm.h"
#include "gcs/takeoff.h"
#include "gcs/land.h"
#include "gcs/startMission.h"
#include "gcs/stopMission.h"
#include "gcs/communicationStatus.h"
#include "gcs/returnToLaunch.h"
#include "gcs/loiter.h"
#include "gcs/continueMission.h"




// TODO Include mavlink to/from message types

#define NAME_AS_STRING(macro) #macro
#define VALUE_AS_STRING(macro) NAME_AS_STRING(macro)

#define DRONE_COMM_CLASS drone_comm

#define DEBUG true
#define HEARTBEAT_MAX_TIMEOUT 5 //Seconds

#define HOLD_TIME 				0.0
#define ACCEPTANCE_RADIUS		1.0

#define ROS_TAKEOFF 0
#define ROS_WAYPOINT 1
#define ROS_LAND 2

#define SUCCESS					0
#define ERROR_CLEARING_MISSION 	1
#define ERROR_PARAM_SET 		2
#define ERROR_SET_MODE			3
#define ERROR_UPLOAD_MISSION	4
#define ERROR					5

enum droneComState {
	WAIT_FOR_CONNECTION,
	SETUP,
	IDLE,
	WAIT_FOR_DEPLOY,
	ARM,
	ENABLE_MISSION_MODE,
	ENROUTE,
	ARRIVED,
	ERROR_STATE,
	INTERVENTION_RTL,
	INTERVENTION_LOITER,
	INTERVENTION_LAND_HERE,
	INTERVENTION_TERMINATE
};

// #define SETUP									0
// #define	IDLE									1
// #define WAIT_FOR_DEPLOY				3
// #define ARM										4
// #define ENABLE_MISSION_MODE		5
// #define ENROUTE								6
// #define ARRIVED								7
// #define ERROR_STATE						8

struct drone_status{
	sensor_msgs::NavSatFix gpsPosition;
	mavros_msgs::State state;
};

class DRONE_COMM_CLASS
{
public:
	DRONE_COMM_CLASS(ros::NodeHandle);
	~DRONE_COMM_CLASS();
	void checkHeartbeat();
	void control();

private:
	// Methods
	void sendSetup();
	bool setup();
	bool arm();
	bool enableMissionMode();
	bool wait_for_connection();


	// Attributes
	ros::NodeHandle nh;
	double heartbeatTimestamp;
	drone_status curDroneStatus;

	droneComState controlStatus = WAIT_FOR_CONNECTION;
	int current_waypoint = 0;
	int last_waypoint = 0;
	int number_of_waypoints = 0;
	bool deploy = false;
	bool missionReceived = false;
	bool missionCleared = false;
	bool NAV_DLL_ACT_set = false;

	int setup_trials = 0;
	int missionmode_trials = 0;
	int arm_trials = 0;

	// Service servers
	ros::ServiceServer uploadMissionService;
	bool uploadMissionCallback(gcs::uploadMission::Request &req, gcs::uploadMission::Response &res);
	bool uploadMissionCallback2(gcs::uploadMission::Request &req, gcs::uploadMission::Response &res);
	ros::ServiceServer clearMissionService;
	bool clearMissionCallback(gcs::clearMission::Request &req, gcs::clearMission::Response &res);
	ros::ServiceServer armService;
	bool armCallback(gcs::arm::Request &req, gcs::arm::Response &res);
	ros::ServiceServer disarmService;
	bool disarmCallback(gcs::disarm::Request &req, gcs::disarm::Response &res);
	ros::ServiceServer takeoffService;
	bool takeoffCallback(gcs::takeoff::Request &req, gcs::takeoff::Response &res);
	ros::ServiceServer landService;
	bool landCallback(gcs::land::Request &req, gcs::land::Response &res);
	ros::ServiceServer startMissionServer;
	bool startMissionCallback(gcs::startMission::Request &req, gcs::startMission::Response &res);
	bool startMissionCallback2(gcs::startMission::Request &req, gcs::startMission::Response &res);
	ros::ServiceServer stopMissionServer;
	bool stopMissionCallback(gcs::stopMission::Request &req, gcs::stopMission::Response &res);
	ros::ServiceServer returnToLaunchServer;
	bool returnToLaunchCallback(gcs::returnToLaunch::Request &req, gcs::returnToLaunch::Response &res);
	ros::ServiceServer loiterServer;
	bool loiterCallback(gcs::loiter::Request &req, gcs::loiter::Response &res);
	ros::ServiceServer continueMissionServer;
	bool continueMissionCallback(gcs::continueMission::Request &req, gcs::continueMission::Response &res);


	// Service client
	ros::ServiceClient uploadMissionServiceClient;
	ros::ServiceClient clearMissionServiceClient;
	ros::ServiceClient armServiceClient;
	ros::ServiceClient takeoffServiceClient;
	ros::ServiceClient landServiceClient;
	ros::ServiceClient startMissionServiceClient;
	ros::ServiceClient stopMissionServiceClient;
	ros::ServiceClient setParameterServiceClient;
	ros::ServiceClient setModeServiceClient;
	ros::ServiceClient getParameterServiceClient;

	// Published topics
	ros::Publisher communicationStatusPublisher;
	ros::Publisher dronePositionPublisher;
	ros::Publisher statePublisher;
	ros::Publisher missionStatusPublisher;
	ros::Publisher batteryStatusPublisher;

	// Subscribed topics
	ros::Subscriber stateSubscriber;
  void stateCallback(const mavros_msgs::State &msg);
	ros::Subscriber globalPositionSubscriber;
	void globalPositionCallback(const sensor_msgs::NavSatFix &msg);
	ros::Subscriber missionStatusSubscriber;
	void missionStatusSubscriberCallback(const mavros_msgs::WaypointList &msg);
	ros::Subscriber batteryStatusSubscriber;
	void batteryStatusSubscriberCallback(const mavros_msgs::BatteryStatus &msg);

};

DRONE_COMM_CLASS::DRONE_COMM_CLASS(ros::NodeHandle n)
{
	nh = n;

	// Services
	uploadMissionService = nh.advertiseService("/drone_communication/uploadMission",&DRONE_COMM_CLASS::uploadMissionCallback2, this);
 	clearMissionService = nh.advertiseService("/drone_communication/clearMission",&DRONE_COMM_CLASS::clearMissionCallback,this);
	armService = nh.advertiseService("/drone_communication/arm",&DRONE_COMM_CLASS::armCallback,this);
	disarmService = nh.advertiseService("/drone_communication/disarm",&DRONE_COMM_CLASS::disarmCallback,this);
	takeoffService = nh.advertiseService("/drone_communication/takeoff",&DRONE_COMM_CLASS::takeoffCallback,this);
	landService = nh.advertiseService("/drone_communication/land",&DRONE_COMM_CLASS::landCallback,this);
	startMissionServer = nh.advertiseService("/drone_communication/startMission",&DRONE_COMM_CLASS::startMissionCallback2,this);
	stopMissionServer = nh.advertiseService("/drone_communication/stopMission",&DRONE_COMM_CLASS::stopMissionCallback,this);
	returnToLaunchServer = nh.advertiseService("drone_communication/returnToLaunch",&DRONE_COMM_CLASS::returnToLaunchCallback,this);
	loiterServer = nh.advertiseService("drone_communication/loiter",&DRONE_COMM_CLASS::loiterCallback,this);
	continueMissionServer = nh.advertiseService("drone_communication/continueMission",&DRONE_COMM_CLASS::continueMissionCallback,this);

	// Service clients
	uploadMissionServiceClient = nh.serviceClient<mavros_msgs::WaypointPush>("/mavros1/mission/push");
	clearMissionServiceClient = nh.serviceClient<mavros_msgs::WaypointClear>("/mavros1/mission/clear");
	armServiceClient = nh.serviceClient<mavros_msgs::CommandBool>("/mavros1/cmd/arming");
	takeoffServiceClient = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros1/cmd/takeoff");
	landServiceClient = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros1/cmd/land");
	startMissionServiceClient = nh.serviceClient<mavros_msgs::CommandLong>("/mavros1/cmd/Command");
	stopMissionServiceClient = nh.serviceClient<mavros_msgs::CommandLong>("/mavros1/cmd/Command");
	setParameterServiceClient = nh.serviceClient<mavros_msgs::ParamSet>("/mavros1/param/set");
	setModeServiceClient = nh.serviceClient<mavros_msgs::SetMode>("/mavros1/set_mode");
	getParameterServiceClient = nh.serviceClient<mavros_msgs::ParamGet>("/mavros1/param/get");

	// Publishers
	communicationStatusPublisher = nh.advertise<gcs::communicationStatus>("/drone_communication/communiationStatus",1);
	dronePositionPublisher = nh.advertise<sensor_msgs::NavSatFix>("/drone_communication/globalPosition",1);
	statePublisher = nh.advertise<mavros_msgs::State>("/drone_communication/droneState",1);
	missionStatusPublisher = nh.advertise<mavros_msgs::WaypointList>("/drone_communication/missionState",1);
	batteryStatusPublisher = nh.advertise<mavros_msgs::BatteryStatus>("drone_communication/batteryStatus",1);
	// Subscribers
	stateSubscriber = nh.subscribe("/mavros1/state",1,&DRONE_COMM_CLASS::stateCallback,this);
	globalPositionSubscriber = nh.subscribe("/mavros1/global_position/global",1,&DRONE_COMM_CLASS::globalPositionCallback,this);
	missionStatusSubscriber = nh.subscribe("/mavros1/mission/waypoints",1,&DRONE_COMM_CLASS::missionStatusSubscriberCallback, this);
	batteryStatusSubscriber = nh.subscribe("/mavros1/battery",1,&DRONE_COMM_CLASS::batteryStatusSubscriberCallback,this);

	heartbeatTimestamp = ros::Time::now().toSec();
}

DRONE_COMM_CLASS::~DRONE_COMM_CLASS()
{}

void DRONE_COMM_CLASS::stateCallback(const mavros_msgs::State &msg)<commit_msg>Changed mavros1 to mavros 1 in drone_communication_node topics<commit_after>//General

//custom

//ROS
#include <ros/ros.h>
#include "mavros_msgs/State.h"
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/CommandCode.h" // For CMD and NAV enums
#include "mavros_msgs/WaypointClear.h"
#include "mavros_msgs/CommandBool.h"
#include "mavros_msgs/CommandTOL.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/CommandLong.h"
#include "mavros_msgs/ParamSet.h"
#include "mavros_msgs/SetMode.h"
#include "mavros_msgs/WaypointList.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/ParamGet.h"


#include "mavlink_defines.h"	// For mavlink defines not accessible in other files

#include "gcs/path.h"
#include "gcs/uploadMission.h"
#include "gcs/clearMission.h"
#include "gcs/arm.h"
#include "gcs/disarm.h"
#include "gcs/takeoff.h"
#include "gcs/land.h"
#include "gcs/startMission.h"
#include "gcs/stopMission.h"
#include "gcs/communicationStatus.h"
#include "gcs/returnToLaunch.h"
#include "gcs/loiter.h"
#include "gcs/continueMission.h"




// TODO Include mavlink to/from message types

#define NAME_AS_STRING(macro) #macro
#define VALUE_AS_STRING(macro) NAME_AS_STRING(macro)

#define DRONE_COMM_CLASS drone_comm

#define DEBUG true
#define HEARTBEAT_MAX_TIMEOUT 5 //Seconds

#define HOLD_TIME 				0.0
#define ACCEPTANCE_RADIUS		1.0

#define ROS_TAKEOFF 0
#define ROS_WAYPOINT 1
#define ROS_LAND 2

#define SUCCESS					0
#define ERROR_CLEARING_MISSION 	1
#define ERROR_PARAM_SET 		2
#define ERROR_SET_MODE			3
#define ERROR_UPLOAD_MISSION	4
#define ERROR					5

enum droneComState {
	WAIT_FOR_CONNECTION,
	SETUP,
	IDLE,
	WAIT_FOR_DEPLOY,
	ARM,
	ENABLE_MISSION_MODE,
	ENROUTE,
	ARRIVED,
	ERROR_STATE,
	INTERVENTION_RTL,
	INTERVENTION_LOITER,
	INTERVENTION_LAND_HERE,
	INTERVENTION_TERMINATE
};

// #define SETUP									0
// #define	IDLE									1
// #define WAIT_FOR_DEPLOY				3
// #define ARM										4
// #define ENABLE_MISSION_MODE		5
// #define ENROUTE								6
// #define ARRIVED								7
// #define ERROR_STATE						8

struct drone_status{
	sensor_msgs::NavSatFix gpsPosition;
	mavros_msgs::State state;
};

class DRONE_COMM_CLASS
{
public:
	DRONE_COMM_CLASS(ros::NodeHandle);
	~DRONE_COMM_CLASS();
	void checkHeartbeat();
	void control();

private:
	// Methods
	void sendSetup();
	bool setup();
	bool arm();
	bool enableMissionMode();
	bool wait_for_connection();


	// Attributes
	ros::NodeHandle nh;
	double heartbeatTimestamp;
	drone_status curDroneStatus;

	droneComState controlStatus = WAIT_FOR_CONNECTION;
	int current_waypoint = 0;
	int last_waypoint = 0;
	int number_of_waypoints = 0;
	bool deploy = false;
	bool missionReceived = false;
	bool missionCleared = false;
	bool NAV_DLL_ACT_set = false;

	int setup_trials = 0;
	int missionmode_trials = 0;
	int arm_trials = 0;

	// Service servers
	ros::ServiceServer uploadMissionService;
	bool uploadMissionCallback(gcs::uploadMission::Request &req, gcs::uploadMission::Response &res);
	bool uploadMissionCallback2(gcs::uploadMission::Request &req, gcs::uploadMission::Response &res);
	ros::ServiceServer clearMissionService;
	bool clearMissionCallback(gcs::clearMission::Request &req, gcs::clearMission::Response &res);
	ros::ServiceServer armService;
	bool armCallback(gcs::arm::Request &req, gcs::arm::Response &res);
	ros::ServiceServer disarmService;
	bool disarmCallback(gcs::disarm::Request &req, gcs::disarm::Response &res);
	ros::ServiceServer takeoffService;
	bool takeoffCallback(gcs::takeoff::Request &req, gcs::takeoff::Response &res);
	ros::ServiceServer landService;
	bool landCallback(gcs::land::Request &req, gcs::land::Response &res);
	ros::ServiceServer startMissionServer;
	bool startMissionCallback(gcs::startMission::Request &req, gcs::startMission::Response &res);
	bool startMissionCallback2(gcs::startMission::Request &req, gcs::startMission::Response &res);
	ros::ServiceServer stopMissionServer;
	bool stopMissionCallback(gcs::stopMission::Request &req, gcs::stopMission::Response &res);
	ros::ServiceServer returnToLaunchServer;
	bool returnToLaunchCallback(gcs::returnToLaunch::Request &req, gcs::returnToLaunch::Response &res);
	ros::ServiceServer loiterServer;
	bool loiterCallback(gcs::loiter::Request &req, gcs::loiter::Response &res);
	ros::ServiceServer continueMissionServer;
	bool continueMissionCallback(gcs::continueMission::Request &req, gcs::continueMission::Response &res);


	// Service client
	ros::ServiceClient uploadMissionServiceClient;
	ros::ServiceClient clearMissionServiceClient;
	ros::ServiceClient armServiceClient;
	ros::ServiceClient takeoffServiceClient;
	ros::ServiceClient landServiceClient;
	ros::ServiceClient startMissionServiceClient;
	ros::ServiceClient stopMissionServiceClient;
	ros::ServiceClient setParameterServiceClient;
	ros::ServiceClient setModeServiceClient;
	ros::ServiceClient getParameterServiceClient;

	// Published topics
	ros::Publisher communicationStatusPublisher;
	ros::Publisher dronePositionPublisher;
	ros::Publisher statePublisher;
	ros::Publisher missionStatusPublisher;
	ros::Publisher batteryStatusPublisher;

	// Subscribed topics
	ros::Subscriber stateSubscriber;
  void stateCallback(const mavros_msgs::State &msg);
	ros::Subscriber globalPositionSubscriber;
	void globalPositionCallback(const sensor_msgs::NavSatFix &msg);
	ros::Subscriber missionStatusSubscriber;
	void missionStatusSubscriberCallback(const mavros_msgs::WaypointList &msg);
	ros::Subscriber batteryStatusSubscriber;
	void batteryStatusSubscriberCallback(const mavros_msgs::BatteryStatus &msg);

};

DRONE_COMM_CLASS::DRONE_COMM_CLASS(ros::NodeHandle n)
{
	nh = n;

	// Services
	uploadMissionService = nh.advertiseService("/drone_communication/uploadMission",&DRONE_COMM_CLASS::uploadMissionCallback2, this);
 	clearMissionService = nh.advertiseService("/drone_communication/clearMission",&DRONE_COMM_CLASS::clearMissionCallback,this);
	armService = nh.advertiseService("/drone_communication/arm",&DRONE_COMM_CLASS::armCallback,this);
	disarmService = nh.advertiseService("/drone_communication/disarm",&DRONE_COMM_CLASS::disarmCallback,this);
	takeoffService = nh.advertiseService("/drone_communication/takeoff",&DRONE_COMM_CLASS::takeoffCallback,this);
	landService = nh.advertiseService("/drone_communication/land",&DRONE_COMM_CLASS::landCallback,this);
	startMissionServer = nh.advertiseService("/drone_communication/startMission",&DRONE_COMM_CLASS::startMissionCallback2,this);
	stopMissionServer = nh.advertiseService("/drone_communication/stopMission",&DRONE_COMM_CLASS::stopMissionCallback,this);
	returnToLaunchServer = nh.advertiseService("drone_communication/returnToLaunch",&DRONE_COMM_CLASS::returnToLaunchCallback,this);
	loiterServer = nh.advertiseService("drone_communication/loiter",&DRONE_COMM_CLASS::loiterCallback,this);
	continueMissionServer = nh.advertiseService("drone_communication/continueMission",&DRONE_COMM_CLASS::continueMissionCallback,this);

	// Service clients
	uploadMissionServiceClient = nh.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
	clearMissionServiceClient = nh.serviceClient<mavros_msgs::WaypointClear>("/mavros/mission/clear");
	armServiceClient = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
	takeoffServiceClient = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
	landServiceClient = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
	startMissionServiceClient = nh.serviceClient<mavros_msgs::CommandLong>("/mavros/cmd/Command");
	stopMissionServiceClient = nh.serviceClient<mavros_msgs::CommandLong>("/mavros/cmd/Command");
	setParameterServiceClient = nh.serviceClient<mavros_msgs::ParamSet>("/mavros/param/set");
	setModeServiceClient = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
	getParameterServiceClient = nh.serviceClient<mavros_msgs::ParamGet>("/mavros/param/get");

	// Publishers
	communicationStatusPublisher = nh.advertise<gcs::communicationStatus>("/drone_communication/communiationStatus",1);
	dronePositionPublisher = nh.advertise<sensor_msgs::NavSatFix>("/drone_communication/globalPosition",1);
	statePublisher = nh.advertise<mavros_msgs::State>("/drone_communication/droneState",1);
	missionStatusPublisher = nh.advertise<mavros_msgs::WaypointList>("/drone_communication/missionState",1);
	batteryStatusPublisher = nh.advertise<mavros_msgs::BatteryStatus>("drone_communication/batteryStatus",1);
	// Subscribers
	stateSubscriber = nh.subscribe("/mavros/state",1,&DRONE_COMM_CLASS::stateCallback,this);
	globalPositionSubscriber = nh.subscribe("/mavros/global_position/global",1,&DRONE_COMM_CLASS::globalPositionCallback,this);
	missionStatusSubscriber = nh.subscribe("/mavros/mission/waypoints",1,&DRONE_COMM_CLASS::missionStatusSubscriberCallback, this);
	batteryStatusSubscriber = nh.subscribe("/mavros/battery",1,&DRONE_COMM_CLASS::batteryStatusSubscriberCallback,this);

	heartbeatTimestamp = ros::Time::now().toSec();
}

DRONE_COMM_CLASS::~DRONE_COMM_CLASS()
{}

void DRONE_COMM_CLASS::stateCallback(const mavros_msgs::State &msg)
{'fraction_non_alphanumeric': 0.11787514540519581, 'fraction_numerical': 0.005428460643660334, 'mean_word_length': 8.277492291880781, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 35, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3206065', 'n_tokens_mistral': 6478, 'n_tokens_neox': 6091, 'n_words': 926}