costmap_to_dynamic_obstacles.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Notes:
37  * The following code makes use of the OpenCV library.
38  * OpenCV is licensed under the terms of the 3-clause BSD License.
39  *
40  * Authors: Franz Albers, Christoph Rösmann
41  *********************************************************************/
42 
43 #ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_
44 #define COSTMAP_TO_DYNAMIC_OBSTACLES_H_
45 
46 // ROS
48 #include <nav_msgs/Odometry.h>
49 #include <pluginlib/class_loader.h>
50 #include <ros/ros.h>
51 
52 // OpenCV
53 #include <cv_bridge/cv_bridge.h>
54 #include <opencv2/features2d/features2d.hpp>
55 #include <opencv2/video/tracking.hpp>
56 
57 // dynamic reconfigure
58 #include <costmap_converter/CostmapToDynamicObstaclesConfig.h>
59 #include <dynamic_reconfigure/server.h>
60 
61 // Own includes
65 
66 // STL
67 #include <memory>
68 
69 namespace costmap_converter {
70 
78 {
79 public:
84 
89 
94  virtual void initialize(ros::NodeHandle nh);
95 
100  virtual void compute();
101 
107  virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);
108 
113  virtual void updateCostmap2D();
114 
122 
132  virtual void setOdomTopic(const std::string& odom_topic)
133  {
134  odom_topic_ = odom_topic;
135  }
136 
142  void visualize(const std::string& name, const cv::Mat& image);
143 
144 protected:
152  Point_t getEstimatedVelocityOfObject(unsigned int idx);
153 
161  void getContour(unsigned int idx, std::vector<Point_t>& contour);
162 
170 
171 private:
172  boost::mutex mutex_;
174  cv::Mat costmap_mat_;
176  cv::Mat fg_mask_;
177  std::unique_ptr<BackgroundSubtractor> bg_sub_;
178  cv::Ptr<BlobDetector> blob_det_;
179  std::vector<cv::KeyPoint> keypoints_;
180  std::unique_ptr<CTracker> tracker_;
183 
184  std::string odom_topic_ = "/odom";
186 
187  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*
189 
197  void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
198 
207  void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);
208 };
209 
210 } // end namespace costmap_converter
211 
212 #endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */
dynamic_reconfigure::Server< CostmapToDynamicObstaclesConfig > * dynamic_recfg_
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
modifications at runtime
cv::Point3_< track_t > Point_t
Definition: defines.h:8
virtual void setOdomTopic(const std::string &odom_topic)
Set name of robot&#39;s odometry topic.
virtual void setCostmap2D(costmap_2d::Costmap2D *costmap)
Pass a pointer to the costmap to the plugin.
virtual void updateCostmap2D()
Get updated data from the previously set Costmap2D.
virtual void initialize(ros::NodeHandle nh)
Initialize the plugin.
void updateObstacleContainer(ObstacleArrayPtr obstacles)
Thread-safe update of the internal obstacle container (that is shared using getObstacles() from outsi...
Point_t getEstimatedVelocityOfObject(unsigned int idx)
Converts the estimated velocity of a tracked object to m/s and returns it.
virtual void compute()
This method performs the actual work (conversion of the costmap to obstacles)
This class converts the costmap_2d into dynamic obstacles.
ObstacleArrayConstPtr getObstacles()
Get a shared instance of the current obstacle container.
void getContour(unsigned int idx, std::vector< Point_t > &contour)
Gets the last observed contour of a object and converts it from [px] to [m].
void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level)
Callback for the dynamic_reconfigure node.
void visualize(const std::string &name, const cv::Mat &image)
OpenCV Visualization.
std::unique_ptr< BackgroundSubtractor > bg_sub_


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18