diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp index 7262fd5..755f1a6 100644 --- a/src/VoronoiDiagramGenerator.cpp +++ b/src/VoronoiDiagramGenerator.cpp @@ -224,7 +224,7 @@ struct Halfedge* VoronoiDiagramGenerator::ELleftbnd(struct Point* p) he = he->ELright; } while (he != ELrightend && right_of(he, p)); // keep going right on the list until either the end is reached, or // you find the 1st edge which the point - he = he->ELleft; // isn't to the right of + he = he->ELleft; // isn't to the right of } else // if the point is to the left of the HalfEdge, then search left for the HE just to the left of the point do @@ -947,7 +947,7 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate) v = lbnd->vertex; // get the vertex that caused this event makevertex(v); // set the vertex number - couldn't do this earlier since we didn't know when it would be - // processed + // processed endpoint(lbnd->ELedge, lbnd->ELpm, v); // set the endpoint of the left HalfEdge to be this vector endpoint(rbnd->ELedge, rbnd->ELpm, v); // set the endpoint of the right HalfEdge to be this vector ELdelete(lbnd); // mark the lowest HE for deletion - can't delete yet because there might be pointers to it in @@ -968,8 +968,8 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate) // the formula of the line, and assigns a line number to it bisector = HEcreate(e, pm); // create a HE from the Edge 'e', and make it point to that edge with its ELedge // field - ELinsert(llbnd, bisector); // insert the new bisector to the right of the left HE - endpoint(e, re - pm, v); // set one endpoint to the new edge to be the vector point 'v'. + ELinsert(llbnd, bisector); // insert the new bisector to the right of the left HE + endpoint(e, re - pm, v); // set one endpoint to the new edge to be the vector point 'v'. // If the site to the left of this bisector is higher than the right // Site, then this endpoint is put in position 0; otherwise in pos 1 deref(v); // delete the vector 'v' diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d2afea8..852b54f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -619,7 +619,6 @@ int voronoi_center(buzzvm_t vm) return buzzvm_ret0(vm); } - ROS_WARN("NP: %d, Sites: %d", Poly_vert, count); float* xValues = new float[count]; float* yValues = new float[count]; @@ -1171,23 +1170,28 @@ int buzzuav_update_battery(buzzvm_t vm) return vm->state; } -void store_bounding_boxes(std::vector bbox){ +void store_bounding_boxes(std::vector bbox) +{ yolo_boxes.clear(); - for(int i = 0; i< bbox.size(); i++){ + 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){ +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++){ + + 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)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ba5c569..7e9e546 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -301,7 +301,7 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) // Obtain standby script to run during update n_c.getParam("stand_by", stand_by); - // Obtain collision avoidance mode + // Obtain collision avoidance mode n_c.getParam("ca_on", ca_on); if (n_c.getParam("xbee_plugged", xbeeplugged)) @@ -1305,49 +1305,51 @@ 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)); +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; + 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); + 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)); + 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)); + 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)); + memcpy((void*)&xmin, (void*)(message_obt + tot), sizeof(uint64_t)); tot += sizeof(uint64_t); - memcpy((void*)&ymin, (void*)(message_obt+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)); + memcpy((void*)&xmax, (void*)(message_obt + tot), sizeof(uint64_t)); tot += sizeof(uint64_t); - memcpy((void*)&ymax, (void*)(message_obt+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)); + 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); + 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); + buzzuav_closures::bounding_box cur_box(obj_class, probability, xmin, ymin, xmax, ymax); box.push_back(cur_box); } buzzuav_closures::store_bounding_boxes(box);