Public Member Functions | Protected Member Functions | Private Member Functions | Private Attributes | List of all members
costmap_converter::CostmapToDynamicObstacles Class Reference

This class converts the costmap_2d into dynamic obstacles. More...

#include <costmap_to_dynamic_obstacles.h>

Inheritance diagram for costmap_converter::CostmapToDynamicObstacles:
Inheritance graph
[legend]

Public Member Functions

virtual void compute ()
 This method performs the actual work (conversion of the costmap to obstacles) More...
 
 CostmapToDynamicObstacles ()
 Constructor. More...
 
ObstacleArrayConstPtr getObstacles ()
 Get a shared instance of the current obstacle container. More...
 
virtual void initialize (ros::NodeHandle nh)
 Initialize the plugin. More...
 
virtual void setCostmap2D (costmap_2d::Costmap2D *costmap)
 Pass a pointer to the costmap to the plugin. More...
 
virtual void setOdomTopic (const std::string &odom_topic)
 Set name of robot's odometry topic. More...
 
virtual void updateCostmap2D ()
 Get updated data from the previously set Costmap2D. More...
 
void visualize (const std::string &name, const cv::Mat &image)
 OpenCV Visualization. More...
 
virtual ~CostmapToDynamicObstacles ()
 Destructor. More...
 
- Public Member Functions inherited from costmap_converter::BaseCostmapToDynamicObstacles
void convertStaticObstacles ()
 Apply the underlying plugin for static costmap conversion. More...
 
PolygonContainerConstPtr getStaticPolygons ()
 Get the converted polygons from the underlying plugin. More...
 
void loadStaticCostmapConverterPlugin (const std::string &plugin_name, ros::NodeHandle nh_parent)
 Load underlying static costmap conversion plugin via plugin-loader. More...
 
void setStaticCostmap (boost::shared_ptr< costmap_2d::Costmap2D > static_costmap)
 Set the costmap for the underlying plugin. More...
 
void setStaticCostmapConverterPlugin (boost::shared_ptr< BaseCostmapToPolygons > static_costmap_converter)
 Set the underlying plugin for subsequent costmap conversion of the static background of the costmap. More...
 
bool stackedCostmapConversion ()
 Determines whether an additional plugin for subsequent costmap conversion is specified. More...
 
- Public Member Functions inherited from costmap_converter::BaseCostmapToPolygons
virtual PolygonContainerConstPtr getPolygons ()
 Get a shared instance of the current polygon container. More...
 
void startWorker (ros::Rate rate, costmap_2d::Costmap2D *costmap, bool spin_thread=false)
 Instantiate a worker that repeatedly coverts the most recent costmap to polygons. The worker is implemented as a timer event that is invoked at a specific rate. The passed costmap pointer must be valid at the complete time and must be lockable. By specifying the argument spin_thread the timer event is invoked in a separate thread and callback queue or otherwise it is called from the global callback queue (of the node in which the plugin is used). More...
 
void stopWorker ()
 Stop the worker that repeatedly converts the costmap to polygons. More...
 
virtual ~BaseCostmapToPolygons ()
 Destructor. More...
 

Protected Member Functions

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]. More...
 
Point_t getEstimatedVelocityOfObject (unsigned int idx)
 Converts the estimated velocity of a tracked object to m/s and returns it. More...
 
void updateObstacleContainer (ObstacleArrayPtr obstacles)
 Thread-safe update of the internal obstacle container (that is shared using getObstacles() from outside this class) More...
 
- Protected Member Functions inherited from costmap_converter::BaseCostmapToDynamicObstacles
 BaseCostmapToDynamicObstacles ()
 Protected constructor that should be called by subclasses. More...
 
- Protected Member Functions inherited from costmap_converter::BaseCostmapToPolygons
 BaseCostmapToPolygons ()
 Protected constructor that should be called by subclasses. More...
 
void spinThread ()
 Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) More...
 
void workerCallback (const ros::TimerEvent &)
 The callback of the worker that performs the actual work (updating the costmap and converting it to polygons) More...
 

