diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index e7439e9..adb8139 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -6,15 +6,16 @@ include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. +include "vstigenv.bzz" -include "graphs/shapes_L.bzz" +include "graphs/shapes_Y.bzz" ROBOT_RADIUS=50 ROBOT_DIAMETER=2.0*ROBOT_RADIUS ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER # max velocity in cm/step -ROBOT_MAXVEL = 500.0 +ROBOT_MAXVEL = 250.0 # # Global variables @@ -90,8 +91,7 @@ step_cunt=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 13.5 #the LJ parameter for other robots -EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1 +EPSILON = 4000 #13.5 the LJ parameter for other robots # Lennard-Jones interaction magnitude @@ -277,7 +277,7 @@ log(id,"I pass value=",recv_value) var return_table={.Label=0.0,.Bearing=0.0} return_table.Label=qian if(bai==1){ - b=b*-1.0 + b=b*-1.0 } return_table.Bearing=b return return_table @@ -288,10 +288,10 @@ function target4label(nei_id){ var return_val="miss" var i=0 while(i= threshold) { - #barrier = nil +# getlowest() transf() } else if(timeW>=BARRIER_TIMEOUT) { barrier = nil @@ -44,4 +44,19 @@ function barrier_wait(threshold, transf, resumef) { timeW=0 } timeW = timeW+1 +} + +# get the lowest id of the fleet, but requires too much bandwidth +function getlowest(){ + Lid = 20; + u=20 + while(u>=0){ + tab = barrier.get(u) + if(tab!=nil){ + if(tab LOWEST ID:",Lid) } \ No newline at end of file diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/graphs/shapes_L.bzz index dc507c9..16f2272 100644 --- a/buzz_scripts/include/graphs/shapes_L.bzz +++ b/buzz_scripts/include/graphs/shapes_L.bzz @@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab m_vecNodes[1] = { .Label = 1, .Pred = 0, - .distance = 2000, + .distance = 800, .bearing = 0.0, .height = 5000, .State="UNASSIGNED", @@ -23,7 +23,7 @@ m_vecNodes[1] = { m_vecNodes[2] = { .Label = 2, .Pred = 0, - .distance = 2000, + .distance = 800, .bearing = 1.57, .height = 7000, .State="UNASSIGNED", @@ -32,7 +32,7 @@ m_vecNodes[2] = { m_vecNodes[3] = { .Label = 3, .Pred = 1, - .distance = 2000, + .distance = 800, .bearing = 0.0, .height = 9000, .State="UNASSIGNED", @@ -41,7 +41,7 @@ m_vecNodes[3] = { m_vecNodes[4] = { .Label = 4, .Pred = 2, - .distance = 2000, + .distance = 800, .bearing = 1.57, .height = 11000, .State="UNASSIGNED", @@ -50,7 +50,7 @@ m_vecNodes[4] = { m_vecNodes[5] = { .Label = 5, .Pred = 4, - .distance = 2000, + .distance = 800, .bearing = 1.57, .height = 14000, .State="UNASSIGNED", diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/graphs/shapes_Y.bzz index b3e23ce..29c78ec 100644 --- a/buzz_scripts/include/graphs/shapes_Y.bzz +++ b/buzz_scripts/include/graphs/shapes_Y.bzz @@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab m_vecNodes[1] = { .Label = 1, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 4.71, .height = 5000, .State="UNASSIGNED", @@ -23,7 +23,7 @@ m_vecNodes[1] = { m_vecNodes[2] = { .Label = 2, .Pred = 0, - .distance = 1414, + .distance = 707, .bearing = 2.36, .height = 7000, .State="UNASSIGNED", @@ -32,7 +32,7 @@ m_vecNodes[2] = { m_vecNodes[3] = { .Label = 3, .Pred = 0, - .distance = 1414, + .distance = 707, .bearing = 0.79, .height = 9000, .State="UNASSIGNED", @@ -41,7 +41,7 @@ m_vecNodes[3] = { m_vecNodes[4] = { .Label = 4, .Pred = 2, - .distance = 1414, + .distance = 707, .bearing = 2.36, .height = 11000, .State="UNASSIGNED", @@ -50,7 +50,7 @@ m_vecNodes[4] = { m_vecNodes[5] = { .Label = 5, .Pred = 3, - .distance = 1414, + .distance = 707, .bearing = 0.79, .height = 14000, .State="UNASSIGNED", diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index fef18ac..4ecac31 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -103,6 +103,9 @@ function uav_rccmd() { flight.rc_cmd=0 uav_disarm() neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() } } diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index 494ffbe..c983fcc 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -3,6 +3,7 @@ GROUND_VSTIG = 11 WAIT4STEP = 10 v_status = {} v_ground = {} +b_updating = 0 function uav_initstig() { v_status = stigmergy.create(STATUS_VSTIG) @@ -11,29 +12,48 @@ function uav_initstig() { counter=WAIT4STEP function uav_updatestig() { + # TODO: Push values on update only. if(counter<=0) { - var ls={.g=0,.b=battery.capacity,.x=xbee_status.rssi,.f=flight.status} + b_updating=1 + #var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status} + ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status log("Pushing ", ls, "on vstig with id:", id) v_status.put(id, ls) counter=WAIT4STEP - } else + } else { + b_updating=0 counter=counter-1 + } +} + +function unpackstatus(recv_value){ + gps=(recv_value-recv_value%1000000)/1000000 + recv_value=recv_value-gps*1000000 + batt=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-batt*1000 + xbee=(recv_value-recv_value%10)/10 + recv_value=recv_value-xbee*10 + fc=recv_value + log("- GPS ", gps) + log("- Battery ", batt) + log("- Xbee ", xbee) + log("- Status ", fc) } function checkusers() { - # Read a value from the structure - if(size(users)>0) - log("Got a user!") + # Read a value from the structure + if(size(users)>0) + log("Got a user!") -# log(users) -# users_print(users.dataG) -# if(size(users.dataG)>0) -# vt.put("p", users.dataG) - - # Get the number of keys in the structure - #log("The vstig has ", vt.size(), " elements") - # users_save(vt.get("p")) - # table_print(users.dataL) +# log(users) +# users_print(users.dataG) +# if(size(users.dataG)>0) +# vt.put("p", users.dataG) + + # Get the number of keys in the structure +# log("The vstig has ", vt.size(), " elements") +# users_save(vt.get("p")) +# table_print(users.dataL) } function users_save(t) { @@ -56,17 +76,37 @@ function usertab_print(t) { function stattab_print() { if(v_status.size()>0) { - u=0 - while(u<8){ - tab = v_status.get(u) - if(tab!=nil) { - log("id: ", u) - log("- GPS ", tab.g) - log("- Battery ", tab.b) - log("- Xbee ", tab.x) - log("- Status ", tab.f) + if(b_updating==0) { + u=0 + while(u<8){ + tab = v_status.get(u) + if(tab!=nil) + unpackstatus(tab) + u=u+1 + } + } + } +} + +function stattab_send() { + if(v_status.size()>0) { + if(b_updating==0) { + u=0 + while(u<8){ + tab = v_status.get(u) + if(tab!=nil){ + recv_value=tab + gps=(recv_value-recv_value%1000000)/1000000 + recv_value=recv_value-gps*1000000 + batt=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-batt*1000 + xbee=(recv_value-recv_value%10)/10 + recv_value=recv_value-xbee*10 + fc=recv_value + add_neighborStatus(u,gps,batt,xbee,fc) + } + u=u+1 } - u=u+1 } } } \ No newline at end of file diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 40be6f0..4909ed8 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -29,6 +29,14 @@ struct rb_struct }; typedef struct rb_struct RB_struct; +struct neiStatus +{ + uint gps_strenght = 0; + uint batt_lvl = 0; + uint xbee = 0; + uint flight_status = 0; +}; typedef struct neiStatus neighbors_status ; + uint16_t* u64_cvt_u16(uint64_t u64); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index fac506c..c205c47 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -5,9 +5,9 @@ #include #include "uav_utility.h" #include "mavros_msgs/CommandCode.h" +#include "mavros_msgs/Mavlink.h" #include "ros/ros.h" #include "buzz_utility.h" -//#include "roscontroller.h" #define EARTH_RADIUS (double) 6371000.0 #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) @@ -61,6 +61,9 @@ void flight_status_update(uint8_t state); /* Update neighbors table */ void neighbour_pos_callback(int id, float range, float bearing, float elevation); void update_neighbors(buzzvm_t vm); +int buzzuav_addNeiStatus(buzzvm_t vm); +mavros_msgs::Mavlink get_status(); + /*Flight status*/ void set_obstacle_dist(float dist[]); diff --git a/include/roscontroller.h b/include/roscontroller.h index fe54707..31e7537 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -69,7 +69,7 @@ private: double latitude=0.0; float altitude=0.0; }; typedef struct gps GPS ; // not useful in cpp - + GPS target, home, cur_pos; double cur_rel_altitude; @@ -91,7 +91,7 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0 ; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; + std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; std::string setpoint_nonraw; @@ -102,6 +102,7 @@ private: ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; ros::Publisher payload_pub; + ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; @@ -155,6 +156,8 @@ private: /*Neighbours pos publisher*/ void neighbours_pos_publisher(); + void send_MPpayload(); + /*Prepare messages and publish*/ void prepare_msg_and_publish(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 7a5eaf2..6039dc7 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -325,6 +325,9 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); + buzzvm_gstore(VM); return VM->state; } @@ -367,6 +370,9 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); return VM->state; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index fcfb752..019fe35 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -25,13 +25,15 @@ namespace buzzuav_closures{ static int buzz_cmd=0; static float height=0; static bool deque_full = false; - static float rssi = 0.0; + static int rssi = 0; + static int message_number = 0; static float raw_packet_loss = 0.0; - static float filtered_packet_loss = 0.0; + static int filtered_packet_loss = 0; static float api_rssi = 0.0; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; + std::map< int, buzz_utility::neighbors_status> neighbors_status_map; /****************************************/ /****************************************/ @@ -218,6 +220,48 @@ namespace buzzuav_closures{ return 0; } + int buzzuav_addNeiStatus(buzzvm_t vm){ + buzzvm_lnum_assert(vm, 5); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzz_utility::neighbors_status newRS; + uint8_t id = buzzvm_stack_at(vm, 5)->i.value; + newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; + map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); + if(it!=neighbors_status_map.end()) + neighbors_status_map.erase(it); + neighbors_status_map.insert(make_pair(id, newRS)); + return vm->state; + } + + mavros_msgs::Mavlink get_status(){ + mavros_msgs::Mavlink payload_out; + map< int, buzz_utility::neighbors_status >::iterator it; + for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){ + payload_out.payload64.push_back(it->first); + payload_out.payload64.push_back(it->second.gps_strenght); + payload_out.payload64.push_back(it->second.batt_lvl); + payload_out.payload64.push_back(it->second.xbee); + payload_out.payload64.push_back(it->second.flight_status); + } + /*Add Robot id and message number to the published message*/ + payload_out.sysid = (uint8_t)neighbors_status_map.size(); + /*payload_out.msgid = (uint32_t)message_number; + + message_number++;*/ + return payload_out; + } /*----------------------------------------/ / Buzz closure to go directly to a GPS destination from the Mission Planner /----------------------------------------*/ @@ -247,7 +291,7 @@ namespace buzzuav_closures{ } /*---------------------------------------/ - / Buss closure for basic UAV commands + / Buzz closure for basic UAV commands /---------------------------------------*/ int buzzuav_takeoff(buzzvm_t vm) { buzzvm_lnum_assert(vm, 1); @@ -334,7 +378,7 @@ namespace buzzuav_closures{ buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); - buzzvm_pushf(vm, batt[2]); + buzzvm_pushi(vm, (int)batt[2]); buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; @@ -347,7 +391,7 @@ namespace buzzuav_closures{ void set_rssi(float value) { - rssi = value; + rssi = round(value); } void set_raw_packet_loss(float value) @@ -357,7 +401,7 @@ namespace buzzuav_closures{ void set_filtered_packet_loss(float value) { - filtered_packet_loss = value; + filtered_packet_loss = round(100*value); } void set_api_rssi(float value) @@ -374,7 +418,7 @@ namespace buzzuav_closures{ buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); - buzzvm_pushf(vm, rssi); + buzzvm_pushi(vm, rssi); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); @@ -382,7 +426,7 @@ namespace buzzuav_closures{ buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); - buzzvm_pushf(vm, filtered_packet_loss); + buzzvm_pushi(vm, filtered_packet_loss); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 306b9d3..ce77de7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -172,6 +172,10 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result) return srv_response.success; } +void roscontroller::send_MPpayload(){ + MPpayload_pub.publish(buzzuav_closures::get_status()); +} + /*------------------------------------------------- /rosbuzz_node loop method executed once every step /--------------------------------------------------*/ @@ -193,6 +197,7 @@ void roscontroller::RosControllerRun() // buzz_closure::neighbour_pos_callback(neighbours_pos_map); /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); + send_MPpayload(); /*Check updater state and step code*/ update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ @@ -387,6 +392,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node"); } + + node_handle.getParam("obstacles", obstacles_topic); } /*-------------------------------------------------------- @@ -398,23 +405,18 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) Subscribe(n_c); - // current_position_sub = n_c.subscribe("/global_position", 1000, - // &roscontroller::current_pos,this); - // battery_sub = n_c.subscribe("/power_status", 1000, - // &roscontroller::battery,this); 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, + n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + + obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); + /*publishers*/ - payload_pub = n_c.advertise(out_payload, 1000); - neigh_pos_pub = n_c.advertise("neighbours_pos", 1000); + payload_pub = n_c.advertise(out_payload, 5); + MPpayload_pub = n_c.advertise("fleet_status", 5); + neigh_pos_pub = n_c.advertise("neighbours_pos", 5); localsetpoint_nonraw_pub = - n_c.advertise(setpoint_name, 1000); + n_c.advertise(setpoint_name, 5); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); @@ -427,8 +429,8 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) stream_client = n_c.serviceClient(stream_client_name); - users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos, this); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000, + 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; @@ -449,13 +451,13 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) it->first, 100, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = - n_c.subscribe(it->first, 1000, &roscontroller::battery, this); + n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { current_position_sub = - n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); + n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); } else if (it->second == "std_msgs/Float64") { relative_altitude_sub = - n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this); + n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -1125,14 +1127,17 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, rc_goto[0] = req.param5; rc_goto[1] = req.param6; rc_goto[2] = req.param7; - // for test - // SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0); - buzzuav_closures::rc_set_goto(rc_goto); rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; + case 666: + ROS_INFO("RC_Call: Update Fleet Status!!!!"); + rc_cmd = 666; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; default: res.success = false; break;