commit
187e05aca8
|
@ -54,7 +54,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
|
|||
src/buzzuav_closures.cpp
|
||||
src/uav_utility.cpp
|
||||
src/buzz_update.cpp)
|
||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
|
||||
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
|
||||
|
||||
# Executables and libraries for installation to do
|
||||
|
|
|
@ -30,6 +30,8 @@ int buzz_listen(const char* type,
|
|||
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
||||
|
||||
int buzz_update_users_stigmergy(buzzobj_t tbl);
|
||||
|
||||
void in_msg_process(uint64_t* payload);
|
||||
|
||||
uint64_t* out_msg_process();
|
||||
|
@ -49,7 +51,7 @@ int buzz_script_done();
|
|||
|
||||
int update_step_test();
|
||||
|
||||
uint16_t get_robotid();
|
||||
int get_robotid();
|
||||
|
||||
buzzvm_t get_vm();
|
||||
|
||||
|
|
|
@ -46,6 +46,7 @@ void rc_call(int rc_cmd);
|
|||
void set_battery(float voltage,float current,float remaining);
|
||||
/* sets current position */
|
||||
void set_currentpos(double latitude, double longitude, double altitude);
|
||||
void set_userspos(double latitude, double longitude, double altitude);
|
||||
/*retuns the current go to position */
|
||||
double* getgoto();
|
||||
/* updates flight status*/
|
||||
|
@ -82,7 +83,7 @@ int buzzuav_update_battery(buzzvm_t vm);
|
|||
* Updates current position in Buzz
|
||||
*/
|
||||
int buzzuav_update_currentpos(buzzvm_t vm);
|
||||
|
||||
buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
|
||||
|
||||
/*
|
||||
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
|
||||
|
|
|
@ -46,7 +46,7 @@ namespace rosbzz_node{
|
|||
class roscontroller{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle n_c);
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
@ -61,6 +61,14 @@ private:
|
|||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
|
||||
// WGS84 constants
|
||||
double equatorial_radius = 6378137.0;
|
||||
double flattening = 1.0/298.257223563;
|
||||
double excentrity2 = 2*flattening - flattening*flattening;
|
||||
// default reference position
|
||||
double DEFAULT_REFERENCE_LATITUDE = 45.457817;
|
||||
double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
|
||||
|
||||
double cur_pos[3];
|
||||
double home[3];
|
||||
double cur_rel_altitude;
|
||||
|
@ -70,6 +78,7 @@ private:
|
|||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
//int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
|
@ -85,15 +94,17 @@ private:
|
|||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
|
@ -118,12 +129,12 @@ private:
|
|||
ros::ServiceClient mode_client;
|
||||
|
||||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle n_c);
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
void Compile_bzz();
|
||||
|
@ -166,6 +177,7 @@ private:
|
|||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
@ -183,7 +195,7 @@ private:
|
|||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
|
||||
/*Get publisher and subscriber from YML file*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle node_handle);
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
|
||||
/*Arm/disarm method that can be called from buzz*/
|
||||
void Arm();
|
||||
|
@ -192,7 +204,7 @@ private:
|
|||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle n_c);
|
||||
void Subscribe(ros::NodeHandle& n_c);
|
||||
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
|
|
|
@ -1,13 +1,13 @@
|
|||
topics:
|
||||
gps : /global_position
|
||||
battery : /power_status
|
||||
status : /flight_status
|
||||
fcclient : /dji_mavcmd
|
||||
setpoint : /setpoint_position/local
|
||||
armclient: /dji_mavarm
|
||||
modeclient: /dji_mavmode
|
||||
altitude: /rel_alt
|
||||
stream: /set_stream_rate
|
||||
gps : global_position
|
||||
battery : power_status
|
||||
status : flight_status
|
||||
fcclient : dji_mavcmd
|
||||
setpoint : setpoint_position/local
|
||||
armclient: dji_mavarm
|
||||
modeclient: dji_mavmode
|
||||
altitude: rel_alt
|
||||
stream: set_stream_rate
|
||||
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
|
|
|
@ -1,13 +1,13 @@
|
|||
topics:
|
||||
gps : /mavros/global_position/global
|
||||
battery : /mavros/battery
|
||||
status : /mavros/state
|
||||
fcclient: /mavros/cmd/command
|
||||
setpoint: /mavros/setpoint_position/local
|
||||
armclient: /mavros/cmd/arming
|
||||
modeclient: /mavros/set_mode
|
||||
stream: /mavros/set_stream_rate
|
||||
altitude: /mavros/global_position/rel_alt
|
||||
gps : mavros/global_position/global
|
||||
battery : mavros/battery
|
||||
status : mavros/state
|
||||
fcclient: mavros/cmd/command
|
||||
setpoint: mavros/setpoint_position/local
|
||||
armclient: mavros/cmd/arming
|
||||
modeclient: mavros/set_mode
|
||||
stream: mavros/set_stream_rate
|
||||
altitude: mavros/global_position/rel_alt
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
# for SITL Solo
|
||||
|
|
|
@ -20,6 +20,8 @@ namespace buzz_utility{
|
|||
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
|
||||
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||
static int Robot_id = 0;
|
||||
buzzobj_t usersvstig;
|
||||
buzzobj_t key;
|
||||
|
||||
/****************************************/
|
||||
/*adds neighbours position*/
|
||||
|
@ -51,6 +53,9 @@ namespace buzz_utility{
|
|||
return out;
|
||||
}
|
||||
|
||||
int get_robotid() {
|
||||
return Robot_id;
|
||||
}
|
||||
/***************************************************/
|
||||
/*Appends obtained messages to buzz in message Queue*/
|
||||
/***************************************************/
|
||||
|
@ -252,14 +257,6 @@ namespace buzz_utility{
|
|||
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id) {
|
||||
//cout<<"bo file name"<<bo_filename;
|
||||
/* Get hostname */
|
||||
//char hstnm[30];
|
||||
//char* hstnm ="M1003";
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Mnn */
|
||||
//int id = strtol(hstnm+4, NULL, 10);
|
||||
cout << " Robot ID: " <<robot_id<< endl;
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
|
@ -286,55 +283,6 @@ namespace buzz_utility{
|
|||
return 0;
|
||||
}
|
||||
fclose(fd);
|
||||
/* Read debug information */
|
||||
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||
// buzzvm_destroy(&VM);
|
||||
// buzzdebug_destroy(&DBG_INFO);
|
||||
// perror(bdbg_filename);
|
||||
// return 0;
|
||||
//}
|
||||
/* Set byte code */
|
||||
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
// buzzvm_destroy(&VM);
|
||||
// buzzdebug_destroy(&DBG_INFO);
|
||||
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
|
||||
// return 0;
|
||||
//}
|
||||
/* Register hook functions */
|
||||
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
|
||||
return 0;
|
||||
}*/
|
||||
/* Save bytecode file name */
|
||||
//BO_FNAME = strdup(bo_filename);
|
||||
/* Execute the global part of the script */
|
||||
//buzzvm_execute_script(VM);
|
||||
/* Call the Init() function */
|
||||
//buzzvm_function_call(VM, "init", 0);
|
||||
/* All OK */
|
||||
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Sets a new update */
|
||||
/****************************************/
|
||||
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
||||
/* Get hostname */
|
||||
//char hstnm[30];
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Mnn */
|
||||
//int id = strtol(hstnm + 4, NULL, 10);
|
||||
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(Robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
||||
/* Read debug information */
|
||||
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
@ -343,13 +291,95 @@ namespace buzz_utility{
|
|||
return 0;
|
||||
}
|
||||
/* Set byte code */
|
||||
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
|
||||
return 0;
|
||||
}
|
||||
/* Register hook functions */
|
||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
buzzvm_dup(VM);
|
||||
// usersvstig = stigmergy.create(123)
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
|
||||
// value of the stigmergy id
|
||||
buzzvm_pushi(VM, 5);
|
||||
// get the stigmergy table from the global scope
|
||||
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
|
||||
// buzzvm_gload(VM);
|
||||
// get the create method from the stigmergy table
|
||||
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
|
||||
// buzzvm_tget(VM);
|
||||
// call the stigmergy.create() method
|
||||
// buzzvm_closure_call(VM, 1);
|
||||
// now the stigmergy is at the top of the stack - save it for future reference
|
||||
// usersvstig = buzzvm_stack_at(VM, 0);
|
||||
//buzzvm_pop(VM);
|
||||
// assign the virtual stigmergy to the global symbol v
|
||||
// create also a global variable for it, so the garbage collector does not remove it
|
||||
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
|
||||
//buzzvm_push(VM, usersvstig);
|
||||
buzzvm_gstore(VM);
|
||||
|
||||
/* Save bytecode file name */
|
||||
BO_FNAME = strdup(bo_filename);
|
||||
/* Execute the global part of the script */
|
||||
buzzvm_execute_script(VM);
|
||||
/* Call the Init() function */
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
/* All OK */
|
||||
|
||||
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
}
|
||||
|
||||
int buzz_update_users_stigmergy(buzzobj_t tbl){
|
||||
// push the key (here it's an int with value 46)
|
||||
buzzvm_pushi(VM, 46);
|
||||
// push the table
|
||||
buzzvm_push(VM, tbl);
|
||||
// the stigmergy is stored in the vstig variable
|
||||
// let's push it on the stack
|
||||
buzzvm_push(VM, usersvstig);
|
||||
// get the put method from myvstig
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||
buzzvm_tget(VM);
|
||||
// call the vstig.put() method
|
||||
buzzvm_closure_call(VM, 2);
|
||||
return 1;
|
||||
}
|
||||
/****************************************/
|
||||
/*Sets a new update */
|
||||
/****************************************/
|
||||
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
||||
/* // Reset the Buzz VM
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(Robot_id);
|
||||
// Get rid of debug info
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
||||
// Read debug information
|
||||
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
perror(bdbg_filename);
|
||||
return 0;
|
||||
}
|
||||
// Set byte code
|
||||
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||
return 0;
|
||||
}
|
||||
/* Register hook functions */
|
||||
// Register hook functions
|
||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
|
@ -357,46 +387,40 @@ namespace buzz_utility{
|
|||
return 0;
|
||||
}
|
||||
|
||||
/* Execute the global part of the script */
|
||||
// Execute the global part of the script
|
||||
buzzvm_execute_script(VM);
|
||||
/* Call the Init() function */
|
||||
// Call the Init() function
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
/* All OK */
|
||||
return 1;
|
||||
// All OK
|
||||
*/ return 1;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Performs a initialization test */
|
||||
/****************************************/
|
||||
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
||||
/* Get hostname */
|
||||
//char hstnm[30];
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Mnn */
|
||||
//int id = strtol(hstnm + 4, NULL, 10);
|
||||
/* Reset the Buzz VM */
|
||||
/* // Reset the Buzz VM
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(Robot_id);
|
||||
/* Get rid of debug info */
|
||||
// Get rid of debug info
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
||||
/* Read debug information */
|
||||
// Read debug information
|
||||
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
perror(bdbg_filename);
|
||||
return 0;
|
||||
}
|
||||
/* Set byte code */
|
||||
// Set byte code
|
||||
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||
return 0;
|
||||
}
|
||||
/* Register hook functions */
|
||||
// Register hook functions
|
||||
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
|
@ -404,12 +428,12 @@ namespace buzz_utility{
|
|||
return 0;
|
||||
}
|
||||
|
||||
/* Execute the global part of the script */
|
||||
// Execute the global part of the script
|
||||
buzzvm_execute_script(VM);
|
||||
/* Call the Init() function */
|
||||
// Call the Init() function
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
/* All OK */
|
||||
return 1;
|
||||
// All OK
|
||||
*/ return 1;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
@ -425,14 +449,15 @@ namespace buzz_utility{
|
|||
void check_swarm_members(const void* key, void* data, void* params) {
|
||||
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
|
||||
int* status = (int*)params;
|
||||
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
|
||||
if(*status == 3) return;
|
||||
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
|
||||
if(buzzdarray_size(e->swarms) != 1) {
|
||||
fprintf(stderr, "Swarm list size is not 1\n");
|
||||
*status = 3;
|
||||
}
|
||||
else {
|
||||
int sid = 1;
|
||||
if(!buzzdict_isempty(VM->swarms)) {
|
||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
||||
|
@ -441,6 +466,8 @@ namespace buzz_utility{
|
|||
*status = 3;
|
||||
return;
|
||||
}
|
||||
}
|
||||
if(buzzdict_size(VM->swarms)>1) {
|
||||
sid = 2;
|
||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||
|
@ -452,6 +479,7 @@ namespace buzz_utility{
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*Step through the buzz script*/
|
||||
|
||||
void buzz_script_step() {
|
||||
|
@ -461,7 +489,12 @@ namespace buzz_utility{
|
|||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
|
||||
//buzz_update_users_stigmergy(tbl);
|
||||
|
||||
|
||||
/*
|
||||
* Call Buzz step() function
|
||||
*/
|
||||
|
@ -471,21 +504,16 @@ namespace buzz_utility{
|
|||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
}
|
||||
//buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
|
||||
//usleep(10000);
|
||||
/*Print swarm*/
|
||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
|
||||
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
|
||||
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
|
||||
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
|
||||
|
||||
/* Check swarm state -- SOMETHING CRASHING HERE!! */
|
||||
int status = 1;
|
||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||
/* if(status == 1 &&
|
||||
buzzdict_size(VM->swarmmembers) < 9)
|
||||
status = 2;*/
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
|
||||
buzzvm_pushi(VM, status);
|
||||
buzzvm_gstore(VM);
|
||||
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
|
||||
// int status = 1;
|
||||
// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
|
@ -19,6 +19,7 @@ namespace buzzuav_closures{
|
|||
static float batt[3];
|
||||
static float obst[5]={0,0,0,0,0};
|
||||
static double cur_pos[3];
|
||||
static double users_pos[3];
|
||||
static uint8_t status;
|
||||
static int cur_cmd = 0;
|
||||
static int rc_cmd=0;
|
||||
|
@ -30,6 +31,7 @@ namespace buzzuav_closures{
|
|||
int buzzros_print(buzzvm_t vm) {
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
buzzvm_lload(vm, i);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
|
@ -244,6 +246,11 @@ namespace buzzuav_closures{
|
|||
cur_pos[1]=longitude;
|
||||
cur_pos[2]=altitude;
|
||||
}
|
||||
void set_userspos(double latitude, double longitude, double altitude){
|
||||
users_pos[0]=latitude;
|
||||
users_pos[1]=longitude;
|
||||
users_pos[2]=altitude;
|
||||
}
|
||||
/****************************************/
|
||||
int buzzuav_update_currentpos(buzzvm_t vm) {
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
|
||||
|
@ -263,6 +270,26 @@ namespace buzzuav_closures{
|
|||
buzzvm_gstore(vm);
|
||||
return vm->state;
|
||||
}
|
||||
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
|
||||
buzzvm_pushf(vm, users_pos[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
|
||||
buzzvm_pushf(vm, users_pos[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
|
||||
buzzvm_pushf(vm, users_pos[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
|
||||
return buzzvm_stack_at(vm, 0);
|
||||
//return vm->state;
|
||||
}
|
||||
|
||||
void flight_status_update(uint8_t state){
|
||||
status=state;
|
||||
|
|
|
@ -16,8 +16,9 @@ int main(int argc, char **argv)
|
|||
{
|
||||
/*Initialize rosBuzz node*/
|
||||
ros::init(argc, argv, "rosBuzz");
|
||||
ros::NodeHandle n_c("~");
|
||||
rosbzz_node::roscontroller RosController(n_c);
|
||||
ros::NodeHandle nh_priv("~");
|
||||
ros::NodeHandle nh;
|
||||
rosbzz_node::roscontroller RosController(nh, nh_priv);
|
||||
/*Register ros controller object inside buzz*/
|
||||
//buzzuav_closures::set_ros_controller_ptr(&RosController);
|
||||
RosController.RosControllerRun();
|
||||
|
|
|
@ -5,11 +5,11 @@ namespace rosbzz_node{
|
|||
/*---------------
|
||||
/Constructor
|
||||
---------------*/
|
||||
roscontroller::roscontroller(ros::NodeHandle n_c)
|
||||
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
|
||||
{
|
||||
ROS_INFO("Buzz_node");
|
||||
/*Obtain parameters from ros parameter server*/
|
||||
Rosparameters_get(n_c);
|
||||
Rosparameters_get(n_c_priv);
|
||||
/*Initialize publishers, subscribers and client*/
|
||||
Initialize_pub_sub(n_c);
|
||||
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
||||
|
@ -23,12 +23,13 @@ namespace rosbzz_node{
|
|||
// set stream rate - wait for the FC to be started
|
||||
SetStreamRate(0, 10, 1);
|
||||
/// Get Robot Id - wait for Xbee to be started
|
||||
if(xbeeplugged)
|
||||
GetRobotId();
|
||||
else
|
||||
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
|
||||
setpoint_counter = 0;
|
||||
fcu_timeout = TIMEOUT;
|
||||
home[0] = 0.0;
|
||||
home[1] = 0.0;
|
||||
home[2] = 0.0;
|
||||
home[0]=0.0;home[1]=0.0;home[2]=0.0;
|
||||
}
|
||||
|
||||
/*---------------------
|
||||
|
@ -67,7 +68,10 @@ namespace rosbzz_node{
|
|||
/* Set the Buzz bytecode */
|
||||
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
init_update_monitor(bcfname.c_str(),stand_by.c_str());
|
||||
//init_update_monitor(bcfname.c_str(),stand_by.c_str());
|
||||
///////////////////////////////////////////////////////
|
||||
// MAIN LOOP
|
||||
//////////////////////////////////////////////////////
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
/*Update neighbors position inside Buzz*/
|
||||
|
@ -75,7 +79,7 @@ namespace rosbzz_node{
|
|||
/*Neighbours of the robot published with id in respective topic*/
|
||||
neighbours_pos_publisher();
|
||||
/*Check updater state and step code*/
|
||||
update_routine(bcfname.c_str(), dbgfname.c_str());
|
||||
//update_routine(bcfname.c_str(), dbgfname.c_str());
|
||||
/*Step buzz script */
|
||||
buzz_utility::buzz_script_step();
|
||||
/*Prepare messages and publish them in respective topic*/
|
||||
|
@ -83,10 +87,10 @@ namespace rosbzz_node{
|
|||
/*call flight controler service to set command long*/
|
||||
flight_controller_service_call();
|
||||
/*Set multi message available after update*/
|
||||
if(get_update_status()){
|
||||
/*if(get_update_status()){
|
||||
set_read_update_status();
|
||||
multi_msg=true;
|
||||
}
|
||||
}*/
|
||||
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
|
||||
//no_of_robots=get_number_of_robots();
|
||||
get_number_of_robots();
|
||||
|
@ -117,7 +121,7 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/ Get all required parameters from the ROS launch file
|
||||
/-------------------------------------------------------*/
|
||||
void roscontroller::Rosparameters_get(ros::NodeHandle n_c){
|
||||
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c){
|
||||
|
||||
/*Obtain .bzz file name from parameter server*/
|
||||
if(n_c.getParam("bzzfile_name", bzzfile_name));
|
||||
|
@ -126,11 +130,8 @@ namespace rosbzz_node{
|
|||
if(n_c.getParam("rcclient", rcclient)){
|
||||
if(rcclient==true){
|
||||
/*Service*/
|
||||
if(n_c.getParam("rcservice_name", rcservice_name)){
|
||||
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
|
||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||
}
|
||||
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(!n_c.getParam("rcservice_name", rcservice_name))
|
||||
{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
}
|
||||
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
||||
}
|
||||
|
@ -144,7 +145,13 @@ namespace rosbzz_node{
|
|||
n_c.getParam("in_payload", in_payload);
|
||||
/*Obtain standby script to run during update*/
|
||||
n_c.getParam("stand_by", stand_by);
|
||||
n_c.getParam("xbee_status_srv", xbeesrv_name);
|
||||
|
||||
if(n_c.getParam("xbee_plugged", xbeeplugged));
|
||||
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(!xbeeplugged){
|
||||
if(n_c.getParam("name", robot_name));
|
||||
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
}
|
||||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
|
@ -153,11 +160,12 @@ namespace rosbzz_node{
|
|||
/*-----------------------------------------------------------------------------------
|
||||
/Obtains publisher, subscriber and services from yml file based on the robot used
|
||||
/-----------------------------------------------------------------------------------*/
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle){
|
||||
//todo: make it as an array in yaml?
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic, gps_type;
|
||||
node_handle.getParam("topics/gps", gps_topic);
|
||||
if(node_handle.getParam("topics/gps", gps_topic));
|
||||
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
node_handle.getParam("type/gps", gps_type);
|
||||
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
|
||||
|
||||
|
@ -192,7 +200,7 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/ Create all required subscribers, publishers and ROS service clients
|
||||
/-------------------------------------------------------*/
|
||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
|
||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){
|
||||
/*subscribers*/
|
||||
|
||||
Subscribe(n_c);
|
||||
|
@ -202,24 +210,29 @@ namespace rosbzz_node{
|
|||
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
|
||||
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
|
||||
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
|
||||
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos",1000);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||
if(rcclient==true)
|
||||
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
|
||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
|
||||
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this);
|
||||
|
||||
multi_msg=true;
|
||||
}
|
||||
/*---------------------------------------
|
||||
/Robot independent subscribers
|
||||
/--------------------------------------*/
|
||||
void roscontroller::Subscribe(ros::NodeHandle n_c){
|
||||
void roscontroller::Subscribe(ros::NodeHandle& n_c){
|
||||
|
||||
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
|
||||
if(it->second == "mavros_msgs/ExtendedState"){
|
||||
|
@ -345,35 +358,35 @@ namespace rosbzz_node{
|
|||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
/*Check for updater message if present send*/
|
||||
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
|
||||
/*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
|
||||
uint8_t* buff_send = 0;
|
||||
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
|
||||
int tot=0;
|
||||
mavros_msgs::Mavlink update_packets;
|
||||
fprintf(stdout,"Transfering code \n");
|
||||
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
|
||||
/*allocate mem and clear it*/
|
||||
// allocate mem and clear it
|
||||
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
|
||||
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
|
||||
/*Append updater msg size*/
|
||||
// Append updater msg size
|
||||
*(uint16_t*)(buff_send + tot)=updater_msgSize;
|
||||
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
|
||||
tot += sizeof(uint16_t);
|
||||
/*Append updater msgs*/
|
||||
// Append updater msgs
|
||||
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
|
||||
tot += updater_msgSize;
|
||||
/*Destroy the updater out msg queue*/
|
||||
// Destroy the updater out msg queue
|
||||
destroy_out_msg_queue();
|
||||
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
|
||||
uint64_t* payload_64 = new uint64_t[total_size];
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||
free(buff_send);
|
||||
/*Send a constant number to differenciate updater msgs*/
|
||||
// Send a constant number to differenciate updater msgs
|
||||
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
|
||||
for(int i=0;i<total_size;i++){
|
||||
update_packets.payload64.push_back(payload_64[i]);
|
||||
}
|
||||
/*Add Robot id and message number to the published message*/
|
||||
// Add Robot id and message number to the published message
|
||||
if (message_number < 0) message_number=0;
|
||||
else message_number++;
|
||||
update_packets.sysid=(uint8_t)robot_id;
|
||||
|
@ -381,7 +394,7 @@ namespace rosbzz_node{
|
|||
payload_pub.publish(update_packets);
|
||||
multi_msg=false;
|
||||
delete[] payload_64;
|
||||
}
|
||||
}*/
|
||||
|
||||
}
|
||||
/*---------------------------------------------------------------------------------
|
||||
|
@ -399,8 +412,11 @@ namespace rosbzz_node{
|
|||
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
|
||||
switch(buzzuav_closures::getcmd()){
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
if(current_mode != "LAND")
|
||||
{SetMode("LAND", 0);}
|
||||
if(current_mode != "LAND") {
|
||||
SetMode("LAND", 0);
|
||||
armstate = 0;
|
||||
Arm();
|
||||
}
|
||||
if (mav_client.call(cmd_srv)){
|
||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||
} else
|
||||
|
@ -413,6 +429,8 @@ namespace rosbzz_node{
|
|||
armstate = 1;
|
||||
Arm();
|
||||
ros::Duration(0.5).sleep();
|
||||
// Registering HOME POINT.
|
||||
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
|
||||
}
|
||||
if(current_mode != "GUIDED")
|
||||
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
|
||||
|
@ -451,8 +469,7 @@ namespace rosbzz_node{
|
|||
Arm();
|
||||
}
|
||||
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
|
||||
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
|
||||
roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
|
||||
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
|
||||
|
||||
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
|
||||
}
|
||||
|
@ -504,22 +521,133 @@ namespace rosbzz_node{
|
|||
----------------------------------------------------------- */
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
|
||||
|
||||
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
|
||||
double hyp_az, hyp_el;
|
||||
double sin_el, cos_el, sin_az, cos_az;
|
||||
|
||||
/* Convert reference point to spherical earth centered coordinates. */
|
||||
hyp_az = sqrt(ref_ecef[0]*ref_ecef[0] + ref_ecef[1]*ref_ecef[1]);
|
||||
hyp_el = sqrt(hyp_az*hyp_az + ref_ecef[2]*ref_ecef[2]);
|
||||
sin_el = ref_ecef[2] / hyp_el;
|
||||
cos_el = hyp_az / hyp_el;
|
||||
sin_az = ref_ecef[1] / hyp_az;
|
||||
cos_az = ref_ecef[0] / hyp_az;
|
||||
|
||||
M[0][0] = -sin_el * cos_az;
|
||||
M[0][1] = -sin_el * sin_az;
|
||||
M[0][2] = cos_el;
|
||||
M[1][0] = -sin_az;
|
||||
M[1][1] = cos_az;
|
||||
M[1][2] = 0.0;
|
||||
M[2][0] = -cos_el * cos_az;
|
||||
M[2][1] = -cos_el * sin_az;
|
||||
M[2][2] = -sin_el;
|
||||
}
|
||||
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
|
||||
const double *b, double *c)
|
||||
{
|
||||
uint32_t i, j, k;
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < p; j++) {
|
||||
c[p*i + j] = 0;
|
||||
for (k = 0; k < m; k++)
|
||||
c[p*i + j] += a[m*i+k] * b[p*k + j];
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
|
||||
|
||||
// calculate earth radii
|
||||
double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
|
||||
double prime_vertical_radius = equatorial_radius * sqrt(temp);
|
||||
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
|
||||
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
|
||||
|
||||
/*double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned[3];
|
||||
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
|
||||
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
|
||||
double ecef[3];
|
||||
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
|
||||
double d = WGS84_E * sin(llh[0]);
|
||||
double N = WGS84_A / sqrt(1. - d*d);
|
||||
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
|
||||
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
|
||||
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
|
||||
double ref_ecef[3];
|
||||
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
|
||||
d = WGS84_E * sin(llh[0]);
|
||||
N = WGS84_A / sqrt(1. - d*d);
|
||||
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
|
||||
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
|
||||
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
|
||||
double M[3][3];
|
||||
ecef2ned_matrix(ref_ecef, M);
|
||||
double ned[3];
|
||||
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
|
||||
|
||||
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
|
||||
out[0] = std::floor(out[0] * 1000000) / 1000000;
|
||||
out[1] = atan2(ned[1],ned[0]);
|
||||
out[1] = std::floor(out[1] * 1000000) / 1000000;
|
||||
out[2] = 0.0;*/
|
||||
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
//out[0] = std::floor(out[0] * 1000000) / 1000000;
|
||||
out[1] = atan2(ned_y,ned_x);
|
||||
//out[1] = std::floor(out[1] * 1000000) / 1000000;
|
||||
out[2] = 0.0;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
|
||||
// calculate earth radii
|
||||
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
|
||||
double prime_vertical_radius = equatorial_radius * sqrt(temp);
|
||||
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
|
||||
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
|
||||
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
|
||||
out[0] = std::floor(out[0] * 1000000) / 1000000;
|
||||
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
|
||||
out[1] = std::floor(out[1] * 1000000) / 1000000;
|
||||
out[2] = cur[2];
|
||||
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
|
||||
double ecef[3];
|
||||
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
|
||||
double d = WGS84_E * sin(llh[0]);
|
||||
double N = WGS84_A / sqrt(1. - d*d);
|
||||
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
|
||||
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
|
||||
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
|
||||
double ref_ecef[3];
|
||||
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
|
||||
d = WGS84_E * sin(llh[0]);
|
||||
N = WGS84_A / sqrt(1. - d*d);
|
||||
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
|
||||
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
|
||||
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
|
||||
double M[3][3];
|
||||
ecef2ned_matrix(ref_ecef, M);
|
||||
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
|
||||
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
//out[0] = std::floor(out[0] * 1000000) / 1000000;
|
||||
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
out[2] = cur[2];
|
||||
//out[1] = std::floor(out[1] * 1000000) / 1000000;
|
||||
out[2] = 0.0;
|
||||
}
|
||||
|
||||
/*------------------------------------------------------
|
||||
|
@ -557,14 +685,37 @@ namespace rosbzz_node{
|
|||
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
|
||||
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
|
||||
fcu_timeout = TIMEOUT;
|
||||
if(home[0]==0){
|
||||
home[0]=msg->latitude;
|
||||
home[1]=msg->longitude;
|
||||
//double lat = std::floor(msg->latitude * 1000000) / 1000000;
|
||||
//double lon = std::floor(msg->longitude * 1000000) / 1000000;
|
||||
/*if(cur_rel_altitude<1.2){
|
||||
home[0]=lat;
|
||||
home[1]=lon;
|
||||
home[2]=cur_rel_altitude;
|
||||
}
|
||||
}*/
|
||||
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
|
||||
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
|
||||
}
|
||||
void roscontroller::users_pos(const rosbuzz::neigh_pos data){
|
||||
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
|
||||
|
||||
double us[3];
|
||||
int n = data.pos_neigh.size();
|
||||
// ROS_INFO("Neighbors array size: %i\n", n);
|
||||
if(n>0)
|
||||
{
|
||||
for(int it=0; it<n; ++it)
|
||||
{
|
||||
us[0] = data.pos_neigh[it].latitude;
|
||||
us[1] = data.pos_neigh[it].longitude;
|
||||
us[2] = data.pos_neigh[it].altitude;
|
||||
double out[3];
|
||||
cvt_rangebearing_coordinates(us, out, cur_pos);
|
||||
buzzuav_closures::set_userspos(out[0], out[1], out[2]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
/*-------------------------------------------------------------
|
||||
/ Update altitude into BVM from subscriber
|
||||
/-------------------------------------------------------------*/
|
||||
|
@ -583,6 +734,66 @@ namespace rosbzz_node{
|
|||
buzzuav_closures::set_obstacle_dist(obst);
|
||||
}
|
||||
|
||||
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
|
||||
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
|
||||
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
|
||||
|
||||
geometry_msgs::PoseStamped moveMsg;
|
||||
moveMsg.header.stamp = ros::Time::now();
|
||||
moveMsg.header.seq = setpoint_counter++;
|
||||
moveMsg.header.frame_id = 1;
|
||||
double local_pos[3];
|
||||
cvt_ned_coordinates(cur_pos,local_pos,home);
|
||||
ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
|
||||
ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
|
||||
|
||||
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
|
||||
moveMsg.pose.position.x = local_pos[1]+y;
|
||||
moveMsg.pose.position.y = local_pos[0]+x;
|
||||
moveMsg.pose.position.z = z;
|
||||
|
||||
moveMsg.pose.orientation.x = 0;
|
||||
moveMsg.pose.orientation.y = 0;
|
||||
moveMsg.pose.orientation.z = 0;
|
||||
moveMsg.pose.orientation.w = 1;
|
||||
|
||||
// To prevent drifting from stable position.
|
||||
if(fabs(x)>0.1 || fabs(y)>0.1) {
|
||||
localsetpoint_nonraw_pub.publish(moveMsg);
|
||||
ROS_INFO("Sent local NON RAW position message!");
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
|
||||
// wait if necessary
|
||||
if (delay_miliseconds != 0){
|
||||
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
|
||||
}
|
||||
// set mode
|
||||
mavros_msgs::SetMode set_mode_message;
|
||||
set_mode_message.request.base_mode = 0;
|
||||
set_mode_message.request.custom_mode = mode;
|
||||
current_mode = mode;
|
||||
if(mode_client.call(set_mode_message)) {
|
||||
ROS_INFO("Set Mode Service call successful!");
|
||||
} else {
|
||||
ROS_INFO("Set Mode Service call failed!");
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::SetStreamRate(int id, int rate, int on_off){
|
||||
mavros_msgs::StreamRate message;
|
||||
message.request.stream_id = id;
|
||||
message.request.message_rate = rate;
|
||||
message.request.on_off = on_off;
|
||||
|
||||
while(!stream_client.call(message)){
|
||||
ROS_INFO("Set stream rate call failed!, trying again...");
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
ROS_INFO("Set stream rate call successful");
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------
|
||||
/Push payload into BVM FIFO
|
||||
/-------------------------------------------------------------*/
|
||||
|
@ -777,66 +988,5 @@ namespace rosbzz_node{
|
|||
}
|
||||
*/
|
||||
}
|
||||
/*
|
||||
* SOLO SPECIFIC FUNCTIONS
|
||||
*/
|
||||
|
||||
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
|
||||
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
|
||||
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
|
||||
|
||||
geometry_msgs::PoseStamped moveMsg;
|
||||
moveMsg.header.stamp = ros::Time::now();
|
||||
moveMsg.header.seq = setpoint_counter++;
|
||||
moveMsg.header.frame_id = 1;
|
||||
double local_pos[3];
|
||||
cvt_ned_coordinates(cur_pos,local_pos,home);
|
||||
moveMsg.pose.position.x = local_pos[0]+x;
|
||||
moveMsg.pose.position.y = local_pos[1]+y;
|
||||
moveMsg.pose.position.z = z;
|
||||
|
||||
moveMsg.pose.orientation.x = 0;
|
||||
moveMsg.pose.orientation.y = 0;
|
||||
moveMsg.pose.orientation.z = 0;
|
||||
moveMsg.pose.orientation.w = 1;
|
||||
|
||||
std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] <<std::endl;
|
||||
|
||||
localsetpoint_pub.publish(moveMsg);
|
||||
|
||||
ROS_INFO("Sent local NON RAW position message!");
|
||||
}
|
||||
|
||||
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
|
||||
// wait if necessary
|
||||
if (delay_miliseconds != 0){
|
||||
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
|
||||
}
|
||||
// set mode
|
||||
mavros_msgs::SetMode set_mode_message;
|
||||
set_mode_message.request.base_mode = 0;
|
||||
set_mode_message.request.custom_mode = mode;
|
||||
current_mode = mode;
|
||||
if(mode_client.call(set_mode_message)) {
|
||||
ROS_INFO("Set Mode Service call successful!");
|
||||
} else {
|
||||
ROS_INFO("Set Mode Service call failed!");
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::SetStreamRate(int id, int rate, int on_off){
|
||||
mavros_msgs::StreamRate message;
|
||||
message.request.stream_id = id;
|
||||
message.request.message_rate = rate;
|
||||
message.request.on_off = on_off;
|
||||
|
||||
while(!stream_client.call(message)){
|
||||
ROS_INFO("Set stream rate call failed!, trying again...");
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
ROS_INFO("Set stream rate call successful");
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
|
@ -0,0 +1,213 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
####################################################################################################
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 10.0
|
||||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 12.0
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function hexagon() {
|
||||
statef=hexagon
|
||||
CURSTATE = "HEXAGON"
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
|
||||
uav_moveto(accum.x,accum.y)
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 1
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
barrier.put(id, 1)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
WAIT_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf) {
|
||||
barrier.get(id)
|
||||
CURSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=land
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# flight status
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
CURSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
log("TakeOff: ", flight.status)
|
||||
log("Relative position: ", position.altitude)
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
function land() {
|
||||
CURSTATE = "LAND"
|
||||
statef=land
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else {
|
||||
timeW=0
|
||||
barrier = nil
|
||||
statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
CURSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
CURSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
statef = idle
|
||||
uav_goto()
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
}
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and CURSTATE=="IDLE") {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400 and CURSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
} else if(value==401 and CURSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
}
|
||||
}
|
||||
|
||||
)
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
log("User position: ", users.range)
|
||||
|
||||
# Read a value from the structure
|
||||
#x = usersvstig.get(46)
|
||||
# Get the number of keys in the structure
|
||||
#log("The vstig has ", usersvstig.size(), " elements")
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
Loading…
Reference in New Issue