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

local pos

This commit is contained in:
isvogor 2017-05-12 10:46:39 -04:00
parent 9dc997e45c
commit 1e8f0d054a
9 changed files with 155 additions and 800 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/roscontroller.h
View File

@ -110,6 +110,11 @@ private:
ros::Subscriber obstacle_sub; ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub; ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub; ros::Subscriber relative_altitude_sub;
ros::Subscriber local_pos_sub;
double local_pos_new[3];
ros::ServiceClient stream_client; ros::ServiceClient stream_client;
int setpoint_counter; int setpoint_counter;
@ -182,6 +187,7 @@ private:
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg); void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -209,6 +215,8 @@ private:
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c); void Subscribe(ros::NodeHandle& n_c);
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
//void WaypointMissionSetup(float lat, float lng, float alt); //void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup(); void fc_command_setup();

BIN
script/testsolo.bdb
View File

Binary file not shown.

BIN
script/testsolo.bo
View File

Binary file not shown.

208
script/update_sim/testflockfev5.bzz
View File

@ -1,208 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 18.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

208
script/update_sim/testflockfev6.bzz
View File

@ -1,208 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 18.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

208
script/update_sim/testflockfev7.bzz
View File

@ -1,208 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 18.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

88
src/buzz_update.cpp
View File

