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

changed absolute_position to fit argos implementation

This commit is contained in:
dave 2017-12-21 13:28:06 -05:00 committed by vivek-shankar
parent 292c4fda8a
commit 4e4bacb25d
3 changed files with 52 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
buzz_scripts/include/rrtstar.bzz
View File

@ -136,8 +136,8 @@ function getproxobs (m_curpos) {
reduce(proximity, reduce(proximity,
function(key, value, acc) { function(key, value, acc) {
obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw)) obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw))
obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw)) obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw))
per = math.vec2.sub(obs,cur_cell) per = math.vec2.sub(obs,cur_cell)
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)

32
buzz_scripts/include/uavstates.bzz
View File

@ -34,13 +34,13 @@ function idle() {
function takeoff() { function takeoff() {
BVMSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude} homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1) barrier_set(ROBOTS, action, land, -1)
barrier_ready() barrier_ready()
} else { } else {
log("Altitude: ", absolute_position.altitude) log("Altitude: ", pose.position.altitude)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
@ -105,8 +105,8 @@ function gotoWPRRT(transf) {
# create the map # create the map
if(map==nil) { if(map==nil) {
dyn_init_map(rc_goal) dyn_init_map(rc_goal)
homegps.lat = absolute_position.latitude homegps.lat = pose.position.latitude
homegps.long = absolute_position.longitude homegps.long = pose.position.longitude
} }
if(Path==nil) { if(Path==nil) {
@ -122,7 +122,7 @@ function gotoWPRRT(transf) {
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
populateGrid(m_pos) populateGrid(m_pos)
if(check_path(Path, cur_goal_l, m_pos, 0)) { if(check_path(Path, cur_goal_l, m_pos, 0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude) uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
Path = nil Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
# re-start the planner # re-start the planner
@ -130,7 +130,7 @@ function gotoWPRRT(transf) {
cur_goal_l = 1 cur_goal_l = 1
} else { } else {
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude) uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude)
} }
} else } else
cur_goal_l = cur_goal_l + 1 cur_goal_l = cur_goal_l + 1
@ -148,11 +148,11 @@ function gotoWP(transf) {
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else else
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
} }
function follow() { function follow() {
@ -260,8 +260,8 @@ function LimitAngle(angle){
} }
function vec_from_gps(lat, lon, home_ref) { function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - absolute_position.longitude d_lon = lon - pose.position.longitude
d_lat = lat - absolute_position.latitude d_lat = lat - pose.position.latitude
if(home_ref == 1) { if(home_ref == 1) {
d_lon = lon - homegps.long d_lon = lon - homegps.long
d_lat = lat - homegps.lat d_lat = lat - homegps.lat
@ -277,17 +277,17 @@ function gps_from_vec(vec) {
Lgoal = {.latitude=0, .longitude=0} Lgoal = {.latitude=0, .longitude=0}
Vrange = math.vec2.length(vec) Vrange = math.vec2.length(vec)
Vbearing = LimitAngle(math.atan(vec.y, vec.x)) Vbearing = LimitAngle(math.atan(vec.y, vec.x))
# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude) # print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
latR = absolute_position.latitude*math.pi/180.0; latR = pose.position.latitude*math.pi/180.0;
lonR = absolute_position.longitude*math.pi/180.0; lonR = pose.position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.latitude = target_lat*180.0/math.pi;
Lgoal.longitude = target_lon*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi;
#d_lat = (vec.x / 6371000.0)*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
#goal.latitude = d_lat + absolute_position.latitude; #goal.latitude = d_lat + pose.position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + absolute_position.longitude; #goal.longitude = d_lon + pose.position.longitude;
return Lgoal return Lgoal
} }

44
src/buzzuav_closures.cpp
View File

@ -678,25 +678,49 @@ int buzzuav_update_currentpos(buzzvm_t vm)
/ Update the BVM position table / Update the BVM position table
/------------------------------------------------------*/ /------------------------------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzobj_t tPoseTable = buzzvm_stack_at(vm, 1);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_gstore(vm);
// Create table for i-th read
buzzvm_pusht(vm);
buzzobj_t tPosition = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 0));
buzzvm_pushf(vm, cur_pos[0]); buzzvm_pushf(vm, cur_pos[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0));
buzzvm_pushf(vm, cur_pos[1]); buzzvm_pushf(vm, cur_pos[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0));
buzzvm_pushf(vm, cur_pos[2]); buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); // Store read table in the proximity table
buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1)); buzzvm_push(vm, tPoseTable);
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0));
buzzvm_push(vm, tPosition);
buzzvm_tput(vm);
// Create table for i-th read
buzzvm_pusht(vm);
buzzobj_t tOrientation = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tOrientation);
buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 0));
buzzvm_pushf(vm, cur_pos[3]); buzzvm_pushf(vm, cur_pos[3]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); // Store read table in the proximity table
buzzvm_push(vm, tPoseTable);
buzzvm_pushs(vm, buzzvm_string_register(vm, "orientation", 0));
buzzvm_push(vm, tOrientation);
buzzvm_tput(vm);
return vm->state; return vm->state;
} }