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

full v.stig. users

Browse Source
This commit is contained in:
dave 2017-05-10 21:32:10 -04:00
parent 5368728d79
commit a08e121228
5 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -71,6 +71,7 @@ private:
double cur_pos[3]; double cur_pos[3];
double home[3]; double home[3];
double target[3];
double cur_rel_altitude; double cur_rel_altitude;
uint64_t payload; uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;

4
script/testflocksim.bzz
View File

@ -153,8 +153,8 @@ function users_save(t) {
# printing the contents of a table: a custom function # printing the contents of a table: a custom function
function table_print(t) { function table_print(t) {
if(size(t)>0) { if(size(t)>0) {
foreach(t, function(key, value) { foreach(t, function(u, tab) {
log(key, " -> ", value) log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
}) })
} }
} }

12
src/buzz_utility.cpp
View File

@ -50,13 +50,9 @@ namespace buzz_utility{
(it->second).x, (it->second).x,
(it->second).y, (it->second).y,
(it->second).z); (it->second).z);
buzzusers_add(it->first+1,
(it->second).x,
(it->second).y,
(it->second).z);
} }
}else }/*else
ROS_INFO("[%i] No new users",Robot_id); ROS_INFO("[%i] No new users",Robot_id);*/
} }
int buzzusers_reset() { int buzzusers_reset() {
@ -95,7 +91,7 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_tget(VM); buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { 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_pop(VM);
buzzvm_push(VM, nbr); buzzvm_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); 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_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_push(VM,t); buzzvm_push(VM,t);
buzzvm_gstore(VM); 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_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pusht(VM); buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1); buzzobj_t data = buzzvm_stack_at(VM, 1);

2
src/buzzuav_closures.cpp
View File

@ -139,7 +139,7 @@ namespace buzzuav_closures{
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_tget(vm); buzzvm_tget(vm);
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { 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_pop(vm);
buzzvm_push(vm, nbr); buzzvm_push(vm, nbr);
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));

14
src/roscontroller.cpp
View File

@ -7,6 +7,10 @@ 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;
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"); 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);
@ -32,7 +36,6 @@ namespace rosbzz_node{
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; 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; moveMsg.header.frame_id = 1;
double local_pos[3]; double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home); 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 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)*/
moveMsg.pose.position.x = local_pos[1]+y; target[0]+=y; target[1]+=x;
moveMsg.pose.position.y = local_pos[0]+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.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;