diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 23a04bd..d725137 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -18,6 +18,7 @@ #include #include #include +#include using namespace std; /****************************************/ /****************************************/ @@ -36,17 +37,19 @@ static uint8_t* STREAM_SEND_BUF = NULL; /****************************************/ -void neighbour_pos_callback(std::vector pos_vect){ +void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){ /* Reset neighbor information */ buzzneighbors_reset(VM); /* Get robot id and update neighbor information */ -for(int i=0;i::iterator it; + for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ + buzzneighbors_add(VM, + it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + } @@ -63,38 +66,38 @@ return out; } -void in_msg_process(unsigned long int payload[]){ +void in_msg_process(uint64_t* payload){ - printf("inside payload"); /* Go through messages and add them to the FIFO */ - for(size_t i = 0; i < sizeof(payload)/sizeof(payload[0]); ++i) { + uint16_t* data= u64_cvt_u16((uint64_t)payload[0]); + uint16_t size=data[0]*sizeof(uint64_t); + delete[] data; + uint8_t* pl =(uint8_t*)malloc(size); + memset(pl, 0,size); /* Copy packet into temporary buffer */ - long unsigned int* pl = (long unsigned int*) &payload[i]; - size_t tot = 0; - //fprintf(stderr, "[DEBUG] Processing packet %p from %f\n", pl, neighbour[0]); + memcpy(pl, payload ,size); + + size_t tot = sizeof(uint32_t); /* Go through the messages until there's nothing else to read */ uint16_t unMsgSize; do { /* Get payload size */ unMsgSize = *(uint16_t*)(pl + tot); - tot += sizeof(uint16_t); + tot += sizeof(uint16_t); /* Append message to the Buzz input message queue */ - if(unMsgSize > 0 && unMsgSize <= MSG_SIZE - tot) { + if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) { buzzinmsg_queue_append(VM->inmsgs, buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); tot += unMsgSize; - } - }while(MSG_SIZE - tot > sizeof(uint16_t) && unMsgSize > 0); - } + }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); + /* Process messages */ buzzvm_process_inmsgs(VM); - } uint64_t* out_msg_process(){ - // buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM); uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE); memset(buff_send, 0, MSG_SIZE); @@ -112,7 +115,6 @@ uint64_t* out_msg_process(){ if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) > MSG_SIZE) { - printf("buzz payload does not fit"); buzzmsg_payload_destroy(&m); break; } @@ -125,8 +127,6 @@ uint64_t* out_msg_process(){ memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); tot += buzzmsg_payload_size(m); - //fprintf(stderr, "[DEBUG] coppied = %u\n", - // *(uint64_t*)(buff_send+tot)); /* Get rid of message */ buzzoutmsg_queue_next(VM->outmsgs); buzzmsg_payload_destroy(&m); @@ -134,35 +134,12 @@ uint64_t* out_msg_process(){ int total_size =(ceil((float)tot/(float)sizeof(uint64_t))); *(uint16_t*)buff_send = (uint16_t) total_size; - - for(int i=0;i<16;i++){ - uint16_t* temp_buff =(uint16_t*)(buff_send+(i*2)); - cout << "buff_send address :" << (uint16_t*) buff_send+(i*2); - uint16_t *p = reinterpret_cast(temp_buff); - cout <<" buff_send data " << *p <(payload_64[0]).to_string(); //to binary - // std::cout<<"payload_64 binary " <(binary).to_ulong(); - // std::cout<< " decimal "<swarmmembers, VM->robot); /* Check swarm state */ - int status = 1; + /* 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); + buzzvm_gstore(VM);*/ } /****************************************/ diff --git a/src/buzz_utility.h b/src/buzz_utility.h index b7079b9..54ac04f 100644 --- a/src/buzz_utility.h +++ b/src/buzz_utility.h @@ -2,19 +2,21 @@ #define BUZZ_UTILITY_H #include #include +#include struct pos_struct { - int id; double x,y,z; - pos_struct(int id,double x,double y,double z):id(id),x(x),y(y),z(z){}; + pos_struct(double x,double y,double z):x(x),y(y),z(z){}; + pos_struct(){} }; +typedef struct pos_struct Pos_struct ; uint16_t* u64_cvt_u16(uint64_t u64); extern int buzz_listen(const char* type, int msg_size); -void neighbour_pos_callback(std::vector pos_vect); -void in_msg_process(unsigned long int payload[]); +void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); +void in_msg_process(uint64_t* payload); uint64_t* out_msg_process(); diff --git a/src/out.basm b/src/out.basm index f1d36f3..6223250 100644 --- a/src/out.basm +++ b/src/out.basm @@ -1,22 +1,40 @@ -!5 +!23 'init 'i +'v +'stigmergy +'create 'step +'s +'swarm +'join +'neighbors +'foreach +'print +'robot +': +'distance = +'distance +', +'azimuth = +'azimuth +'elevation = +'elevation 'reset 'destroy pushs 0 pushcn @__label_1 gstore - pushs 2 + pushs 5 pushcn @__label_2 gstore - pushs 3 - pushcn @__label_3 - gstore - pushs 4 + pushs 21 pushcn @__label_4 gstore + pushs 22 + pushcn @__label_5 + gstore nop @__label_0 @@ -28,13 +46,68 @@ pushs 1 |3,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz - ret0 |4,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 3 |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gload |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 4 |4,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 1 |4,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 1 |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + callc |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gstore |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + ret0 |6,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz @__label_2 - ret0 |21,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 6 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 7 |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gload |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 4 |10,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |10,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 10 |10,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 1 |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + callc |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gstore |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 6 |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gload |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 8 |11,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |11,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 0 |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + callc |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 9 |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gload |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 10 |13,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |13,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushl @__label_3 |14,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 1 |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + callc |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz @__label_3 - ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 11 |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + gload |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 12 |15,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + lload 1 |15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 13 |15,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 14 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 15 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |16,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 16 |16,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |17,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 16 |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 19 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + lload 2 |18,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushs 20 |18,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + tget |18,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + pushi 11 |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + callc |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + ret0 |18,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz @__label_4 - ret0 |29,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz + +@__label_5 + ret0 |38,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz diff --git a/src/out.bdbg b/src/out.bdbg index c0caeb2..34fafc5 100644 Binary files a/src/out.bdbg and b/src/out.bdbg differ diff --git a/src/out.bo b/src/out.bo index 51311ee..15e6a6d 100644 Binary files a/src/out.bo and b/src/out.bo differ diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 4db5f6f..613efa6 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -25,19 +25,32 @@ #include #include #include +#include using namespace std; -std::vector pos_vect; // vector of struct to store neighbours position + static int done = 0; static double cur_pos[3]; -uint64_t payload; - +static uint64_t payload; +static std::map< int, Pos_struct> neighbours_pos_map; +static int timer_step=0; /** * This program implements Buzz node in ros. */ -/* +void maintain_pos(int tim_step){ +if(timer_step >=10){ +neighbours_pos_map.clear(); +timer_step=0; +} - * Print usage information - */ +} +void neighbours_pos_maintain(int id, Pos_struct pos_arr ){ +map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id); +if(it!=neighbours_pos_map.end()) +neighbours_pos_map.erase(it); +neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +/*usage*/ void usage(const char* path, int status) { fprintf(stderr, "Usage:\n"); fprintf(stderr, "\t%s \n\n", path); @@ -60,20 +73,16 @@ cur_pos [2] =altitude; } /*convert from catresian to spherical coordinate system callback */ -void cvt_spherical_coordinates(){ +double* cvt_spherical_coordinates(double neighbours_pos_payload []){ double latitude,longitude,altitude; -for(int i=0;ilatitude,msg->longitude,msg->altitude); } -/*payload callback*/ +/*payload callback callback*/ void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { -long unsigned int message_obt[(msg->payload64.end())-(msg->payload64.begin())]; +uint64_t message_obt[(msg->payload64.end())-(msg->payload64.begin())]; int i = 0; + // print all the remaining numbers for(std::vector::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it) { - message_obt[i] = *it; + message_obt[i] =(uint64_t) *it; + cout<<" obtaind message "<::const_iterator it = msg->pos_neigh.begin(); it != msg->pos_neigh.end(); ++it){ -sensor_msgs::NavSatFix cur_neigh = *it; -sensor_msgs::NavSatStatus stats = cur_neigh.status; -pos_vect.push_back(pos_struct(stats.status,(cur_neigh.latitude-cur_pos[0]),(cur_neigh.longitude-cur_pos[1]),(cur_neigh.altitude-cur_pos[2]))); - // ROS_INFO("[Debug]I heard neighbour: %d from latitude: [%15f]",pos_vect[i].id, pos_vect[i].x); - // ROS_INFO("[Debug]I heard neighbour: %d from longitude: [%15f]",pos_vect[i].id, pos_vect[i].y); - // ROS_INFO("[Debug]I heard neighbour: %d from altitude: [%15f]",pos_vect[i].id, pos_vect[i].z); -i++; -} -cvt_spherical_coordinates(); +for(i=0;i<3;i++){ +neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i]; } + double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload); +//cout<<" neighbours pos cvt " <<" lat " << cvt_neighbours_pos_payload[0]<<" long "<("go_to", 1000); - - //ros::Publisher cmds_pub = n_c.advertise("newstate", 1000); - - ros::Publisher payload_pub = n_c.advertise("pay_load_out", 1000); + ros::Publisher payload_pub = n_c.advertise("payload_out", 1000); /* Clients*/ ros::ServiceClient mav_client = n_c.serviceClient("djicmd"); @@ -219,6 +222,7 @@ int main(int argc, char **argv) /*Services*/ ros::ServiceServer service = n_c.advertiseService("djicmd_rc", rc_callback); ROS_INFO("Ready to receive Mav Commands from dji RC client"); + /*loop rate of ros*/ ros::Rate loop_rate(1); @@ -236,14 +240,17 @@ int main(int argc, char **argv) if(buzz_script_set(bcfname, dbgfname)) { fprintf(stdout, "Bytecode file found and set\n"); -// buzz setting + mavros_msgs::CommandInt cmd_srv; int count = 0; while (ros::ok() && !done && !buzz_script_done()) { - + + /*Update neighbors position inside Buzz*/ + neighbour_pos_callback(neighbours_pos_map); + /* Main loop */ - buzz_script_step(); + buzz_script_step(); /*prepare the goto publish message */ @@ -251,34 +258,25 @@ int main(int argc, char **argv) cmd_srv.request.param1 = goto_pos[0]; cmd_srv.request.param2 = goto_pos[1]; cmd_srv.request.param3 = goto_pos[2]; - //ROS_INFO("set value X = %f, Y =%f, Z= %f ",cmd_srv.request.param1,cmd_srv.request.param2,cmd_srv.request.param3); - /*prepare commands for takeoff,land and gohome*/ - //char tmp[20]; - //sprintf(tmp,"%i",getcmd()); + cmd_srv.request.command = getcmd(); /* diji client call*/ if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else{ ROS_ERROR("Failed to call service 'djicmd'"); } /*Prepare Pay load to be sent*/ uint64_t* payload_out_ptr= out_msg_process(); - uint16_t* out = u64_cvt_u16(payload_out_ptr[0]); + uint64_t position[3]; + memcpy(position, cur_pos, 3*sizeof(uint64_t)); mavros_msgs::Mavlink payload_out; + payload_out.payload64.push_back(position[0]); + payload_out.payload64.push_back(position[1]); + payload_out.payload64.push_back(position[2]); + + uint16_t* out = u64_cvt_u16(payload_out_ptr[0]); for(int i=0;i::iterator it = payload_out.payload64.begin() ; it != payload_out.payload64.end(); ++it){ - out = u64_cvt_u16((uint64_t)*it); - for(int i=0; i<4;i++){ - cout<< "[ROS BUZZ] cvt value"<::iterator it; + for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ + std::cout << it->first << " => " << (it->second).x <<" " << (it->second).y <<" " << (it->second).z<