From 73c81fb22b377abd674d1b9b50c1b38392559725 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 15 Apr 2017 18:17:19 -0400 Subject: [PATCH] swarm table test --- include/roscontroller.h | 1 + src/buzz_utility.cpp | 162 +++++++++++++++++----------------------- src/roscontroller.cpp | 62 ++++++++------- 3 files changed, 107 insertions(+), 118 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index a337cf7..d2a4ad4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -70,6 +70,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; diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index f4bb43a..15ba455 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -252,18 +252,10 @@ namespace buzz_utility{ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - //cout<<"bo file name"<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*/ @@ -471,21 +454,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); } /****************************************/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1b43b42..21457a9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -23,7 +23,10 @@ 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; } @@ -63,7 +66,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*/ @@ -71,7 +77,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*/ @@ -79,10 +85,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(); @@ -142,7 +148,11 @@ namespace rosbzz_node{ 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");} + 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? @@ -205,13 +215,13 @@ namespace rosbzz_node{ /*publishers*/ payload_pub = n_c.advertise(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); - localsetpoint_pub = n_c.advertise(setpoint_name,1000); + localsetpoint_pub = n_c.advertise("/"+robot_name+setpoint_name,1000); /* Service Clients*/ - arm_client = n_c.serviceClient(armclient); - mode_client = n_c.serviceClient(modeclient); - mav_client = n_c.serviceClient(fcclient_name); - xbeestatus_srv = n_c.serviceClient(xbeesrv_name); - stream_client = n_c.serviceClient(stream_client_name); + arm_client = n_c.serviceClient("/"+robot_name+armclient); + mode_client = n_c.serviceClient("/"+robot_name+modeclient); + mav_client = n_c.serviceClient("/"+robot_name+fcclient_name); + xbeestatus_srv = n_c.serviceClient("/"+robot_name+xbeesrv_name); + stream_client = n_c.serviceClient("/"+robot_name+stream_client_name); multi_msg=true; } @@ -222,19 +232,19 @@ namespace rosbzz_node{ for(std::map::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){ if(it->second == "mavros_msgs/ExtendedState"){ - flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_extended_status_update, this); } else if(it->second == "mavros_msgs/State"){ - flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_status_update, this); } else if(it->second == "mavros_msgs/BatteryStatus"){ - battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this); + battery_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::battery, this); } else if(it->second == "sensor_msgs/NavSatFix"){ - current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_pos, this); } else if(it->second == "std_msgs/Float64"){ - relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_rel_alt, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -344,35 +354,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