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

Merge branch 'dev' 

Conflicts:
	src/roscontroller.cpp
This commit is contained in:
isvogor 2017-04-25 21:06:53 -04:00
parent e01e6e6c5e 52bed0536f
commit 187e05aca8
11 changed files with 668 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -54,7 +54,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/buzzuav_closures.cpp
src/uav_utility.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
# Executables and libraries for installation to do

4
include/buzz_utility.h
View File

@ -30,6 +30,8 @@ int buzz_listen(const char* type,
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
int buzz_update_users_stigmergy(buzzobj_t tbl);
void in_msg_process(uint64_t* payload);
uint64_t* out_msg_process();
@ -49,7 +51,7 @@ int buzz_script_done();
int update_step_test();
uint16_t get_robotid();
int get_robotid();
buzzvm_t get_vm();

3
include/buzzuav_closures.h
View File

@ -46,6 +46,7 @@ void rc_call(int rc_cmd);
void set_battery(float voltage,float current,float remaining);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
void set_userspos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
double* getgoto();
/* updates flight status*/
@ -82,7 +83,7 @@ int buzzuav_update_battery(buzzvm_t vm);
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it

24
include/roscontroller.h
View File

@ -46,7 +46,7 @@ namespace rosbzz_node{
class roscontroller{
public:
roscontroller(ros::NodeHandle n_c);
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
@ -61,6 +61,14 @@ private:
}; typedef struct num_robot_count Num_robot_count ;
// WGS84 constants
double equatorial_radius = 6378137.0;
double flattening = 1.0/298.257223563;
double excentrity2 = 2*flattening - flattening*flattening;
// default reference position
double DEFAULT_REFERENCE_LATITUDE = 45.457817;
double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
double cur_pos[3];
double home[3];
double cur_rel_altitude;
@ -70,6 +78,7 @@ private:
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
@ -85,15 +94,17 @@ private:
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
@ -118,12 +129,12 @@ private:
ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle n_c);
void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle n_c);
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*compiles buzz script from the specified .bzz file*/
void Compile_bzz();
@ -166,6 +177,7 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -183,7 +195,7 @@ private:
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle node_handle);
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm();
@ -192,7 +204,7 @@ private:
void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c);
void Subscribe(ros::NodeHandle& n_c);
//void WaypointMissionSetup(float lat, float lng, float alt);

18
launch/launch_config/m100.yaml
View File

@ -1,13 +1,13 @@
topics:
gps : /global_position
battery : /power_status
status : /flight_status
fcclient : /dji_mavcmd
setpoint : /setpoint_position/local
armclient: /dji_mavarm
modeclient: /dji_mavmode
altitude: /rel_alt
stream: /set_stream_rate
gps : global_position
battery : power_status
status : flight_status
fcclient : dji_mavcmd
setpoint : setpoint_position/local
armclient: dji_mavarm
modeclient: dji_mavmode
altitude: rel_alt
stream: set_stream_rate
type:
gps : sensor_msgs/NavSatFix

18
launch/launch_config/solo.yaml
View File

@ -1,13 +1,13 @@
topics:
gps : /mavros/global_position/global
battery : /mavros/battery
status : /mavros/state
fcclient: /mavros/cmd/command
setpoint: /mavros/setpoint_position/local
armclient: /mavros/cmd/arming
modeclient: /mavros/set_mode
stream: /mavros/set_stream_rate
altitude: /mavros/global_position/rel_alt
gps : mavros/global_position/global
battery : mavros/battery
status : mavros/state
fcclient: mavros/cmd/command
setpoint: mavros/setpoint_position/local
armclient: mavros/cmd/arming
modeclient: mavros/set_mode
stream: mavros/set_stream_rate
altitude: mavros/global_position/rel_alt
type:
gps : sensor_msgs/NavSatFix
# for SITL Solo

222
src/buzz_utility.cpp
View File

