diff --git a/include/roscontroller.h b/include/roscontroller.h index 87bd038..d9aa948 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -37,6 +37,7 @@ #define UPDATER_MESSAGE_CONSTANT 987654321 #define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_STOP_TRANSMISSION 4355356352 +#define TIMEOUT 60 using namespace std; @@ -61,6 +62,7 @@ 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; @@ -70,6 +72,7 @@ private: int robot_id=0; //int oldcmdID=0; int rc_cmd; + float fcu_timeout; int armstate; int barrier; int message_number=0; @@ -127,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(); @@ -151,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); diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml index f9cb809..1ffd380 100644 --- a/launch/launch_config/m100.yaml +++ b/launch/launch_config/m100.yaml @@ -3,8 +3,7 @@ topics: battery : /power_status status : /flight_status fcclient : /dji_mavcmd - setpoint : /setpoint_raw/local - setpoint_nonraw : /setpoint_position/local + setpoint : /setpoint_position/local armclient: /dji_mavarm modeclient: /dji_mavmode altitude: /rel_alt diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index 650ba45..8930701 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -3,8 +3,7 @@ topics: battery : /mavros/battery status : /mavros/state fcclient: /mavros/cmd/command - setpoint: /mavros/setpoint_raw/local - setpoint_nonraw: /mavros/setpoint_position/local + setpoint: /mavros/setpoint_position/local armclient: /mavros/cmd/arming modeclient: /mavros/set_mode stream: /mavros/set_stream_rate 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 80dfb55..386563c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -25,6 +25,7 @@ namespace rosbzz_node{ /// Get Robot Id - wait for Xbee to be started GetRobotId(); setpoint_counter = 0; + fcu_timeout = TIMEOUT; } /*--------------------- @@ -76,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(); @@ -94,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); @@ -178,8 +183,6 @@ namespace rosbzz_node{ 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");} - if(node_handle.getParam("topics/setpoint_nonraw", setpoint_nonraw)); - else {ROS_ERROR("Provide a mode setpoint non raw client in Launch file"); system("rosnode kill rosbuzz_node");} } /*-------------------------------------------------------- @@ -199,8 +202,7 @@ 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_nonraw_pub = n_c.advertise(setpoint_nonraw,1000); + localsetpoint_pub = n_c.advertise(setpoint_name,1000); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); @@ -381,7 +383,7 @@ 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(); @@ -506,15 +508,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]; } /*------------------------------------------------------ @@ -551,6 +552,12 @@ 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; + 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); } @@ -774,51 +781,15 @@ namespace rosbzz_node{ // 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 - mavros_msgs::PositionTarget moveMsg; - const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3); - - moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED; - moveMsg.type_mask = ignore_all_except_xyz_y; - - moveMsg.header.frame_id = 1; - moveMsg.header.seq = setpoint_counter++; - moveMsg.header.stamp = ros::Time::now(); - - my_x += x; - my_y += y; - - moveMsg.position.x = x; - moveMsg.position.y = y; - moveMsg.position.z = 3; - moveMsg.yaw_rate = 0.0; - moveMsg.yaw = 0.0; - - - geometry_msgs::Vector3 zeroVec; - zeroVec.x = 0.0; zeroVec.y = 0.0; zeroVec.z = 0.0; - moveMsg.acceleration_or_force = zeroVec; - moveMsg.velocity = zeroVec; - - localsetpoint_pub.publish(moveMsg); - - ROS_INFO("Sent local position message!"); - } - - void roscontroller::SetLocalPositionNonRaw(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; - - my_x += x; - my_y += y; - - moveMsg.pose.position.x = my_x; - moveMsg.pose.position.y = my_y; - moveMsg.pose.position.z = 3; + 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;