@ -29,7 +29,7 @@ static int updated=0;
/*Initialize updater*/ /*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script){ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
fprintf(stdout,"intiialized file monitor.\n"); ROS_INFO("intiialized file monitor.\n");
fd=inotify_init1(IN_NONBLOCK); fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) { if ( fd < 0 ) {
perror( "inotify_init error" ); perror( "inotify_init error" );
@ -48,7 +48,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
BO_BUF = (uint8_t*)malloc(bcode_size); BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
perror(bo_filename); perror(bo_filename);
fclose(fp); //fclose(fp);
//return 0; //return 0;
} }
fclose(fp); fclose(fp);
@ -65,7 +65,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
perror(stand_by_script); perror(stand_by_script);
fclose(fp); //fclose(fp);
//return 0; //return 0;
} }
fclose(fp); fclose(fp);
@ -147,7 +147,7 @@ void code_message_outqueue_append(){
void code_message_inqueue_append(uint8_t* msg,uint16_t size){ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
fprintf(stdout,"in ms append code size %d\n", (int) size); //ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size); memcpy(updater->inmsg_queue->queue, msg, size);
@ -156,9 +156,9 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
void code_message_inqueue_process(){ void code_message_inqueue_process(){
int size=0; int size=0;
fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
if( *(int*) (updater->mode) == CODE_RUNNING){ if( *(int*) (updater->mode) == CODE_RUNNING){
//fprintf(stdout,"[debug]Inside inmsg code running"); //fprintf(stdout,"[debug]Inside inmsg code running");
@ -191,7 +191,6 @@ void update_routine(const char* bcfname,
const char* dbgfname){ const char* dbgfname){
dbgf_name=(char*)dbgfname; dbgf_name=(char*)dbgfname;
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -199,37 +198,13 @@ void update_routine(const char* bcfname,
if(*(int*)updater->mode==CODE_RUNNING){ if(*(int*)updater->mode==CODE_RUNNING){
buzzvm_function_call(VM, "updated_neigh", 0); buzzvm_function_call(VM, "updated_neigh", 0);
if(check_update()){ if(check_update()){
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); ROS_INFO("Update found \nUpdating script ...\n");
bzzfile_in_compile<<path<<"/";
path = bzzfile_in_compile.str(); if(compile_bzz()){
bzzfile_in_compile.str(""); ROS_WARN("Errors in comipilg script so staying on old script\n");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
FILE *fp;
int comp=0;
char buf[128];
fprintf(stdout,"Update found \nUpdating script ...\n");
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
comp=1;
} }
bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
}
if(pclose(fp) || comp) {
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
}
else { else {
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit FILE* fp = fopen(bcfname, "rb"); // to change file edit
@ -250,12 +225,12 @@ void update_routine(const char* bcfname,
*(uint16_t*)(updater->update_no) =update_no +1; *(uint16_t*)(updater->update_no) =update_no +1;
code_message_outqueue_append(); code_message_outqueue_append();
VM = buzz_utility::get_vm(); VM = buzz_utility::get_vm();
fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
neigh=-1; neigh=-1;
fprintf(stdout,"Sending code... \n"); ROS_INFO("Sending code... \n");
code_message_outqueue_append(); code_message_outqueue_append();
} }
delete_p(BO_BUF); delete_p(BO_BUF);
@ -268,7 +243,7 @@ void update_routine(const char* bcfname,
else{ else{
//gettimeofday(&t1, NULL); //gettimeofday(&t1, NULL);
if(neigh==0 && (!is_msg_present())){ if(neigh==0 && (!is_msg_present())){
fprintf(stdout,"Sending code... \n"); ROS_INFO("Sending code... \n");
code_message_outqueue_append(); code_message_outqueue_append();
} }
@ -277,7 +252,7 @@ void update_routine(const char* bcfname,
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM); buzzvm_pop(VM);
fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value); ROS_INFO("Barrier ..................... %i \n",tObj->i.value);
if(tObj->i.value==no_of_robot) { if(tObj->i.value==no_of_robot) {
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL); gettimeofday(&t2, NULL);
@ -307,12 +282,12 @@ return (uint8_t*)updater->outmsg_queue->size;
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
fprintf(stdout,"Initializtion of script test passed\n"); ROS_WARN("Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){ if(buzz_utility::update_step_test()){
/*data logging*/ /*data logging*/
//start =1; //start =1;
/*data logging*/ /*data logging*/
fprintf(stdout,"Step test passed\n"); ROS_WARN("Step test passed\n");
*(int*) (updater->mode) = CODE_STANDBY; *(int*) (updater->mode) = CODE_STANDBY;
//fprintf(stdout,"updater value = %i\n",updater->mode); //fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode); delete_p(updater->bcode);
@ -330,12 +305,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
/*Unable to step something wrong*/ /*Unable to step something wrong*/
else{ else{
if(*(int*) (updater->mode) == CODE_RUNNING){ if(*(int*) (updater->mode) == CODE_RUNNING){
fprintf(stdout,"step test failed, stick to old script\n"); ROS_ERROR("step test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size));
} }
else{ else{
/*You will never reach here*/ /*You will never reach here*/
fprintf(stdout,"step test failed, Return to stand by\n"); ROS_ERROR("step test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
@ -350,12 +325,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
} }
else { else {
if(*(int*) (updater->mode) == CODE_RUNNING){ if(*(int*) (updater->mode) == CODE_RUNNING){
fprintf(stdout,"Initialization test failed, stick to old script\n"); ROS_ERROR("Initialization test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size));
} }
else{ else{
/*You will never reach here*/ /*You will never reach here*/
fprintf(stdout,"Initialization test failed, Return to stand by\n"); ROS_ERROR("Initialization test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
@ -420,6 +395,23 @@ void updates_set_robots(int robots){
no_of_robot=robots; no_of_robot=robots;
} }
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/
int compile_bzz(){
/*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
bzzfile_in_compile << " -b " << path << name << ".bo";
bzzfile_in_compile << " -d " << path << name << ".bdb ";
bzzfile_in_compile << bzzfile_name;
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str());
}
void collect_data(){ void collect_data(){
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); //fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;

52
src/buzz_utility.cpp
View File

@ -502,13 +502,20 @@ static int create_stig_tables() {
/* Save bytecode file name */ /* Save bytecode file name */
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM); // Execute the global part of the script
/* Call the Init() function */ if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
buzzvm_function_call(VM, "init", 0); ROS_ERROR("Error executing global part, VM state : %i",VM->state);
return 0;
}
// Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
/* All OK */ /* All OK */
ROS_INFO("[%i] INIT DONE!!!", Robot_id); ROS_INFO("[%i] INIT DONE!!!", Robot_id);
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
@ -554,10 +561,17 @@ static int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
} }
// Execute the global part of the script // Execute the global part of the script
buzzvm_execute_script(VM); if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
return 0;
}
// Call the Init() function // Call the Init() function
buzzvm_function_call(VM, "init", 0); if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK // All OK
return 1; return 1;
} }
@ -604,9 +618,15 @@ static int create_stig_tables() {
return 0; return 0;
} }
// Execute the global part of the script // Execute the global part of the script
buzzvm_execute_script(VM); if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
return 0;
}
// Call the Init() function // Call the Init() function
buzzvm_function_call(VM, "init", 0); if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK // All OK
return 1; return 1;
} }
@ -725,19 +745,25 @@ static int create_stig_tables() {
} }
int update_step_test() { int update_step_test() {
/*Process available messages*/
in_message_process();
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM); buzzuav_closures::update_neighbors(VM);
update_users(); update_users();
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
int a = buzzvm_function_call(VM, "step", 0); int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY) {
if(a!= BUZZVM_STATE_READY) {
ROS_ERROR("%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
fprintf(stdout, "step test VM state %i\n",a); fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally\n\n"); }
}
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }

183
src/roscontroller.cpp
View File

@ -7,11 +7,6 @@ namespace rosbzz_node{
---------------*/ ---------------*/
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{ {
home[0]=0.0;home[1]=0.0;home[2]=0.0;
target[0]=0.0;target[1]=0.0;target[2]=0.0;
cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0;
ROS_INFO("Buzz_node"); ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/ /*Obtain parameters from ros parameter server*/
Rosparameters_get(n_c_priv); Rosparameters_get(n_c_priv);
@ -34,7 +29,11 @@ namespace rosbzz_node{
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
while(cur_pos[2] == 0.0f){ cur_pos.longitude = 0;
cur_pos.latitude = 0;
cur_pos.altitude = 0;
while(cur_pos.latitude == 0.0f){
ROS_INFO("Waiting for GPS. "); ROS_INFO("Waiting for GPS. ");
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
ros::spinOnce(); ros::spinOnce();
@ -113,10 +112,11 @@ namespace rosbzz_node{
get_number_of_robots(); get_number_of_robots();
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1; //if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
//buzz_utility::set_robot_var(no_of_robots); //buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates*/ /*Set no of robots for updates TODO only when not updating*/
//if(multi_msg)
updates_set_robots(no_of_robots); updates_set_robots(no_of_robots);
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
loop_rate.sleep(); loop_rate.sleep();
@ -128,7 +128,7 @@ namespace rosbzz_node{
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
std::cout<< "HOME: " << home[0] << ", " << home[1]; std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
} }
/* Destroy updater and Cleanup */ /* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1); //update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -215,6 +215,10 @@ namespace rosbzz_node{
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/stream", stream_client_name)); if(node_handle.getParam("topics/stream", stream_client_name));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
} }
/*-------------------------------------------------------- /*--------------------------------------------------------
@ -246,6 +250,8 @@ namespace rosbzz_node{
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name); stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this); users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this);
local_pos_sub = n_c.subscribe("/mavros/local_position/pose", 1000, &roscontroller::local_pos_callback, this);
multi_msg=true; multi_msg=true;
} }
@ -363,7 +369,8 @@ namespace rosbzz_node{
uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t* payload_out_ptr= buzz_utility::obt_out_msg();
uint64_t position[3]; uint64_t position[3];
/*Appened current position to message*/ /*Appened current position to message*/
memcpy(position, cur_pos, 3*sizeof(uint64_t)); double tmp[3];tmp[0]=cur_pos.latitude;tmp[1]=cur_pos.longitude;tmp[2]=cur_pos.altitude;
memcpy(position, tmp, 3*sizeof(uint64_t));
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[0]);
payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[1]);
@ -457,13 +464,7 @@ namespace rosbzz_node{
Arm(); Arm();
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
// Registering HOME POINT. // Registering HOME POINT.
if(home[0] == 0){ home = cur_pos;
//test #1: set home only once -- ok
home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2];
//test #2: set home mavros -- nope
//SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]);
}
} }
if(current_mode != "GUIDED") if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@ -543,9 +544,9 @@ namespace rosbzz_node{
void roscontroller::set_cur_pos(double latitude, void roscontroller::set_cur_pos(double latitude,
double longitude, double longitude,
double altitude){ double altitude){
cur_pos [0] = latitude; cur_pos.latitude =latitude;
cur_pos [1] = longitude; cur_pos.longitude =longitude;
cur_pos [2] = altitude; cur_pos.altitude =altitude;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -586,48 +587,10 @@ namespace rosbzz_node{
} }
} }
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ void roscontroller::gps_rb(GPS nei_pos, double out[])
{
// calculate earth radii float ned_x=0.0, ned_y=0.0;
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); gps_ned_cur(ned_x, ned_y, nei_pos);
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/
/*double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned[3];
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned[1],ned[0]);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
//out[0] = std::floor(out[0] * 1000000) / 1000000; //out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x); out[1] = atan2(ned_y,ned_x);
@ -635,48 +598,30 @@ namespace rosbzz_node{
out[2] = 0.0; out[2] = 0.0;
} }
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t)
// calculate earth radii {
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); gps_convert_ned(ned_x, ned_y,
double prime_vertical_radius = equatorial_radius * sqrt(temp); t.longitude, t.latitude,
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; cur_pos.longitude, cur_pos.latitude);
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = cur[2];
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
} }
void roscontroller::gps_ned_home(float &ned_x, float &ned_y)
{
gps_convert_ned(ned_x, ned_y,
cur_pos.longitude, cur_pos.latitude,
home.longitude, home.latitude);
}
void roscontroller::gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat)
{
double d_lon = gps_t_lon - gps_r_lon;
double d_lat = gps_t_lat - gps_r_lat;
ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat));
};
/*------------------------------------------------------ /*------------------------------------------------------
/ Update battery status into BVM from subscriber / Update battery status into BVM from subscriber
/------------------------------------------------------*/ /------------------------------------------------------*/
@ -722,6 +667,13 @@ namespace rosbzz_node{
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
} }
void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose){
local_pos_new[0] = pose->pose.position.x;
local_pos_new[1] = pose->pose.position.y;
local_pos_new[2] = pose->pose.position.z;
}
void roscontroller::users_pos(const rosbuzz::neigh_pos data){ void roscontroller::users_pos(const rosbuzz::neigh_pos data){
//ROS_INFO("Altitude out: %f", cur_rel_altitude); //ROS_INFO("Altitude out: %f", cur_rel_altitude);
@ -735,9 +687,7 @@ namespace rosbzz_node{
us[0] = data.pos_neigh[it].latitude; us[0] = data.pos_neigh[it].latitude;
us[1] = data.pos_neigh[it].longitude; us[1] = data.pos_neigh[it].longitude;
us[2] = data.pos_neigh[it].altitude; us[2] = data.pos_neigh[it].altitude;
double out[3];
cvt_rangebearing_coordinates(us, out, cur_pos);
//buzzuav_closures::set_userspos(out[0], out[1], out[2]);
buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude); buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
} }
@ -770,16 +720,15 @@ namespace rosbzz_node{
moveMsg.header.stamp = ros::Time::now(); moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++; moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = 1;
double local_pos[3]; float ned_x, ned_y;
cvt_ned_coordinates(cur_pos,local_pos,home); gps_ned_home(ned_x, ned_y);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
target[0]+=y; //target[0]+=y; target[1]+=x;
target[1]+=x; moveMsg.pose.position.x = local_pos_new[0]+y;//ned_y+y;
moveMsg.pose.position.x = target[0];//local_pos[1]+y; moveMsg.pose.position.y = local_pos_new[1]+x;//ned_x+x;
moveMsg.pose.position.y = target[1];
moveMsg.pose.position.z = z; moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;
@ -874,9 +823,13 @@ namespace rosbzz_node{
double neighbours_pos_payload[3]; double neighbours_pos_payload[3];
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
// cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; GPS nei_pos;
nei_pos.latitude=neighbours_pos_payload[0];
nei_pos.longitude=neighbours_pos_payload[1];
nei_pos.altitude=neighbours_pos_payload[2];
double cvt_neighbours_pos_payload[3]; double cvt_neighbours_pos_payload[3];
cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos); // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;