@ -20,6 +20,8 @@ namespace buzz_utility{
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0;
buzzobj_t usersvstig;
buzzobj_t key;
/****************************************/
/*adds neighbours position*/
@ -51,6 +53,9 @@ namespace buzz_utility{
return out;
}
int get_robotid() {
return Robot_id;
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
@ -252,18 +257,10 @@ namespace buzz_utility{
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
//cout<<"bo file name"<<bo_filename;
/* Get hostname */
//char hstnm[30];
//char* hstnm ="M1003";
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
Robot_id =robot_id;
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
@ -287,69 +284,102 @@ namespace buzz_utility{
}
fclose(fd);
/* Read debug information */
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// perror(bdbg_filename);
// return 0;
//}
/* Set byte code */
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
// return 0;
//}
/* Register hook functions */
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
perror(bdbg_filename);
return 0;
}*/
/* Save bytecode file name */
//BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
//buzzvm_execute_script(VM);
/* Call the Init() function */
//buzzvm_function_call(VM, "init", 0);
/* All OK */
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
buzzvm_dup(VM);
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// get the stigmergy table from the global scope
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
// buzzvm_gload(VM);
// get the create method from the stigmergy table
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
// buzzvm_tget(VM);
// call the stigmergy.create() method
// buzzvm_closure_call(VM, 1);
// now the stigmergy is at the top of the stack - save it for future reference
// usersvstig = buzzvm_stack_at(VM, 0);
//buzzvm_pop(VM);
// assign the virtual stigmergy to the global symbol v
// create also a global variable for it, so the garbage collector does not remove it
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
//buzzvm_push(VM, usersvstig);
buzzvm_gstore(VM);
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
int buzz_update_users_stigmergy(buzzobj_t tbl){
// push the key (here it's an int with value 46)
buzzvm_pushi(VM, 46);
// push the table
buzzvm_push(VM, tbl);
// the stigmergy is stored in the vstig variable
// let's push it on the stack
buzzvm_push(VM, usersvstig);
// get the put method from myvstig
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
// call the vstig.put() method
buzzvm_closure_call(VM, 2);
return 1;
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
/* // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
/* Get rid of debug info */
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
// Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
// Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -357,46 +387,40 @@ namespace buzz_utility{
return 0;
}
/* Execute the global part of the script */
// Execute the global part of the script
buzzvm_execute_script(VM);
/* Call the Init() function */
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
// All OK
*/ return 1;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
/* // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
/* Get rid of debug info */
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
// Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
// Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -404,12 +428,12 @@ namespace buzz_utility{
return 0;
}
/* Execute the global part of the script */
// Execute the global part of the script
buzzvm_execute_script(VM);
/* Call the Init() function */
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
// All OK
*/ return 1;
}
/****************************************/
@ -425,31 +449,35 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
if(!buzzdict_isempty(VM->swarms)) {
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
if(buzzdict_size(VM->swarms)>1) {
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
}
/*Step through the buzz script*/
@ -461,7 +489,12 @@ namespace buzz_utility{
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
/*
* Call Buzz step() function
*/
@ -471,21 +504,16 @@ namespace buzz_utility{
buzz_error_info());
buzzvm_dump(VM);
}
// usleep(10000);
//buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
//usleep(10000);
/*Print swarm*/
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- SOMETHING CRASHING HERE!! */
int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
/* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
// int status = 1;
// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
}
/****************************************/

27
src/buzzuav_closures.cpp
View File

@ -19,6 +19,7 @@ namespace buzzuav_closures{
static float batt[3];
static float obst[5]={0,0,0,0,0};
static double cur_pos[3];
static double users_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd=0;
@ -30,6 +31,7 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) {
int i;
char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1);
@ -244,6 +246,11 @@ namespace buzzuav_closures{
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
void set_userspos(double latitude, double longitude, double altitude){
users_pos[0]=latitude;
users_pos[1]=longitude;
users_pos[2]=altitude;
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
@ -263,6 +270,26 @@ namespace buzzuav_closures{
buzzvm_gstore(vm);
return vm->state;
}
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, users_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, users_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
buzzvm_pushf(vm, users_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return buzzvm_stack_at(vm, 0);
//return vm->state;
}
void flight_status_update(uint8_t state){
status=state;

5
src/rosbuzz.cpp
View File

@ -16,8 +16,9 @@ int main(int argc, char **argv)
{
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle n_c("~");
rosbzz_node::roscontroller RosController(n_c);
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();

366
src/roscontroller.cpp
View File

@ -5,11 +5,11 @@ namespace rosbzz_node{
/*---------------
/Constructor
---------------*/
roscontroller::roscontroller(ros::NodeHandle n_c)
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{
ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/
Rosparameters_get(n_c);
Rosparameters_get(n_c_priv);
/*Initialize publishers, subscribers and client*/
Initialize_pub_sub(n_c);
/*Compile the .bzz file to .basm, .bo and .bdbg*/
@ -23,12 +23,13 @@ namespace rosbzz_node{
// set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started
GetRobotId();
if(xbeeplugged)
GetRobotId();
else
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
home[0] = 0.0;
home[1] = 0.0;
home[2] = 0.0;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
}
/*---------------------
@ -67,7 +68,10 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
init_update_monitor(bcfname.c_str(),stand_by.c_str());
//init_update_monitor(bcfname.c_str(),stand_by.c_str());
///////////////////////////////////////////////////////
// MAIN LOOP
//////////////////////////////////////////////////////
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
@ -75,7 +79,7 @@ namespace rosbzz_node{
/*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher();
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
//update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
@ -83,10 +87,10 @@ namespace rosbzz_node{
/*call flight controler service to set command long*/
flight_controller_service_call();
/*Set multi message available after update*/
if(get_update_status()){
/*if(get_update_status()){
set_read_update_status();
multi_msg=true;
}
}*/
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots();
get_number_of_robots();
@ -117,7 +121,7 @@ namespace rosbzz_node{
/*--------------------------------------------------------
/ Get all required parameters from the ROS launch file
/-------------------------------------------------------*/
void roscontroller::Rosparameters_get(ros::NodeHandle n_c){
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c){
/*Obtain .bzz file name from parameter server*/
if(n_c.getParam("bzzfile_name", bzzfile_name));
@ -126,11 +130,8 @@ namespace rosbzz_node{
if(n_c.getParam("rcclient", rcclient)){
if(rcclient==true){
/*Service*/
if(n_c.getParam("rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
if(!n_c.getParam("rcservice_name", rcservice_name))
{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
}
else if(rcclient==false){ROS_INFO("RC service is disabled");}
}
@ -144,7 +145,13 @@ namespace rosbzz_node{
n_c.getParam("in_payload", in_payload);
/*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by);
n_c.getParam("xbee_status_srv", xbeesrv_name);
if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
if(!xbeeplugged){
if(n_c.getParam("name", robot_name));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -153,11 +160,12 @@ namespace rosbzz_node{
/*-----------------------------------------------------------------------------------
/Obtains publisher, subscriber and services from yml file based on the robot used
/-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle){
//todo: make it as an array in yaml?
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
node_handle.getParam("topics/gps", gps_topic);
if(node_handle.getParam("topics/gps", gps_topic));
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
@ -192,7 +200,7 @@ namespace rosbzz_node{
/*--------------------------------------------------------
/ Create all required subscribers, publishers and ROS service clients
/-------------------------------------------------------*/
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){
/*subscribers*/
Subscribe(n_c);
@ -202,24 +210,29 @@ namespace rosbzz_node{
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos",1000);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if(rcclient==true)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this);
multi_msg=true;
}
/*---------------------------------------
/Robot independent subscribers
/--------------------------------------*/
void roscontroller::Subscribe(ros::NodeHandle n_c){
void roscontroller::Subscribe(ros::NodeHandle& n_c){
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
if(it->second == "mavros_msgs/ExtendedState"){
@ -345,35 +358,35 @@ namespace rosbzz_node{
delete[] out;
delete[] payload_out_ptr;
/*Check for updater message if present send*/
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
/*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
int tot=0;
mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
/*allocate mem and clear it*/
// allocate mem and clear it
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/
// Append updater msg size
*(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t);
/*Append updater msgs*/
// Append updater msgs
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize;
/*Destroy the updater out msg queue*/
// Destroy the updater out msg queue
destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send);
/*Send a constant number to differenciate updater msgs*/
// Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]);
}
/*Add Robot id and message number to the published message*/
// Add Robot id and message number to the published message
if (message_number < 0) message_number=0;
else message_number++;
update_packets.sysid=(uint8_t)robot_id;
@ -381,7 +394,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------
@ -399,8 +412,11 @@ namespace rosbzz_node{
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
switch(buzzuav_closures::getcmd()){
case mavros_msgs::CommandCode::NAV_LAND:
if(current_mode != "LAND")
{SetMode("LAND", 0);}
if(current_mode != "LAND") {
SetMode("LAND", 0);
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv)){
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
@ -413,6 +429,8 @@ namespace rosbzz_node{
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
}
if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@ -451,8 +469,7 @@ namespace rosbzz_node{
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
}
@ -504,22 +521,133 @@ namespace rosbzz_node{
----------------------------------------------------------- */
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
double hyp_az, hyp_el;
double sin_el, cos_el, sin_az, cos_az;
/* Convert reference point to spherical earth centered coordinates. */
hyp_az = sqrt(ref_ecef[0]*ref_ecef[0] + ref_ecef[1]*ref_ecef[1]);
hyp_el = sqrt(hyp_az*hyp_az + ref_ecef[2]*ref_ecef[2]);
sin_el = ref_ecef[2] / hyp_el;
cos_el = hyp_az / hyp_el;
sin_az = ref_ecef[1] / hyp_az;
cos_az = ref_ecef[0] / hyp_az;
M[0][0] = -sin_el * cos_az;
M[0][1] = -sin_el * sin_az;
M[0][2] = cos_el;
M[1][0] = -sin_az;
M[1][1] = cos_az;
M[1][2] = 0.0;
M[2][0] = -cos_el * cos_az;
M[2][1] = -cos_el * sin_az;
M[2][2] = -sin_el;
}
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
const double *b, double *c)
{
uint32_t i, j, k;
for (i = 0; i < n; i++)
for (j = 0; j < p; j++) {
c[p*i + j] = 0;
for (k = 0; k < m; k++)
c[p*i + j] += a[m*i+k] * b[p*k + j];
}
}
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
// calculate earth radii
double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
/*double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned[3];
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned[1],ned[0]);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x);
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
// calculate earth radii
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = cur[2];
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
/*------------------------------------------------------
@ -557,13 +685,36 @@ namespace rosbzz_node{
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT;
if(home[0]==0){
home[0]=msg->latitude;
home[1]=msg->longitude;
//double lat = std::floor(msg->latitude * 1000000) / 1000000;
//double lon = std::floor(msg->longitude * 1000000) / 1000000;
/*if(cur_rel_altitude<1.2){
home[0]=lat;
home[1]=lon;
home[2]=cur_rel_altitude;
}
set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
}*/
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
}
void roscontroller::users_pos(const rosbuzz::neigh_pos data){
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
double us[3];
int n = data.pos_neigh.size();
// ROS_INFO("Neighbors array size: %i\n", n);
if(n>0)
{
for(int it=0; it<n; ++it)
{
us[0] = data.pos_neigh[it].latitude;
us[1] = data.pos_neigh[it].longitude;
us[2] = data.pos_neigh[it].altitude;
double out[3];
cvt_rangebearing_coordinates(us, out, cur_pos);
buzzuav_closures::set_userspos(out[0], out[1], out[2]);
}
}
}
/*-------------------------------------------------------------
/ Update altitude into BVM from subscriber
@ -582,6 +733,66 @@ namespace rosbzz_node{
obst[i]=msg->ranges[i];
buzzuav_closures::set_obstacle_dist(obst);
}
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
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]);
/*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;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
moveMsg.pose.orientation.y = 0;
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position.
if(fabs(x)>0.1 || fabs(y)>0.1) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
}
/*-------------------------------------------------------------
/Push payload into BVM FIFO
@ -777,66 +988,5 @@ namespace rosbzz_node{
}
*/
}
/*
* SOLO SPECIFIC FUNCTIONS
*/
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1;
double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home);
moveMsg.pose.position.x = local_pos[0]+x;
moveMsg.pose.position.y = local_pos[1]+y;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
moveMsg.pose.orientation.y = 0;
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] <<std::endl;
localsetpoint_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
}
}

213
src/testflocksim.bzz Normal file
View File

@ -0,0 +1,213 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 12.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
log("User position: ", users.range)
# Read a value from the structure
#x = usersvstig.get(46)
# Get the number of keys in the structure
#log("The vstig has ", usersvstig.size(), " elements")
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}