locomotor_action_server.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
36 #include <nav_2d_utils/path_ops.h>
37 #include <string>
38 
39 namespace locomotor
40 {
41 
43  : navigate_action_server_(nh, name, false), goal_cb_(cb)
44 {
48 }
49 void LocomotorActionServer::publishFeedback(const locomotor_msgs::NavigationState& nav_state)
50 {
51  if (!navigate_action_server_.isActive()) return;
52 
53  // Update The Feedback
54  const geometry_msgs::Pose2D& pose = nav_state.global_pose.pose;
55  if (feedback_.state.global_pose.header.frame_id.length() > 0)
56  {
57  // If we already have a global pose saved, calculate the distance traveled
58  const geometry_msgs::Pose2D& prev_pose = feedback_.state.global_pose.pose;
59  feedback_.distance_traveled += nav_2d_utils::poseDistance(prev_pose, pose);
60  }
61 
62  feedback_.state = nav_state;
63 
64  if (feedback_.state.global_plan.poses.size() > 0)
65  {
66  feedback_.estimated_distance_remaining = nav_2d_utils::getPlanLength(feedback_.state.global_plan, pose);
67  double total_distance = feedback_.distance_traveled + feedback_.estimated_distance_remaining;
68  if (total_distance != 0.0)
69  feedback_.percent_complete = feedback_.distance_traveled / total_distance;
70  }
71 
73 }
74 
76 {
77  if (!navigate_action_server_.isActive()) return;
79 }
80 void LocomotorActionServer::failNavigation(const locomotor_msgs::ResultCode& result_code)
81 {
82  if (!navigate_action_server_.isActive()) return;
83  locomotor_msgs::NavigateToPoseResult result;
84  result.result_code = result_code;
85  navigate_action_server_.setAborted(result, result_code.message);
86 }
87 
89 {
90  feedback_.distance_traveled = 0.0;
91  feedback_.percent_complete = 0.0;
92  feedback_.estimated_distance_remaining = 0.0;
93  feedback_.state = locomotor_msgs::NavigationState();
94 
95  auto full_goal = navigate_action_server_.acceptNewGoal();
96  goal_cb_(full_goal->goal);
97 }
98 
100 {
101  if (!navigate_action_server_.isActive()) return;
102  locomotor_msgs::NavigateToPoseResult result;
103  result.result_code.result_code = -1;
104  result.result_code.message = "Preempted.";
105  navigate_action_server_.setPreempted(result, result.result_code.message);
106 }
107 
108 } // namespace locomotor
boost::shared_ptr< const Goal > acceptNewGoal()
void failNavigation(const locomotor_msgs::ResultCode &result_code)
void publishFeedback(const FeedbackConstPtr &feedback)
LocomotorActionServer(const ros::NodeHandle nh, NewGoalCallback cb, const std::string name="navigate")
std::function< void(const nav_2d_msgs::Pose2DStamped &)> NewGoalCallback
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void publishFeedback(const locomotor_msgs::NavigationState &nav_state)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
void registerPreemptCallback(boost::function< void()> cb)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
double getPlanLength(const nav_2d_msgs::Path2D &plan, const unsigned int start_index=0)
actionlib::SimpleActionServer< locomotor_msgs::NavigateToPoseAction > navigate_action_server_
void registerGoalCallback(boost::function< void()> cb)
locomotor_msgs::NavigateToPoseFeedback feedback_


locomotor
Author(s):
autogenerated on Sun Jan 10 2021 04:08:39