pose_action.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012-2016, Institute of Flight Systems and Automatic Control,
3 // Technische Universität Darmstadt.
4 // All rights reserved.
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of hector_quadrotor nor the names of its contributors
14 // may be used to endorse or promote products derived from this software
15 // without specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #include <ros/ros.h>
31 
32 #include <hector_uav_msgs/PoseAction.h>
33 #include <geometry_msgs/PoseStamped.h>
34 
37 
39 {
40 
42 {
43 public:
45  : pose_server_(nh, "action/pose", boost::bind(&PoseActionServer::poseActionCb, this, _1))
46  {
47  pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("command/pose", 1);
48 
49  nh.param<double>("dist_tolerance", dist_tolerance_, 0.05);
50  nh.param<double>("yaw_tolerance", yaw_tolerance_, 0.35);
51  nh.param<double>("time_in_tolerance", time_in_tolerance_, 1.0);
52  nh.param<double>("action_frequency", frequency_, 10.0);
53  nh.param<double>("action_timeout", action_timeout_, 30.0);
54  }
55 
56  void poseActionCb(const hector_uav_msgs::PoseGoalConstPtr &goal)
57  {
59 
60  geometry_msgs::PoseStamped pose = goal->target_pose;
61 
63  ros::Time start = ros::Time::now();
64  ros::Time last_time_out_of_tolerance_ = ros::Time::now();
65 
66  while (ros::ok() && pose_server_.get()->isActive())
67  {
68  if (pose_server_.get()->isPreemptRequested())
69  {
70 
71  if (!pose_server_.get()->isNewGoalAvailable())
72  {
73  //Stop moving
75  }
76  pose_server_.get()->setPreempted();
77  return;
78  }
79 
80  pose.header.stamp = ros::Time::now();
81  pose_pub_.publish(pose);
82 
83  hector_uav_msgs::PoseFeedback feedback;
84  feedback.current_pose = *pose_server_.getPose();
85  pose_server_.get()->publishFeedback(feedback);
86 
87  if(!hector_quadrotor_interface::poseWithinTolerance(feedback.current_pose.pose, goal->target_pose.pose, dist_tolerance_, yaw_tolerance_))
88  {
89  last_time_out_of_tolerance_ = ros::Time::now();
90  }
91  else if (last_time_out_of_tolerance_ + ros::Duration(time_in_tolerance_ ) < ros::Time::now())
92  {
93  pose_server_.get()->setSucceeded();
94  return;
95  }
96 
98  {
99  pose_server_.get()->setAborted();
100  return;
101  }
102 
103  ros::spinOnce();
104  r.sleep();
105  }
106  }
107 
108 private:
111 
113 };
114 
115 } // namespace hector_quadrotor_actions
116 
117 
118 int main(int argc, char **argv)
119 {
120  ros::init(argc, argv, "pose_action");
121 
122  ros::NodeHandle nh;
124 
125  ros::spin();
126 
127  return 0;
128 }
void publish(const boost::shared_ptr< M > &message) const
bool poseWithinTolerance(const geometry_msgs::Pose &pose_current, const geometry_msgs::Pose &pose_target, const double dist_tolerance, const double yaw_tolerance)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
boost::shared_ptr< ActionServer > get()
Definition: base_action.h:72
bool param(const std::string &param_name, T &param_val, const T &default_val) const
geometry_msgs::PoseStampedConstPtr getPose()
Definition: base_action.h:67
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void poseActionCb(const hector_uav_msgs::PoseGoalConstPtr &goal)
Definition: pose_action.cpp:56
bool sleep()
static Time now()
hector_quadrotor_actions::BaseActionServer< hector_uav_msgs::PoseAction > pose_server_
ROSCPP_DECL void spinOnce()


hector_quadrotor_actions
Author(s): Paul Bovbel
autogenerated on Mon Jun 10 2019 13:36:49