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

clean prints

This commit is contained in:
dave 2017-05-17 16:23:51 -04:00
parent 6b36c948ce
commit f6be6ce034
5 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -38,6 +38,7 @@
#define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
#define BUZZRATE 50
using namespace std;

6
script/testflocksim.bzz
View File

@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 12.0
EPSILON = 0.5
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -224,13 +224,13 @@ neighbors.listen("cmd",
log("Swarm size: ",ROBOTS)
# Read a value from the structure
log(users)
# log(users)
#users_print(users.dataG)
if(size(users.dataG)>0)
vt.put("p", users.dataG)
# Get the number of keys in the structure
log("The vstig has ", vt.size(), " elements")
#log("The vstig has ", vt.size(), " elements")
users_save(vt.get("p"))
table_print(users.dataL)
}

2
src/buzz_utility.cpp
View File

@ -124,7 +124,7 @@ namespace buzz_utility{
/* Save entry into data table */
buzzvm_push(VM, entry);
buzzvm_tput(VM);
ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
// forcing the new table into the stigmergy....
/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));

6
src/buzzuav_closures.cpp
View File

@ -125,8 +125,8 @@ namespace buzzuav_closures{
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
}
@ -189,7 +189,7 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) {
printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
return users_add2localtable(vm, uid, rb[0], rb[1]);
} else
printf(" ---------- User too far %f\n",rb[0]);

16
src/roscontroller.cpp
View File

@ -118,17 +118,17 @@ namespace rosbzz_node{
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
ros::Rate loop_rate(10);
ros::Rate loop_rate(BUZZRATE);
loop_rate.sleep();
if(fcu_timeout<=0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else
fcu_timeout -= 1/10;
fcu_timeout -= 1/BUZZRATE;
/*sleep for the mentioned loop rate*/
timer_step+=1;
maintain_pos(timer_step);
std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
//std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
}
/* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -513,7 +513,7 @@ namespace rosbzz_node{
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){
if(timer_step >=BUZZRATE){
neighbours_pos_map.clear();
//raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear !
timer_step=0;
@ -732,10 +732,10 @@ namespace rosbzz_node{
moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position.
if(fabs(x)>0.05 || fabs(y)>0.05) {
//if(fabs(x)>0.005 || fabs(y)>0.005) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
//ROS_INFO("Sent local NON RAW position message!");
// }
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
@ -827,7 +827,7 @@ namespace rosbzz_node{
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;
//cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/