# PicsData.msg
Header header
float64 height
float64 wide
string data
# PicsProcessResult.msg
Header header
string result
#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include "geometry_msgs/Point.h"
#include "uav_msgs/Task.h"
#include "uav_msgs/PicsData.h"
#include "uav_msgs/PicsProcessResult.h"
/*
function:
1. provide the path of uav.
2. process pics data and return value to uav.
*/
// picture data subscriber
ros::Subscriber pics_data_sub;
// picture process result publisher
ros::Publisher pics_process_res_pub;
// uav name
std::string uav_name;
/*
*Description:
* Generate a gemetry_msgs::point type variable
*Parameters:
* x, y, z : position 3D
*Return:
* a gemetry_msgs::point variable
*/
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:
* accept pics data & do some process
*Parameters:
* pics_data : picture data
*/
void doPicsProcess(const uav_msgs::PicsData::ConstPtr &pics_data) {
// accept pics data
ROS_INFO("Accepted picture data :");
ROS_INFO(" [ Height : %.2f, Wide : %.2f, Data : %s ]",
pics_data->height,
pics_data->wide,
pics_data->data.c_str());
// do process
uav_msgs::PicsProcessResult res;
res.header.stamp = ros::Time::now();
res.result = "Everything is ok. Nothing should be worried.";
ROS_INFO("Pics process result :");
ROS_INFO(" [ %s ]", res.result.c_str());
// publish process result
pics_process_res_pub.publish(res);
}
/*
*Description:
* give a response for service execute_task
*Parameters:
* req : srv request part
* res : srv response part
*Return:
* service call result : true or false
*/
bool doTaskResponse(uav_msgs::Task::Request &req,
uav_msgs::Task::Response &res) {
// accepted request content
bool ready = req.ready;
// response task & path
if(!ready) {
ROS_INFO("No uav ready!");
} else {
uav_name = req.uav_name;
ROS_INFO("Uav %s connected!", uav_name.c_str());
ros::NodeHandle nh;
// subscribe pics data
pics_data_sub = nh.subscribe<uav_msgs::PicsData>(std::string("/"+uav_name+"/pics_data").c_str(), 10, doPicsProcess);
// publish pics process result
pics_process_res_pub = nh.advertise<uav_msgs::PicsProcessResult>(std::string("/"+uav_name+"/pics_process_result").c_str(), 10);
// time stamp
res.header.stamp = ros::Time::now();
// set uav path
res.path.push_back(getPoint(1.0, 2.0, 3.0));
res.path.push_back(getPoint(3.0, 4.0, 4.0));
res.path.push_back(getPoint(4.0, 8.0, 9.0));
res.path.push_back(getPoint(-2.0, 5.0, 5.0));
res.path.push_back(getPoint(7.0, 3.0, 8.0));
// set task start pos
res.task_pos = res.path[3];
ROS_INFO("Already send task data to uav.");
ROS_INFO("Task pos is : [ <%.2f, %.2f, %.2f> ]",
res.task_pos.x,
res.task_pos.y,
res.task_pos.z);
return true;
}
return false;
}
bool shutdown(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
ros::shutdown();
return true;
}
int main(int argc, char *argv[])
{
// init ros
ros::init(argc, argv, "ctl_center");
ros::NodeHandle nh;
// create service "excute_task"
ros::ServiceServer task_srv = nh.advertiseService("execute_task", &doTaskResponse);
ros::ServiceServer shutdown_srv = nh.advertiseService("/ctl_center/shutdown", shutdown);
ROS_INFO("Control center is ready.");
ROS_INFO("Waiting for a service call ...");
ros::spin();
return 0;
}
#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;
}
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
<depend>uav_msgs</depend>
no matching function for call to ‘ros::AdvertiseServiceOptions::initBySpecType(const string&, const boost::function<bool(uav_task_execution::Task&)>&)’
