From 4c99bb6acc3b50421c296fd847d0bf4905c46f32 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 11 Apr 2017 20:23:21 -0400 Subject: [PATCH 01/11] 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 fcf9b17f0fa737cab8d4b66ac4285783582012bf Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 14 Apr 2017 14:54:29 -0400 Subject: [PATCH 02/11] 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 03/11] 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 04/11] 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!"); } From a14b753b7eaf91c9220d32c81be4fa8f5fcb8b65 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 20 Apr 2017 23:23:36 -0400 Subject: [PATCH 05/11] minor fixes --- include/roscontroller.h | 4 +- src/roscontroller.cpp | 168 +++++++++++++++++++++------------------- 2 files changed, 91 insertions(+), 81 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index d9aa948..ab8eab4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -80,18 +80,18 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; + std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name, robot_name; std::string stream_client_name; 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; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5a13086..3e8fb24 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -23,9 +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()+4, NULL, 10); setpoint_counter = 0; fcu_timeout = TIMEOUT; + home[0]=0;home[1]=0;home[2]=0; } /*--------------------- @@ -137,7 +141,11 @@ namespace rosbzz_node{ n_c.getParam("in_payload", in_payload); /*Obtain standby script to run during update*/ n_c.getParam("stand_by", stand_by); + + if(n_c.getParam("xbee_plugged", xbeeplugged)); + else {ROS_ERROR("Provide the xbee_plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} n_c.getParam("xbee_status_srv", xbeesrv_name); + n_c.getParam("robot_name", robot_name); GetSubscriptionParameters(n_c); // initialize topics to null? @@ -197,15 +205,15 @@ namespace rosbzz_node{ //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(setpoint_name,1000); + localsetpoint_nonraw_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; } @@ -216,22 +224,22 @@ 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; + std::cout << "Subscribed to: " << "/"+robot_name+it->first << endl; } } @@ -503,7 +511,9 @@ namespace rosbzz_node{ 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; } @@ -511,7 +521,9 @@ namespace rosbzz_node{ 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] = cur[2]; } @@ -550,13 +562,15 @@ 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<0.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(lat,lon, cur_rel_altitude);//msg->altitude); + buzzuav_closures::set_currentpos(lat,lon, cur_rel_altitude);//msg->altitude); } /*------------------------------------------------------------- / Update altitude into BVM from subscriber @@ -575,6 +589,62 @@ 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); + 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; + + + if(fabs(x)>0.01 && fabs(y)>0.01) { + 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 @@ -770,65 +840,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; - - - 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"); - } - - } From ee8ae38aaf339bed8a08aa60db3ff0afd9e92e55 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 21 Apr 2017 12:22:39 -0400 Subject: [PATCH 06/11] local pose tests --- CMakeLists.txt | 2 +- include/roscontroller.h | 21 +++++++++ src/roscontroller.cpp | 102 ++++++++++++++++++++++++++++++++++++---- 3 files changed, 116 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 385dec8..9cb3fac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/include/roscontroller.h b/include/roscontroller.h index 08617bf..05b28b5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,6 +39,19 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 +/** Semi-major axis of the Earth, \f$ a \f$, in meters. + * This is a defining parameter of the WGS84 ellipsoid. */ +#define WGS84_A 6378137.0 +/** Inverse flattening of the Earth, \f$ 1/f \f$. + * This is a defining parameter of the WGS84 ellipsoid. */ +#define WGS84_IF 298.257223563 +/** The flattening of the Earth, \f$ f \f$. */ +#define WGS84_F (1/WGS84_IF) +/** Semi-minor axis of the Earth in meters, \f$ b = a(1-f) \f$. */ +#define WGS84_B (WGS84_A*(1-WGS84_F)) +/** Eccentricity of the Earth, \f$ e \f$ where \f$ e^2 = 2f - f^2 \f$ */ +#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F)) + using namespace std; namespace rosbzz_node{ @@ -61,6 +74,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; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b661098..c249d62 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -151,7 +151,7 @@ namespace rosbzz_node{ 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("robot_name", robot_name)); + if(n_c.getParam("name", robot_name)); else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} } @@ -514,26 +514,111 @@ 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[]){ + + // 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_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); + 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_y,ned_x); + out[1] = atan2(ned[1],ned[0]); 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[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + 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);*/ } /*------------------------------------------------------ @@ -609,6 +694,7 @@ namespace rosbzz_node{ moveMsg.header.frame_id = 1; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); + ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]); moveMsg.pose.position.x = local_pos[0]+x; moveMsg.pose.position.y = local_pos[1]+y; moveMsg.pose.position.z = z; @@ -619,7 +705,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.w = 1; - if(fabs(x)>0.01 && fabs(y)>0.01) { + if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } From 84d64a197961323b9fdecc0c675424e22a9b49b4 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 21 Apr 2017 13:29:53 -0400 Subject: [PATCH 07/11] test bzz script --- src/testflocksim.bzz | 207 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 src/testflocksim.bzz diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz new file mode 100644 index 0000000..21ca80b --- /dev/null +++ b/src/testflocksim.bzz @@ -0,0 +1,207 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +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 = 5.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 +EPSILON = 8.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) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From 421d78bacc4e72c32a1b481fa61fa279e3c20a39 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 21 Apr 2017 16:31:05 -0400 Subject: [PATCH 08/11] new fix for localpose --- src/roscontroller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c249d62..70efbe0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -461,8 +461,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); } @@ -695,8 +694,9 @@ namespace rosbzz_node{ double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]); - moveMsg.pose.position.x = local_pos[0]+x; - moveMsg.pose.position.y = local_pos[1]+y; + /*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; @@ -705,10 +705,10 @@ namespace rosbzz_node{ moveMsg.pose.orientation.w = 1; - if(fabs(x)>0.01 || fabs(y)>0.01) { + //if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); - } + //} } void roscontroller::SetMode(std::string mode, int delay_miliseconds){ From 61d1cab41b63079dd0712d9d0bdec07cfc88128f Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 24 Apr 2017 14:20:59 -0400 Subject: [PATCH 09/11] implement vstig users --- include/buzz_utility.h | 2 ++ include/buzzuav_closures.h | 3 +- include/roscontroller.h | 15 ++------ src/buzz_utility.cpp | 49 ++++++++++++++++++++++++- src/buzzuav_closures.cpp | 26 ++++++++++++++ src/roscontroller.cpp | 74 ++++++++++++++++++++++++++++++-------- src/testflocksim.bzz | 10 ++++-- 7 files changed, 148 insertions(+), 31 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index e276e76..3667363 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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(); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index dcfa26a..eff6835 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 diff --git a/include/roscontroller.h b/include/roscontroller.h index 05b28b5..802306f 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,19 +39,6 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 -/** Semi-major axis of the Earth, \f$ a \f$, in meters. - * This is a defining parameter of the WGS84 ellipsoid. */ -#define WGS84_A 6378137.0 -/** Inverse flattening of the Earth, \f$ 1/f \f$. - * This is a defining parameter of the WGS84 ellipsoid. */ -#define WGS84_IF 298.257223563 -/** The flattening of the Earth, \f$ f \f$. */ -#define WGS84_F (1/WGS84_IF) -/** Semi-minor axis of the Earth in meters, \f$ b = a(1-f) \f$. */ -#define WGS84_B (WGS84_A*(1-WGS84_F)) -/** Eccentricity of the Earth, \f$ e \f$ where \f$ e^2 = 2f - f^2 \f$ */ -#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F)) - using namespace std; namespace rosbzz_node{ @@ -117,6 +104,7 @@ private: 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; @@ -189,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); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 01769fd..c9cbd69 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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*/ @@ -302,6 +304,30 @@ namespace buzz_utility{ 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 */ @@ -309,9 +335,25 @@ namespace buzz_utility{ /* 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 */ /****************************************/ @@ -447,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 */ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ae7557e..5d1433f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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; @@ -244,6 +245,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 +269,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; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 70efbe0..401f3d8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -223,7 +223,9 @@ namespace rosbzz_node{ 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); - + + users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this); + multi_msg=true; } /*--------------------------------------- @@ -424,6 +426,8 @@ namespace rosbzz_node{ 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) if (mav_client.call(cmd_srv)) @@ -578,20 +582,32 @@ namespace rosbzz_node{ double M[3][3]; ecef2ned_matrix(ref_ecef, M); double ned[3]; - matrix_multiply(3, 3, 1, (double *)M, ecef, ned);*/ + matrix_multiply(3, 3, 1, (double *)M, ecef, ned); + + 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;*/ + 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; + } 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)); + // 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]; @@ -617,7 +633,15 @@ namespace rosbzz_node{ 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);*/ + 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;*/ } /*------------------------------------------------------ @@ -657,14 +681,35 @@ namespace rosbzz_node{ fcu_timeout = TIMEOUT; double lat = std::floor(msg->latitude * 1000000) / 1000000; double lon = std::floor(msg->longitude * 1000000) / 1000000; - if(cur_rel_altitude<0.2){ + /*if(cur_rel_altitude<1.2){ home[0]=lat; home[1]=lon; home[2]=cur_rel_altitude; - } + }*/ set_cur_pos(lat,lon, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(lat,lon, 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; it0.01 || fabs(y)>0.01) { + // To prevent drifting from stable position. + if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); - //} + } } void roscontroller::SetMode(std::string mode, int delay_miliseconds){ diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 21ca80b..695c6cb 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -11,11 +11,11 @@ function updated_neigh(){ neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 5.0 +TARGET_ALTITUDE = 10.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 +TARGET = 12.0 EPSILON = 8.0 # Lennard-Jones interaction magnitude @@ -196,6 +196,12 @@ neighbors.listen("cmd", 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. From bb1ec20e37e1125d25311630aad2402b7f4e898d Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 24 Apr 2017 17:55:54 -0400 Subject: [PATCH 10/11] fix namespace use --- include/roscontroller.h | 10 +++--- launch/launch_config/m100.yaml | 18 +++++----- launch/launch_config/solo.yaml | 18 +++++----- src/rosbuzz.cpp | 5 +-- src/roscontroller.cpp | 62 +++++++++++++++++----------------- src/testflocksim.bzz | 2 +- 6 files changed, 58 insertions(+), 57 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 802306f..e1e00c8 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -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(); @@ -129,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(); @@ -195,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(); @@ -204,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); diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml index 1ffd380..5ad5f46 100644 --- a/launch/launch_config/m100.yaml +++ b/launch/launch_config/m100.yaml @@ -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 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index 8930701..c53989a 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -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 diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 5190944..d997e5c 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -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(); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 401f3d8..e334b51 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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*/ @@ -120,7 +120,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)); @@ -129,11 +129,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");} } @@ -162,7 +159,7 @@ 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; @@ -202,55 +199,58 @@ 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); //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("/"+robot_name+in_payload, 1000, &roscontroller::payload_obt,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); - 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("/"+robot_name+out_payload, 1000); - neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); - localsetpoint_nonraw_pub = n_c.advertise("/"+robot_name+setpoint_name,1000); + payload_pub = n_c.advertise(out_payload, 1000); + neigh_pos_pub = n_c.advertise("neighbours_pos",1000); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name,1000); /* Service Clients*/ - 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); + arm_client = n_c.serviceClient(armclient); + mode_client = n_c.serviceClient(modeclient); + mav_client = n_c.serviceClient(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(xbeesrv_name); + stream_client = n_c.serviceClient(stream_client_name); - users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this); + 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::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){ if(it->second == "mavros_msgs/ExtendedState"){ - flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); } else if(it->second == "mavros_msgs/State"){ - flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); } else if(it->second == "mavros_msgs/BatteryStatus"){ - battery_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::battery, this); + battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this); } else if(it->second == "sensor_msgs/NavSatFix"){ - current_position_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); } else if(it->second == "std_msgs/Float64"){ - relative_altitude_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this); } - std::cout << "Subscribed to: " << "/"+robot_name+it->first << endl; + std::cout << "Subscribed to: " << it->first << endl; } } @@ -425,9 +425,9 @@ 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]; } - // 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) if (mav_client.call(cmd_srv)) @@ -751,7 +751,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.w = 1; // To prevent drifting from stable position. - if(fabs(x)>0.01 || fabs(y)>0.01) { + if(fabs(x)>0.1 || fabs(y)>0.1) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 695c6cb..2e490e1 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF" # Lennard-Jones parameters TARGET = 12.0 -EPSILON = 8.0 +EPSILON = 10.0 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { From 52bed0536f588d9982b7469f9e6f3ecb5ecb203e Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 25 Apr 2017 20:27:28 -0400 Subject: [PATCH 11/11] first uesrs stigmergy --- src/roscontroller.cpp | 52 ++++++++++++++++++++++++------------------- src/testflocksim.bzz | 2 +- 2 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e334b51..d4f0830 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -411,8 +411,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 @@ -560,12 +563,12 @@ namespace rosbzz_node{ 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_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 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); @@ -584,27 +587,29 @@ namespace rosbzz_node{ 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[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;*/ - - 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[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 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)); @@ -617,7 +622,7 @@ namespace rosbzz_node{ 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 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); @@ -633,15 +638,15 @@ namespace rosbzz_node{ 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); + 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[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;*/ + //out[1] = std::floor(out[1] * 1000000) / 1000000; + out[2] = 0.0; } /*------------------------------------------------------ @@ -679,15 +684,15 @@ namespace rosbzz_node{ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ //ROS_INFO("Altitude out: %f", cur_rel_altitude); fcu_timeout = TIMEOUT; - double lat = std::floor(msg->latitude * 1000000) / 1000000; - double lon = std::floor(msg->longitude * 1000000) / 1000000; + //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(lat,lon, cur_rel_altitude);//msg->altitude); - buzzuav_closures::set_currentpos(lat,lon, 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); @@ -738,7 +743,8 @@ namespace rosbzz_node{ moveMsg.header.frame_id = 1; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); - ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]); + 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; diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 2e490e1..107edbf 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF" # Lennard-Jones parameters TARGET = 12.0 -EPSILON = 10.0 +EPSILON = 12.0 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) {