diff --git a/include/roscontroller.h b/include/roscontroller.h index c6c2ec7..f009be4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -111,21 +111,9 @@ private: void set_cur_pos(double latitude, double longitude, double altitude); - /*convert from cartesian to spherical coordinate system callback */ - void cvt_spherical_coordinates(double neighbours_pos_payload [], double out[]); - - /*convert from spherical to cartesian coordinate system callback */ - void cvt_cartesian_coordinates(double neighbours_pos_payload [], double out[]); - /*convert from spherical to cartesian coordinate system callback */ void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); - /*convert from spherical to cartesian coordinate system callback */ - void cvt_rangebearingGB_coordinates(double neighbours_pos_payload [], double out[], double pos[]); - - /*convert from spherical to cartesian coordinate system callback */ - void cvt_rangebearing2D_coordinates(double neighbours_pos_payload [], double out[], double pos[]); - /*battery status callback*/ void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 74674cf..f9af619 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -9,6 +9,9 @@ #include "buzzuav_closures.h" #include "buzz_utility.h" namespace buzzuav_closures{ + +// TODO: Minimize the required global variables and put them in the header + static double goto_pos[3]; static double rc_goto_pos[3]; static float batt[3]; @@ -62,42 +65,13 @@ int buzzros_print(buzzvm_t vm) { return buzzvm_ret0(vm); } -/****************************************/ -/****************************************/ -#define EARTH_RADIUS 6371000.0 -/*convert from spherical to cartesian coordinate system callback */ - void cartesian_coordinates(double spherical_pos_payload [], double out[]){ - double latitude, longitude, rho; - latitude = spherical_pos_payload[0] / 180.0 * M_PI; - longitude = spherical_pos_payload[1] / 180.0 * M_PI; - rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth - try { - out[0] = cos(latitude) * cos(longitude) * rho; - out[1] = cos(latitude) * sin(longitude) * rho; - out[2] = sin(latitude) * rho; // z is 'up' - } catch (std::overflow_error e) { -// std::cout << e.what() << " Error in convertion to cartesian coordinate system "<f.value; - //double cur_pos_cartesian[3]; - //cartesian_coordinates(cur_pos,cur_pos_cartesian); - //double goto_cartesian[3]; - //goto_cartesian[0] = dx + cur_pos_cartesian[0]; - //goto_cartesian[1] = dy + cur_pos_cartesian[1]; - //goto_cartesian[2] = cur_pos_cartesian[2]; - //spherical_coordinates(goto_cartesian, goto_pos); -// goto_pos[0]=dx; -// goto_pos[1]=dy; - //goto_pos[2]=height; // force a constant altitude to avoid loop increases gps_from_rb(d, b, goto_pos); -/* if(dx == 1.0){ - if((int)buzz_utility::get_robotid()==1){ - goto_pos[0]=hcpos1[0];goto_pos[1]=hcpos1[1];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==2){ - goto_pos[0]=hcpos2[0];goto_pos[1]=hcpos2[1];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==3){ - goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height; - } - } - if(dx == 2.0){OB - if((int)buzz_utility::get_robotid()==1){ - goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==2){ - goto_pos[0]=hcpos2[2];goto_pos[1]=hcpos2[3];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==3){ - goto_pos[0]=hcpos3[2];goto_pos[1]=hcpos3[3];goto_pos[2]=height; - } - }*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd=2; return buzzvm_ret0(vm); } +/*----------------------------------------/ +/ Buzz closure to go directly to a GPS destination from the Mission Planner +/----------------------------------------*/ int buzzuav_goto(buzzvm_t vm) { rc_goto_pos[2]=height; set_goto(rc_goto_pos); @@ -171,6 +119,9 @@ int buzzuav_goto(buzzvm_t vm) { return buzzvm_ret0(vm); } +/*----------------------------------------/ +/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and runing +/----------------------------------------*/ int buzzuav_arm(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); @@ -183,12 +134,44 @@ int buzzuav_disarm(buzzvm_t vm) { buzz_cmd=4; return buzzvm_ret0(vm); } -/******************************/ +/*---------------------------------------/ +/ Buss closure for basic UAV commands +/---------------------------------------*/ +int buzzuav_takeoff(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* Altitude */ + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; + height = goto_pos[2]; + cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; + printf(" Buzz requested Take off !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); +} + +int buzzuav_land(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); +} + +int buzzuav_gohome(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + printf(" Buzz requested gohome !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); +} + + +/*------------------------------- +/ Get/Set to transfer variable from Roscontroller to buzzuav +/------------------------------------*/ double* getgoto() { return goto_pos; } -/******************************/ + int getcmd() { return cur_cmd; } @@ -221,43 +204,11 @@ void set_obstacle_dist(float dist[]) { obst[i] = dist[i]; } - -/****************************************/ -/****************************************/ - -int buzzuav_takeoff(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 1); - buzzvm_lload(vm, 1); /* Altitude */ - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; - height = goto_pos[2]; - cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; - printf(" Buzz requested Take off !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} - -int buzzuav_land(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_LAND; - printf(" Buzz requested Land !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} - -int buzzuav_gohome(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - printf(" Buzz requested gohome !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} - -/****************************************/ void set_battery(float voltage,float current,float remaining){ batt[0]=voltage; batt[1]=current; batt[2]=remaining; } -/****************************************/ int buzzuav_update_battery(buzzvm_t vm) { //static char BATTERY_BUF[256]; @@ -305,7 +256,10 @@ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_gstore(vm); return vm->state; } -/*obstacle*/ +/*----------------------------------------- +/ Create an obstacle Buzz table from proximity sensors +/ TODO: swap to proximity function below +--------------------------------------------*/ int buzzuav_update_obstacle(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1)); @@ -333,24 +287,16 @@ int buzzuav_update_obstacle(buzzvm_t vm) { buzzvm_gstore(vm); return vm->state; } -/**************************************/ -/*Flight status ratinale*/ -/**************************************/ -/***************************************************************** - DJI_SDK -Mavlink ExtendedState landed_state -STATUS_GROUND_STANDBY = 1, 1 -STATUS_TAKE_OFF = 2, 3 -STATUS_SKY_STANDBY = 3, 2 -STATUS_LANDING = 4, 4 -STATUS_FINISHING_LANDING = 5 0 -******************************************************************/ -/****************************************/ -/*flight status update*/ -/***************************************/ + void flight_status_update(uint8_t state){ status=state; } -/****************************************/ + +/*---------------------------------------------------- +/ Create the generic robot table with status, remote controller current comand and destination +/ and current position of the robot +/ TODO: change global name for robot +/------------------------------------------------------*/ int buzzuav_update_flight_status(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); buzzvm_pusht(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 64a318c..001b4fc 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -15,6 +15,9 @@ namespace rosbzz_node{ /*Compile the .bzz file to .basm, .bo and .bdbg*/ Compile_bzz(); set_bzz_file(bzzfile_name.c_str()); + /*Initialize variables*/ + armstate = 0; + multi_msg = true; } /***Destructor***/ @@ -83,6 +86,9 @@ namespace rosbzz_node{ } } + /*-------------------------------------------------------- + / Get all required parameters from the ROS launch file + /-------------------------------------------------------*/ void roscontroller::Rosparameters_get(ros::NodeHandle n_c){ /*Obtain .bzz file name from parameter server*/ @@ -146,6 +152,9 @@ namespace rosbzz_node{ else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} } + /*-------------------------------------------------------- + / Create all required subscribers, publishers and ROS service clients + /-------------------------------------------------------*/ void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){ /*subscribers*/ @@ -191,6 +200,9 @@ namespace rosbzz_node{ } } + /*-------------------------------------------------------- + / Create Buzz bytecode from the bzz script inputed + /-------------------------------------------------------*/ void roscontroller::Compile_bzz(){ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; @@ -215,6 +227,9 @@ namespace rosbzz_node{ } + /*-------------------------------------------------------- + / Fonctions handling the local MAV ROS fligh controller + /-------------------------------------------------------*/ void roscontroller::Arm(){ mavros_msgs::CommandBool arming_message; arming_message.request.value = armstate; @@ -240,31 +255,22 @@ namespace rosbzz_node{ } - void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){ - mavros_msgs::WaypointPush srv; - mavros_msgs::Waypoint waypoint; - - // prepare waypoint mission package - waypoint.frame = mavros_msgs::Waypoint::FRAME_GLOBAL; - waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT; - waypoint.is_current = 2; // IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter - waypoint.autocontinue = 0; - waypoint.x_lat = lat; - waypoint.y_long = lng; - waypoint.z_alt = alt; - - srv.request.waypoints.push_back(waypoint); - if(mission_client.call(srv)){ - ROS_INFO("Waypoint setup service called!"); - } else { - ROS_INFO("Waypoint setup service failed!"); - } - - } - - void roscontroller::fc_command_setup(){ + /*----------------------------------------------------------------- + /Prepare Buzz messages payload for each step and publish + / + /*******************************************************************************************************/ + /* Message format of payload (Each slot is uint64_t) */ + /* _________________________________________________________________________________________________ */ + /*| | | | | | */ + /*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */ + /*|_____|_____|_____|________________________________________________|______________________________| */ + /*******************************************************************************************************/ + void roscontroller::prepare_msg_and_publish(){ + /* flight controller client call if requested from Buzz*/ /*FC call for takeoff,land and gohome*/ + /* TODO: this should go in a separate function and be called by the main Buzz step */ + int tmp = buzzuav_closures::bzz_cmd(); double* goto_pos = buzzuav_closures::getgoto(); @@ -478,10 +484,12 @@ namespace rosbzz_node{ } - #define EARTH_RADIUS 6371000.0 - #define LAT_AVERAGE 45.564898 - /*rectangular projection range and bearing coordinate system callback */ - void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){ + /*----------------------------------------------------------- + / Compute Range and Bearing of a neighbor in a local reference frame + / from GPS coordinates + ----------------------------------------------------------- */ + #define EARTH_RADIUS (double) 6371000.0 + void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ double lat1 = cur[0]*M_PI/180.0; double lon1 = cur[1]*M_PI/180.0; double lat2 = nei[0]*M_PI/180.0; @@ -493,146 +501,7 @@ namespace rosbzz_node{ out[2] = 0.0; } - /*rectangular projection range and bearing coordinate system callback */ - void roscontroller::cvt_rangebearing2D_coordinates(double spherical_pos_payload [], double out[], double cur[]){ - double nei_lat = spherical_pos_payload[0] / 180.0 * M_PI; - double nei_long = spherical_pos_payload[1] / 180.0 * M_PI; - double cur_lat = cur[0] / 180.0 * M_PI; - double cur_long = cur[1] / 180.0 * M_PI; - double rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth - double x_nei = rho * nei_long * cos(LAT_AVERAGE); - double y_nei = rho * nei_lat; - double x_cur = rho * cur_long * cos(LAT_AVERAGE); - double y_cur = rho * cur_lat; - out[0] = sqrt(pow(x_nei-x_cur,2.0)+pow(y_nei-y_cur,2.0)); - out[1] = atan2(y_nei-y_cur,x_nei-x_cur); - out[2] = 0; - } - - /*convert spherical to cartesian coordinate system callback */ - void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){ - double latitude = spherical_pos_payload[0] / 180.0 * M_PI; - double longitude = spherical_pos_payload[1] / 180.0 * M_PI; - double rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth - try { - out[0] = cos(latitude) * cos(longitude) * rho; - out[1] = cos(latitude) * sin(longitude) * rho; - out[2] = sin(latitude) * rho; // z is 'up' - } catch (std::overflow_error e) { - std::cout << e.what() << " Error in convertion to cartesian coordinate system "< 1) && (change < 0.0000000000001)) { - //cout << "CONVERSION CONVERGED at " << i << " !!!!" << endl; - converged = 1; - break; - } - } - - // eq. 19 - out[0] = b * A * (sigma - deltasigma); - - // didn't converge? must be N/S - if (!converged) { - if (pos[0] > spherical_pos[0]) { - out[1] = M_PI/2; - out[2] = 0; - } else if (pos[0] < spherical_pos[0]) { - out[1] = 0; - out[2] = M_PI/2; - } - } else { // else, it converged, so do the math - - // eq. 20 - out[1] = atan2(cosU2 * sin(lambda), (cosU1*sinU2 - sinU1*cosU2 * cos(lambda))); - if (out[1] < 0.0) out[1] += 2*M_PI; - - // eq. 21 - out[2] = atan2(cosU1 * sin(lambda), (-sinU1*cosU2 + cosU1*sinU2 * cos(lambda))) + 3.1416; - if (out[2] < 0.0) out[2] += 2*M_PI; - } - - if (out[1] >= M_PI) out[1] -= M_PI; - if (out[2] >= M_PI) out[2] -= M_PI; - - } catch (std::overflow_error e) { - std::cout << e.what() << " Error in convertion to range and bearing "<voltage,msg->current,msg->remaining); //ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining); @@ -729,23 +598,11 @@ namespace rosbzz_node{ memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; - double cvt_neighbours_pos_test[3]; - cvt_rangebearingGB_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos); - /*Convert obtained position to relative CARTESIAN position*/ - double cartesian_neighbours_pos[3], cartesian_cur_pos[3]; - cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos); - cvt_cartesian_coordinates(cur_pos, cartesian_cur_pos); - /*Compute the relative position*/ - for(int i=0;i<3;i++){ - neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i]; - } - double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test; - //double cvt_neighbours_pos_payload[3]; - //cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload); + double cvt_neighbours_pos_payload[3]; + cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos); /*Extract robot id of the neighbour*/ uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; - // cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl; /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); /*Put RID and pos*/ @@ -757,42 +614,16 @@ namespace rosbzz_node{ } - 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; - if(mode_client.call(set_mode_message)) { - ROS_INFO("Set Mode Service call successful!"); - } else { - ROS_INFO("Set Mode Service call failed!"); - } - } - - /* RC command service */ + /*----------------------------------------------------------- + / Catch the ROS service call from a custom remote controller (Mission Planner) + / and send the requested commands to Buzz + ----------------------------------------------------------- */ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res){ int rc_cmd; -/* if(req.command==oldcmdID) - return true; - else oldcmdID=req.command;*/ switch(req.command){ case mavros_msgs::CommandCode::NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; - /* arming */ - SetMode(); - cout << "ARM: " << armstate <