Private Member Functions

void odomCallback (const nav_msgs::Odometry::ConstPtr &msg)
 modifications at runtime More...
 
void reconfigureCB (CostmapToDynamicObstaclesConfig &config, uint32_t level)
 Callback for the dynamic_reconfigure node. More...
 

Private Attributes

std::unique_ptr< BackgroundSubtractorbg_sub_
 
cv::Ptr< BlobDetectorblob_det_
 
costmap_2d::Costmap2Dcostmap_
 
cv::Mat costmap_mat_
 
dynamic_reconfigure::Server< CostmapToDynamicObstaclesConfig > * dynamic_recfg_
 
Point_t ego_vel_
 
cv::Mat fg_mask_
 
std::vector< cv::KeyPoint > keypoints_
 
boost::mutex mutex_
 
ObstacleArrayPtr obstacles_
 
ros::Subscriber odom_sub_
 
std::string odom_topic_ = "/odom"
 
bool publish_static_obstacles_ = true
 
std::unique_ptr< CTrackertracker_
 

Detailed Description

This class converts the costmap_2d into dynamic obstacles.

Static obstacles are treated as point obstacles.

Definition at line 77 of file costmap_to_dynamic_obstacles.h.

Constructor & Destructor Documentation

costmap_converter::CostmapToDynamicObstacles::CostmapToDynamicObstacles ( )

Constructor.

Definition at line 11 of file costmap_to_dynamic_obstacles.cpp.

costmap_converter::CostmapToDynamicObstacles::~CostmapToDynamicObstacles ( )
virtual

Destructor.

Definition at line 18 of file costmap_to_dynamic_obstacles.cpp.

Member Function Documentation

void costmap_converter::CostmapToDynamicObstacles::compute ( )
virtual

This method performs the actual work (conversion of the costmap to obstacles)

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 144 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::getContour ( unsigned int  idx,
std::vector< Point_t > &  contour 
)
protected

Gets the last observed contour of a object and converts it from [px] to [m].

Parameters
[in]idxIndex of the tracked object in the tracker
[out]contourvector of Point_t, which represents the last observed contour in [m] in x,y,z coordinates

Definition at line 445 of file costmap_to_dynamic_obstacles.cpp.

Point_t costmap_converter::CostmapToDynamicObstacles::getEstimatedVelocityOfObject ( unsigned int  idx)
protected

Converts the estimated velocity of a tracked object to m/s and returns it.

Parameters
idxIndex of the tracked object in the tracker
Returns
Point_t, which represents velocity in [m/s] of object(idx) in x,y,z coordinates

Definition at line 370 of file costmap_to_dynamic_obstacles.cpp.

ObstacleArrayConstPtr costmap_converter::CostmapToDynamicObstacles::getObstacles ( )
virtual

Get a shared instance of the current obstacle container.

Remarks
If compute() or startWorker() has not been called before, this method returns an empty instance!
Returns
Shared instance of the current obstacle container

Reimplemented from costmap_converter::BaseCostmapToPolygons.

Definition at line 358 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::initialize ( ros::NodeHandle  nh)
virtual

Initialize the plugin.

Parameters
nhNodehandle that defines the namespace for parameters

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 24 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::odomCallback ( const nav_msgs::Odometry::ConstPtr &  msg)
private

modifications at runtime

Callback for the odometry messages of the observing robot.

Used to convert the velocity of obstacles to the /map frame.

Parameters
msgThe Pointer to the nav_msgs::Odometry of the observing robot

Definition at line 380 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::reconfigureCB ( CostmapToDynamicObstaclesConfig &  config,
uint32_t  level 
)
private

Callback for the dynamic_reconfigure node.

This callback allows to modify parameters dynamically at runtime without restarting the node

Parameters
configReference to the dynamic reconfigure config
levelDynamic reconfigure level

Definition at line 397 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::setCostmap2D ( costmap_2d::Costmap2D costmap)
virtual

