waypoint_server.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include <ros/ros.h>
00033 #include <asctec_hl_comm/mav_ctrl.h>
00034 #include <geometry_msgs/PoseStamped.h>
00035 #include <tf/tf.h>
00036 
00037 // action server
00038 #include <actionlib/server/simple_action_server.h>
00039 #include <asctec_hl_comm/WaypointAction.h>
00040 
00041 typedef actionlib::SimpleActionServer<asctec_hl_comm::WaypointAction> WaypointActionServer;
00042 
00043 class WPServer
00044 {
00045 private:
00046   ros::NodeHandle nh_;
00047   ros::Publisher wp_pub_;
00048   ros::Subscriber pose_sub_;
00049 
00050   WaypointActionServer *wp_server_;
00051 
00052   geometry_msgs::PoseStamped current_pose_;
00053   boost::mutex current_pose_mutex_;
00054 
00055 
00057   void poseCallback(const geometry_msgs::PoseStampedConstPtr & pose)
00058   {
00059     boost::mutex::scoped_lock lock(current_pose_mutex_);
00060     current_pose_ = *pose;
00061   }
00062 
00064   void wpExecuteCB(const asctec_hl_comm::WaypointGoalConstPtr & goal)
00065   {
00066     const geometry_msgs::Point32 & wp = goal->goal_pos;
00067     const float & yaw = goal->goal_yaw;
00068 
00069     ROS_INFO("got new waypoint: x=%f y=%f z=%f yaw=%f v_xy=%f v_z=%f accuracy=%f timeout=%f", wp.x, wp.y, wp.z, goal->goal_yaw, goal->max_speed.x, goal->max_speed.z, goal->accuracy_position, goal->timeout);
00070 
00071     asctec_hl_comm::mav_ctrlPtr new_wp(new asctec_hl_comm::mav_ctrl);
00072     new_wp->x = wp.x;
00073     new_wp->y = wp.y;
00074     new_wp->z = wp.z;
00075     new_wp->yaw = yaw;
00076     new_wp->v_max_xy = goal->max_speed.x;
00077     new_wp->v_max_z = goal->max_speed.z;
00078     new_wp->type = asctec_hl_comm::mav_ctrl::position;
00079     wp_pub_.publish(new_wp);
00080 
00081     std::stringstream feedback_msg;
00082 
00083     ros::Rate r(2);
00084     bool success = true;
00085     static asctec_hl_comm::WaypointResult result;
00086     static asctec_hl_comm::WaypointFeedback feedback;
00087 
00088     float dist_to_wp = 1e9;
00089     float dist_yaw = M_PI;
00090     float dx, dy, dz;
00091 
00092     int i = 0;
00093     int max_cycles = ceil(goal->timeout / r.expectedCycleTime().toSec());
00094 
00095     while (ros::ok() && i < max_cycles)
00096     {
00097       if (wp_server_->isPreemptRequested() || !ros::ok())
00098       {
00099         ROS_INFO("waypoint server: Preempted");
00100         // set the action state to preempted
00101         wp_server_->setPreempted();
00102         success = false;
00103         break;
00104       }
00105 
00106       boost::mutex::scoped_lock lock(current_pose_mutex_);
00107 
00108       dx = wp.x - current_pose_.pose.position.x;
00109       dy = wp.y - current_pose_.pose.position.y;
00110       dz = wp.z - current_pose_.pose.position.z;
00111       dist_to_wp = sqrt(dx * dx + dy * dy + dz * dz);
00112 
00113       dist_yaw = fabs(yaw - tf::getYaw(current_pose_.pose.orientation));
00114       dist_yaw = dist_yaw > M_PI ? fabs(dist_yaw - M_PI) : dist_yaw;
00115 
00116       feedback_msg.clear();
00117       feedback_msg.str("");
00118       feedback_msg << "dist: " << dist_to_wp << " dist_yaw" << dist_yaw;
00119       if (dist_to_wp <= goal->accuracy_position && dist_yaw <= goal->accuracy_orientation)
00120         break;
00121 
00122       feedback.current_pos.x = current_pose_.pose.position.x;
00123       feedback.current_pos.y = current_pose_.pose.position.y;
00124       feedback.current_pos.z = current_pose_.pose.position.z;
00125       feedback.current_yaw = tf::getYaw(current_pose_.pose.orientation);
00126       feedback.status = feedback_msg.str();
00127 
00128       lock.unlock();
00129 
00130       wp_server_->publishFeedback(feedback);
00131 
00132       r.sleep();
00133       i++;
00134     }
00135 
00136     result.result_pos.x = current_pose_.pose.position.x;
00137     result.result_pos.y = current_pose_.pose.position.y;
00138     result.result_pos.z = current_pose_.pose.position.z;
00139     result.result_yaw = tf::getYaw(current_pose_.pose.orientation);
00140 
00141     if (i == max_cycles)
00142     {
00143       success = false;
00144       wp_server_->setAborted(result, "Timed Out!");
00145     }
00146 
00147     if (success)
00148     {
00149       result.status = "Waypoint reached!";
00150       wp_server_->setSucceeded(result);
00151     }
00152   }
00153 
00154 public:
00155   WPServer() :
00156     nh_(""), wp_server_(NULL)
00157   {
00158     wp_pub_ = nh_.advertise<asctec_hl_comm::mav_ctrl> ("fcu/control", 1);
00159     pose_sub_ = nh_.subscribe("fcu/current_pose", 10, &WPServer::poseCallback, this);
00160     wp_server_ = new WaypointActionServer(nh_, "fcu/waypoint", boost::bind(&WPServer::wpExecuteCB, this, _1), false);
00161     wp_server_->start();
00162   }
00163   ~WPServer()
00164   {
00165     wp_server_->shutdown();
00166     delete wp_server_;
00167   }
00168 };
00169 
00170 int main(int argc, char** argv)
00171 {
00172   ros::init(argc, argv, "waypoint_server");
00173   WPServer wps;
00174   ROS_INFO("Waypoint server started ... waiting for requests");
00175   ros::spin();
00176   return 0;
00177 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Jan 7 2014 11:05:25