2016-10-10 21:22:17 -03:00
|
|
|
#pragma once
|
2017-02-10 15:18:44 -04:00
|
|
|
#include <ros/ros.h>
|
|
|
|
#include <sensor_msgs/NavSatFix.h>
|
2017-02-20 00:30:45 -04:00
|
|
|
#include <std_msgs/UInt8.h>
|
2016-10-10 21:22:17 -03:00
|
|
|
#include "mavros_msgs/GlobalPositionTarget.h"
|
|
|
|
#include "mavros_msgs/CommandCode.h"
|
2017-02-16 21:58:21 -04:00
|
|
|
#include "mavros_msgs/CommandLong.h"
|
2017-02-17 17:35:08 -04:00
|
|
|
#include "mavros_msgs/CommandBool.h"
|
2016-11-18 15:05:31 -04:00
|
|
|
#include "mavros_msgs/ExtendedState.h"
|
2017-02-17 17:35:08 -04:00
|
|
|
#include "mavros_msgs/SetMode.h"
|
2016-10-10 21:22:17 -03:00
|
|
|
#include "mavros_msgs/State.h"
|
|
|
|
#include "mavros_msgs/BatteryStatus.h"
|
|
|
|
#include "mavros_msgs/Mavlink.h"
|
2017-03-13 19:11:01 -03:00
|
|
|
#include "mavros_msgs/PositionTarget.h"
|
2017-02-23 22:10:49 -04:00
|
|
|
#include "sensor_msgs/NavSatStatus.h"
|
2017-02-21 17:09:16 -04:00
|
|
|
#include "mavros_msgs/WaypointPush.h"
|
|
|
|
#include "mavros_msgs/Waypoint.h"
|
2017-03-14 12:09:01 -03:00
|
|
|
#include "mavros_msgs/PositionTarget.h"
|
2017-03-29 15:57:31 -03:00
|
|
|
#include "mavros_msgs/StreamRate.h"
|
2017-04-03 16:41:03 -03:00
|
|
|
#include "mavros_msgs/ParamGet.h"
|
2017-04-10 21:23:04 -03:00
|
|
|
#include "geometry_msgs/PoseStamped.h"
|
2017-04-03 20:50:09 -03:00
|
|
|
#include "std_msgs/Float64.h"
|
2017-02-10 15:18:44 -04:00
|
|
|
#include <sensor_msgs/LaserScan.h>
|
2017-01-09 18:15:33 -04:00
|
|
|
#include <rosbuzz/neigh_pos.h>
|
2016-10-10 21:22:17 -03:00
|
|
|
#include <sstream>
|
|
|
|
#include <buzz/buzzasm.h>
|
|
|
|
#include "buzz_utility.h"
|
|
|
|
#include "uav_utility.h"
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <math.h>
|
|
|
|
#include <signal.h>
|
|
|
|
#include <ostream>
|
|
|
|
#include <map>
|
2016-12-24 21:59:00 -04:00
|
|
|
#include "buzzuav_closures.h"
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-01-27 23:00:11 -04:00
|
|
|
#define UPDATER_MESSAGE_CONSTANT 987654321
|
2017-01-29 04:03:29 -04:00
|
|
|
#define XBEE_MESSAGE_CONSTANT 586782343
|
|
|
|
#define XBEE_STOP_TRANSMISSION 4355356352
|
2017-04-12 20:23:53 -03:00
|
|
|
#define TIMEOUT 60
|
2017-05-17 17:23:51 -03:00
|
|
|
#define BUZZRATE 50
|
2017-01-27 23:00:11 -04:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
using namespace std;
|
|
|
|
|
|
|
|
namespace rosbzz_node{
|
2016-11-28 21:35:33 -04:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
class roscontroller{
|
|
|
|
|
|
|
|
public:
|
2017-04-24 18:55:54 -03:00
|
|
|
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
2016-10-10 21:22:17 -03:00
|
|
|
~roscontroller();
|
|
|
|
//void RosControllerInit();
|
|
|
|
void RosControllerRun();
|
2016-11-14 18:12:12 -04:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
private:
|
2017-05-11 22:17:50 -03:00
|
|
|
struct num_robot_count
|
2017-04-02 23:46:48 -03:00
|
|
|
{
|
|
|
|
uint8_t history[10];
|
|
|
|
uint8_t index=0;
|
|
|
|
uint8_t current=0;
|
|
|
|
num_robot_count(){}
|
|
|
|
|
|
|
|
}; typedef struct num_robot_count Num_robot_count ;
|
|
|
|
|
2017-05-11 22:17:50 -03:00
|
|
|
struct gps
|
|
|
|
{
|
|
|
|
double longitude=0.0;
|
|
|
|
double latitude=0.0;
|
|
|
|
float altitude=0.0;
|
|
|
|
}; typedef struct gps GPS ;
|
|
|
|
|
|
|
|
GPS target, home, cur_pos;
|
2017-04-03 20:50:09 -03:00
|
|
|
double cur_rel_altitude;
|
2017-05-11 22:17:50 -03:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
uint64_t payload;
|
|
|
|
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
2017-01-11 14:14:05 -04:00
|
|
|
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
2017-01-13 16:03:21 -04:00
|
|
|
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
2016-10-10 21:22:17 -03:00
|
|
|
int timer_step=0;
|
|
|
|
int robot_id=0;
|
2017-04-15 19:17:19 -03:00
|
|
|
std::string robot_name = "";
|
2016-12-24 21:59:00 -04:00
|
|
|
//int oldcmdID=0;
|
2016-10-10 21:22:17 -03:00
|
|
|
int rc_cmd;
|
2017-04-12 20:23:53 -03:00
|
|
|
float fcu_timeout;
|
2017-02-23 12:49:10 -04:00
|
|
|
int armstate;
|
2017-01-08 14:17:13 -04:00
|
|
|
int barrier;
|
2017-02-19 20:22:47 -04:00
|
|
|
int message_number=0;
|
2017-04-05 18:58:03 -03:00
|
|
|
uint8_t no_of_robots=0;
|
2017-04-03 00:41:44 -03:00
|
|
|
/*tmp to be corrected*/
|
2017-04-05 18:58:03 -03:00
|
|
|
uint8_t no_cnt=0;
|
|
|
|
uint8_t old_val=0;
|
2017-04-05 19:11:27 -03:00
|
|
|
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
2017-04-03 09:45:38 -03:00
|
|
|
std::string stream_client_name;
|
2017-04-03 20:50:09 -03:00
|
|
|
std::string relative_altitude_sub_name;
|
2017-04-10 21:23:04 -03:00
|
|
|
std::string setpoint_nonraw;
|
2016-10-10 21:22:17 -03:00
|
|
|
bool rcclient;
|
2017-04-21 00:23:36 -03:00
|
|
|
bool xbeeplugged = false;
|
2017-01-29 04:03:29 -04:00
|
|
|
bool multi_msg;
|
2017-04-02 23:46:48 -03:00
|
|
|
Num_robot_count count_robots;
|
2016-10-10 21:22:17 -03:00
|
|
|
ros::ServiceClient mav_client;
|
2017-04-02 15:19:45 -03:00
|
|
|
ros::ServiceClient xbeestatus_srv;
|
2016-10-10 21:22:17 -03:00
|
|
|
ros::Publisher payload_pub;
|
2017-01-09 18:15:33 -04:00
|
|
|
ros::Publisher neigh_pos_pub;
|
2017-04-10 21:23:04 -03:00
|
|
|
ros::Publisher localsetpoint_nonraw_pub;
|
2016-10-10 21:22:17 -03:00
|
|
|
ros::ServiceServer service;
|
|
|
|
ros::Subscriber current_position_sub;
|
2017-04-24 15:20:59 -03:00
|
|
|
ros::Subscriber users_sub;
|
2016-10-10 21:22:17 -03:00
|
|
|
ros::Subscriber battery_sub;
|
|
|
|
ros::Subscriber payload_sub;
|
2016-11-18 15:05:31 -04:00
|
|
|
ros::Subscriber flight_status_sub;
|
2017-02-10 15:18:44 -04:00
|
|
|
ros::Subscriber obstacle_sub;
|
2017-02-20 00:30:45 -04:00
|
|
|
ros::Subscriber Robot_id_sub;
|
2017-04-03 20:50:09 -03:00
|
|
|
ros::Subscriber relative_altitude_sub;
|
2017-05-12 11:46:39 -03:00
|
|
|
|
2017-05-12 12:04:42 -03:00
|
|
|
std::string local_pos_sub_name;
|
2017-05-12 11:46:39 -03:00
|
|
|
ros::Subscriber local_pos_sub;
|
|
|
|
double local_pos_new[3];
|
|
|
|
|
|
|
|
|
2017-03-29 18:32:56 -03:00
|
|
|
ros::ServiceClient stream_client;
|
2017-03-14 12:09:01 -03:00
|
|
|
|
2017-04-10 21:23:04 -03:00
|
|
|
int setpoint_counter;
|
|
|
|
double my_x = 0, my_y = 0;
|
2017-05-17 14:54:32 -03:00
|
|
|
|
|
|
|
std::ofstream log;
|
2017-04-10 21:23:04 -03:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*Commands for flight controller*/
|
2017-02-16 21:58:21 -04:00
|
|
|
//mavros_msgs::CommandInt cmd_srv;
|
|
|
|
mavros_msgs::CommandLong cmd_srv;
|
2017-02-15 19:08:41 -04:00
|
|
|
std::vector<std::string> m_sMySubscriptions;
|
|
|
|
std::map<std::string, std::string> m_smTopic_infos;
|
|
|
|
|
2017-02-17 17:35:08 -04:00
|
|
|
mavros_msgs::CommandBool m_cmdBool;
|
|
|
|
ros::ServiceClient arm_client;
|
|
|
|
|
|
|
|
mavros_msgs::SetMode m_cmdSetMode;
|
|
|
|
ros::ServiceClient mode_client;
|
2017-03-14 00:08:18 -03:00
|
|
|
|
|
|
|
/*Initialize publisher and subscriber, done in the constructor*/
|
2017-04-24 18:55:54 -03:00
|
|
|
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
2017-02-21 17:09:16 -04:00
|
|
|
|
2017-03-01 16:48:16 -04:00
|
|
|
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*Obtain data from ros parameter server*/
|
2017-04-24 18:55:54 -03:00
|
|
|
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2016-11-18 15:05:31 -04:00
|
|
|
/*compiles buzz script from the specified .bzz file*/
|
2017-05-09 14:07:11 -03:00
|
|
|
std::string Compile_bzz(std::string bzzfile_name);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*Flight controller service call*/
|
2017-04-12 20:23:53 -03:00
|
|
|
void flight_controller_service_call();
|
2017-03-14 00:08:18 -03:00
|
|
|
|
|
|
|
/*Neighbours pos publisher*/
|
|
|
|
void neighbours_pos_publisher();
|
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*Prepare messages and publish*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void prepare_msg_and_publish();
|
2016-10-10 21:22:17 -03:00
|
|
|
|
|
|
|
|
|
|
|
/*Refresh neighbours Position for every ten step*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void maintain_pos(int tim_step);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-01-11 14:14:05 -04:00
|
|
|
/*Puts neighbours position inside neigbours_pos_map*/
|
|
|
|
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
|
|
|
|
|
|
|
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
|
|
|
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-05-11 22:17:50 -03:00
|
|
|
/*Set the current position of the robot callback*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void set_cur_pos(double latitude,
|
2016-10-10 21:22:17 -03:00
|
|
|
double longitude,
|
|
|
|
double altitude);
|
2017-01-27 00:19:16 -04:00
|
|
|
/*convert from spherical to cartesian coordinate system callback */
|
2017-05-11 22:17:50 -03:00
|
|
|
void gps_rb(GPS nei_pos, double out[]);
|
|
|
|
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
|
|
|
void gps_ned_home(float &ned_x, float &ned_y);
|
|
|
|
void gps_convert_ned(float &ned_x, float &ned_y,
|
|
|
|
double gps_t_lon, double gps_t_lat,
|
|
|
|
double gps_r_lon, double gps_r_lat);
|
2017-01-30 22:58:48 -04:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*battery status callback*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
|
|
|
|
2017-02-15 19:08:41 -04:00
|
|
|
/*flight extended status callback*/
|
|
|
|
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
|
|
|
|
2016-11-18 15:05:31 -04:00
|
|
|
/*flight status callback*/
|
2017-02-15 19:08:41 -04:00
|
|
|
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
2016-11-18 15:05:31 -04:00
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*current position callback*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
2017-05-11 22:17:50 -03:00
|
|
|
void users_pos(const rosbuzz::neigh_pos msg);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-05-12 11:46:39 -03:00
|
|
|
|
2017-04-03 20:50:09 -03:00
|
|
|
/*current relative altitude callback*/
|
|
|
|
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
|
|
|
|
2016-10-10 21:22:17 -03:00
|
|
|
/*payload callback callback*/
|
2016-11-18 15:05:31 -04:00
|
|
|
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
|
|
|
/* RC commands service */
|
2017-02-16 21:58:21 -04:00
|
|
|
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-02-20 00:30:45 -04:00
|
|
|
/*robot id sub callback*/
|
|
|
|
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*Obstacle distance table callback*/
|
2017-02-10 15:18:44 -04:00
|
|
|
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
2016-10-10 21:22:17 -03:00
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*Get publisher and subscriber from YML file*/
|
2017-04-24 18:55:54 -03:00
|
|
|
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
2017-02-15 19:08:41 -04:00
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*Arm/disarm method that can be called from buzz*/
|
2017-02-17 17:35:08 -04:00
|
|
|
void Arm();
|
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*set mode like guided for solo*/
|
2017-03-15 12:54:42 -03:00
|
|
|
void SetMode(std::string mode, int delay_miliseconds);
|
2017-02-17 17:35:08 -04:00
|
|
|
|
2017-03-14 00:08:18 -03:00
|
|
|
/*Robot independent subscribers*/
|
2017-04-24 18:55:54 -03:00
|
|
|
void Subscribe(ros::NodeHandle& n_c);
|
2017-02-20 18:59:35 -04:00
|
|
|
|
2017-05-12 11:46:39 -03:00
|
|
|
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
|
|
|
|
2017-03-15 12:54:42 -03:00
|
|
|
//void WaypointMissionSetup(float lat, float lng, float alt);
|
2017-02-21 17:09:16 -04:00
|
|
|
|
2017-02-23 22:10:49 -04:00
|
|
|
void fc_command_setup();
|
2017-03-14 12:09:01 -03:00
|
|
|
|
2017-03-15 12:54:42 -03:00
|
|
|
void SetLocalPosition(float x, float y, float z, float yaw);
|
2017-03-29 15:57:31 -03:00
|
|
|
|
2017-04-10 21:23:04 -03:00
|
|
|
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
|
|
|
|
|
2017-03-29 15:57:31 -03:00
|
|
|
void SetStreamRate(int id, int rate, int on_off);
|
2017-04-02 23:46:48 -03:00
|
|
|
|
2017-04-03 00:41:44 -03:00
|
|
|
void get_number_of_robots();
|
2017-04-05 18:02:35 -03:00
|
|
|
void GetRobotId();
|
2016-10-10 21:22:17 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
}
|