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

Yolo bounding box integration into the BVM.

This commit is contained in:
vivek-shankar 2019-01-08 14:26:38 -05:00
parent fbb0f108b1
commit 6a8ff44ee7
10 changed files with 262 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
buzz_scripts/include/act/naviguation.bzz
View File

@ -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)
}
}

5
buzz_scripts/include/act/neighborcomm.bzz
View File

@ -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
}
}
}
})

93
buzz_scripts/include/act/states.bzz
View File

@ -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"

7
buzz_scripts/main.bzz
View File

@ -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.

24
include/buzzuav_closures.h
View File

@ -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<bounding_box> bbox);
/*
* sets the xbee network status
*/

5
include/roscontroller.h
View File

@ -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<std::string, std::string> 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);

1
launch/launch_config/topics.yaml
View File

@ -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

1
src/buzz_utility.cpp
View File

@ -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()

54
src/buzzuav_closures.cpp
View File

@ -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<bounding_box> 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<bounding_box> 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
----------------------------------------------------------------------*/

65
src/roscontroller.cpp
View File

@ -365,6 +365,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
node_handle.getParam("topics/inpayload", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(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<mavros_msgs::Mavlink>(topic, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>(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<rosbuzz::neigh_pos>(topic, 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(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<buzzuav_closures::bounding_box> 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)