diff --git a/CMakeLists.txt b/CMakeLists.txt index 28df175..f9d152b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,11 @@ if(UNIX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") endif() +if(SIM) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1") +else() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0") +endif() ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/include/buzz_update.h b/include/buzz_update.h index a0be091..7a2a8f5 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,7 +1,7 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H /*Simulation or robot check*/ -#define SIMULATION 1 +//#define SIMULATION 1 // set in CMAKELIST #include #include diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 692de52..d2a4e60 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -42,13 +42,8 @@ uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); - -void add_user(int id, double latitude, double longitude, float altitude); -void update_users(); int make_table(buzzobj_t* t); -int buzzusers_add(int id, double latitude, double longitude, double altitude); int buzzusers_reset(); - int compute_users_rb(); int create_stig_tables(); void in_msg_append(uint64_t* payload); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 6a5d18a..2c1d387 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -1,6 +1,5 @@ #pragma once -//#ifndef BUZZUAV_CLOSURES_H -//#define BUZZUAV_CLOSURES_H + #include #include #include "uav_utility.h" diff --git a/include/roscontroller.h b/include/roscontroller.h index a2d72d6..0dbe19e 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -212,7 +212,6 @@ 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*/ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 017c632..bba7a5b 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -1,8 +1,8 @@ /** @file buzz_utility.cpp * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. - * @author Vivek Shankar Varadharajan + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge * @copyright 2016 MistLab. All rights reserved. */ @@ -17,119 +17,13 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets + static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; std::map< int, Pos_struct> users_map; /****************************************/ - void add_user(int id, double latitude, double longitude, float altitude) - { - Pos_struct pos_arr; - pos_arr.x=latitude; - pos_arr.y=longitude; - pos_arr.z=altitude; - map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id); - if(it!=users_map.end()) - users_map.erase(it); - users_map.insert(make_pair(id, pos_arr)); - //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); - } - - void update_users(){ - if(users_map.size()>0) { - // Reset users information -// buzzusers_reset(); -// create_stig_tables(); - // Get user id and update user information - map< int, Pos_struct >::iterator it; - for (it=users_map.begin(); it!=users_map.end(); ++it){ - //ROS_INFO("Buzz_utility will save user %i.", it->first); - buzzusers_add(it->first, - (it->second).x, - (it->second).y, - (it->second).z); - } - } - } - - /*int buzzusers_reset() { - if(VM->state != BUZZVM_STATE_READY) return VM->state; - //Make new table - buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); - //make_table(&t); - if(VM->state != BUZZVM_STATE_READY) return VM->state; - //Register table as global symbol - //buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); - buzzvm_push(VM, t); - buzzvm_gstore(VM); - //buzzvm_pushi(VM, 2); - //buzzvm_callc(VM); - return VM->state; - }*/ - - int buzzusers_add(int id, double latitude, double longitude, double altitude) { - if(VM->state != BUZZVM_STATE_READY) return VM->state; - // Get users "p" table - /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); - buzzvm_tget(VM);*/ - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_tget(VM); - if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { - //ROS_INFO("Empty data, create a new table"); - buzzvm_pop(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_pusht(VM); - buzzobj_t data = buzzvm_stack_at(VM, 1); - buzzvm_tput(VM); - buzzvm_push(VM, data); - } - // When we get here, the "data" table is on top of the stack - // Push user id - buzzvm_pushi(VM, id); - // Create entry table - buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); - // Insert latitude - buzzvm_push(VM, entry); - buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1)); - buzzvm_pushf(VM, latitude); - buzzvm_tput(VM); - // Insert longitude - buzzvm_push(VM, entry); - buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1)); - buzzvm_pushf(VM, longitude); - buzzvm_tput(VM); - // Insert altitude - buzzvm_push(VM, entry); - buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1)); - buzzvm_pushf(VM, altitude); - buzzvm_tput(VM); - // Save entry into data table - buzzvm_push(VM, entry); - buzzvm_tput(VM); - //ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); - // forcing the new table into the stigmergy.... - /*buzzobj_t newt = buzzvm_stack_at(VM, 0); - buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); - buzzvm_push(VM, nbr); - buzzvm_pushi(VM, 2); - buzzvm_callc(VM);*/ - //buzzvm_gstore(VM); - return VM->state; - } /**************************************************************************/ /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ /**************************************************************************/ @@ -388,74 +282,6 @@ void in_message_process(){ return VM->state; } -int create_stig_tables() { -/* - // usersvstig = stigmergy.create(123) - buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - // 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); - // value of the stigmergy id - buzzvm_pushi(VM, 5); - // call the stigmergy.create() method - buzzvm_dump(VM); -// buzzvm_closure_call(VM, 1); - buzzvm_pushi(VM, 1); - buzzvm_callc(VM); - buzzvm_gstore(VM); - buzzvm_dump(VM); - - //buzzusers_reset(); - buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); - - buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); - buzzvm_push(VM, t); - buzzvm_dump(VM); -// buzzvm_closure_call(VM, 2); - buzzvm_pushi(VM, 2); - buzzvm_callc(VM); - //buzzvm_gstore(VM); - //buzzvm_dump(VM); - - buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1)); - buzzvm_pusht(VM); - buzzvm_pushi(VM, 2); - buzzvm_call(VM, 0); - buzzvm_gstore(VM);*/ - - buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_push(VM,t); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); - buzzvm_pusht(VM); - buzzobj_t data = buzzvm_stack_at(VM, 1); - buzzvm_tput(VM); - buzzvm_push(VM, data); - - buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1)); - buzzvm_pusht(VM); - data = buzzvm_stack_at(VM, 1); - buzzvm_tput(VM); - buzzvm_push(VM, data); - - return VM->state; -} /****************************************/ /*Sets the .bzz and .bdbg file*/ /****************************************/ @@ -526,16 +352,6 @@ int create_stig_tables() { buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); - /* Create vstig tables - if(create_stig_tables() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id); - //cout << "ERROR!!!! ---------- " << VM->errormsg << endl; - //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; - return 0; - }*/ - // Execute the global part of the script if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ ROS_ERROR("Error executing global part, VM state : %i",VM->state); @@ -588,15 +404,6 @@ int create_stig_tables() { buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); - /* Create vstig tables - if(create_stig_tables() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id); - //cout << "ERROR!!!! ---------- " << VM->errormsg << endl; - //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; - return 0; - }*/ // Execute the global part of the script if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ ROS_ERROR("Error executing global part, VM state : %i",VM->state); @@ -682,16 +489,6 @@ int create_stig_tables() { buzz_error_info()); buzzvm_dump(VM); } - - /*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); - - - /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ - //int status = 1; - //buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ @@ -733,9 +530,7 @@ int create_stig_tables() { buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); - //update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); - //set_robot_var(buzzdict_size(VM->swarmmembers)+1); int a = buzzvm_function_call(VM, "step", 0); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b1aa6c0..180df2f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -1,11 +1,11 @@ /** @file buzzuav_closures.cpp * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. - * @author Vivek Shankar Varadharajan + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge * @copyright 2016 MistLab. All rights reserved. */ -//#define _GNU_SOURCE + #include "buzzuav_closures.h" #include "math.h" @@ -192,11 +192,6 @@ int buzzros_print(buzzvm_t vm) goto_pos[0]=dx; goto_pos[1]=dy; goto_pos[2]=height+dh; - /*double b = atan2(dy,dx); //bearing - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - gps_from_rb(d, b, goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ - //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd = COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); @@ -205,12 +200,8 @@ int buzzros_print(buzzvm_t vm) int buzzuav_update_targets(buzzvm_t vm) { if(vm->state != BUZZVM_STATE_READY) return vm->state; buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1)); - //buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - //buzzvm_push(vm, t); buzzvm_pusht(vm); buzzobj_t targettbl = buzzvm_stack_at(vm, 1); - //buzzvm_tput(vm); - //buzzvm_dup(vm); double rb[3], tmp[3]; map< int, buzz_utility::RB_struct >::iterator it; for (it=targets_map.begin(); it!=targets_map.end(); ++it){ @@ -736,31 +727,6 @@ int buzzros_print(buzzvm_t vm) buzzvm_pushi(vm, 4); buzzvm_push(vm, tProxRead); buzzvm_tput(vm); - - /* - buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); - buzzvm_pushf(vm, obst[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); - buzzvm_pushf(vm, obst[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); - buzzvm_pushf(vm, obst[2]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); - buzzvm_pushf(vm, obst[3]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); - buzzvm_pushf(vm, obst[4]); - buzzvm_tput(vm); - buzzvm_gstore(vm);*/ return vm->state; } @@ -770,12 +736,4 @@ int buzzros_print(buzzvm_t vm) int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} - /***********************************************/ - /* Store Ros controller object pointer */ - /***********************************************/ - - //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){ - //roscontroller_ptr = roscontroller_ptrin; - //} - } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index d997e5c..4481734 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -1,8 +1,8 @@ /** @file rosbuzz.cpp * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. - * @author Vivek Shankar Varadharajan + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge * @copyright 2016 MistLab. All rights reserved. */ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9674e03..c388d0d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1,3 +1,11 @@ +/** @file roscontroller.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "roscontroller.h" #include namespace rosbzz_node { @@ -424,7 +432,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) capture_srv = n_c.serviceClient(capture_srv_name); stream_client = n_c.serviceClient(stream_client_name); - users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); multi_msg = true; @@ -877,19 +884,6 @@ void roscontroller::local_pos_callback( local_pos_new[2] = pose->pose.position.z; } -void roscontroller::users_pos(const rosbuzz::neigh_pos data) { - - int n = data.pos_neigh.size(); - // ROS_INFO("Users array size: %i\n", n); - if (n > 0) { - for (int it = 0; it < n; ++it) { - buzz_utility::add_user(data.pos_neigh[it].position_covariance_type, - data.pos_neigh[it].latitude, - data.pos_neigh[it].longitude, - data.pos_neigh[it].altitude); - } - } -} /*------------------------------------------------------------- / Update altitude into BVM from subscriber /-------------------------------------------------------------*/ @@ -1126,41 +1120,6 @@ void roscontroller::get_number_of_robots() { no_cnt = 0; } } - /* - if(count_robots.current !=0){ - std::map< int, int> count_count; - uint8_t index=0; - count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; - //count_robots.current=neighbours_pos_map.size()+1; - count_robots.index++; - if(count_robots.index >9) count_robots.index =0; - for(int i=0;i<10;i++){ - if(count_robots.history[i]==count_robots.current){ - current_count++; - } - else{ - if(count_robots.history[i]!=0){ - odd_count++; - odd_val=count_robots.history[i]; - } - } - } - if(odd_count>current_count){ - count_robots.current=odd_val; - } - } - else{ - if(neighbours_pos_map.size()!=0){ - count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; - //count_robots.current=neighbours_pos_map.size()+1; - count_robots.index++; - if(count_robots.index >9){ - count_robots.index =0; - count_robots.current=neighbours_pos_map.size()+1; - } - } - } - */ } void roscontroller::get_xbee_status()