costmap_controller_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_controller_execution.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
41 
42 namespace mbf_costmap_nav
43 {
44 
46  const std::string &controller_name,
47  const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
48  const ros::Publisher &vel_pub,
49  const ros::Publisher &goal_pub,
50  const TFPtr &tf_listener_ptr,
51  const CostmapWrapper::Ptr &costmap_ptr,
52  const MoveBaseFlexConfig &config)
53  : AbstractControllerExecution(controller_name, controller_ptr, vel_pub, goal_pub,
54  tf_listener_ptr, toAbstract(config)),
55  costmap_ptr_(costmap_ptr)
56 {
57  ros::NodeHandle private_nh("~");
58  private_nh.param("controller_lock_costmap", lock_costmap_, true);
59 }
60 
62 {
63 }
64 
65 mbf_abstract_nav::MoveBaseFlexConfig CostmapControllerExecution::toAbstract(const MoveBaseFlexConfig &config)
66 {
67  // copy the controller-related abstract configuration common to all MBF-based navigation
68  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
69  abstract_config.controller_frequency = config.controller_frequency;
70  abstract_config.controller_patience = config.controller_patience;
71  abstract_config.controller_max_retries = config.controller_max_retries;
72  abstract_config.oscillation_timeout = config.oscillation_timeout;
73  abstract_config.oscillation_distance = config.oscillation_distance;
74  return abstract_config;
75 }
76 
78  const geometry_msgs::PoseStamped &robot_pose,
79  const geometry_msgs::TwistStamped &robot_velocity,
80  geometry_msgs::TwistStamped &vel_cmd,
81  std::string &message)
82 {
83  // Lock the costmap while planning, but following issue #4, we allow to move the responsibility to the planner itself
84  if (lock_costmap_)
85  {
86  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
87  return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
88  }
89  return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
90 }
91 
93 {
94  // Check that the observation buffers for the costmap are current, we don't want to drive blind
95  if (!costmap_ptr_->isCurrent())
96  {
97  ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
98  return false;
99  }
100  return true;
101 }
102 
103 } /* namespace mbf_costmap_nav */
mbf_abstract_nav::AbstractControllerExecution::controller_
mbf_abstract_core::AbstractController::Ptr controller_
ros::Publisher
boost::shared_ptr
mbf_costmap_nav::CostmapControllerExecution::CostmapControllerExecution
CostmapControllerExecution(const std::string &controller_name, const mbf_costmap_core::CostmapController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const CostmapWrapper::Ptr &costmap_ptr, const MoveBaseFlexConfig &config)
Constructor.
Definition: costmap_controller_execution.cpp:45
mbf_costmap_nav::CostmapControllerExecution::computeVelocityCmd
virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &robot_pose, const geometry_msgs::TwistStamped &robot_velocity, geometry_msgs::TwistStamped &vel_cmd, std::string &message)
Request plugin for a new velocity command. We override this method so we can lock the local costmap b...
Definition: costmap_controller_execution.cpp:77
costmap_controller_execution.h
mbf_costmap_nav::CostmapControllerExecution::toAbstract
mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config)
Definition: costmap_controller_execution.cpp:65
mbf_costmap_nav::CostmapControllerExecution::~CostmapControllerExecution
virtual ~CostmapControllerExecution()
Destructor.
Definition: costmap_controller_execution.cpp:61
mbf_costmap_nav
Definition: costmap_controller_execution.h:51
ROS_WARN
#define ROS_WARN(...)
mbf_costmap_nav::CostmapControllerExecution::costmap_ptr_
const CostmapWrapper::Ptr & costmap_ptr_
Shared pointer to thr local costmap.
Definition: costmap_controller_execution.h:134
mbf_costmap_nav::CostmapControllerExecution::safetyCheck
bool safetyCheck()
Implementation-specific safety check, called during execution to ensure it's safe to drive....
Definition: costmap_controller_execution.cpp:92
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mbf_costmap_nav::CostmapControllerExecution::lock_costmap_
bool lock_costmap_
Whether to lock costmap before calling the controller (see issue #4 for details)
Definition: costmap_controller_execution.h:137
ros::NodeHandle


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