diff --git a/src/buzz_utility.h b/src/buzz_utility.h deleted file mode 100644 index b6b8e0e..0000000 --- a/src/buzz_utility.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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(); - -} diff --git a/src/buzzuav_closures.h b/src/buzzuav_closures.h deleted file mode 100644 index 4937812..0000000 --- a/src/buzzuav_closures.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -//#ifndef BUZZUAV_CLOSURES_H -//#define BUZZUAV_CLOSURES_H -#include -#include -#include -#include -#include - -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 -} diff --git a/src/roscontroller.h b/src/roscontroller.h deleted file mode 100644 index d498c82..0000000 --- a/src/roscontroller.h +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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); - - - -}; - -}