From 4c99bb6acc3b50421c296fd847d0bf4905c46f32 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 11 Apr 2017 20:23:21 -0400 Subject: [PATCH 1/5] delete corrected to free for malloc --- src/buzz_utility.cpp | 4 ++-- src/roscontroller.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 905e1a9..9139c12 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -91,7 +91,7 @@ namespace buzz_utility{ tot += unMsgSize; } }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - delete[] pl; + free(pl); /* Process messages */ buzzvm_process_inmsgs(VM); } @@ -141,7 +141,7 @@ namespace buzz_utility{ uint64_t* payload_64 = new uint64_t[total_size]; memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); - delete[] buff_send; + free(buff_send); /*for(int i=0;ipayload64.size() > 3){ From 3af2a03ffc8b08be7a34f094af1f7426e33a81b7 Mon Sep 17 00:00:00 2001 From: isvogor Date: Thu, 13 Apr 2017 09:39:45 -0400 Subject: [PATCH 2/5] topic fix --- include/roscontroller.h | 1 - src/roscontroller.cpp | 6 ++++-- src/testflockfev.bzz | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index d9aa948..738371a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -92,7 +92,6 @@ private: 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 battery_sub; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5a13086..47473d1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -42,6 +42,7 @@ namespace rosbzz_node{ void roscontroller::GetRobotId() { + 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)){ @@ -50,6 +51,8 @@ namespace rosbzz_node{ } robot_id=robot_id_srv_response.value.integer; + + //robot_id = 8; } /*------------------------------------------------- @@ -793,8 +796,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.z = 0; moveMsg.pose.orientation.w = 1; - - localsetpoint_nonraw_pub.publish(moveMsg); + localsetpoint_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 7b1df3f..30e5b80 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -50,7 +50,7 @@ function hexagon() { statef=land } else { timeW = timeW+1 - uav_moveto(0.0,0.35) + uav_moveto(0.0,0.0) } } From fcf9b17f0fa737cab8d4b66ac4285783582012bf Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 14 Apr 2017 14:54:29 -0400 Subject: [PATCH 3/5] without xbee --- include/roscontroller.h | 1 + src/roscontroller.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 31f66fc..80ab0d2 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -73,6 +73,7 @@ private: int old_val=0; std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name; bool rcclient; + bool xbeeplugged; bool multi_msg; Num_robot_count count_robots; ros::ServiceClient mav_client; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 73938fa..ad1cf31 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -123,11 +123,11 @@ namespace rosbzz_node{ n_c.getParam("out_payload", out_payload); /*Obtain in payload name*/ n_c.getParam("in_payload", in_payload); - /*Obtain Number of robots for barrier*/ - 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); + if(n_c.getParam("xbee_plugged", xbeeplugged)); + else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} GetSubscriptionParameters(n_c); // initialize topics to null? @@ -140,7 +140,8 @@ namespace rosbzz_node{ //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 (gps_topic, gps_type)); From 73c81fb22b377abd674d1b9b50c1b38392559725 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 15 Apr 2017 18:17:19 -0400 Subject: [PATCH 4/5] 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 Date: Thu, 20 Apr 2017 16:07:36 -0400 Subject: [PATCH 5/5] modif for sim --- include/buzz_utility.h | 2 +- src/buzz_utility.cpp | 3 +++ src/buzzuav_closures.cpp | 1 + src/roscontroller.cpp | 10 +++++++--- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 6b2a1aa..e276e76 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -49,7 +49,7 @@ int buzz_script_done(); int update_step_test(); -uint16_t get_robotid(); +int get_robotid(); buzzvm_t get_vm(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 15ba455..01769fd 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -51,6 +51,9 @@ namespace buzz_utility{ return out; } + int get_robotid() { + return Robot_id; + } /***************************************************/ /*Appends obtained messages to buzz in message Queue*/ /***************************************************/ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6fa15b9..ae7557e 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -30,6 +30,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); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 21457a9..2a20791 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -29,6 +29,7 @@ namespace rosbzz_node{ 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; } /*--------------------- @@ -208,12 +209,12 @@ namespace rosbzz_node{ //current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this); //battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); - payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); + payload_sub = n_c.subscribe("/"+robot_name+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); /*publishers*/ - payload_pub = n_c.advertise(out_payload, 1000); + payload_pub = n_c.advertise("/"+robot_name+out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); localsetpoint_pub = n_c.advertise("/"+robot_name+setpoint_name,1000); /* Service Clients*/ @@ -797,7 +798,7 @@ namespace rosbzz_node{ geometry_msgs::PoseStamped moveMsg; moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; - moveMsg.header.frame_id = 1; + moveMsg.header.frame_id = "/world"; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); moveMsg.pose.position.x = local_pos[0]+x; @@ -811,6 +812,9 @@ namespace rosbzz_node{ localsetpoint_pub.publish(moveMsg); + ROS_INFO("Home: %f %f %f", home[0],home[1],home[2]); + ROS_INFO("Current: %f %f %f", cur_pos[0],cur_pos[1],cur_pos[2]); + ROS_INFO("Offset NED: %f %f", local_pos[0], local_pos[1]); ROS_INFO("Sent local NON RAW position message!"); }