diff --git a/src/mocap_path_follow.cpp b/src/mocap_path_follow.cpp deleted file mode 100644 index 7d1035c..0000000 --- a/src/mocap_path_follow.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/** - * @file path_follow.cpp - * @brief Offboard path trajectory tracking -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // for std::abs - -/********* CALLBACK FUNCTIONS **********************/ - -// Callback function which will save the current state of the autopilot. -// Allows to check connection, arming, and Offboard tags*/ -mavros_msgs::State current_state; -bool land = false; -void state_cb(const mavros_msgs::State::ConstPtr& msg){ - current_state = *msg; -} - -// Cb to determine attitude target -mavros_msgs::AttitudeTarget att_targ; -void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){ - att_targ = *msg; -} - -// Cb to recieve pose information -geometry_msgs::PoseStamped buff_pose; -geometry_msgs::Quaternion q_init; -geometry_msgs::PoseStamped mavPose; -bool gps_read = false; -double current_altitude; -void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ - mavPose = *msg; - current_altitude = mavPose.pose.position.z; - while(gps_read == false){ - q_init = mavPose.pose.orientation; - if(q_init.x == 0.0 && q_init.w == 0.0){ - ROS_INFO("Waiting..."); - } else { - buff_pose.pose.orientation.x = 0.0; - buff_pose.pose.orientation.y = 0.0; - buff_pose.pose.orientation.z = 0.0; - buff_pose.pose.orientation.w = 1.0; - gps_read = true; - } - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "path_follow"); - ros::NodeHandle nh; - - /********************** SUBSCRIBERS **********************/ - // Get current state - ros::Subscriber state_sub = nh.subscribe - ("mavros/state", 10, state_cb); - - // Get attitude target from klausen control - ros::Subscriber att_target_sub = nh.subscribe - ("command/att_target",10,att_targ_cb); - - // Pose subscriber - ros::Subscriber mavPose_sub = nh.subscribe - ("mavros/local_position/pose",10,mavPose_cb); - - /********************** PUBLISHERS **********************/ - // Initiate publisher to publish commanded local position - ros::Publisher local_pos_pub = nh.advertise - ("mavros/setpoint_position/local", 10); - - // Publish attitude and thrust commands - ros::Publisher att_targ_pub = nh.advertise - ("mavros/setpoint_raw/attitude",10); - - // Service Clients - ros::ServiceClient arming_client = nh.serviceClient - ("mavros/cmd/arming"); - ros::ServiceClient set_mode_client = nh.serviceClient - ("mavros/set_mode"); - ros::ServiceClient takeoff_cl = nh.serviceClient - ("mavros/cmd/takeoff"); - ros::ServiceClient waypoint_cl = nh.serviceClient - ("status/waypoint_tracker"); - - //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms - ros::Rate rate_wait(20.0); - - // wait for FCU connection - while(ros::ok() && !current_state.connected){ - ros::spinOnce(); - ROS_INFO("Waiting for FCU connection"); - rate_wait.sleep(); - } - - if (current_state.connected){ - ROS_INFO("Connected to FCU"); - } else { - ROS_INFO("Never Connected"); - } - - /*********** Initiate variables ************************/ - //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms - ros::Rate rate_pub(25.0); - - // Desired mode is offboard - mavros_msgs::SetMode offb_set_mode; - offb_set_mode.request.custom_mode = "OFFBOARD"; - - // Arm UAV - mavros_msgs::CommandBool arm_cmd; - arm_cmd.request.value = true; - - // Take off command - bool takeoff = false, att_cmds = false; - bool oscillation_damp = true; - - // Keep track of time since requests - ros::Time tkoff_req = ros::Time::now(); - ros::Time last_request = ros::Time::now(); - - //send a few setpoints before starting - for(int i = 100; ros::ok() && i > 0; --i){ - local_pos_pub.publish(buff_pose); - ros::spinOnce(); - ROS_INFO("Publishing position setpoints"); - rate_wait.sleep(); - } - - // Retrieve desired waypoints - oscillation_ctrl::WaypointTrack wpoint_srv; - wpoint_srv.request.query = false; - if (waypoint_cl.call(wpoint_srv)){ - ROS_INFO("Waypoints received"); - // populate desired waypoints - this is only for original hover as - // a change of waypoints is handled by ref_signal.py - buff_pose.pose.position.x = wpoint_srv.response.xd.x; - buff_pose.pose.position.y = wpoint_srv.response.xd.y; - buff_pose.pose.position.z = wpoint_srv.response.xd.z; - } else { - ROS_INFO("NO waypoints received"); - } - double alt_des = wpoint_srv.response.xd.z; // Desired height - while(ros::ok()){ - if(current_state.mode == "AUTO.LAND"){ - land = true; - while(land == true){ - ROS_INFO("Des Altitude: LANDING"); - } - } else { - if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ - if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ - } else { - ROS_INFO("Could not enter offboard mode"); - } - //last_request = ros::Time::now(); - } else { - if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ - if( arming_client.call(arm_cmd) && arm_cmd.response.success){ - ROS_INFO("Vehicle armed"); - } - last_request = ros::Time::now(); - } - } - if(current_state.mode == "OFFBOARD" && current_state.armed){ - ROS_INFO_ONCE("Spiri is taking off"); - if(!takeoff){ - tkoff_req = ros::Time::now(); - takeoff = true; - } - } - // Check if we want to use oscillation controller - //if (ros::param::get("/use_ctrl", oscillation_damp) == true){ - if (ros::param::has("/status/use_ctrl")){ - ros::param::get("/status/use_ctrl", oscillation_damp); - if(oscillation_damp){ - ROS_INFO("USING ATTITUDE CTRL"); - att_targ.header.stamp = ros::Time::now(); - att_targ_pub.publish(att_targ); - } else { - // Check if waypoints have changed - if (waypoint_cl.call(wpoint_srv)) - { - // populate desired waypoints - buff_pose.pose.position.x = wpoint_srv.response.xd.x; - buff_pose.pose.position.y = wpoint_srv.response.xd.y; - buff_pose.pose.position.z = wpoint_srv.response.xd.z; - } - local_pos_pub.publish(buff_pose); - ROS_INFO("USING POSITION CTRL"); - } - } - ROS_INFO("Des Altitude: %.2f", alt_des); - ROS_INFO("Cur Altitude: %.2f", current_altitude); - ROS_INFO("---------------------------"); - ros::spinOnce(); - rate_pub.sleep(); - } - } - - return 0; -} \ No newline at end of file