diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index ddcf448..3588ed8 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -567,7 +567,6 @@ function set_rc_goto() { m_cMeToPred.Bearing=m_MessageBearing[IDofPred] var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing) - var S2Target=math.vec2.add(S2Pred,P2Target) gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 55ee509..48a28a7 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -5,7 +5,7 @@ ######################################## TARGET_ALTITUDE = 5.0 # m. UAVSTATE = "TURNEDOFF" -PICTURE_WAIT = 40 # steps +PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 2 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. @@ -35,10 +35,10 @@ function takeoff() { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", TARGET_ALTITUDE) + log("Altitude: ", position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) - } + } } function land() { @@ -48,7 +48,7 @@ function land() { neighbors.broadcast("cmd", 21) uav_land() - if(flight.status != 2 and flight.status != 3){ + if(flight.status != 2 and flight.status != 3) { barrier_set(ROBOTS,turnedoff,land, 21) barrier_ready() } @@ -63,7 +63,7 @@ function set_goto(transf) { if(rc_goto.id==id){ if(s!=nil){ if(s.in()) - s.leave() + s.leave() } } else { neighbors.broadcast("cmd", 16) @@ -75,10 +75,10 @@ ptime=0 function picture() { statef=picture UAVSTATE="PICTURE" - #TODO: change gimbal orientation - if(ptime==PICTURE_WAIT/2) + uav_setgimbal(0.0, 0.0, -90.0, 20.0) + if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() - else if(ptime>=PICTURE_WAIT) { + } else if(ptime>=PICTURE_WAIT) { # wait for the picture statef=action ptime=0 } @@ -86,13 +86,12 @@ function picture() { } function gotoWP(transf) { - print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", goal.range) + print(" has to move ", goal.range, goal.bearing) m_navigation = math.vec2.newp(goal.range, goal.bearing) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far.") + log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) @@ -183,7 +182,6 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { - # print("gps2rb d: ",lat,lon) # print("gps2rb h: ",position.latitude,position.longitude) d_lon = lon - position.longitude diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 47c5388..eeb9184 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -10,7 +10,8 @@ #include "buzz_utility.h" #define EARTH_RADIUS (double) 6371000.0 -#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) +#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0))) +#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI))) namespace buzzuav_closures{ typedef enum { @@ -23,6 +24,7 @@ namespace buzzuav_closures{ COMMAND_GOTO, COMMAND_MOVETO, COMMAND_PICTURE, + COMMAND_GIMBAL, } Custom_CommandCode; /* @@ -38,6 +40,7 @@ void setWPlist(std::string path); */ int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); +int buzzuav_setgimbal(buzzvm_t vm); void parse_gpslist(); int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ @@ -47,7 +50,7 @@ void set_goto(double pos[]); /*Sets goto position from rc client*/ void rc_set_goto(int id, double latitude, double longitude, double altitude); /*Sets gimbal orientation from rc client*/ -void rc_set_gimbal(int id, float yaw, float pitch); +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); /*sets rc requested command */ void rc_call(int rc_cmd); /* sets the battery state */ @@ -61,6 +64,8 @@ void set_api_rssi(float value); void set_currentpos(double latitude, double longitude, double altitude); /*retuns the current go to position */ double* getgoto(); +std::string getuavstate(); +float* getgimbal(); /* updates flight status*/ void flight_status_update(uint8_t state); /* Update neighbors table */ diff --git a/include/roscontroller.h b/include/roscontroller.h index cebee9d..a2d72d6 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -71,7 +71,7 @@ private: double longitude=0.0; double latitude=0.0; float altitude=0.0; - }; typedef struct gps GPS ; // not useful in cpp + }; typedef struct gps GPS ; GPS target, home, cur_pos; double cur_rel_altitude; diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index b191b90..1a02c36 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -307,6 +307,9 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); buzzvm_gstore(VM); @@ -352,6 +355,12 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ed9ef9b..6140310 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -16,7 +16,7 @@ namespace buzzuav_closures{ //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); static double goto_pos[3]; static double rc_goto_pos[3]; - static float rc_gimbal[2]; + static float rc_gimbal[4]; static float batt[3]; static float obst[5]={0,0,0,0,0}; static double cur_pos[3]; @@ -28,11 +28,10 @@ namespace buzzuav_closures{ static float height=0; static bool deque_full = false; static int rssi = 0; - static int message_number = 0; static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; - string WPlistname = ""; + string WPlistname = ""; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::RB_struct> wplist_map; @@ -94,12 +93,12 @@ namespace buzzuav_closures{ /----------------------------------------*/ void gps_from_rb(double range, double bearing, double out[3]) { - double lat = cur_pos[0]*M_PI/180.0; - double lon = cur_pos[1]*M_PI/180.0; + double lat = RAD2DEG(cur_pos[0]); + double lon = RAD2DEG(cur_pos[1]); out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); - out[0] = out[0]*180.0/M_PI; - out[1] = out[1]*180.0/M_PI; + out[0] = RAD2DEG(out[0]); + out[1] = RAD2DEG(out[1]); out[2] = height; //constant height. } @@ -321,11 +320,34 @@ namespace buzzuav_closures{ / Buzz closure to take a picture here. /----------------------------------------*/ int buzzuav_takepicture(buzzvm_t vm) { - cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; buzz_cmd = COMMAND_PICTURE; return buzzvm_ret0(vm); } + /*----------------------------------------/ + / Buzz closure to change locally the gimbal orientation + /----------------------------------------*/ + int buzzuav_setgimbal(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // time + buzzvm_lload(vm, 2); // pitch + buzzvm_lload(vm, 3); // roll + buzzvm_lload(vm, 4); // yaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; + rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; + rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; + rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; + + ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]); + buzz_cmd = COMMAND_GIMBAL; + return buzzvm_ret0(vm); + } + /*----------------------------------------/ / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ @@ -413,6 +435,20 @@ namespace buzzuav_closures{ return goto_pos; } + float* getgimbal() { + return rc_gimbal; + } + + string getuavstate() { + static buzzvm_t VM = buzz_utility::get_vm(); + std::stringstream state_buff; + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); + buzzvm_gload(VM); + buzzobj_t uav_state = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + return uav_state->s.value.str; + } + int getcmd() { return cur_cmd; } @@ -438,11 +474,13 @@ namespace buzzuav_closures{ } - void rc_set_gimbal(int id, float yaw, float pitch) { + void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) { rc_id = id; rc_gimbal[0] = yaw; - rc_gimbal[1] = pitch; + rc_gimbal[1] = roll; + rc_gimbal[2] = pitch; + rc_gimbal[3] = t; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3383569..bed9e10 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,7 +195,9 @@ void roscontroller::RosControllerRun() robot_id)) { ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -220,15 +222,8 @@ void roscontroller::RosControllerRun() } const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) - log<::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -262,40 +257,28 @@ void roscontroller::RosControllerRun() << "," << (double)it->second.z; } log << std::endl;*/ - } + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF - SCRIPT IS NOT graphform.bzz*/ - static buzzvm_t VM = buzz_utility::get_vm(); - std::stringstream state_buff; - //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); - // buzzvm_gload(VM); - // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); - // buzzvm_pop(VM); - // state_buff<< graph_state->s.value.str<<","; - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); - buzzvm_gload(VM); - buzzobj_t uav_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - state_buff<< uav_state->s.value.str; - log<0) no_of_robots // =neighbours_pos_map.size()+1; // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); - // ROS_INFO("ROBOTS: %i , acutal : - // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); - get_xbee_status(); + //updates_set_robots(no_of_robots); + //get_xbee_status(); // commented out because it may slow down the node too much, to be tested /*run once*/ ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); + // make sure to sleep for the remainder of our cycle time + if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, loop_rate.cycleTime().toSec()); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); else @@ -633,7 +616,7 @@ void roscontroller::prepare_msg_and_publish() delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && + /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && multi_msg) { uint8_t *buff_send = 0; @@ -675,7 +658,7 @@ void roscontroller::prepare_msg_and_publish() payload_pub.publish(update_packets); multi_msg = false; delete[] payload_64; - } + }*/ } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -685,95 +668,119 @@ script void roscontroller::flight_controller_service_call() { /* flight controller client call if requested from Buzz*/ - /*FC call for takeoff,land, gohome and arm/disarm*/ - int tmp = buzzuav_closures::bzz_cmd(); - double *goto_pos = buzzuav_closures::getgoto(); - if (tmp == buzzuav_closures::COMMAND_TAKEOFF || - tmp == buzzuav_closures::COMMAND_LAND || - tmp == buzzuav_closures::COMMAND_GOHOME) { - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - // std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl; - // 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); - armstate = 0; - Arm(); - } - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else { - ROS_ERROR("Failed to call service from flight controller"); - } - armstate = 0; - break; - case mavros_msgs::CommandCode::NAV_TAKEOFF: - if (!armstate) { + double *goto_pos; + float *gimbal; + switch (buzzuav_closures::bzz_cmd()) { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home = cur_pos; - } - if (current_mode != "GUIDED") - SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it - // should always be in loiter after arm/disarm) + case buzzuav_closures::COMMAND_TAKEOFF: + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (!armstate) { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + // Registering HOME POINT. + home = cur_pos; + } + 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)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case buzzuav_closures::COMMAND_LAND: + cmd_srv.request.command = buzzuav_closures::getcmd(); + 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 { + ROS_ERROR("Failed to call service from flight controller"); + } + armstate = 0; + break; + + case buzzuav_closures::COMMAND_GOHOME: // NOT FULLY IMPLEMENTED/TESTED !!! + cmd_srv.request.param5 = home.latitude; + cmd_srv.request.param6 = home.longitude; + cmd_srv.request.param7 = home.altitude; + cmd_srv.request.command = buzzuav_closures::getcmd(); if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller"); break; - } - } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ - /*prepare the goto publish message */ - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else - ROS_ERROR("Failed to call service from flight controller"); - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else - ROS_ERROR("Failed to call service from flight controller"); - } else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/ - if (!armstate) { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - } - } else if (tmp == buzzuav_closures::COMMAND_DISARM) { - if (armstate) { - armstate = 0; - SetMode("LOITER", 0); - Arm(); - } - } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); - } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ - ROS_INFO("TAKING A PICTURE HERE!! --------------"); - cmd_srv.request.param1 = 90; - cmd_srv.request.param2 = 0; - cmd_srv.request.param3 = 0; - cmd_srv.request.param4 = 0; - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else - ROS_ERROR("Failed to call service from flight controller"); - mavros_msgs::CommandBool capture_command; - capture_srv.call(capture_command); + case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!! + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param5 = goto_pos[0]; + cmd_srv.request.param6 = goto_pos[1]; + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case buzzuav_closures::COMMAND_ARM: + if (!armstate) { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + } + break; + + case buzzuav_closures::COMMAND_DISARM: + if (armstate) { + armstate = 0; + SetMode("LOITER", 0); + Arm(); + } + break; + + case buzzuav_closures::COMMAND_MOVETO: + goto_pos = buzzuav_closures::getgoto(); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); + break; + + case buzzuav_closures::COMMAND_GIMBAL: + gimbal = buzzuav_closures::getgimbal(); + cmd_srv.request.param1 = gimbal[0]; + cmd_srv.request.param2 = gimbal[1]; + cmd_srv.request.param3 = gimbal[2]; + cmd_srv.request.param4 = gimbal[3]; + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case buzzuav_closures::COMMAND_PICTURE: + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + if (capture_srv.call(capture_command)) { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } else + ROS_ERROR("Failed to call service from camera streamer"); + break; } } + /*---------------------------------------------- /Refresh neighbours Position for every ten step /---------------------------------------------*/ @@ -823,39 +830,6 @@ void roscontroller::set_cur_pos(double latitude, double longitude, / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates ----------------------------------------------------------- */ -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]; - } -} - float roscontroller::constrainAngle(float x) { x = fmod(x,2*M_PI); if (x < 0.0) @@ -989,18 +963,10 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) { moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - // float ned_x, ned_y; - // gps_ned_home(ned_x, ned_y); - // 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)*/ - // target[0]+=y; target[1]+=x; - moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y; - moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x; + //ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); + moveMsg.pose.position.x = local_pos_new[0] + y; + moveMsg.pose.position.y = local_pos_new[1] + x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -1111,7 +1077,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); -// ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); + ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], @@ -1175,7 +1141,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, break; case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); - buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); + buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3,req.param4,req.param5); rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true;