From 6a8ff44ee73fd84fc05726d04ec91fee8e252788 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 8 Jan 2019 14:26:38 -0500 Subject: [PATCH] Yolo bounding box integration into the BVM. --- buzz_scripts/include/act/naviguation.bzz | 19 ++++- buzz_scripts/include/act/neighborcomm.bzz | 5 ++ buzz_scripts/include/act/states.bzz | 93 +++++++++++++++++++++++ buzz_scripts/main.bzz | 7 +- include/buzzuav_closures.h | 24 ++++++ include/roscontroller.h | 5 ++ launch/launch_config/topics.yaml | 1 + src/buzz_utility.cpp | 1 + src/buzzuav_closures.cpp | 54 +++++++++++++ src/roscontroller.cpp | 65 +++++++++++++--- 10 files changed, 262 insertions(+), 12 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 80488b1..1017195 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -1,4 +1,4 @@ -GOTO_MAXVEL = 2.5 # m/steps +GOTO_MAXVEL = 12.5 # m/steps GOTO_MAXDIST = 250 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. @@ -41,3 +41,20 @@ function LimitSpeed(vel_vec, factor){ vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) return vel_vec } + +# Core naviguation function to travel to provided GPS target location. +function goto_gps_in(transf, gps_to_go) { + m_navigation = vec_from_gps(gps_to_go.latitude, gps_to_go.longitude, 0) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination + ; + # transf() + } else { + #log("move to x", m_navigation.x," y ", m_navigation.y) + m_navigation = LimitSpeed(m_navigation, 1.0) + #log("after limit speed move to x", m_navigation.x," y ", m_navigation.y) + goto_abs(m_navigation.x, m_navigation.y, gps_to_go.altitude - pose.position.altitude, 0.0) + } +} \ No newline at end of file diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 7e5089d..e6f6075 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -126,6 +126,11 @@ function nei_cmd_listen() { #barrier_ready(904) #neighbors.broadcast("cmd", 904) } + else if(value==555 and BVMSTATE=="YOLO_DEMO"){ + YOLO_NEI_TARGET = 1 + } else if(value==556 and BVMSTATE=="YOLO_DEMO"){ + YOLO_NEI_TARGET = 2 + } } } }) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 4c6f3aa..658a763 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -22,6 +22,16 @@ pic_time = 0 g_it = 0 homegps = {} +YOLO_TARGET = 1 +YOLO_NEI_TARGET = 0 +yolo_init = 0 +yolo_left_buff = 0 +yolo_right_buff = 0 +YOLO_L_GOAL_1 = math.vec2.new(20,0,0) +YOLO_L_GOAL_2 = math.vec2.new(-20,0,0) +YOLO_GPS_1 = {.lat =0, .long=0} +YOLO_GPS_2 = {.lat =0, .long=0} + # Core state function when on the ground function turnedoff() { BVMSTATE = "TURNEDOFF" @@ -32,6 +42,89 @@ function idle() { BVMSTATE = "IDLE" } +# yolo Demostration +function yolo_demo(){ + var left_count = 0 + var right_count = 0 + var img_bound_x={.l=0,.r=320} # assumed left image pixel x boundary + var img_bound_y={.t=0,.b=480} # assumed left image pixel y boundary + if(yolo_init == 0){ + yolo_init = 1 + yolo_gps_in = {.latitude=pose.position.latitude, .longitude=pose.position.longitude, .altitude= pose.position.altitude} + yolo_left_buff = 0 + yolo_right_buff = 0 + YOLO_GPS_1 = gps_from_vec(YOLO_L_GOAL_1) + YOLO_GPS_2 = gps_from_vec(YOLO_L_GOAL_2) + } + #log("POS lat : ",pose.position.latitude, " long: ", pose.position.longitude ) + if(yolo_boxes != nil){ + # log("yolo tab size",size(yolo_boxes)) + var yol_i = 0 + while(yol_i < yolo_boxes.size){ + var table_id = string.tostring(yol_i) + if(yolo_boxes[table_id].class == "person"){ + var diff_x = (yolo_boxes[table_id].xmax - yolo_boxes[table_id].xmin) / 2 + var mid_x = yolo_boxes[table_id].xmax - diff_x + if( (mid_x >=img_bound_x.l and mid_x <=img_bound_x.r)){ + # person is in the left side of the image + left_count = left_count + 1 + } + else{ + # person is in the right side of the image + right_count = right_count + 1 + } + } + yol_i = yol_i + 1 + } + # log("left_count: ", left_count, " right_count: ", right_count) + if(left_count > right_count){ + if(1){ + YOLO_TARGET = 1 + neighbors.broadcast("cmd", 555) + #storegoal(YOLO_GPS_1.latitude, YOLO_GPS_1.longitude, pose.position.altitude) + yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude} + # log("left dominating") + yolo_left_buff = 0 + yolo_right_buff = 0 + } + else{ + yolo_left_buff = yolo_left_buff + 1 + yolo_right_buff = 0 + } + } + else if(left_count < right_count){ + if(1){ + YOLO_TARGET = 2 + neighbors.broadcast("cmd", 556) + yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude} + # log("right dominating") + yolo_right_buff = 0 + yolo_left_buff = 0 + } + else{ + yolo_right_buff = yolo_right_buff + 1 + yolo_left_buff = 0 + } + } + goto_gps_in(yolo_demo, yolo_gps_in) + } + if( YOLO_NEI_TARGET == 1){ + # YOLO_NEI_TARGET =0 + YOLO_TARGET = 1 + yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude} + # log("left dominating") + goto_gps_in(yolo_demo, yolo_gps_in) + } + else if(YOLO_NEI_TARGET == 2){ + YOLO_TARGET = 2 + # YOLO_NEI_TARGET = 0 + yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude} + # log("right dominating") + goto_gps_in(yolo_demo, yolo_gps_in) + } +} + + # Core state function to launch the robot: takeoff and wait for others, or stop (land) function launch() { BVMSTATE = "LAUNCH" diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 758005f..99d5372 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "YOLO_DEMO" LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE. ##### @@ -39,7 +39,7 @@ function init() { nei_cmd_listen() # Starting state: TURNEDOFF to wait for user input. - BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCH" } # Executed at each time step. @@ -58,6 +58,8 @@ function step() { statef=turnedoff else if(BVMSTATE=="CUSFUN") statef=cusfun + else if(BVMSTATE == "YOLO_DEMO") + statef=yolo_demo else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE @@ -107,6 +109,7 @@ function step() { log("Current state: ", BVMSTATE) + } # Executed once when the robot (or the simulator) is reset. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 71a1d6a..6321da3 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -14,6 +14,22 @@ namespace buzzuav_closures { +struct bounding_box_struct +{ + std::string obj_class; + double probability; + int64_t xmin; + int64_t ymin; + int64_t xmax; + int64_t ymax; + bounding_box_struct(std::string c, double prob, int64_t xmi, int64_t ymi, int64_t xma, int64_t yma) + : obj_class(c), probability(prob), xmin(xmi), ymin(ymi), xmax(xma), ymax(yma){}; + bounding_box_struct() + { + } +}; +typedef struct bounding_box_struct bounding_box; + /* * prextern int() function in Buzz * This function is used to print data from buzz @@ -76,6 +92,14 @@ void rc_call(int rc_cmd); * sets the battery state */ void set_battery(float voltage, float current, float remaining); +/* + * Update yolo boxes into buzz + */ +int buzzuav_update_yolo_boxes(buzzvm_t vm); +/* + *Stores the received bounding boxes + */ +void store_bounding_boxes(std::vector bbox); /* * sets the xbee network status */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 23ef2a1..7c2684a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -147,6 +147,7 @@ private: std::string bcfname, dbgfname; std::string stand_by; std::string capture_srv_name; + std::string yolobox_sub_name; // ROS service, publishers and subscribers ros::ServiceClient mav_client; @@ -171,6 +172,7 @@ private: ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; ros::Subscriber local_pos_sub; + ros::Subscriber yolo_sub; std::map m_smTopic_infos; @@ -257,6 +259,9 @@ private: /*payload callback callback*/ void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); + /*yolo bounding box callback function*/ + void yolo_box_process(const mavros_msgs::Mavlink::ConstPtr& msg); + /* RC commands service */ bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res); diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index a3c2851..29ce56c 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -14,6 +14,7 @@ topics: npose: neighbours_pos fstatus: fleet_status targetf: targets_found + yolobox: intermediator_ros/bounding_boxes services: fcclient: cmd/command armclient: cmd/arming diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index af2606a..ce911f0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -483,6 +483,7 @@ void update_sensors() buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); buzzuav_closures::buzzuav_update_flight_status(VM); + buzzuav_closures::buzzuav_update_yolo_boxes(VM); } void buzz_script_step() diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index fd04090..d21f06c 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -38,6 +38,7 @@ static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; static bool logVoronoi = false; +static std::vector yolo_boxes; std::ofstream voronoicsv; @@ -1170,6 +1171,59 @@ int buzzuav_update_battery(buzzvm_t vm) return vm->state; } +void store_bounding_boxes(std::vector bbox){ + yolo_boxes.clear(); + for(int i = 0; i< bbox.size(); i++){ + yolo_boxes.push_back(bbox[i]); + } +} + +int buzzuav_update_yolo_boxes(buzzvm_t vm){ + if(yolo_boxes.size()>0){ + buzzvm_pushs(vm, buzzvm_string_register(vm, "yolo_boxes", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "size", 1)); + buzzvm_pushf(vm, yolo_boxes.size()); + buzzvm_tput(vm); + + for(int i=0; i< yolo_boxes.size(); i++){ + buzzvm_dup(vm); + std::string index = std::to_string(i); + buzzvm_pushs(vm, buzzvm_string_register(vm, index.c_str(), 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "class", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, yolo_boxes[i].obj_class.c_str(), 1)); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "probability", 1)); + buzzvm_pushf(vm, yolo_boxes[i].probability); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "xmin", 1)); + buzzvm_pushf(vm, yolo_boxes[i].xmin); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "xmax", 1)); + buzzvm_pushf(vm, yolo_boxes[i].xmax); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "ymin", 1)); + buzzvm_pushf(vm, yolo_boxes[i].ymin); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "ymax", 1)); + buzzvm_pushf(vm, yolo_boxes[i].ymax); + buzzvm_tput(vm); + buzzvm_tput(vm); + } + buzzvm_gstore(vm); + yolo_boxes.clear(); + } + return vm->state; +} + /* / Set of function to update interface variable of xbee network status ----------------------------------------------------------------------*/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d42e5cd..43940cf 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -365,6 +365,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) node_handle.getParam("topics/inpayload", topic); m_smTopic_infos.insert(pair(topic, "mavros_msgs::Mavlink")); + + node_handle.getParam("topics/yolobox", yolobox_sub_name); } void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handle) @@ -433,7 +435,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/fstatus", topic)) - MPpayload_pub = n_c.advertise(topic, 5); + MPpayload_pub = n_c.advertise(topic, 1); else { ROS_ERROR("Provide a fleet status out topic name in YAML file"); @@ -447,7 +449,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/npose", topic)) - neigh_pos_pub = n_c.advertise(topic, 5); + neigh_pos_pub = n_c.advertise(topic, 1); else { ROS_ERROR("Provide a Neighbor pose out topic name in YAML file"); @@ -477,7 +479,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_ // Subscribers Subscribe(n_c); - + yolo_sub = n_c.subscribe(yolobox_sub_name, 1, &roscontroller::yolo_box_process, this); // Publishers and service Clients PubandServ(n_c, n_c_priv); @@ -1217,12 +1219,9 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) /---------------------------------------------------------------------------------------- / Message format of payload (Each slot is uint64_t) / _________________________________________________________________________________________________ -/| | | | | - * | -/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs - * with size......... | -/|_____|_____|_____|________________________________________________|______________________________| - +/| | | | | | | | | +/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | +/|_____|_____|_____|________________|________|_____________|________|______________________________| -----------------------------------------------------------------------------------------------------*/ { // decode msg header @@ -1302,6 +1301,54 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) } } +void roscontroller::yolo_box_process(const mavros_msgs::Mavlink::ConstPtr& msg){ + uint64_t size = (msg->payload64.size()*sizeof(uint64_t)); + uint8_t message_obt[size]; + size=0; + // Go throught the obtained payload + for (int i = 0; i < (int)msg->payload64.size(); i++) + { + uint64_t msg_tmp = msg->payload64[i]; + memcpy((void*)(message_obt+size), (void*)(&msg_tmp), sizeof(uint64_t)); + size +=sizeof(uint64_t); + } + + std::vector box; + int tot = 0; + size = 0; + memcpy((void*)&size, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + for(int i=0; i< size; i++){ + double probability=0; + int64_t xmin=0; + int64_t ymin=0; + int64_t xmax=0; + int64_t ymax=0; + memcpy((void*)&probability, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + memcpy((void*)&xmin, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + memcpy((void*)&ymin, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + memcpy((void*)&xmax, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + memcpy((void*)&ymax, (void*)(message_obt+tot), sizeof(uint64_t)); + tot += sizeof(uint64_t); + uint16_t str_size=0; + memcpy((void*)&str_size, (void*)(message_obt+tot), sizeof(uint16_t)); + tot += sizeof(uint16_t); + char char_class[str_size * sizeof(char) + 1]; + memcpy((void*)char_class, (void*)(message_obt+tot), str_size); + /* Set the termination character */ + char_class[str_size] = '\0'; + tot += str_size; + string obj_class(char_class); + buzzuav_closures::bounding_box cur_box(obj_class,probability,xmin,ymin,xmax,ymax); + box.push_back(cur_box); + } + buzzuav_closures::store_bounding_boxes(box); +} + bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) /* / Catch the ROS service call from a custom remote controller (Mission Planner)