waypoint_client.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 <actionlib/client/simple_action_client.h>
00034 #include <actionlib/client/terminal_state.h>
00035 
00036 #include <asctec_hl_comm/WaypointAction.h>
00037 
00038 void feedbackCB(const asctec_hl_comm::WaypointFeedbackConstPtr & fb)
00039 {
00040   const geometry_msgs::Point32 & wp = fb->current_pos;
00041   ROS_INFO("got feedback: %fm %fm %fm %f° ", wp.x, wp.y, wp.z, fb->current_yaw*180/M_PI);
00042 }
00043 
00044 void activeCb()
00045 {
00046   ROS_INFO("Goal just went active");
00047 }
00048 
00049 void doneCb(const actionlib::SimpleClientGoalState& state, const asctec_hl_comm::WaypointResultConstPtr & result)
00050 {
00051   if (state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED)
00052     ROS_INFO("Finished in state [%s]", state.toString().c_str());
00053   else
00054     ROS_WARN("Finished in state [%s]", state.toString().c_str());
00055 
00056   const geometry_msgs::Point32 & wp = result->result_pos;
00057   ROS_INFO("Reached waypoint: %fm %fm %fm %f°",wp.x, wp.y, wp.z, result->result_yaw*180/M_PI);
00058 }
00059 
00060 int main(int argc, char** argv)
00061 {
00062 
00063   ros::init(argc, argv, "waypoint_client");
00064   ros::NodeHandle nh;
00065 
00066   float maxSpeed;
00067   float accuracy;
00068   ros::Duration timeout(15);
00069 
00070   if (argc == 5)
00071   {
00072     maxSpeed = 0.5;
00073     accuracy = 0.3;
00074   }
00075   else if (argc == 6)
00076   {
00077     maxSpeed = atof(argv[5]);
00078     accuracy = 0.3;
00079   }
00080   else if (argc == 7)
00081   {
00082     maxSpeed = atof(argv[5]);
00083     accuracy = atof(argv[6]);
00084   }
00085   else if (argc == 8)
00086   {
00087     maxSpeed = atof(argv[5]);
00088     accuracy = atof(argv[6]);
00089     timeout = ros::Duration(atof(argv[7]));
00090   }
00091   else
00092   {
00093     std::cout << "Wrong number of arguments! \nusage:" << "\twpclient x y z yaw \n"
00094         << "\twpclient x y z yaw max_speed\n" << "\twpclient x y z yaw max_speed accuracy\n"
00095         << "\twpclient x y z yaw max_speed accuracy timeout\n" << std::endl;
00096     return -1;
00097   }
00098 
00099   actionlib::SimpleActionClient<asctec_hl_comm::WaypointAction> ac(nh, "fcu/waypoint", true);
00100 
00101   asctec_hl_comm::WaypointGoal goal;
00102 
00103   ROS_INFO("Waiting for action server to start.");
00104   ac.waitForServer(); //will wait for infinite time
00105   ROS_INFO("Action server started, sending goal.");
00106 
00107   goal.goal_pos.x = atof(argv[1]);
00108   goal.goal_pos.y = atof(argv[2]);
00109   goal.goal_pos.z = atof(argv[3]);
00110 
00111   goal.max_speed.x = maxSpeed;
00112   goal.max_speed.y = maxSpeed;
00113   goal.max_speed.z = maxSpeed;
00114 
00115   goal.goal_yaw = atof(argv[4]);
00116 
00117   goal.accuracy_position = accuracy;
00118   goal.accuracy_orientation = 0;
00119 
00120   goal.timeout = timeout.toSec();
00121 
00122   ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCB);
00123 
00124   //wait for the action to return
00125   bool finished_before_timeout = ac.waitForResult(timeout);
00126 
00127   if (!finished_before_timeout)
00128   {
00129     ROS_WARN("Action did not finish before the time out.");
00130   }
00131 
00132   return 0;
00133 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52