diff --git a/include/roscontroller.h b/include/roscontroller.h index 6dc1859..4169307 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -110,6 +110,11 @@ private: ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; + + ros::Subscriber local_pos_sub; + double local_pos_new[3]; + + ros::ServiceClient stream_client; int setpoint_counter; @@ -182,6 +187,7 @@ private: void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void users_pos(const rosbuzz::neigh_pos msg); + /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); @@ -209,6 +215,8 @@ private: /*Robot independent subscribers*/ 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 fc_command_setup(); diff --git a/script/testsolo.bdb b/script/testsolo.bdb index e3f057f..d25ea48 100644 Binary files a/script/testsolo.bdb and b/script/testsolo.bdb differ diff --git a/script/testsolo.bo b/script/testsolo.bo index 294076a..d44323f 100644 Binary files a/script/testsolo.bo and b/script/testsolo.bo differ diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev5.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -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() { -} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev6.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -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() { -} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev7.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -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() { -} diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 3a77dba..69d9ec8 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -29,7 +29,7 @@ static int updated=0; /*Initialize updater*/ 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); if ( fd < 0 ) { 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); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { perror(bo_filename); - fclose(fp); + //fclose(fp); //return 0; } 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); if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { perror(stand_by_script); - fclose(fp); + //fclose(fp); //return 0; } fclose(fp); @@ -147,7 +147,7 @@ void code_message_outqueue_append(){ void code_message_inqueue_append(uint8_t* msg,uint16_t size){ 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->size = (uint8_t*)malloc(sizeof(uint16_t)); 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(){ int size=0; - fprintf(stdout,"[debug]Updater 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)) ); - fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + 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){ //fprintf(stdout,"[debug]Inside inmsg code running"); @@ -191,7 +191,6 @@ void update_routine(const char* bcfname, const char* dbgfname){ dbgf_name=(char*)dbgfname; buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_gstore(VM); @@ -199,37 +198,13 @@ void update_routine(const char* bcfname, if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); 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("\\/")); - bzzfile_in_compile<update_no) =update_no +1; code_message_outqueue_append(); 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_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_gstore(VM); neigh=-1; - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } delete_p(BO_BUF); @@ -268,7 +243,7 @@ void update_routine(const char* bcfname, else{ //gettimeofday(&t1, NULL); if(neigh==0 && (!is_msg_present())){ - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } @@ -277,7 +252,7 @@ void update_routine(const char* bcfname, buzzvm_gload(VM); buzzobj_t tObj = buzzvm_stack_at(VM, 1); 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) { *(int*)(updater->mode) = CODE_RUNNING; 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 ){ 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()){ /*data logging*/ //start =1; /*data logging*/ - fprintf(stdout,"Step test passed\n"); + ROS_WARN("Step test passed\n"); *(int*) (updater->mode) = CODE_STANDBY; //fprintf(stdout,"updater value = %i\n",updater->mode); 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*/ else{ 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)); } else{ /*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, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); 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 { 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)); } else{ /*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, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -420,6 +395,23 @@ void updates_set_robots(int 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/"; //<<" "<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 */ - 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); } @@ -554,10 +561,17 @@ static int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; } + // 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 - 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 return 1; } @@ -604,9 +618,15 @@ static int create_stig_tables() { return 0; } // 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 - 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 return 1; } @@ -725,19 +745,25 @@ static int create_stig_tables() { } int update_step_test() { - + /*Process available messages*/ + in_message_process(); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); + //set_robot_var(buzzdict_size(VM->swarmmembers)+1); 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, " execution terminated abnormally\n\n"); - } + } + return a == BUZZVM_STATE_READY; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 273c912..1f27497 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,11 +7,6 @@ namespace rosbzz_node{ ---------------*/ 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"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -34,7 +29,11 @@ namespace rosbzz_node{ setpoint_counter = 0; 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::Duration(0.5).sleep(); ros::spinOnce(); @@ -113,10 +112,11 @@ namespace rosbzz_node{ get_number_of_robots(); //if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1; //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); - /*run once*/ - ros::spinOnce(); + /*run once*/ + ros::spinOnce(); /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); @@ -128,7 +128,7 @@ namespace rosbzz_node{ timer_step+=1; maintain_pos(timer_step); - std::cout<< "HOME: " << home[0] << ", " << home[1]; + std::cout<< "HOME: " << home.latitude << ", " << home.longitude; } /* Destroy updater and Cleanup */ //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");} 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");} + + + + } /*-------------------------------------------------------- @@ -246,6 +250,8 @@ namespace rosbzz_node{ stream_client = n_c.serviceClient(stream_client_name); 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; } @@ -363,7 +369,8 @@ namespace rosbzz_node{ uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*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; payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[1]); @@ -457,13 +464,7 @@ namespace rosbzz_node{ Arm(); ros::Duration(0.5).sleep(); // Registering HOME POINT. - if(home[0] == 0){ - //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]); - - } + home = cur_pos; } if(current_mode != "GUIDED") 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, double longitude, double altitude){ - cur_pos [0] = latitude; - cur_pos [1] = longitude; - cur_pos [2] = altitude; + cur_pos.latitude =latitude; + cur_pos.longitude =longitude; + cur_pos.altitude =altitude; } /*----------------------------------------------------------- @@ -586,48 +587,10 @@ namespace rosbzz_node{ } } - void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ - - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - 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])); + void roscontroller::gps_rb(GPS nei_pos, double out[]) + { + float ned_x=0.0, ned_y=0.0; + gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); @@ -635,48 +598,30 @@ namespace rosbzz_node{ out[2] = 0.0; } - void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - 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]; - 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_cur(float &ned_x, float &ned_y, GPS t) + { + gps_convert_ned(ned_x, ned_y, + t.longitude, t.latitude, + cur_pos.longitude, cur_pos.latitude); } + 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 /------------------------------------------------------*/ @@ -722,6 +667,13 @@ namespace rosbzz_node{ 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); } + + 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){ //ROS_INFO("Altitude out: %f", cur_rel_altitude); @@ -735,9 +687,7 @@ namespace rosbzz_node{ us[0] = data.pos_neigh[it].latitude; us[1] = data.pos_neigh[it].longitude; 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); } @@ -770,16 +720,15 @@ namespace rosbzz_node{ moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - double local_pos[3]; - cvt_ned_coordinates(cur_pos,local_pos,home); + float ned_x, ned_y; + gps_ned_home(ned_x, ned_y); // 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]); - /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - target[0]+=y; - target[1]+=x; - moveMsg.pose.position.x = target[0];//local_pos[1]+y; - moveMsg.pose.position.y = target[1]; + /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ + //target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = local_pos_new[0]+y;//ned_y+y; + moveMsg.pose.position.y = local_pos_new[1]+x;//ned_x+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -874,9 +823,13 @@ namespace rosbzz_node{ double neighbours_pos_payload[3]; 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]); - // 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]; - 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*/ 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;