costmap_wrapper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019, 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_wrapper.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
42 
43 
44 namespace mbf_costmap_nav
45 {
46 
47 
48 CostmapWrapper::CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr) :
49  costmap_2d::Costmap2DROS(name, *tf_listener_ptr),
50  shutdown_costmap_(false), costmap_users_(0), private_nh_("~")
51 {
52  // even if shutdown_costmaps is a dynamically reconfigurable parameter, we
53  // need it here to decide whether to start or not the costmap on starting up
54  private_nh_.param("shutdown_costmaps", shutdown_costmap_, false);
55  private_nh_.param("clear_on_shutdown", clear_on_shutdown_, false);
56 
58  // initialize costmap stopped if shutdown_costmaps parameter is true
59  stop();
60  else
61  // otherwise costmap_users_ is at least 1, as costmap is always active
63 }
64 
66 {
68 }
69 
70 
71 void CostmapWrapper::reconfigure(double shutdown_costmap, double shutdown_costmap_delay)
72 {
73  shutdown_costmap_delay_ = ros::Duration(shutdown_costmap_delay);
75  ROS_WARN("Zero shutdown costmaps delay is not recommended, as it forces us to enable costmaps on each action");
76 
77  if (shutdown_costmap_ && !shutdown_costmap)
78  {
79  checkActivate();
80  shutdown_costmap_ = shutdown_costmap;
81  }
82  if (!shutdown_costmap_ && shutdown_costmap)
83  {
84  shutdown_costmap_ = shutdown_costmap;
86  }
87 }
88 
90 {
91  // lock and clear costmap
92  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*getCostmap()->getMutex());
93  resetLayers();
94 }
95 
97 {
98  boost::mutex::scoped_lock sl(check_costmap_mutex_);
99 
101 
102  // Activate costmap if we shutdown them when not moving and they are not already active. This method must be
103  // synchronized because start costmap can take up to 1/update freq., and concurrent calls to it can lead to segfaults
105  {
106  start();
107  ROS_DEBUG_STREAM("" << name_ << " activated");
108  }
109  ++costmap_users_;
110 }
111 
113 {
114  boost::mutex::scoped_lock sl(check_costmap_mutex_);
115 
116  --costmap_users_;
117  ROS_ASSERT_MSG(costmap_users_ >= 0, "Negative number (%d) of active users count!", costmap_users_);
119  {
120  // Delay costmap shutdown by shutdown_costmap_delay so we don't need to enable at each step of a normal
121  // navigation sequence, what is terribly inefficient; the timer is stopped on costmap re-activation and
122  // reset after every new call to deactivate
125  }
126 }
127 
129 {
130  boost::mutex::scoped_lock sl(check_costmap_mutex_);
131 
132  ROS_ASSERT_MSG(!costmap_users_, "Deactivating costmap with %d active users!", costmap_users_);
133  if (clear_on_shutdown_)
134  clear(); // do before stop, as some layers (e.g. obstacle and voxel) reactivate their subscribers on reset
135  stop();
136  ROS_DEBUG_STREAM("" << name_ << " deactivated");
137 }
138 
139 } /* namespace mbf_costmap_nav */
bool clear_on_shutdown_
clear the costmap, when shutting down
bool shutdown_costmap_
don&#39;t update costmap when not using it
void stop()
void reconfigure(double shutdown_costmap, double shutdown_costmap_delay)
Reconfiguration method called by dynamic reconfigure.
void checkActivate()
Check whether the costmap should be activated.
ros::Duration shutdown_costmap_delay_
costmap delayed shutdown delay
#define ROS_WARN(...)
boost::mutex check_costmap_mutex_
Start/stop costmap mutex; concurrent calls to start can lead to segfault.
void deactivate(const ros::TimerEvent &event)
Timer-triggered deactivation of the costmap.
void checkDeactivate()
Check whether the costmap should and could be deactivated.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
#define ROS_DEBUG_STREAM(args)
ros::Timer shutdown_costmap_timer_
costmap delayed shutdown timer
virtual ~CostmapWrapper()
Destructor.
int16_t costmap_users_
keep track of plugins using costmap
Costmap2D * getCostmap()
CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr)
Constructor.
ros::NodeHandle private_nh_
Private node handle.


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Fri Nov 6 2020 03:56:29