diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 81f7a73..0fb8cbf 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -13,6 +13,7 @@ include "act/neighborcomm.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps +WP_STIG = 8 path_it = 0 pic_time = 0 @@ -100,7 +101,7 @@ firsttimeinwp = 1 wpreached = 1 function indiWP() { if(firsttimeinwp) { - v_wp = stigmergy.create(8) + v_wp = stigmergy.create(WP_STIG) storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) firsttimeinwp = 0 } @@ -111,21 +112,25 @@ function indiWP() { storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) return } else { - v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0}) + var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0) + v_wp.put(rc_goto.id,ls) reset_rc() } } - wp = v_wp.get(id) - if(wp!=nil) { - #log(wp.pro,wpreached) - if(wp.pro == 0) { - wpreached = 0 - storegoal(wp.lat, wp.lon, pose.position.altitude) - v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1}) - return - } - } + #if(vstig_buzzy == 0) { + wpi = v_wp.get(id) + if(wpi!=nil) { + wp = unpackWP2i(wpi) + if(wp.pro == 0) { + wpreached = 0 + storegoal(wp.lat, wp.lon, pose.position.altitude) + var ls = packWP2i(wp.lat, wp.lon, 1) + v_wp.put(id,ls) + return + } + } + #} if(wpreached!=1) goto_gps(indiWP_done) diff --git a/buzz_scripts/include/plan/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz index c3d3393..209aed0 100644 --- a/buzz_scripts/include/plan/mapmatrix.bzz +++ b/buzz_scripts/include/plan/mapmatrix.bzz @@ -102,21 +102,6 @@ function is_in(dictionary, x, y){ return 0 } - -function table_print(t) { - foreach(t, function(key, value) { - log(key, " -> ", value) - }) -} - -function table_copy(t) { - var t2 = {} - foreach(t, function(key, value) { - t2[key] = value - }) - return t2 -} - function print_pos(t) { var ir=1 while(ir <= size(t)) { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index f020e2e..b796a1f 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -6,6 +6,7 @@ include "taskallocate/graphs/shapes_P.bzz" include "taskallocate/graphs/shapes_O.bzz" include "taskallocate/graphs/shapes_L.bzz" include "taskallocate/graphs/shapes_Y.bzz" +#include "utils/table.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -82,60 +83,6 @@ m_lockstig = 1 # Lennard-Jones parameters, may need change EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots -# -#return the number of value in table -# -function count(table,value){ - var number=0 - var i=0 - while(i ", value) + }) +} + +function table_copy(t) { + var t2 = {} + foreach(t, function(key, value) { + t2[key] = value + }) + return t2 +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i0) { - if(b_updating==0) { - u=0 - while(u0) { - if(b_updating==0) { - u=0 - while(u indoor wheeled vehicle V_TYPE = 0 -goal_list = { - .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} -} # Executed once at init time. function init() { @@ -53,7 +51,7 @@ function step() { rc_cmd_listen() # update the vstig (status/net/batt/...) - # uav_updatestig() + uav_updatestig() # # State machine diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f8ae4d5..967f521 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -20,7 +20,8 @@ namespace buzzuav_closures * The command to use in Buzz is buzzros_print takes any available datatype in Buzz */ int buzzros_print(buzzvm_t vm); -void setWPlist(std::string path); +void setWPlist(std::string file); +void check_targets_sim(double lat, double lon, double *res); /* * closure to move following a vector diff --git a/include/roscontroller.h b/include/roscontroller.h index 74f12ef..599f116 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -115,6 +115,7 @@ private: uint64_t payload; std::map neighbours_pos_map; + std::map targets_found; std::map raw_neighbours_pos_map; std::map neighbours_time_map; int timer_step = 0; @@ -143,7 +144,7 @@ private: bool debug = false; bool setmode = false; bool BClpose = false; - std::string bzzfile_name; + std::string bzzfile_name, WPfile; std::string bcfname, dbgfname; std::string stand_by; std::string capture_srv_name; @@ -155,6 +156,7 @@ private: ros::ServiceClient stream_client; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; + ros::Publisher targetf_pub; ros::Publisher neigh_pos_pub; ros::Publisher bvmstate_pub; ros::Publisher grid_pub; diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index 82efabe..a3c2851 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -13,6 +13,7 @@ topics: bstate: bvmstate npose: neighbours_pos fstatus: fleet_status + targetf: targets_found services: fcclient: cmd/command armclient: cmd/arming diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 6ab3f50..010de1a 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -6,6 +6,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index 0b34233..20912ec 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -6,6 +6,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index d9a80ca..4ce60b5 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ static buzzvm_t VM = 0; static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; -static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets +static uint32_t MAX_MSG_SIZE = 210;//250; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; std::map users_map; @@ -291,6 +291,12 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 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 a18a093..8cdfa92 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -54,7 +54,7 @@ int buzzros_print(buzzvm_t vm) ----------------------------------------------------------- */ { std::ostringstream buffer(std::ostringstream::ate); - buffer << "[" << buzz_utility::get_robotid() << "] "; + buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] "; for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) { buzzvm_lload(vm, index); @@ -98,12 +98,13 @@ int buzzros_print(buzzvm_t vm) return buzzvm_ret0(vm); } -void setWPlist(string path) +void setWPlist(string file) /* / set the absolute path for a csv list of waypoints ----------------------------------------------------------- */ { - WPlistname = path + "include/taskallocate/waypointlist.csv"; + WPlistname = file;//path + "include/taskallocate/waypointlist.csv"; + parse_gpslist(); } float constrainAngle(float x) @@ -181,6 +182,28 @@ void parse_gpslist() fin.close(); } +void check_targets_sim(double lat, double lon, double *res) +/* +/ check if a listed target is close +----------------------------------------------------------- */ +{ + map::iterator it; + for (it = wplist_map.begin(); it != wplist_map.end(); ++it) + { + double rb[3]; + double ref[2]={lat, lon}; + double tar[2]={it->second.latitude, it->second.longitude}; + rb_from_gps(tar, rb, ref); + if(rb[0]<3.0){ + ROS_WARN("FOUND A TARGET!!! [%i]", it->first); + res[0] = it->first; + res[1] = it->second.latitude; + res[2] = it->second.longitude; + res[3] = it->second.altitude; + } + } +} + int buzz_exportmap(buzzvm_t vm) /* / Buzz closure to export a 2D map @@ -288,23 +311,42 @@ int buzzuav_addNeiStatus(buzzvm_t vm) / closure to add neighbors status to the BVM /----------------------------------------*/ { - 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); + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); // state table + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); + buzzobj_t t = buzzvm_stack_at(vm, 1); + if(buzzdict_size(t->t.value) != 5) { + ROS_WARN("Wrong neighbor status size."); + return vm->state; + } + 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; + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_tget(vm); + uint8_t id = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "ba", 1)); + buzzvm_tget(vm); + newRS.batt_lvl = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "gp", 1)); + buzzvm_tget(vm); + newRS.gps_strenght = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "xb", 1)); + buzzvm_tget(vm); + newRS.xbee = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "st", 1)); + buzzvm_tget(vm); newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + map::iterator it = neighbors_status_map.find(id); if (it != neighbors_status_map.end()) neighbors_status_map.erase(it); @@ -417,8 +459,10 @@ void set_gpsgoal(double goal[3]) rb_from_gps(goal, rb, cur_pos); if (fabs(rb[0]) < 150.0) { goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; - ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); - } + ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } else + ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } int buzzuav_arm(buzzvm_t vm) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 20d955e..1656185 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -28,7 +28,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); - buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); + buzzuav_closures::setWPlist(WPfile); // Initialize variables if(setmode) SetMode("LOITER", 0); @@ -259,6 +259,13 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } + if (n_c.getParam("WPfile", WPfile)) + ; + else + { + ROS_ERROR("Provide a .csv file to with target WP list"); + system("rosnode kill rosbuzz_node"); + } // Obtain debug mode from launch file parameter if (n_c.getParam("debug", debug)) ; @@ -417,8 +424,15 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl ROS_ERROR("Provide a fleet status out topic name in YAML file"); system("rosnode kill rosbuzz_node"); } + if (node_handle.getParam("topics/targetf", topic)) + targetf_pub = n_c.advertise(topic, 5); + else + { + ROS_ERROR("Provide a fleet status out topic name in YAML file"); + system("rosnode kill rosbuzz_node"); + } if (node_handle.getParam("topics/npose", topic)) - neigh_pos_pub = n_c.advertise(topic, MAX_NUMBER_OF_ROBOTS); + neigh_pos_pub = n_c.advertise(topic, 5); else { ROS_ERROR("Provide a Neighbor pose out topic name in YAML file"); @@ -529,11 +543,14 @@ std::string roscontroller::Compile_bzz(std::string bzzfile_name) void roscontroller::neighbours_pos_publisher() /* -/ Publish neighbours pos and id in neighbours pos topic +/ Publish neighbours pos and id in neighbours pos topic AND TARGET ACQUIRED (SIMULATED) /----------------------------------------------------*/ { auto current_time = ros::Time::now(); map::iterator it; + rosbuzz::neigh_pos msg_target_out; + msg_target_out.header.frame_id = "/world"; + msg_target_out.header.stamp = current_time; rosbuzz::neigh_pos neigh_pos_array; neigh_pos_array.header.frame_id = "/world"; neigh_pos_array.header.stamp = current_time; @@ -551,7 +568,32 @@ void roscontroller::neighbours_pos_publisher() // cout<<"iterator it val: "<< it-> first << " After convertion: " // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<second).x, (it->second).y, tf); + if(tf[0]!=-1){ + buzz_utility::Pos_struct pos_tmp; + pos_tmp.x = tf[1]; + pos_tmp.y = tf[2]; + pos_tmp.z = tf[3]; + map::iterator it = targets_found.find(round(tf[0])); + if (it != targets_found.end()) + targets_found.erase(it); + targets_found.insert(make_pair(round(tf[0]), pos_tmp)); + } } + for (it = targets_found.begin(); it != targets_found.end(); ++it) + { + sensor_msgs::NavSatFix target_tmp; + target_tmp.header.stamp = current_time; + target_tmp.header.frame_id = "/world"; + target_tmp.position_covariance_type = it->first; // custom robot id storage + target_tmp.latitude = (it->second).x; + target_tmp.longitude = (it->second).y; + target_tmp.altitude = (it->second).z; + msg_target_out.pos_neigh.push_back(target_tmp); + } + targetf_pub.publish(msg_target_out); neigh_pos_pub.publish(neigh_pos_array); }