costmap_planner_execution.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * costmap_planner_execution.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
41 #include <mbf_msgs/GetPathResult.h>
42 
44 
45 namespace mbf_costmap_nav
46 {
47 CostmapPlannerExecution::CostmapPlannerExecution(const std::string& planner_name,
48  const mbf_costmap_core::CostmapPlanner::Ptr& planner_ptr,
49  const TFPtr& tf_listener_ptr, const CostmapWrapper::Ptr& costmap_ptr,
50  const MoveBaseFlexConfig& config)
51  : AbstractPlannerExecution(planner_name, planner_ptr, tf_listener_ptr, toAbstract(config)), costmap_ptr_(costmap_ptr)
52 {
53  ros::NodeHandle private_nh("~");
54  private_nh.param("planner_lock_costmap", lock_costmap_, true);
55 }
56 
58 {
59 }
60 
61 mbf_abstract_nav::MoveBaseFlexConfig CostmapPlannerExecution::toAbstract(const MoveBaseFlexConfig &config)
62 {
63  // copy the planner-related abstract configuration common to all MBF-based navigation
64  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
65  abstract_config.planner_frequency = config.planner_frequency;
66  abstract_config.planner_patience = config.planner_patience;
67  abstract_config.planner_max_retries = config.planner_max_retries;
68  return abstract_config;
69 }
70 
71 uint32_t CostmapPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
72  const geometry_msgs::PoseStamped &goal,
73  double tolerance,
74  std::vector<geometry_msgs::PoseStamped> &plan,
75  double &cost,
76  std::string &message)
77 {
78  // transform the input to the global frame of the costmap, since this is an
79  // "implicit" requirement for most planners
80  // note: costmap_2d::Costmap2DROS::getTransformTolerance might be a good idea,
81  // but it's not part of the class API in ros-kinetic
82  const ros::Duration timeout(0.5);
83  const std::string frame = costmap_ptr_->getGlobalFrameID();
84  geometry_msgs::PoseStamped g_start, g_goal;
85 
86  if (!mbf_utility::transformPose(*tf_listener_ptr_, frame, timeout, start, g_start))
87  return mbf_msgs::GetPathResult::TF_ERROR;
88 
89  if (!mbf_utility::transformPose(*tf_listener_ptr_, frame, timeout, goal, g_goal))
90  return mbf_msgs::GetPathResult::TF_ERROR;
91 
92  if (lock_costmap_)
93  {
94  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
95  return planner_->makePlan(g_start, g_goal, tolerance, plan, cost, message);
96  }
97  return planner_->makePlan(g_start, g_goal, tolerance, plan, cost, message);
98 }
99 
100 } /* namespace mbf_costmap_nav */
wrapper_global_planner.h
boost::shared_ptr
mbf_costmap_nav::CostmapPlannerExecution::makePlan
virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance, std::vector< geometry_msgs::PoseStamped > &plan, double &cost, std::string &message)
Calls the planner plugin to make a plan from the start pose to the goal pose with the given tolerance...
Definition: costmap_planner_execution.cpp:71
mbf_abstract_nav::AbstractPlannerExecution::tf_listener_ptr_
const TFPtr tf_listener_ptr_
costmap_planner_execution.h
mbf_utility::transformPose
bool transformPose(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PoseStamped &in, geometry_msgs::PoseStamped &out)
mbf_costmap_nav
Definition: costmap_controller_execution.h:51
goal
int goal[2]
mbf_costmap_nav::CostmapPlannerExecution::~CostmapPlannerExecution
virtual ~CostmapPlannerExecution()
Destructor.
Definition: costmap_planner_execution.cpp:57
mbf_costmap_nav::CostmapPlannerExecution::lock_costmap_
bool lock_costmap_
Whether to lock costmap before calling the planner (see issue #4 for details)
Definition: costmap_planner_execution.h:125
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mbf_costmap_nav::CostmapPlannerExecution::costmap_ptr_
const CostmapWrapper::Ptr & costmap_ptr_
Shared pointer to the global planner costmap.
Definition: costmap_planner_execution.h:122
mbf_costmap_nav::CostmapPlannerExecution::CostmapPlannerExecution
CostmapPlannerExecution(const std::string &planner_name, const mbf_costmap_core::CostmapPlanner::Ptr &planner_ptr, const TFPtr &tf_listener_ptr, const CostmapWrapper::Ptr &costmap_ptr, const MoveBaseFlexConfig &config)
Constructor.
Definition: costmap_planner_execution.cpp:47
ros::Duration
mbf_abstract_nav::AbstractExecutionBase::start
virtual bool start()
mbf_abstract_nav::AbstractPlannerExecution::planner_
mbf_abstract_core::AbstractPlanner::Ptr planner_
ros::NodeHandle
mbf_costmap_nav::CostmapPlannerExecution::toAbstract
mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config)
Definition: costmap_planner_execution.cpp:61


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:55