Include files correction
This commit is contained in:
parent
63f2349618
commit
021d12b451
|
@ -1,44 +0,0 @@
|
||||||
#pragma once
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <buzz_utility.h>
|
|
||||||
#include <buzzuav_closures.h>
|
|
||||||
#include <buzz/buzzdebug.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <iostream>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <map>
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
namespace buzz_utility{
|
|
||||||
|
|
||||||
struct pos_struct
|
|
||||||
{
|
|
||||||
double x,y,z;
|
|
||||||
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
|
||||||
pos_struct(){}
|
|
||||||
};
|
|
||||||
|
|
||||||
typedef struct pos_struct Pos_struct ;
|
|
||||||
|
|
||||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
|
||||||
|
|
||||||
int buzz_listen(const char* type,
|
|
||||||
int msg_size);
|
|
||||||
|
|
||||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
|
||||||
|
|
||||||
void in_msg_process(uint64_t* payload);
|
|
||||||
|
|
||||||
uint64_t* out_msg_process();
|
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
|
||||||
const char* bdbg_filename, int robot_id);
|
|
||||||
void buzz_script_step();
|
|
||||||
|
|
||||||
void buzz_script_destroy();
|
|
||||||
|
|
||||||
int buzz_script_done();
|
|
||||||
|
|
||||||
}
|
|
|
@ -1,59 +0,0 @@
|
||||||
#pragma once
|
|
||||||
//#ifndef BUZZUAV_CLOSURES_H
|
|
||||||
//#define BUZZUAV_CLOSURES_H
|
|
||||||
#include <buzz/buzzvm.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <uav_utility.h>
|
|
||||||
#include <mavros_msgs/CommandCode.h>
|
|
||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
namespace buzzuav_closures{
|
|
||||||
|
|
||||||
/*
|
|
||||||
* prextern int() function in Buzz
|
|
||||||
* This function is used to print data from buzz
|
|
||||||
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
|
|
||||||
*/
|
|
||||||
int buzzros_print(buzzvm_t vm);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
|
|
||||||
* commands the UAV to go to a position supplied
|
|
||||||
*/
|
|
||||||
int buzzuav_goto(buzzvm_t vm);
|
|
||||||
/* Returns the current command from local variable*/
|
|
||||||
int getcmd();
|
|
||||||
/*Sets goto position could be used for bypassing*/
|
|
||||||
void set_goto(double pos[]);
|
|
||||||
/*sets rc requested command */
|
|
||||||
void rc_call(int rc_cmd);
|
|
||||||
/* sets the battery state */
|
|
||||||
void set_battery(float voltage,float current,float remaining);
|
|
||||||
/*retuns the current go to position */
|
|
||||||
double* getgoto();
|
|
||||||
/*
|
|
||||||
* Commands the UAV to takeoff
|
|
||||||
*/
|
|
||||||
int buzzuav_takeoff(buzzvm_t vm);
|
|
||||||
|
|
||||||
/* Commands the UAV to land
|
|
||||||
*/
|
|
||||||
int buzzuav_land(buzzvm_t vm);
|
|
||||||
|
|
||||||
/* Command the UAV to go to home location
|
|
||||||
*/
|
|
||||||
int buzzuav_gohome(buzzvm_t vm);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Updates battery information in Buzz
|
|
||||||
*/
|
|
||||||
int buzzuav_update_battery(buzzvm_t vm);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Updates IR information in Buzz
|
|
||||||
* Proximity and ground sensors to do !!!!
|
|
||||||
*/
|
|
||||||
int buzzuav_update_prox(buzzvm_t vm);
|
|
||||||
|
|
||||||
//#endif
|
|
||||||
}
|
|
|
@ -1,101 +0,0 @@
|
||||||
#pragma once
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <sensor_msgs/NavSatFix.h>
|
|
||||||
#include <mavros_msgs/GlobalPositionTarget.h>
|
|
||||||
#include <mavros_msgs/CommandCode.h>
|
|
||||||
#include <mavros_msgs/CommandInt.h>
|
|
||||||
#include <mavros_msgs/State.h>
|
|
||||||
#include <mavros_msgs/BatteryStatus.h>
|
|
||||||
#include <mavros_msgs/Mavlink.h>
|
|
||||||
#include <sensor_msgs/NavSatStatus.h>
|
|
||||||
#include <sstream>
|
|
||||||
#include <buzz/buzzasm.h>
|
|
||||||
#include <buzzuav_closures.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>
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
namespace rosbzz_node{
|
|
||||||
|
|
||||||
|
|
||||||
class roscontroller{
|
|
||||||
|
|
||||||
public:
|
|
||||||
roscontroller();
|
|
||||||
~roscontroller();
|
|
||||||
void RosControllerRun();
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
double cur_pos[3];
|
|
||||||
uint64_t payload;
|
|
||||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
|
||||||
int timer_step=0;
|
|
||||||
int robot_id=0;
|
|
||||||
int oldcmdID=0;
|
|
||||||
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
|
||||||
bool rcclient;
|
|
||||||
ros::ServiceClient mav_client;
|
|
||||||
ros::Publisher payload_pub;
|
|
||||||
ros::ServiceServer service;
|
|
||||||
ros::Subscriber current_position_sub;
|
|
||||||
ros::Subscriber battery_sub;
|
|
||||||
ros::Subscriber payload_sub;
|
|
||||||
/*Create node Handler*/
|
|
||||||
ros::NodeHandle n_c;
|
|
||||||
/*Commands for flight controller*/
|
|
||||||
mavros_msgs::CommandInt cmd_srv;
|
|
||||||
/* The bytecode filename */
|
|
||||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
|
||||||
/* The debugging information file name */
|
|
||||||
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
|
||||||
|
|
||||||
void Initialize_pub_sub();
|
|
||||||
|
|
||||||
/*Obtain data from ros parameter server*/
|
|
||||||
void Rosparameters_get();
|
|
||||||
|
|
||||||
void Compile_bzz();
|
|
||||||
|
|
||||||
/*Prepare messages and publish*/
|
|
||||||
void prepare_msg_and_publish();
|
|
||||||
|
|
||||||
|
|
||||||
/*Refresh neighbours Position for every ten step*/
|
|
||||||
void maintain_pos(int tim_step);
|
|
||||||
|
|
||||||
/*Maintain neighbours position*/
|
|
||||||
void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
|
|
||||||
|
|
||||||
/*Set the current position of the robot callback*/
|
|
||||||
void set_cur_pos(double latitude,
|
|
||||||
double longitude,
|
|
||||||
double altitude);
|
|
||||||
/*convert from catresian to spherical coordinate system callback */
|
|
||||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []);
|
|
||||||
|
|
||||||
/*battery status callback*/
|
|
||||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
|
||||||
|
|
||||||
/*current position callback*/
|
|
||||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
|
||||||
|
|
||||||
/*payload callback callback*/
|
|
||||||
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
|
||||||
|
|
||||||
/* RC commands service */
|
|
||||||
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
|
||||||
mavros_msgs::CommandInt::Response &res);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
Loading…
Reference in New Issue