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

full v.stig. users

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 home[3];
double target[3];
double cur_rel_altitude;
uint64_t payload;
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
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)
})
}
}

12
src/buzz_utility.cpp
View File

@ -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);

2
src/buzzuav_closures.cpp
View File

@ -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));

14
src/roscontroller.cpp
View File

@ -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;