From 9a0332aed9802343b0c7cb8773ed453a5f14f4c0 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 9 Aug 2017 19:23:42 -0400 Subject: [PATCH] tweak graph control, fix obstacles and and rc_client fleet_status --- buzz_scripts/graphform.bzz | 62 +++++++++++++----------- buzz_scripts/include/barrier.bzz | 17 ++++++- buzz_scripts/include/graphs/shapes_L.bzz | 10 ++-- buzz_scripts/include/graphs/shapes_Y.bzz | 10 ++-- buzz_scripts/include/uavstates.bzz | 3 ++ buzz_scripts/include/vstigenv.bzz | 23 +++++++++ include/buzz_utility.h | 8 +++ include/buzzuav_closures.h | 5 +- include/roscontroller.h | 7 ++- src/buzz_utility.cpp | 6 +++ src/buzzuav_closures.cpp | 44 +++++++++++++++++ src/roscontroller.cpp | 47 ++++++++++-------- 12 files changed, 179 insertions(+), 63 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 133a834..adb8139 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -8,14 +8,14 @@ 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 = 200.0 +ROBOT_MAXVEL = 250.0 # # Global variables @@ -91,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 @@ -278,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 @@ -289,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 c49f1f8..c983fcc 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -86,4 +86,27 @@ function stattab_print() { } } } +} + +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 + } + } + } } \ 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 516e9d6..019fe35 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -26,12 +26,14 @@ namespace buzzuav_closures{ static float height=0; static bool deque_full = false; static int rssi = 0; + static int message_number = 0; static float raw_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 /----------------------------------------*/ 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;