Merge remote-tracking branch 'refs/remotes/origin/dev'
Conflicts: include/roscontroller.h launch/rosbuzz.launch src/buzz_utility.cpp src/roscontroller.cpp
This commit is contained in:
isvogor
commit
f7eb8d8031
|
|
@ -73,7 +73,7 @@ void update_routine(const char* bcfname,
|
|||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
void init_update_monitor(const char* bo_filename,const char* stand_by_script,int barrier);
|
||||
void init_update_monitor(const char* bo_filename,const char* stand_by_script);
|
||||
|
||||
|
||||
/*********************************************************/
|
||||
|
|
@ -129,4 +129,6 @@ int get_update_status();
|
|||
|
||||
void set_read_update_status();
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -52,4 +52,6 @@ int update_step_test();
|
|||
uint16_t get_robotid();
|
||||
|
||||
buzzvm_t get_vm();
|
||||
|
||||
void set_robot_var(int ROBOTS);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,6 +10,17 @@
|
|||
//#include "roscontroller.h"
|
||||
|
||||
namespace buzzuav_closures{
|
||||
typedef enum {
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_LAND,
|
||||
COMMAND_GOHOME,
|
||||
COMMAND_ARM,
|
||||
COMMAND_DISARM,
|
||||
COMMAND_GOTO,
|
||||
COMMAND_MOVETO,
|
||||
} Custom_CommandCode;
|
||||
|
||||
/*
|
||||
* prextern int() function in Buzz
|
||||
* This function is used to print data from buzz
|
||||
|
|
|
|||
|
|
@ -48,7 +48,15 @@ public:
|
|||
void RosControllerRun();
|
||||
|
||||
private:
|
||||
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index=0;
|
||||
uint8_t current=0;
|
||||
num_robot_count(){}
|
||||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
|
||||
double cur_pos[3];
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
|
|
@ -61,10 +69,17 @@ private:
|
|||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
std::string bzzfile_name, fcclient_name, armclient, stream_client_name, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
|
||||
int no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
int no_cnt=0;
|
||||
int old_val=0;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
|
||||
std::string stream_client_name;
|
||||
bool rcclient;
|
||||
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;
|
||||
|
|
@ -80,7 +95,6 @@ private:
|
|||
/*Commands for flight controller*/
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandLong cmd_srv;
|
||||
|
||||
std::vector<std::string> m_sMySubscriptions;
|
||||
std::map<std::string, std::string> m_smTopic_infos;
|
||||
|
||||
|
|
@ -170,6 +184,8 @@ private:
|
|||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
|
||||
void SetStreamRate(int id, int rate, int on_off);
|
||||
|
||||
void get_number_of_robots();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,8 +8,8 @@
|
|||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
<param name="xbee_status_srv" value="/xbee_status"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
<param name="fcclient_name" value="/dji_mavcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="xbee_status_srv" value="/xbee_status"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ static int neigh=-1;
|
|||
static int updater_msg_ready ;
|
||||
static int updated=0;
|
||||
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script,int barrier){
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
||||
fprintf(stdout,"intiialized file monitor.\n");
|
||||
fd=inotify_init1(IN_NONBLOCK);
|
||||
if ( fd < 0 ) {
|
||||
|
|
@ -91,7 +91,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script,in
|
|||
*(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
|
||||
updater->mode=(int*)malloc(sizeof(int));
|
||||
*(int*)(updater->mode)=CODE_RUNNING;
|
||||
no_of_robot=barrier;
|
||||
//no_of_robot=barrier;
|
||||
updater_msg_ready=0;
|
||||
//neigh = 0;
|
||||
//updater->outmsg_queue=
|
||||
|
|
@ -444,6 +444,11 @@ close(fd);
|
|||
void set_bzz_file(const char* in_bzz_file){
|
||||
bzz_file=in_bzz_file;
|
||||
}
|
||||
|
||||
void updates_set_robots(int robots){
|
||||
no_of_robot=robots;
|
||||
}
|
||||
|
||||
void collect_data(){
|
||||
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
|
||||
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
|
||||
|
|
|
|||
|
|
@ -18,9 +18,9 @@ namespace buzz_utility{
|
|||
static uint8_t* BO_BUF = 0;
|
||||
static buzzdebug_t DBG_INFO = 0;
|
||||
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 MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||
static int Robot_id = 0;
|
||||
|
||||
/****************************************/
|
||||
/*adds neighbours position*/
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
|
||||
|
|
@ -254,16 +254,17 @@ namespace buzz_utility{
|
|||
const char* bdbg_filename, int robot_id) {
|
||||
//cout<<"bo file name"<<bo_filename;
|
||||
/* Get hostname */
|
||||
char hstnm[30];
|
||||
//char hstnm[30];
|
||||
//char* hstnm ="M1003";
|
||||
gethostname(hstnm, 30);
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm+4, NULL, 10);
|
||||
cout << " Robot ID: " <<id<< endl;
|
||||
/* 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);
|
||||
VM = buzzvm_new((int)id);
|
||||
Robot_id =robot_id;
|
||||
VM = buzzvm_new((int)robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
|
@ -321,15 +322,15 @@ namespace buzz_utility{
|
|||
/****************************************/
|
||||
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);
|
||||
//char hstnm[30];
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm + 4, NULL, 10);
|
||||
|
||||
/* NOTE: here we assume that the hostname is in the format Mnn */
|
||||
//int id = strtol(hstnm + 4, NULL, 10);
|
||||
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(id);
|
||||
VM = buzzvm_new(Robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
|
@ -369,14 +370,14 @@ namespace buzz_utility{
|
|||
/****************************************/
|
||||
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);
|
||||
//char hstnm[30];
|
||||
//gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm + 4, NULL, 10);
|
||||
/* NOTE: here we assume that the hostname is in the format Mnn */
|
||||
//int id = strtol(hstnm + 4, NULL, 10);
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(id);
|
||||
VM = buzzvm_new(Robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
|
|
@ -547,6 +548,11 @@ namespace buzz_utility{
|
|||
return VM;
|
||||
}
|
||||
|
||||
void set_robot_var(int ROBOTS){
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
|
||||
buzzvm_pushi(VM, ROBOTS);
|
||||
buzzvm_gstore(VM);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -29,40 +29,42 @@ namespace buzzuav_closures{
|
|||
|
||||
int buzzros_print(buzzvm_t vm) {
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
buzzvm_lload(vm, i);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
switch(o->o.type) {
|
||||
case BUZZTYPE_NIL:
|
||||
ROS_INFO("BUZZ - [nil]");
|
||||
sprintf(buffer,"%s BUZZ - [nil]", buffer);
|
||||
break;
|
||||
case BUZZTYPE_INT:
|
||||
ROS_INFO("%d", o->i.value);
|
||||
sprintf(buffer,"%s %d", buffer, o->i.value);
|
||||
//fprintf(stdout, "%d", o->i.value);
|
||||
break;
|
||||
case BUZZTYPE_FLOAT:
|
||||
ROS_INFO("%f", o->f.value);
|
||||
sprintf(buffer,"%s %f", buffer, o->f.value);
|
||||
break;
|
||||
case BUZZTYPE_TABLE:
|
||||
ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
|
||||
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
|
||||
break;
|
||||
case BUZZTYPE_CLOSURE:
|
||||
if(o->c.value.isnative)
|
||||
ROS_INFO("[n-closure @%d]", o->c.value.ref);
|
||||
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
|
||||
else
|
||||
ROS_INFO("[c-closure @%d]", o->c.value.ref);
|
||||
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
|
||||
break;
|
||||
case BUZZTYPE_STRING:
|
||||
ROS_INFO("%s", o->s.value.str);
|
||||
sprintf(buffer,"%s %s", buffer, o->s.value.str);
|
||||
break;
|
||||
case BUZZTYPE_USERDATA:
|
||||
ROS_INFO("[userdata @%p]", o->u.value);
|
||||
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO(buffer);
|
||||
//fprintf(stdout, "\n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
|
@ -108,7 +110,7 @@ namespace buzzuav_closures{
|
|||
gps_from_rb(d, b, goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
|
||||
printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
|
||||
buzz_cmd=5;
|
||||
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -120,7 +122,7 @@ namespace buzzuav_closures{
|
|||
set_goto(rc_goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||
buzz_cmd=2;
|
||||
buzz_cmd=COMMAND_GOTO;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -130,13 +132,13 @@ namespace buzzuav_closures{
|
|||
int buzzuav_arm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
printf(" Buzz requested Arm \n");
|
||||
buzz_cmd=3;
|
||||
buzz_cmd=COMMAND_ARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzuav_disarm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
|
||||
printf(" Buzz requested Disarm \n");
|
||||
buzz_cmd=4;
|
||||
buzz_cmd=COMMAND_DISARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -151,21 +153,21 @@ namespace buzzuav_closures{
|
|||
height = goto_pos[2];
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
printf(" Buzz requested Take off !!! \n");
|
||||
buzz_cmd = 1;
|
||||
buzz_cmd = COMMAND_TAKEOFF;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_land(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
buzz_cmd = 1;
|
||||
buzz_cmd = COMMAND_LAND;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_gohome(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
printf(" Buzz requested gohome !!! \n");
|
||||
buzz_cmd = 1;
|
||||
buzz_cmd = COMMAND_GOHOME;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -306,9 +308,65 @@ namespace buzzuav_closures{
|
|||
|
||||
/******************************************************/
|
||||
/*Create an obstacle Buzz table from proximity sensors*/
|
||||
/* Acessing proximity in buzz script
|
||||
proximity[0].angle and proximity[0].value - front
|
||||
"" "" "" - right and back
|
||||
proximity[3].angle and proximity[3].value - left
|
||||
proximity[4].angle = -1 and proximity[4].value -bottom */
|
||||
/******************************************************/
|
||||
|
||||
int buzzuav_update_prox(buzzvm_t vm) {
|
||||
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_gstore(vm);
|
||||
|
||||
/* Fill into the proximity table */
|
||||
buzzobj_t tProxRead;
|
||||
float angle =0;
|
||||
for(size_t i = 0; i < 4; ++i) {
|
||||
/* Create table for i-th read */
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i+1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/* Store read table in the proximity table */
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle+=1.5708;
|
||||
}
|
||||
/* Create table for bottom read */
|
||||
angle =-1;
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
|
||||
/*
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
|
|
@ -331,7 +389,7 @@ namespace buzzuav_closures{
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
|
||||
buzzvm_pushf(vm, obst[4]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
buzzvm_gstore(vm);*/
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -40,10 +40,23 @@ namespace rosbzz_node{
|
|||
/rosbuzz_node loop method executed once every step
|
||||
/--------------------------------------------------*/
|
||||
void roscontroller::RosControllerRun(){
|
||||
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
|
||||
mavros_msgs::ParamGet::Response robot_id_srv_response;
|
||||
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
/*loop rate of ros*/
|
||||
ros::Rate loop_rate(10);
|
||||
loop_rate.sleep();
|
||||
/*sleep for the mentioned loop rate*/
|
||||
ROS_ERROR("Waiting for Xbee to respond to get device ID");
|
||||
}
|
||||
robot_id=robot_id_srv_response.value.integer;
|
||||
|
||||
/* 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(),barrier);
|
||||
init_update_monitor(bcfname.c_str(),stand_by.c_str());
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
/*Update neighbors position inside Buzz*/
|
||||
|
|
@ -63,6 +76,13 @@ namespace rosbzz_node{
|
|||
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();
|
||||
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
/*Set no of robots for updates*/
|
||||
updates_set_robots(no_of_robots);
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
/*loop rate of ros*/
|
||||
|
|
@ -71,9 +91,8 @@ namespace rosbzz_node{
|
|||
/*sleep for the mentioned loop rate*/
|
||||
timer_step+=1;
|
||||
maintain_pos(timer_step);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
/* Destroy updater and Cleanup */
|
||||
//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
|
||||
|
|
@ -112,6 +131,7 @@ namespace rosbzz_node{
|
|||
n_c.getParam("No_of_Robots", barrier);
|
||||
/*Obtain standby script to run during update*/
|
||||
n_c.getParam("stand_by", stand_by);
|
||||
n_c.getParam("xbee_status_srv", xbeesrv_name);
|
||||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
|
|
@ -161,7 +181,7 @@ namespace rosbzz_node{
|
|||
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
|
||||
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);
|
||||
//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);
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
|
|
@ -171,6 +191,7 @@ namespace rosbzz_node{
|
|||
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);
|
||||
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
|
||||
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
multi_msg=true;
|
||||
|
|
@ -444,8 +465,16 @@ namespace rosbzz_node{
|
|||
/ from GPS coordinates
|
||||
----------------------------------------------------------- */
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
|
||||
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
|
||||
double lat1 = cur[0]*M_PI/180.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[1] = atan2(ned_y,ned_x);
|
||||
out[2] = 0.0;
|
||||
/* double lat1 = cur[0]*M_PI/180.0;
|
||||
double lon1 = cur[1]*M_PI/180.0;
|
||||
double lat2 = nei[0]*M_PI/180.0;
|
||||
double lon2 = nei[1]*M_PI/180.0;
|
||||
|
|
@ -453,7 +482,7 @@ namespace rosbzz_node{
|
|||
double y = sin(lon1-lon2)*cos(lat1);
|
||||
double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
|
||||
out[1] = atan2(y,x)+M_PI;
|
||||
out[2] = 0.0;
|
||||
out[2] = 0.0;*/
|
||||
}
|
||||
|
||||
/*------------------------------------------------------
|
||||
|
|
@ -633,10 +662,66 @@ namespace rosbzz_node{
|
|||
/Obtain robot id by subscribing to xbee robot id topic
|
||||
/ TODO: check for integrity of this subscriber call back
|
||||
/----------------------------------------------------*/
|
||||
void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
|
||||
robot_id=(int)msg->data;
|
||||
}
|
||||
/*void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
|
||||
|
||||
|
||||
}*/
|
||||
void roscontroller::get_number_of_robots(){
|
||||
|
||||
if(no_of_robots==0){
|
||||
no_of_robots=neighbours_pos_map.size()+1;
|
||||
|
||||
}
|
||||
else{
|
||||
if(no_of_robots!=neighbours_pos_map.size()+1 && no_cnt==0){
|
||||
no_cnt++;
|
||||
old_val=neighbours_pos_map.size()+1;
|
||||
|
||||
}
|
||||
else if(old_val==neighbours_pos_map.size()+1){
|
||||
no_cnt++;
|
||||
if(no_cnt==3){
|
||||
no_of_robots=neighbours_pos_map.size()+1;
|
||||
no_cnt=0;
|
||||
}
|
||||
}
|
||||
else{
|
||||
no_cnt=0;
|
||||
}
|
||||
}
|
||||
//if(count_robots.current !=0){
|
||||
/*std::map< int, int> count_count;
|
||||
uint8_t index=0;
|
||||
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
|
||||
//count_robots.current=neighbours_pos_map.size()+1;
|
||||
count_robots.index++;
|
||||
if(count_robots.index >9) count_robots.index =0;
|
||||
for(int i=0;i<10;i++){
|
||||
if(count_robots.history[i]==count_robots.current){
|
||||
current_count++;
|
||||
}
|
||||
else{
|
||||
if(count_robots.history[i]!=0){
|
||||
odd_count++;
|
||||
odd_val=count_robots.history[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
if(odd_count>current_count){
|
||||
count_robots.current=odd_val;
|
||||
}
|
||||
//}
|
||||
/*else{
|
||||
if(neighbours_pos_map.size()!=0){
|
||||
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
|
||||
//count_robots.current=neighbours_pos_map.size()+1;
|
||||
count_robots.index++;
|
||||
if(count_robots.index >9){
|
||||
count_robots.index =0;
|
||||
count_robots.current=neighbours_pos_map.size()+1;
|
||||
}
|
||||
}
|
||||
}*/
|
||||
/*
|
||||
* SOLO SPECIFIC FUNCTIONS
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF"
|
|||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 10.0 #0.001
|
||||
EPSILON = 18.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -56,7 +56,7 @@ function hexagon() {
|
|||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 1
|
||||
ROBOTS = 4 # number of robots in the swarm
|
||||
# ROBOTS = 3 # number of robots in the swarm
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
|
|
@ -86,12 +86,12 @@ function barrier_wait(threshold, transf) {
|
|||
if(barrier.size() >= threshold) {
|
||||
barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=hexagon #idle
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=land
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# flight status
|
||||
|
|
@ -99,25 +99,12 @@ function barrier_wait(threshold, transf) {
|
|||
function idle() {
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22) {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400) {
|
||||
uav_arm()
|
||||
} else if(value==401){
|
||||
uav_disarm()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
)
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
CURSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
log("TakeOff: ", flight.status)
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
|
|
@ -131,6 +118,8 @@ function takeoff() {
|
|||
}
|
||||
}
|
||||
function land() {
|
||||
CURSTATE = "LAND"
|
||||
statef=land
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 1){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
|
|
@ -165,6 +154,7 @@ function step() {
|
|||
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
|
||||
|
|
@ -175,8 +165,24 @@ function step() {
|
|||
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)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
Loading…
Reference in New Issue