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

Merge commit '33e19e16aedb33f792088967064e5c5da8de13ac' into ros_drones_ws

This commit is contained in:
dave 2018-11-12 17:45:33 -05:00
parent 777c1b43d2 33e19e16ae
commit 9050de5954
4 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,7 +54,7 @@ function launch() {
function launch_switch() {
BVMSTATE = "LAUNCH_SWITCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
barrier_ready(22)
@ -74,11 +74,13 @@ gohomeT=100
function goinghome() {
BVMSTATE = "GOHOME"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
} else
BVMSTATE = AUTO_LAUNCH_STATE
storegoal(homegps.lat, homegps.lng, pose.position.altitude)
goto_gps(AUTO_LAUNCH_STATE)
#if(gohomeT > 0) { # TODO: Make a real check if home is reached
# gohome()
# gohomeT = gohomeT - 1
#} else
# BVMSTATE = AUTO_LAUNCH_STATE
}
}

2
buzz_scripts/include/utils/conversions.bzz
View File

@ -33,7 +33,7 @@ function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - pose.position.longitude
d_lat = lat - pose.position.latitude
if(home_ref == 1) {
d_lon = lon - homegps.long
d_lon = lon - homegps.lng
d_lat = lat - homegps.lat
}
ned_x = d_lat/180*math.pi * 6371000.0;

3
include/roscontroller.h
View File

@ -118,7 +118,6 @@ private:
std::map<int, buzz_utility::Pos_struct> targets_found;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
int timer_step = 0;
int robot_id = 0;
ros::Time logical_clock;
ros::Time previous_step_time;
@ -222,7 +221,7 @@ private:
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
void clear_pos();
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);

18
src/roscontroller.cpp
View File

@ -111,6 +111,7 @@ void roscontroller::RosControllerRun()
/rosbuzz_node main loop method
/--------------------------------------------------*/
{
int timer_step = 0;
// Set the Buzz bytecode
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id))
{
@ -161,6 +162,12 @@ void roscontroller::RosControllerRun()
// Call Step from buzz script
buzz_utility::buzz_script_step();
// Force a refresh on neighbors array once in a while
if (timer_step >= 20*BUZZRATE){
clear_pos();
timer_step = 0;
} else
timer_step++;
// Prepare messages and publish them
prepare_msg_and_publish();
// Call the flight controler service
@ -185,9 +192,6 @@ void roscontroller::RosControllerRun()
buzzuav_closures::rc_call(NAV_LAND);
else
fcu_timeout -= 1 / BUZZRATE;
timer_step += 1;
// Force a refresh on neighbors array once in a while
maintain_pos(timer_step);
// DEBUG
// std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
}
@ -941,19 +945,17 @@ script
}
}
void roscontroller::maintain_pos(int tim_step)
void roscontroller::clear_pos()
/*
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
{
if (timer_step >= BUZZRATE)
{
neighbours_pos_map.clear();
raw_neighbours_pos_map.clear();
buzzuav_closures::clear_neighbours_pos();
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// have to clear !
timer_step = 0;
}
}
void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)