From a08e121228e04c978c6c12e59fd28d5044cb3b79 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 10 May 2017 21:32:10 -0400 Subject: [PATCH] full v.stig. users --- include/roscontroller.h | 1 + script/testflocksim.bzz | 4 ++-- src/buzz_utility.cpp | 12 +++++------- src/buzzuav_closures.cpp | 2 +- src/roscontroller.cpp | 14 +++++++++----- 5 files changed, 18 insertions(+), 15 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 43cb117..123cc8a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -71,6 +71,7 @@ private: double cur_pos[3]; double home[3]; + double target[3]; double cur_rel_altitude; uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index 61976ae..48f47dd 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -153,8 +153,8 @@ function users_save(t) { # printing the contents of a table: a custom function function table_print(t) { if(size(t)>0) { - foreach(t, function(key, value) { - log(key, " -> ", value) + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) }) } } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c86e7d5..f1679c4 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -50,13 +50,9 @@ namespace buzz_utility{ (it->second).x, (it->second).y, (it->second).z); - buzzusers_add(it->first+1, - (it->second).x, - (it->second).y, - (it->second).z); } - }else - ROS_INFO("[%i] No new users",Robot_id); + }/*else + ROS_INFO("[%i] No new users",Robot_id);*/ } int buzzusers_reset() { @@ -95,7 +91,7 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_tget(VM); if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { - ROS_INFO("Empty data, create a new table"); + //ROS_INFO("Empty data, create a new table"); buzzvm_pop(VM); buzzvm_push(VM, nbr); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); @@ -424,6 +420,8 @@ static int create_stig_tables() { buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_push(VM,t); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_pusht(VM); buzzobj_t data = buzzvm_stack_at(VM, 1); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index c64f5c5..47ee6e0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -139,7 +139,7 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); buzzvm_tget(vm); if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { - ROS_INFO("Empty data, create a new table"); + //ROS_INFO("Empty data, create a new table"); buzzvm_pop(vm); buzzvm_push(vm, nbr); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1deee9a..52c543c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,6 +7,10 @@ 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; + cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; + target[0]=0.0;target[1]=0.0;target[2]=0.0; + ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -32,7 +36,6 @@ namespace rosbzz_node{ setpoint_counter = 0; fcu_timeout = TIMEOUT; - home[0]=0.0;home[1]=0.0;home[2]=0.0; } /*--------------------- @@ -748,12 +751,13 @@ namespace rosbzz_node{ moveMsg.header.frame_id = 1; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); - 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 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)*/ - moveMsg.pose.position.x = local_pos[1]+y; - moveMsg.pose.position.y = local_pos[0]+x; + target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = target[0];//local_pos[1]+y; + moveMsg.pose.position.y = target[1];//local_pos[0]+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0;