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

Merge branch 'dev' of https://github.com/MISTLab/ROSBuzz into test

This commit is contained in:
vivek-shankar 2017-05-17 18:01:00 -04:00
parent 2d0824be00 833c2b30c3
commit f39cab4617
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_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352 #define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60 #define TIMEOUT 60
#define BUZZRATE 50
using namespace std; using namespace std;

6
script/testflocksim.bzz
View File

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

2
src/buzz_utility.cpp
View File

@ -124,7 +124,7 @@ namespace buzz_utility{
/* Save entry into data table */ /* Save entry into data table */
buzzvm_push(VM, entry); buzzvm_push(VM, entry);
buzzvm_tput(VM); 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.... // forcing the new table into the stigmergy....
/*buzzobj_t newt = buzzvm_stack_at(VM, 0); /*buzzobj_t newt = buzzvm_stack_at(VM, 0);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); 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); printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos); gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //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(" 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 buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -189,7 +189,7 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) { 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]); return users_add2localtable(vm, uid, rb[0], rb[1]);
} else } else
printf(" ---------- User too far %f\n",rb[0]); printf(" ---------- User too far %f\n",rb[0]);

16
src/roscontroller.cpp
View File

@ -125,17 +125,17 @@ namespace rosbzz_node{
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(10); ros::Rate loop_rate(BUZZRATE);
loop_rate.sleep(); loop_rate.sleep();
if(fcu_timeout<=0) if(fcu_timeout<=0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else else
fcu_timeout -= 1/10; fcu_timeout -= 1/BUZZRATE;
/*sleep for the mentioned loop rate*/ /*sleep for the mentioned loop rate*/
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
std::cout<< "HOME: " << home.latitude << ", " << home.longitude; //std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
} }
/* Destroy updater and Cleanup */ /* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1); //update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -520,7 +520,7 @@ namespace rosbzz_node{
/Refresh neighbours Position for every ten step /Refresh neighbours Position for every ten step
/---------------------------------------------*/ /---------------------------------------------*/
void roscontroller::maintain_pos(int tim_step){ void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){ if(timer_step >=BUZZRATE){
neighbours_pos_map.clear(); neighbours_pos_map.clear();
//raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear ! //raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear !
timer_step=0; timer_step=0;
@ -739,10 +739,10 @@ namespace rosbzz_node{
moveMsg.pose.orientation.w = 1; moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position. // 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); 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){ void roscontroller::SetMode(std::string mode, int delay_miliseconds){
@ -834,7 +834,7 @@ namespace rosbzz_node{
gps_rb(nei_pos, cvt_neighbours_pos_payload); gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); 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*/ /*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]); 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*/ /*Put RID and pos*/