Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

beautified

This commit is contained in:
dave 2019-04-10 14:32:55 -04:00
parent 073e107da7
commit ea2e9348c1
3 changed files with 41 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/buzzuav_closures.cpp
View File

@ -619,7 +619,6 @@ int voronoi_center(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
ROS_WARN("NP: %d, Sites: %d", Poly_vert, count); ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
float* xValues = new float[count]; float* xValues = new float[count];
float* yValues = new float[count]; float* yValues = new float[count];
@ -1171,15 +1170,19 @@ int buzzuav_update_battery(buzzvm_t vm)
return vm->state; return vm->state;
} }
void store_bounding_boxes(std::vector<bounding_box> bbox){ void store_bounding_boxes(std::vector<bounding_box> bbox)
{
yolo_boxes.clear(); 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]); yolo_boxes.push_back(bbox[i]);
} }
} }
int buzzuav_update_yolo_boxes(buzzvm_t vm){ int buzzuav_update_yolo_boxes(buzzvm_t vm)
if(yolo_boxes.size()>0){ {
if (yolo_boxes.size() > 0)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "yolo_boxes", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "yolo_boxes", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -1187,7 +1190,8 @@ int buzzuav_update_yolo_boxes(buzzvm_t vm){
buzzvm_pushf(vm, yolo_boxes.size()); buzzvm_pushf(vm, yolo_boxes.size());
buzzvm_tput(vm); buzzvm_tput(vm);
for(int i=0; i< yolo_boxes.size(); i++){ for (int i = 0; i < yolo_boxes.size(); i++)
{
buzzvm_dup(vm); buzzvm_dup(vm);
std::string index = std::to_string(i); std::string index = std::to_string(i);
buzzvm_pushs(vm, buzzvm_string_register(vm, index.c_str(), 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, index.c_str(), 1));

44
src/roscontroller.cpp
View File

@ -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){ void roscontroller::yolo_box_process(const mavros_msgs::Mavlink::ConstPtr& msg)
uint64_t size = (msg->payload64.size()*sizeof(uint64_t)); {
uint64_t size = (msg->payload64.size() * sizeof(uint64_t));
uint8_t message_obt[size]; uint8_t message_obt[size];
size=0; size = 0;
// Go throught the obtained payload // Go throught the obtained payload
for (int i = 0; i < (int)msg->payload64.size(); i++) for (int i = 0; i < (int)msg->payload64.size(); i++)
{ {
uint64_t msg_tmp = msg->payload64[i]; uint64_t msg_tmp = msg->payload64[i];
memcpy((void*)(message_obt+size), (void*)(&msg_tmp), sizeof(uint64_t)); memcpy((void*)(message_obt + size), (void*)(&msg_tmp), sizeof(uint64_t));
size +=sizeof(uint64_t); size += sizeof(uint64_t);
} }
std::vector<buzzuav_closures::bounding_box> box; std::vector<buzzuav_closures::bounding_box> box;
int tot = 0; int tot = 0;
size = 0; size = 0;
memcpy((void*)&size, (void*)(message_obt+tot), sizeof(uint64_t)); memcpy((void*)&size, (void*)(message_obt + tot), sizeof(uint64_t));
tot += sizeof(uint64_t); tot += sizeof(uint64_t);
for(int i=0; i< size; i++){ for (int i = 0; i < size; i++)
double probability=0; {
int64_t xmin=0; double probability = 0;
int64_t ymin=0; int64_t xmin = 0;
int64_t xmax=0; int64_t ymin = 0;
int64_t ymax=0; int64_t xmax = 0;
memcpy((void*)&probability, (void*)(message_obt+tot), sizeof(uint64_t)); int64_t ymax = 0;
memcpy((void*)&probability, (void*)(message_obt + tot), sizeof(uint64_t));
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); 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); 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); 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); tot += sizeof(uint64_t);
uint16_t str_size=0; uint16_t str_size = 0;
memcpy((void*)&str_size, (void*)(message_obt+tot), sizeof(uint16_t)); memcpy((void*)&str_size, (void*)(message_obt + tot), sizeof(uint16_t));
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
char char_class[str_size * sizeof(char) + 1]; char char_class[str_size * sizeof(char) + 1];
memcpy((void*)char_class, (void*)(message_obt+tot), str_size); memcpy((void*)char_class, (void*)(message_obt + tot), str_size);
/* Set the termination character */ /* Set the termination character */
char_class[str_size] = '\0'; char_class[str_size] = '\0';
tot += str_size; tot += str_size;
string obj_class(char_class); 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); box.push_back(cur_box);
} }
buzzuav_closures::store_bounding_boxes(box); buzzuav_closures::store_bounding_boxes(box);