#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>
#include "uav_msgs/Task.h"
#include "uav_msgs/PicsData.h"
#include "uav_msgs/PicsProcessResult.h"
ros::Publisher trajectory_pub;
ros::Publisher pics_data_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;
geometry_msgs::Point home_pos;
std::vector<geometry_msgs::Point> path;
geometry_msgs::Point task_pos;
bool b_home = false;
bool b_process_res = false;
geometry_msgs::Point getPoint(float x, float y, float z) {
geometry_msgs::Point pt;
pt.x = x;
pt.y = y;
pt.z = z;
return pt;
}
/*
*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.x = msg.point.x;
home_pos.y = msg.point.y;
home_pos.z = 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:
* get pictures process result from topic pics_process_result
*Parameters:
* msg : pictures process result
*/
void getPicsProcessResult(const uav_msgs::PicsProcessResult &msg) {
ROS_INFO("Pics process result is :");
ROS_INFO(" [ %s ]", msg.result.c_str());
b_process_res = true;
}
/*
*Description:
* when uav get to the task pos, we take photos and send them to Topic pics_data
*Parameters:
* none
*/
void executeTask() {
uav_msgs::PicsData pics_data;
pics_data.header.stamp = ros::Time::now();
pics_data.height = 7.0;
pics_data.wide = 10.0;
pics_data.data = "This is a photo.";
pics_data_pub.publish(pics_data);
ROS_INFO("Uav sends picture data : ");
ROS_INFO(" [ Height : %.2f, Wide : %.2f, Data : %s ]",
pics_data.height,
pics_data.wide,
pics_data.data.c_str());
}
/*
*Description:
* calculate the distance between current position and target position
*Parameters:
* target_pos : destination position
*Return:
* double : distance
*/
double getDistanceToDestination(const geometry_msgs::Point& target_pos) {
double distance = 0.0;
distance = pow((target_pos.x - cur_pos_msg.point.x), 2) +
pow((target_pos.y - cur_pos_msg.point.y), 2) +
pow((target_pos.z - 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 geometry_msgs::Point& target_pos, float max_err = 0.2) {
double cur_err = getDistanceToDestination(target_pos);
return cur_err < max_err;
}
bool linearSmoothingNavigationTask(const geometry_msgs::Point& 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);
geometry_msgs::Point next_pos;
if(dist < linear_smoothing_navigation_step) {
next_pos = target_pos;
} else {
next_pos.x = cur_pos_msg.point.x +
(target_pos.x - cur_pos_msg.point.x) / dist * linear_smoothing_navigation_step;
next_pos.y = cur_pos_msg.point.y +
(target_pos.y - cur_pos_msg.point.y) / dist * linear_smoothing_navigation_step;
next_pos.z = cur_pos_msg.point.z +
(target_pos.z - cur_pos_msg.point.z) / dist * linear_smoothing_navigation_step;
}
double target_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(Eigen::Vector3d(target_pos.x, target_pos.y, target_pos.z), target_yaw, &trajectory_msg);
trajectory_pub.publish(trajectory_msg);
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 geometry_msgs::Point target_pos;
target_pos.x = cur_pos_msg.point.x;
target_pos.y = cur_pos_msg.point.y;
target_pos.z = altitude;
double target_yaw = 0.0;
if(isReachDestination(target_pos)) {
return true;
}
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(Eigen::Vector3d(target_pos.x, target_pos.y, target_pos.z),
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 geometry_msgs::Point home_pos_bar = getPoint(home_pos.x,
home_pos.y,
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.");
b_home = true;
}
}
int main(int argc, char *argv[]) {
// set local code
setlocale(LC_ALL, "");
// ros node init
ros::init(argc, argv, "uav_with_task");
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);
// global variable
// publisher: publish uav's position
trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
pics_data_pub = nh.advertise<uav_msgs::PicsData>("pics_data", 10);
// subcribe topic
// uav position info : /odometry_sensor1/position
// subscriber: subscribe uav's current position
ros::Subscriber pos_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, updateUAVPosition);
ros::Subscriber pics_process_res_sub = nh.subscribe(std::string("pics_process_result").c_str(), 10, getPicsProcessResult);
// 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.");
}
// if can't get task, then exit
ROS_INFO("Uav is ready!");
uav_msgs::Task task;
task.request.ready = true;
task.request.uav_name = uav_name;
bool b_task = ros::service::call("/execute_task", task);
if(!b_task) {
ROS_FATAL("Fail to get task content.");
return -1;
} else {
path = task.response.path;
task_pos = task.response.task_pos;
ROS_INFO("We get task conent.");
ROS_INFO("Trajectory has %d points.", path.size());
ROS_INFO("Task pos is : [ <%.2f, %.2f, %.2f> ]",
task_pos.x,
task_pos.y,
task_pos.z);
}
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()) {
if(tasks_ok < path.size()) {
ROS_INFO("task number : %d", tasks_ok);
bool tmp = linearSmoothingNavigationTask(path[tasks_ok]);
if(tmp) {
tasks_ok++;
}
}
if(isReachDestination(task_pos)) {
executeTask();
}
} else if(tasks_ok >= path.size()) {
goHome();
}
if(b_home && b_process_res) {
ros::service::call("/ctl_center/shutdown", srv);
exit(0);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
遇到错误
创建消息时错误
错误1
CMake Error at /home/ml/ros_pros/ros_tutorials/build/uav_msgs/cmake/uav_msgs-genmsg.cmake:3 (message):
Could not find messages which
'/home/ml/ros_pros/ros_tutorials/src/uav_msgs/srv/Task.srv' depends on.
Did you forget to specify generate_messages(DEPENDENCIES ...)?
Cannot locate message [Point]: unknown package [geometry_msgs] on search
path [{{'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'],
'uav_msgs': ['/home/ml/ros_pros/ros_tutorials/src/uav_msgs/msg']}}]
CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:196 (message):
catkin_package() the catkin package 'uav_msgs' has been find_package()-ed
but is not listed as a build dependency in the package.xml
解决
package.xml 中添加
<depend>uav_msgs</depend>
错误3
no matching function for call to ‘ros::AdvertiseServiceOptions::initBySpecType(const string&, const boost::function<bool(uav_task_execution::Task&)>&)’
