diff --git a/CMakeLists.txt b/CMakeLists.txt index 385dec8..9cb3fac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 6b2a1aa..3667363 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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(); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index dcfa26a..eff6835 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 diff --git a/include/roscontroller.h b/include/roscontroller.h index 738371a..e1e00c8 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -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); diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml index 1ffd380..5ad5f46 100644 --- a/launch/launch_config/m100.yaml +++ b/launch/launch_config/m100.yaml @@ -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 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index 8930701..c53989a 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -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 diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index f4bb43a..c9cbd69 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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,18 +257,10 @@ namespace buzz_utility{ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - //cout<<"bo file name"<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_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", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - sid = 2; - 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", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } + 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", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *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) { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } } } /*Step through the buzz script*/ @@ -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); } -// usleep(10000); + //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); } /****************************************/ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 2f1f754..7451a73 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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; diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 5190944..d997e5c 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -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(); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1e086b3..abd99e4 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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 - GetRobotId(); + 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 (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(out_payload, 1000); - neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); - localsetpoint_pub = n_c.advertise(setpoint_name,1000); + neigh_pos_pub = n_c.advertise("neighbours_pos",1000); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name,1000); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(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(xbeesrv_name); stream_client = n_c.serviceClient(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::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;ilatitude; - 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); + }*/ + 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; itranges[i]; 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] < ..." +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() { +}