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

#include <costmap_to_dynamic_obstacles.h>

Inheritance diagram for costmap_converter::CostmapToDynamicObstacles:

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< BackgroundSubtractor >	bg_sub_

cv::Ptr< BlobDetector >	blob_det_

costmap_2d::Costmap2D *	costmap_

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< CTracker >	tracker_

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]	idx	Index of the tracked object in the tracker
[out]	contour	vector 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

idx	Index 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

nh	Nodehandle 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

msg	The 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

config	Reference to the dynamic reconfigure config
level	Dynamic 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

costmap Pointer 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_topic topic 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

obstacles Updated 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

name	Id/name of the opencv window
image	Image 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:

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation