#include <ros/ros.h>
#include <chrono>
#include <thread>
#include <Eigen/Core>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
ros::Publisher trajectory_pub;
geometry_msgs::PointStamped cur_pos_msg;
float linear_smoothing_navigation_step = 2;
bool gps_init_ok = false;
bool take_off_ok = false;
int tasks_ok = 0;
Eigen::Vector3d home_pos;
/*
*Description:
* when gps position changed, we update related parameters.
*Parameters:
* msg : current position message
*/
void updateUAVPosition(const geometry_msgs::PointStamped& msg) {
if(!gps_init_ok) {
// prepare to take off
gps_init_ok = true;
// record current position
home_pos[0] = msg.point.x;
home_pos[1] = msg.point.y;
home_pos[2] = msg.point.z;
ROS_INFO("Home Position is <%f, %f, %f>", msg.point.x, msg.point.y, msg.point.z);
}
cur_pos_msg = msg;
// ROS_INFO("Current Position is <%f, %f, %f>", msg.point.x, msg.point.y, msg.point.z);
}
/*
*Description:
* calculate the distance between current position and target position
*Parameters:
* target_pos : destination position
*Return:
* double : distance
*/
double getDistanceToDestination(const Eigen::Vector3d& target_pos) {
double distance = 0.0;
distance = pow((target_pos[0] - cur_pos_msg.point.x), 2) +
pow((target_pos[1] - cur_pos_msg.point.y), 2) +
pow((target_pos[2] - cur_pos_msg.point.z), 2);
return sqrt(distance);
}
/*
*Description:
*
*Parameters:
* target_pos : destination position
* max_err : max error (default = 0.0001)
*Return:
* true : arrival
* false : not arrival
*/
bool isReachDestination(const Eigen::Vector3d& target_pos, float max_err = 0.2) {
double cur_err = getDistanceToDestination(target_pos);
return cur_err < max_err;
}
bool linearSmoothingNavigationTask(const Eigen::Vector3d& target_pos) {
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
if(isReachDestination(target_pos)) {
return true;
}
double dist = getDistanceToDestination(target_pos);
Eigen::Vector3d next_pos;
if(dist < linear_smoothing_navigation_step) {
next_pos = target_pos;
} else {
next_pos[0] = cur_pos_msg.point.x +
(target_pos[0] - cur_pos_msg.point.x) / dist * linear_smoothing_navigation_step;
next_pos[1] = cur_pos_msg.point.y +
(target_pos[1] - cur_pos_msg.point.y) / dist * linear_smoothing_navigation_step;
next_pos[2] = cur_pos_msg.point.z +
(target_pos[2] - cur_pos_msg.point.z) / dist * linear_smoothing_navigation_step;
}
double target_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(target_pos, target_yaw, &trajectory_msg);
trajectory_pub.publish(trajectory_msg);
ROS_INFO("point %d", tasks_ok);
return false;
}
/*
*Description:
* UAV reach the height of taking off.
*Parameters:
* altitude : the height of taking off
*/
bool takeOff(float altitude) {
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
static Eigen::Vector3d target_pos(cur_pos_msg.point.x, cur_pos_msg.point.y, altitude);
double target_yaw = 0.0;
if(isReachDestination(target_pos)) {
return true;
}
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(target_pos, target_yaw, &trajectory_msg);
trajectory_pub.publish(trajectory_msg);
return false;
}
/*
*Description:
* Go back to the origin position. UAV move to home_pos with current altitude. Then UAV lands.
*/
void goHome() {
static Eigen::Vector3d home_pos_bar(home_pos[0],
home_pos[1],
cur_pos_msg.point.z);
static bool home_pos_ok = false;
if(!home_pos_ok) {
home_pos_ok = linearSmoothingNavigationTask(home_pos_bar);
} else {
linearSmoothingNavigationTask(home_pos);
ROS_INFO("UAV aready go back.");
}
}
int main(int argc, char *argv[]) {
// set local code
setlocale(LC_ALL, "");
// ros node init
ros::init(argc, argv, "rotors_control_node");
ros::NodeHandle nh;
// create a private node for accessing node parameters
ros::NodeHandle nh_private("~");
std::string uav_name = "";
ros::param::get("~mav_name", uav_name);
// subcribe topic
// uav position info : /odometry_sensor1/position
ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUAVPosition);
// global variable
trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
// wait 5s for gazebo successfully startup
ros::Duration(5.0).sleep();
// make gazebo auto-play
std_srvs::Empty srv;
bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
// try to make gazebo auto-play
int i = 0;
while(i <= 10 && !unpaused) {
ROS_INFO("Wait for 1 second before trying to unpause Gazebo again!");
std::this_thread::sleep_for(std::chrono::seconds(1));
unpaused = ros::service::call("/gazebo/unpause_physics", srv);
++i;
}
// if Gazebo is not auto-play, then exit
if(!unpaused) {
ROS_FATAL("Fail to wake up Gazebo.");
return -1;
} else {
ROS_INFO("Unpaused the Gazebo simulation.");
}
// trajectory
std::vector<Eigen::Vector3d> path;
path.push_back(Eigen::Vector3d(5.f, 5.f, 5.f));
path.push_back(Eigen::Vector3d(-5.f, 5.f, 5.f));
path.push_back(Eigen::Vector3d(-5.f, -5.f, 5.f));
path.push_back(Eigen::Vector3d(5.f, -5.f, 5.f));
path.push_back(Eigen::Vector3d(5.f, 5.f, 5.f));
ROS_INFO("Trajectory has %d points.", path.size());
ros::Rate loop_rate(10);
while(ros::ok()) {
if(gps_init_ok && !take_off_ok) {
take_off_ok = takeOff(3);
} else if(take_off_ok && tasks_ok < path.size()) {
ROS_INFO("Move");
if(tasks_ok < path.size()) {
bool tmp = linearSmoothingNavigationTask(path[tasks_ok]);
if(tmp) {
tasks_ok++;
}
}
} else if(tasks_ok >= path.size()) {
goHome();
}
ROS_INFO("take off : %d, task number : %d", take_off_ok, tasks_ok);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