Pass a pointer to the costmap to the plugin.

See also
updateCostmap2D
Parameters
costmapPointer to the costmap2d source

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 332 of file costmap_to_dynamic_obstacles.cpp.

virtual void costmap_converter::CostmapToDynamicObstacles::setOdomTopic ( const std::string &  odom_topic)
inlinevirtual

Set name of robot's odometry topic.

Warning
The method must be called before initialize()

Some plugins might require odometry information to compensate the robot's ego motion

Parameters
odom_topictopic name

Reimplemented from costmap_converter::BaseCostmapToPolygons.

Definition at line 132 of file costmap_to_dynamic_obstacles.h.

void costmap_converter::CostmapToDynamicObstacles::updateCostmap2D ( )
virtual

Get updated data from the previously set Costmap2D.

See also
setCostmap2D

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 342 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::updateObstacleContainer ( ObstacleArrayPtr  obstacles)
protected

Thread-safe update of the internal obstacle container (that is shared using getObstacles() from outside this class)

Parameters
obstaclesUpdated obstacle container

Definition at line 364 of file costmap_to_dynamic_obstacles.cpp.

void costmap_converter::CostmapToDynamicObstacles::visualize ( const std::string &  name,
const cv::Mat &  image 
)

OpenCV Visualization.

Parameters
nameId/name of the opencv window
imageImage to be visualized

Definition at line 466 of file costmap_to_dynamic_obstacles.cpp.

Member Data Documentation

std::unique_ptr<BackgroundSubtractor> costmap_converter::CostmapToDynamicObstacles::bg_sub_
private

Definition at line 177 of file costmap_to_dynamic_obstacles.h.

cv::Ptr<BlobDetector> costmap_converter::CostmapToDynamicObstacles::blob_det_
private

Definition at line 178 of file costmap_to_dynamic_obstacles.h.

costmap_2d::Costmap2D* costmap_converter::CostmapToDynamicObstacles::costmap_
private

Definition at line 173 of file costmap_to_dynamic_obstacles.h.

cv::Mat costmap_converter::CostmapToDynamicObstacles::costmap_mat_
private

Definition at line 174 of file costmap_to_dynamic_obstacles.h.

dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>* costmap_converter::CostmapToDynamicObstacles::dynamic_recfg_
private

Dynamic reconfigure server to allow config

Definition at line 188 of file costmap_to_dynamic_obstacles.h.

Point_t costmap_converter::CostmapToDynamicObstacles::ego_vel_
private

Definition at line 182 of file costmap_to_dynamic_obstacles.h.

cv::Mat costmap_converter::CostmapToDynamicObstacles::fg_mask_
private

Definition at line 176 of file costmap_to_dynamic_obstacles.h.

std::vector<cv::KeyPoint> costmap_converter::CostmapToDynamicObstacles::keypoints_
private

Definition at line 179 of file costmap_to_dynamic_obstacles.h.

boost::mutex costmap_converter::CostmapToDynamicObstacles::mutex_
private

Definition at line 172 of file costmap_to_dynamic_obstacles.h.

ObstacleArrayPtr costmap_converter::CostmapToDynamicObstacles::obstacles_
private

Definition at line 175 of file costmap_to_dynamic_obstacles.h.

ros::Subscriber costmap_converter::CostmapToDynamicObstacles::odom_sub_
private

Definition at line 181 of file costmap_to_dynamic_obstacles.h.

std::string costmap_converter::CostmapToDynamicObstacles::odom_topic_ = "/odom"
private

Definition at line 184 of file costmap_to_dynamic_obstacles.h.

bool costmap_converter::CostmapToDynamicObstacles::publish_static_obstacles_ = true
private

Definition at line 185 of file costmap_to_dynamic_obstacles.h.

std::unique_ptr<CTracker> costmap_converter::CostmapToDynamicObstacles::tracker_
private

Definition at line 180 of file costmap_to_dynamic_obstacles.h.


The documentation for this class was generated from the following files:


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Fri Jun 7 2019 21:48:43