diff --git a/CMakeLists.txt b/CMakeLists.txt index 0df4d4c..385dec8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS ) ############################## -#Generate messages# ############################## add_message_files( diff --git a/include/roscontroller.h b/include/roscontroller.h index 80ab0d2..a337cf7 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -13,8 +13,13 @@ #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" -#include -#include +#include "mavros_msgs/WaypointPush.h" +#include "mavros_msgs/Waypoint.h" +#include "mavros_msgs/PositionTarget.h" +#include "mavros_msgs/StreamRate.h" +#include "mavros_msgs/ParamGet.h" +#include "geometry_msgs/PoseStamped.h" +#include "std_msgs/Float64.h" #include #include #include @@ -32,6 +37,7 @@ #define UPDATER_MESSAGE_CONSTANT 987654321 #define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_STOP_TRANSMISSION 4355356352 +#define TIMEOUT 60 using namespace std; @@ -56,6 +62,8 @@ private: }; typedef struct num_robot_count Num_robot_count ; double cur_pos[3]; + double home[3]; + double cur_rel_altitude; uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; @@ -64,14 +72,18 @@ private: int robot_id=0; //int oldcmdID=0; int rc_cmd; + float fcu_timeout; int armstate; int barrier; int message_number=0; - int no_of_robots=0; + uint8_t no_of_robots=0; /*tmp to be corrected*/ - int no_cnt=0; - int old_val=0; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name; + 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 stream_client_name; + std::string relative_altitude_sub_name; + std::string setpoint_nonraw; bool rcclient; bool xbeeplugged; bool multi_msg; @@ -87,7 +99,13 @@ private: ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; - //ros::Subscriber Robot_id_sub; + ros::Subscriber Robot_id_sub; + ros::Subscriber relative_altitude_sub; + ros::ServiceClient stream_client; + + int setpoint_counter; + double my_x = 0, my_y = 0; + /*Commands for flight controller*/ //mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandLong cmd_srv; @@ -103,6 +121,8 @@ private: /*Initialize publisher and subscriber, done in the constructor*/ 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); @@ -110,7 +130,7 @@ private: void Compile_bzz(); /*Flight controller service call*/ - void flight_controler_service_call(); + void flight_controller_service_call(); /*Neighbours pos publisher*/ void neighbours_pos_publisher(); @@ -134,6 +154,7 @@ private: double altitude); /*convert from spherical to cartesian coordinate system callback */ void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); + void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]); /*battery status callback*/ void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); @@ -147,6 +168,9 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); + /*current relative altitude callback*/ + void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); + /*payload callback callback*/ void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); @@ -166,12 +190,23 @@ private: void Arm(); /*set mode like guided for solo*/ - void SetMode(); + void SetMode(std::string mode, int delay_miliseconds); /*Robot independent subscribers*/ void Subscribe(ros::NodeHandle n_c); + + //void WaypointMissionSetup(float lat, float lng, float alt); + + void fc_command_setup(); + + void SetLocalPosition(float x, float y, float z, float yaw); + + void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + + void SetStreamRate(int id, int rate, int on_off); void get_number_of_robots(); + void GetRobotId(); }; } diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml index b7f1eb8..1ffd380 100644 --- a/launch/launch_config/m100.yaml +++ b/launch/launch_config/m100.yaml @@ -3,9 +3,14 @@ topics: 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 battery : mavros_msgs/BatteryStatus status : mavros_msgs/ExtendedState + altitude: std_msgs/Float64 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index 7d3ceb4..8930701 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -3,8 +3,11 @@ topics: 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 @@ -12,5 +15,6 @@ type: # for solo #battery : mavros_msgs/BatteryStatus status : mavros_msgs/State + altitude: std_msgs/Float64 environment : environment : solo-simulator diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch new file mode 100644 index 0000000..26ba3e7 --- /dev/null +++ b/launch/rosbuzz-solo.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch new file mode 100644 index 0000000..141f0a0 --- /dev/null +++ b/launch/rosbuzz-testing-sitl.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch new file mode 100644 index 0000000..d955758 --- /dev/null +++ b/launch/rosbuzz-testing.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 16e86c2..d79e016 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -2,14 +2,13 @@ - - + + - - + diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh new file mode 100644 index 0000000..e00b73b --- /dev/null +++ b/misc/cmdlinectr.sh @@ -0,0 +1,20 @@ +#! /bin/bash +function takeoff { + rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0 +} +function land { + rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0 +} +function record { + rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth +} +function clean { + sudo rm /var/log/upstart/robot* + sudo rm /var/log/upstart/x3s* +} +function startrobot { + sudo service robot start +} +function stoprobot { + sudo service robot stop +} diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index be9d43e..d622228 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -454,7 +454,7 @@ void collect_data(){ double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; //int bytecodesize=(int); -fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)buzz_utility::get_robotid(),neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); +fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); //FILE *Fileptr; //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 9139c12..f4bb43a 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -49,7 +49,7 @@ namespace buzz_utility{ out[3] = (int32_2 & (0xFFFF0000) ) >> 16; //cout << " values " <data, buzzmsg_payload_size(m)); tot += buzzmsg_payload_size(m); - + /* Get rid of message */ buzzoutmsg_queue_next(VM); buzzmsg_payload_destroy(&m); } while(1); - - uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t))); - *(uint16_t*)buff_send = (uint16_t) total_size; - + + uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t))); + *(uint16_t*)buff_send = (uint16_t) total_size; + uint64_t* payload_64 = new uint64_t[total_size]; - + memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); free(buff_send); /*for(int i=0;iswarms) != 1) { fprintf(stderr, "Swarm list size is not 1\n"); @@ -470,83 +471,84 @@ namespace buzz_utility{ buzz_error_info()); buzzvm_dump(VM); } - /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ - /* look into this currently we don't have swarm feature tested */ - /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ +// usleep(10000); /*Print swarm*/ - //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); - /* Check swarm state */ - /* int status = 1; + 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 && +/* if(status == 1 && buzzdict_size(VM->swarmmembers) < 9) - status = 2; + status = 2;*/ buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1)); buzzvm_pushi(VM, status); - buzzvm_gstore(VM);*/ + buzzvm_gstore(VM); } -/****************************************/ -/*Destroy the bvm and other resorces*/ -/****************************************/ + /****************************************/ + /*Destroy the bvm and other resorces*/ + /****************************************/ -void buzz_script_destroy() { - if(VM) { - if(VM->state != BUZZVM_STATE_READY) { - fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", - BO_FNAME, - buzz_error_info()); - buzzvm_dump(VM); - } - buzzvm_function_call(VM, "destroy", 0); - buzzvm_destroy(&VM); - free(BO_FNAME); - buzzdebug_destroy(&DBG_INFO); - } - fprintf(stdout, "Script execution stopped.\n"); -} + void buzz_script_destroy() { + if(VM) { + if(VM->state != BUZZVM_STATE_READY) { + fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", + BO_FNAME, + buzz_error_info()); + buzzvm_dump(VM); + } + buzzvm_function_call(VM, "destroy", 0); + buzzvm_destroy(&VM); + free(BO_FNAME); + buzzdebug_destroy(&DBG_INFO); + } + fprintf(stdout, "Script execution stopped.\n"); + } -/****************************************/ -/****************************************/ + /****************************************/ + /****************************************/ -/****************************************/ -/*Execution completed*/ -/****************************************/ + /****************************************/ + /*Execution completed*/ + /****************************************/ -int buzz_script_done() { - return VM->state != BUZZVM_STATE_READY; -} + int buzz_script_done() { + return VM->state != BUZZVM_STATE_READY; + } -int update_step_test(){ + int update_step_test() { - buzzuav_closures::buzzuav_update_battery(VM); - buzzuav_closures::buzzuav_update_prox(VM); - buzzuav_closures::buzzuav_update_currentpos(VM); - buzzuav_closures::buzzuav_update_flight_status(VM); + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); - int a = buzzvm_function_call(VM, "step", 0); -if(a != BUZZVM_STATE_READY){ - fprintf(stdout, "step test VM state %i\n",a); - fprintf(stdout, " execution terminated abnormally\n\n"); -} - return a == BUZZVM_STATE_READY; -} + int a = buzzvm_function_call(VM, "step", 0); + if(a != BUZZVM_STATE_READY) { + fprintf(stdout, "step test VM state %i\n",a); + fprintf(stdout, " execution terminated abnormally\n\n"); + } + return a == BUZZVM_STATE_READY; + } -uint16_t get_robotid(){ -/* Get hostname */ - char hstnm[30]; - gethostname(hstnm, 30); - /* Make numeric id from hostname */ - /* NOTE: here we assume that the hostname is in the format Knn */ - int id = strtol(hstnm + 4, NULL, 10); - //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); -return (uint16_t)id; -} +/* uint16_t get_robotid() { + // Get hostname + char hstnm[30]; + gethostname(hstnm, 30); + // Make numeric id from hostname + // NOTE: here we assume that the hostname is in the format Knn + int id = strtol(hstnm + 4, NULL, 10); + //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); + return (uint16_t)id; + }*/ -buzzvm_t get_vm(){ -return VM; -} + buzzvm_t get_vm() { + return VM; + } void set_robot_var(int ROBOTS){ buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); @@ -556,4 +558,3 @@ void set_robot_var(int ROBOTS){ } - diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a77c79d..6fa15b9 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -104,7 +104,7 @@ namespace buzzuav_closures{ double d = sqrt(dx*dx+dy*dy); //range goto_pos[0]=dx; goto_pos[1]=dy; - goto_pos[2]=0; + goto_pos[2]=height; /*double b = atan2(dy,dx); //bearing printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); gps_from_rb(d, b, goto_pos); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ad1cf31..1b43b42 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1,5 +1,5 @@ #include "roscontroller.h" - +#include namespace rosbzz_node{ /*--------------- @@ -16,8 +16,16 @@ namespace rosbzz_node{ Compile_bzz(); set_bzz_file(bzzfile_name.c_str()); /*Initialize variables*/ + // Solo things + SetMode("LOITER", 0); armstate = 0; multi_msg = true; + // set stream rate - wait for the FC to be started + SetStreamRate(0, 10, 1); + /// Get Robot Id - wait for Xbee to be started + GetRobotId(); + setpoint_counter = 0; + fcu_timeout = TIMEOUT; } /*--------------------- @@ -32,23 +40,26 @@ namespace rosbzz_node{ uav_done(); } + void roscontroller::GetRobotId() + { + + mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ + ros::Duration(0.1).sleep(); + ROS_ERROR("Waiting for Xbee to respond to get device ID"); + } + + robot_id=robot_id_srv_response.value.integer; + + //robot_id = 8; + } + /*------------------------------------------------- /rosbuzz_node loop method executed once every step /--------------------------------------------------*/ void roscontroller::RosControllerRun(){ - mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; - mavros_msgs::ParamGet::Response robot_id_srv_response; - while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ - /*run once*/ - ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(10); - loop_rate.sleep(); - /*sleep for the mentioned loop rate*/ - ROS_ERROR("Waiting for Xbee to respond to get device ID"); - } - robot_id=robot_id_srv_response.value.integer; - + /* 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"); @@ -66,7 +77,7 @@ namespace rosbzz_node{ /*Prepare messages and publish them in respective topic*/ prepare_msg_and_publish(); /*call flight controler service to set command long*/ - flight_controler_service_call(); + flight_controller_service_call(); /*Set multi message available after update*/ if(get_update_status()){ set_read_update_status(); @@ -84,6 +95,10 @@ namespace rosbzz_node{ /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); + if(fcu_timeout<=0) + buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); + else + fcu_timeout -= 1/10; /*sleep for the mentioned loop rate*/ timer_step+=1; maintain_pos(timer_step); @@ -128,7 +143,7 @@ namespace rosbzz_node{ 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? @@ -150,19 +165,27 @@ namespace rosbzz_node{ node_handle.getParam("type/battery", battery_type); m_smTopic_infos.insert(pair (battery_topic, battery_type)); - std::string status_topic, status_type; node_handle.getParam("topics/status", status_topic); node_handle.getParam("type/status", status_type); m_smTopic_infos.insert(pair (status_topic, status_type)); + std::string altitude_topic, altitude_type; + node_handle.getParam("topics/altitude", altitude_topic); + node_handle.getParam("type/altitude", altitude_type); + m_smTopic_infos.insert(pair (altitude_topic, altitude_type)); + /*Obtain fc client name from parameter server*/ if(node_handle.getParam("topics/fcclient", fcclient_name)); else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");} + if(node_handle.getParam("topics/setpoint", setpoint_name)); + else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/armclient", armclient)); else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/modeclient", modeclient)); else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} + if(node_handle.getParam("topics/stream", stream_client_name)); + else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} } /*-------------------------------------------------------- @@ -182,12 +205,14 @@ 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_raw/local",1000); + localsetpoint_pub = n_c.advertise(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); + multi_msg=true; } /*--------------------------------------- @@ -208,6 +233,9 @@ namespace rosbzz_node{ else if(it->second == "sensor_msgs/NavSatFix"){ 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(it->first, 1000, &roscontroller::current_rel_alt, this); + } std::cout << "Subscribed to: " << it->first << endl; } @@ -269,6 +297,7 @@ namespace rosbzz_node{ void roscontroller::Arm(){ mavros_msgs::CommandBool arming_message; arming_message.request.value = armstate; + ROS_INFO("FC Arm Service called!------------------------------------------------------"); if(arm_client.call(arming_message)) { if(arming_message.response.success==1) ROS_INFO("FC Arm Service called!"); @@ -278,20 +307,6 @@ namespace rosbzz_node{ ROS_INFO("FC Arm Service call failed!"); } } - /*--------------------------------------------------------- - / Set mode for the solos - /---------------------------------------------------------*/ - void roscontroller::SetMode(){ - mavros_msgs::SetMode set_mode_message; - set_mode_message.request.base_mode = 0; - set_mode_message.request.custom_mode = "GUIDED"; // shit! - if(mode_client.call(set_mode_message)) { - ROS_INFO("Set Mode Service call successful!"); - } else { - ROS_INFO("Set Mode Service call failed!"); - } - } - /*----------------------------------------------------------------------------------------------------- /Prepare Buzz messages payload for each step and publish @@ -323,7 +338,7 @@ namespace rosbzz_node{ else message_number++; payload_out.sysid=(uint8_t)robot_id; payload_out.msgid=(uint32_t)message_number; - + /*publish prepared messages in respective topic*/ payload_pub.publish(payload_out); delete[] out; @@ -371,46 +386,75 @@ namespace rosbzz_node{ /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz script /-------------------------------------------------------------------------------- */ - void roscontroller::flight_controler_service_call(){ + 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){ + 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.z = 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"); - } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ + 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);} + 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){ + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + } + 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; + } + + } 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"); + 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*/ - armstate=1; - Arm(); - } else if (tmp == buzzuav_closures::COMMAND_DISARM){ - armstate=0; - Arm(); - } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ - /*prepare the goto publish message */ - mavros_msgs::PositionTarget pt; - pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | mavros_msgs::PositionTarget::IGNORE_VY | mavros_msgs::PositionTarget::IGNORE_VZ | mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ | mavros_msgs::PositionTarget::IGNORE_YAW_RATE; - pt.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED; - pt.position.x = goto_pos[0]; - pt.position.y = goto_pos[1]; - pt.position.z = goto_pos[2]; - pt.yaw = 0;//goto_pos[3]; - ROS_INFO("Sending local setpoint: %.2f, %.2f, %.2f",pt.position.x,pt.position.y,pt.position.z); - localsetpoint_pub.publish(pt); - } + 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*/ + /*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::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0); + } } /*---------------------------------------------- /Refresh neighbours Position for every ten step @@ -453,7 +497,6 @@ namespace rosbzz_node{ cur_pos [2] =altitude; } - /*----------------------------------------------------------- / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates @@ -468,15 +511,14 @@ namespace rosbzz_node{ out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[1] = atan2(ned_y,ned_x); out[2] = 0.0; -/* double lat1 = cur[0]*M_PI/180.0; - double lon1 = cur[1]*M_PI/180.0; - double lat2 = nei[0]*M_PI/180.0; - double lon2 = nei[1]*M_PI/180.0; - out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS; - double y = sin(lon1-lon2)*cos(lat1); - double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2); - out[1] = atan2(y,x)+M_PI; - out[2] = 0.0;*/ + } + + void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; + out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + out[2] = cur[2]; } /*------------------------------------------------------ @@ -495,11 +537,11 @@ namespace rosbzz_node{ // TODO: Handle landing std::cout << "Message: " << msg->mode << std::endl; if(msg->mode == "GUIDED") - buzzuav_closures::flight_status_update(1); + buzzuav_closures::flight_status_update(2); else if (msg->mode == "LAND") - buzzuav_closures::flight_status_update(4); + buzzuav_closures::flight_status_update(1); else // ground standby = LOITER? - buzzuav_closures::flight_status_update(1);//? + buzzuav_closures::flight_status_update(7);//? } /*------------------------------------------------------------ @@ -512,8 +554,23 @@ namespace rosbzz_node{ / Update current position into BVM from subscriber /-------------------------------------------------------------*/ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ - set_cur_pos(msg->latitude,msg->longitude,msg->altitude); - buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude); + //ROS_INFO("Altitude out: %f", cur_rel_altitude); + fcu_timeout = TIMEOUT; + if(home[0]==0){ + home[0]=msg->latitude; + home[1]=msg->longitude; + 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); + } + /*------------------------------------------------------------- + / Update altitude into BVM from subscriber + /-------------------------------------------------------------*/ + void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){ + //ROS_INFO("Altitude in: %f", msg->data); + cur_rel_altitude = (double)msg->data; + } /*------------------------------------------------------------- /Set obstacle Obstacle distance table into BVM from subscriber @@ -588,7 +645,7 @@ namespace rosbzz_node{ delete[] out; buzz_utility::in_msg_process((message_obt+3)); } - + } /*----------------------------------------------------------- @@ -606,7 +663,7 @@ namespace rosbzz_node{ res.success = true; break; case mavros_msgs::CommandCode::NAV_LAND: - ROS_INFO("RC_Call: LAND!!!!"); + ROS_INFO("RC_Call: LAND!!!! sending land"); rc_cmd=mavros_msgs::CommandCode::NAV_LAND; buzzuav_closures::rc_call(rc_cmd); res.success = true; @@ -632,14 +689,17 @@ namespace rosbzz_node{ res.success = true; break; case mavros_msgs::CommandCode::NAV_WAYPOINT: - ROS_INFO("RC_Call: GO TO!!!! "); + ROS_INFO("RC_Call: GO TO!!!! --- Doing this! "); double rc_goto[3]; + // testing PositionTarget rc_goto[0] = req.param5; rc_goto[1] = req.param6; rc_goto[2] = req.param7; + // for test + //SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0); buzzuav_closures::rc_set_goto(rc_goto); - rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; + rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; @@ -669,9 +729,9 @@ namespace rosbzz_node{ old_val=neighbours_pos_map.size()+1; } - else if(old_val==neighbours_pos_map.size()+1){ + else if(no_cnt!=0 && old_val==neighbours_pos_map.size()+1){ no_cnt++; - if(no_cnt==3){ + if(no_cnt>=4){ no_of_robots=neighbours_pos_map.size()+1; no_cnt=0; } @@ -680,8 +740,9 @@ namespace rosbzz_node{ no_cnt=0; } } - //if(count_robots.current !=0){ - /*std::map< int, int> count_count; + /* + if(count_robots.current !=0){ + std::map< int, int> count_count; uint8_t index=0; count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; //count_robots.current=neighbours_pos_map.size()+1; @@ -701,8 +762,8 @@ namespace rosbzz_node{ if(odd_count>current_count){ count_robots.current=odd_val; } - //} - /*else{ + } + else{ if(neighbours_pos_map.size()!=0){ count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; //count_robots.current=neighbours_pos_map.size()+1; @@ -712,8 +773,67 @@ namespace rosbzz_node{ count_robots.current=neighbours_pos_map.size()+1; } } - }*/ + } + */ } - -} + /* + * 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_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"); + } + + + +} diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 60c5086..30e5b80 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -11,7 +11,7 @@ function updated_neigh(){ neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 5.0 +TARGET_ALTITUDE = 3.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters @@ -43,7 +43,15 @@ function hexagon() { 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) +# 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) + } } ######################################## @@ -78,7 +86,7 @@ function barrier_ready() { # # Executes the barrier # -WAIT_TIMEOUT = 100 +WAIT_TIMEOUT = 200 timeW=0 function barrier_wait(threshold, transf) { barrier.get(id) @@ -106,12 +114,14 @@ 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 if( flight.status !=3){ + else { log("Altitude: ", TARGET_ALTITUDE) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) @@ -126,6 +136,7 @@ function land() { uav_land() } else { + timeW=0 barrier = nil statef=idle } @@ -133,6 +144,9 @@ function land() { # Executed once at init time. function init() { + s = swarm.create(1) +# s.select(1) + s.join() statef=idle CURSTATE = "IDLE" }