costmap_to_dynamic_obstacles.cpp
Go to the documentation of this file.
2 
4 #include <tf/tf.h>
5 
7 
8 namespace costmap_converter
9 {
10 
11 CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
12 {
13  ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
14  costmap_ = nullptr;
15  dynamic_recfg_ = nullptr;
16 }
17 
19 {
20  if(dynamic_recfg_ != nullptr)
21  delete dynamic_recfg_;
22 }
23 
25 {
26  costmap_ = nullptr;
27 
28  // We need the odometry from the robot to compensate the ego motion
29  ros::NodeHandle gn; // create odom topic w.r.t. global node handle
31 
32  nh.param("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
33 
35  // Foreground detection parameters
36  BackgroundSubtractor::Params bg_sub_params;
37 
38  bg_sub_params.alpha_slow = 0.3;
39  nh.param("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
40 
41  bg_sub_params.alpha_fast = 0.85;
42  nh.param("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
43 
44  bg_sub_params.beta = 0.85;
45  nh.param("beta", bg_sub_params.beta, bg_sub_params.beta);
46 
47  bg_sub_params.min_occupancy_probability = 180;
48  nh.param("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
49 
50  bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
51  nh.param("min_sep_between_slow_and_fast_filter", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter);
52 
53  bg_sub_params.max_occupancy_neighbors = 100;
54  nh.param("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
55 
56  bg_sub_params.morph_size = 1;
57  nh.param("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
58 
59  bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
60 
62  // Blob detection parameters
63  BlobDetector::Params blob_det_params;
64 
65  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
66  blob_det_params.blobColor = 255; // Extract light blobs
67  blob_det_params.thresholdStep = 256; // Input for blob detection is already a binary image
68  blob_det_params.minThreshold = 127;
69  blob_det_params.maxThreshold = 255;
70  blob_det_params.minRepeatability = 1;
71 
72  blob_det_params.minDistBetweenBlobs = 10;
73  nh.param("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
74 
75  blob_det_params.filterByArea = true;
76  nh.param("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
77 
78  blob_det_params.minArea = 3; // Filter out blobs with less pixels
79  nh.param("min_area", blob_det_params.minArea, blob_det_params.minArea);
80 
81  blob_det_params.maxArea = 300;
82  nh.param("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
83 
84  blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2
85  nh.param("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
86 
87  blob_det_params.minCircularity = 0.2;
88  nh.param("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
89 
90  blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)
91  nh.param("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
92 
93  blob_det_params.filterByInertia = true; // Filter blobs based on their elongation
94  nh.param("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
95 
96  blob_det_params.minInertiaRatio = 0.2; // minimal 0 (in case of a line)
97  nh.param("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
98 
99  blob_det_params.maxInertiaRatio = 1; // maximal 1 (in case of a circle)
100  nh.param("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
101 
102  blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull
103  nh.param("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
104 
105  blob_det_params.minConvexity = 0; // minimal 0
106  nh.param("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
107 
108  blob_det_params.maxConvexity = 1; // maximal 1
109  nh.param("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
110 
111  blob_det_ = BlobDetector::create(blob_det_params);
112 
114  // Tracking parameters
115  CTracker::Params tracker_params;
116  tracker_params.dt = 0.2;
117  nh.param("dt", tracker_params.dt, tracker_params.dt);
118 
119  tracker_params.dist_thresh = 60.0;
120  nh.param("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
121 
122  tracker_params.max_allowed_skipped_frames = 3;
123  nh.param("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
124 
125  tracker_params.max_trace_length = 10;
126  nh.param("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
127 
128  tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
129 
130 
132  // Static costmap conversion parameters
133  std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
134  nh.param("static_converter_plugin", static_converter_plugin, static_converter_plugin);
135  loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
136 
137 
138  // setup dynamic reconfigure
139  dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
140  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
141  dynamic_recfg_->setCallback(cb);
142 }
143 
145 {
146  if (costmap_mat_.empty())
147  return;
148 
150  // Dynamic obstacles are separated from static obstacles
151  int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
152  int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
153  // ROS_INFO("Origin x [m]: %f Origin_y [m]: %f", costmap_->getOriginX(), costmap_->getOriginY());
154  // ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY);
155 
156  bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
157 
158  // if no foreground object is detected, no ObstacleMsgs need to be published
159  if (fg_mask_.empty())
160  return;
161 
162  cv::Mat bg_mat;
164  {
165  // Get static obstacles
166  bg_mat = costmap_mat_ - fg_mask_;
167  // visualize("bg_mat", bg_mat);
168  }
169 
170 
172  // Centers and contours of Blobs are detected
173  blob_det_->detect(fg_mask_, keypoints_);
174  std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
175 
176 
178  // Objects are assigned to objects from previous frame based on Hungarian Algorithm
179  // Object velocities are estimated using a Kalman Filter
180  std::vector<Point_t> detected_centers(keypoints_.size());
181  for (int i = 0; i < keypoints_.size(); i++)
182  {
183  detected_centers.at(i).x = keypoints_.at(i).pt.x;
184  detected_centers.at(i).y = keypoints_.at(i).pt.y;
185  detected_centers.at(i).z = 0; // Currently unused!
186  }
187 
188  tracker_->Update(detected_centers, contours);
189 
190 
192  /*
193  cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);
194  cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);
195 
196  for (int i = 0; i < (int)tracker_->tracks.size(); ++i)
197  cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),
198  cv::Scalar(0, 0, 255), 1);
199 
200  //visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints);
201  //cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask);
202  //cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints);
203  */
204 
206  ObstacleArrayPtr obstacles(new ObstacleArrayMsg);
207  // header.seq is automatically filled
208  obstacles->header.stamp = ros::Time::now();
209  obstacles->header.frame_id = "/map"; //Global frame /map
210 
211  // For all tracked objects
212  for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
213  {
214  geometry_msgs::Polygon polygon;
215 
216  // TODO directly create polygon inside getContour and avoid copy
217  std::vector<Point_t> contour;
218  getContour(i, contour); // this method also transforms map to world coordinates
219 
220  // convert contour to polygon
221  for (const Point_t& pt : contour)
222  {
223  polygon.points.emplace_back();
224  polygon.points.back().x = pt.x;
225  polygon.points.back().y = pt.y;
226  polygon.points.back().z = 0;
227  }
228 
229  obstacles->obstacles.emplace_back();
230  obstacles->obstacles.back().polygon = polygon;
231 
232  // Set obstacle ID
233  obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
234 
235  // Set orientation
236  geometry_msgs::QuaternionStamped orientation;
237 
239  double yaw = std::atan2(vel.y, vel.x);
240  //ROS_INFO("yaw: %f", yaw);
241  obstacles->obstacles.back().orientation = tf::createQuaternionMsgFromYaw(yaw);
242 
243  // Set velocity
244  geometry_msgs::TwistWithCovariance velocities;
245  //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);
246  //velocities.twist.linear.y = 0;
247  velocities.twist.linear.x = vel.x;
248  velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?
249  velocities.twist.linear.z = 0;
250  velocities.twist.angular.x = 0;
251  velocities.twist.angular.y = 0;
252  velocities.twist.angular.z = 0;
253 
254  // TODO: use correct covariance matrix
255  velocities.covariance = {1, 0, 0, 0, 0, 0,
256  0, 1, 0, 0, 0, 0,
257  0, 0, 1, 0, 0, 0,
258  0, 0, 0, 1, 0, 0,
259  0, 0, 0, 0, 1, 0,
260  0, 0, 0, 0, 0, 1};
261 
262  obstacles->obstacles.back().velocities = velocities;
263  }
264 
267  {
268  uchar* img_data = bg_mat.data;
269  int width = bg_mat.cols;
270  int height = bg_mat.rows;
271  int stride = bg_mat.step;
272 
274  {
275  // Create new costmap with static obstacles (background)
279  costmap_->getOriginX(),
280  costmap_->getOriginY()));
281  for(int i = 0; i < height; i++)
282  {
283  for(int j = 0; j < width; j++)
284  {
285  static_costmap->setCost(j, i, img_data[i * stride + j]);
286  }
287  }
288 
289  // Apply static obstacle conversion plugin
290  setStaticCostmap(static_costmap);
292 
293  // Store converted static obstacles for publishing
294  auto static_polygons = getStaticPolygons();
295  for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
296  {
297  obstacles->obstacles.emplace_back();
298  obstacles->obstacles.back().polygon = *it;
299  obstacles->obstacles.back().velocities.twist.linear.x = 0;
300  obstacles->obstacles.back().velocities.twist.linear.y = 0;
301  obstacles->obstacles.back().id = -1;
302  }
303  }
304  // Otherwise leave static obstacles point-shaped
305  else
306  {
307  for(int i = 0; i < height; i++)
308  {
309  for(int j = 0; j < width; j++)
310  {
311  uchar value = img_data[i * stride + j];
312  if (value > 0)
313  {
314  // obstacle found
315  obstacles->obstacles.emplace_back();
316  geometry_msgs::Point32 pt;
317  pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
318  pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
319  obstacles->obstacles.back().polygon.points.push_back(pt);
320  obstacles->obstacles.back().velocities.twist.linear.x = 0;
321  obstacles->obstacles.back().velocities.twist.linear.y = 0;
322  obstacles->obstacles.back().id = -1;
323  }
324  }
325  }
326  }
327  }
328 
329  updateObstacleContainer(obstacles);
330 }
331 
333 {
334  if (!costmap)
335  return;
336 
337  costmap_ = costmap;
338 
339  updateCostmap2D();
340 }
341 
343 {
344  if (!costmap_->getMutex())
345  {
346  ROS_ERROR("Cannot update costmap since the mutex pointer is null");
347  return;
348  }
349  costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
350 
351  // Initialize costmapMat_ directly with the unsigned char array of costmap_
352  //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
353  // costmap_->getCharMap()).clone(); // Deep copy: TODO
355  costmap_->getCharMap());
356 }
357 
359 {
360  boost::mutex::scoped_lock lock(mutex_);
361  return obstacles_;
362 }
363 
365 {
366  boost::mutex::scoped_lock lock(mutex_);
367  obstacles_ = obstacles;
368 }
369 
371 {
372  // vel [px/s] * costmapResolution [m/px] = vel [m/s]
373  Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
374 
375  //ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z);
376  // velocity in /map frame
377  return vel;
378 }
379 
380 void CostmapToDynamicObstacles::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
381 {
382  ROS_INFO_ONCE("CostmapToDynamicObstacles: odom received.");
383 
384  tf::Quaternion pose;
385  tf::quaternionMsgToTF(msg->pose.pose.orientation, pose);
386 
387  tf::Vector3 twistLinear;
388  tf::vector3MsgToTF(msg->twist.twist.linear, twistLinear);
389 
390  // velocity of the robot in x, y and z coordinates
391  tf::Vector3 vel = tf::quatRotate(pose, twistLinear);
392  ego_vel_.x = vel.x();
393  ego_vel_.y = vel.y();
394  ego_vel_.z = vel.z();
395 }
396 
397 void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
398 {
399  publish_static_obstacles_ = config.publish_static_obstacles;
400 
401  // BackgroundSubtraction Parameters
402  BackgroundSubtractor::Params bg_sub_params;
403  bg_sub_params.alpha_slow = config.alpha_slow;
404  bg_sub_params.alpha_fast = config.alpha_fast;
405  bg_sub_params.beta = config.beta;
406  bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
407  bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
408  bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
409  bg_sub_params.morph_size = config.morph_size;
410  bg_sub_->updateParameters(bg_sub_params);
411 
412  // BlobDetector Parameters
413  BlobDetector::Params blob_det_params;
414  // necessary, because blobDetParams are otherwise initialized with default values for dark blobs
415  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
416  blob_det_params.blobColor = 255; // Extract light blobs
417  blob_det_params.thresholdStep = 256; // Input for blobDetection is already a binary image
418  blob_det_params.minThreshold = 127;
419  blob_det_params.maxThreshold = 255;
420  blob_det_params.minRepeatability = 1;
421  blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected
422  blob_det_params.filterByArea = config.filter_by_area;
423  blob_det_params.minArea = config.min_area;
424  blob_det_params.maxArea = config.max_area;
425  blob_det_params.filterByCircularity = config.filter_by_circularity;
426  blob_det_params.minCircularity = config.min_circularity;
427  blob_det_params.maxCircularity = config.max_circularity;
428  blob_det_params.filterByInertia = config.filter_by_inertia;
429  blob_det_params.minInertiaRatio = config.min_inertia_ratio;
430  blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
431  blob_det_params.filterByConvexity = config.filter_by_convexity;
432  blob_det_params.minConvexity = config.min_convexity;
433  blob_det_params.maxConvexity = config.max_convexity;
434  blob_det_->updateParameters(blob_det_params);
435 
436  // Tracking Parameters
437  CTracker::Params tracking_params;
438  tracking_params.dt = config.dt;
439  tracking_params.dist_thresh = config.dist_thresh;
440  tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
441  tracking_params.max_trace_length = config.max_trace_length;
442  tracker_->updateParameters(tracking_params);
443 }
444 
445 void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
446 {
447  assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
448 
449  contour.clear();
450 
451  // contour [px] * costmapResolution [m/px] = contour [m]
452  std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
453 
454  contour.reserve(contour2i.size());
455 
456  Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
457 
458  for (std::size_t i = 0; i < contour2i.size(); ++i)
459  {
460  contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
461  + costmap_origin); // Shift to /map
462  }
463 
464 }
465 
466 void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
467 {
468  if (!image.empty())
469  {
470  cv::Mat im = image.clone();
471  cv::flip(im, im, 0);
472  cv::imshow(name, im);
473  cv::waitKey(1);
474  }
475 }
476 }
int max_trace_length
Definition: Ctracker.h:80
dynamic_reconfigure::Server< CostmapToDynamicObstaclesConfig > * dynamic_recfg_
void convertStaticObstacles()
Apply the underlying plugin for static costmap conversion.
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int max_allowed_skipped_frames
Definition: Ctracker.h:79
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
modifications at runtime
double alpha_fast
Filter constant (learning rate) of the fast filter part.
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
cv::Point3_< track_t > Point_t
Definition: defines.h:8
double getOriginX() const
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.
double alpha_slow
Filter constant (learning rate) of the slow filter part.
unsigned char * getCharMap() const
void updateObstacleContainer(ObstacleArrayPtr obstacles)
Thread-safe update of the internal obstacle container (that is shared using getObstacles() from outsi...
TFSIMD_FORCE_INLINE const tfScalar & x() const
void loadStaticCostmapConverterPlugin(const std::string &plugin_name, ros::NodeHandle nh_parent)
Load underlying static costmap conversion plugin via plugin-loader.
Point_t getEstimatedVelocityOfObject(unsigned int idx)
Converts the estimated velocity of a tracked object to m/s and returns it.
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
TFSIMD_FORCE_INLINE const tfScalar & z() const
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
virtual void compute()
This method performs the actual work (conversion of the costmap to obstacles)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
track_t dist_thresh
Definition: Ctracker.h:78
This abstract class defines the interface for plugins that convert the costmap into polygon types...
TFSIMD_FORCE_INLINE const tfScalar & y() const
void setStaticCostmap(boost::shared_ptr< costmap_2d::Costmap2D > static_costmap)
Set the costmap for the underlying plugin.
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
This class converts the costmap_2d into dynamic obstacles.
double getOriginY() const
static cv::Ptr< BlobDetector > create(const BlobDetector::Params &params)
Create shared instance of the blob detector with given parameters.
unsigned int getSizeInCellsY() const
Perform background subtraction to extract the "moving" foreground.
unsigned int getSizeInCellsX() const
track_t dt
Definition: Ctracker.h:77
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
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].
static Time now()
void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level)
Callback for the dynamic_reconfigure node.
double getResolution() const
#define ROS_ERROR(...)
void visualize(const std::string &name, const cv::Mat &image)
OpenCV Visualization.
PolygonContainerConstPtr getStaticPolygons()
Get the converted polygons from the underlying plugin.
std::unique_ptr< BackgroundSubtractor > bg_sub_
bool stackedCostmapConversion()
Determines whether an additional plugin for subsequent costmap conversion is specified.


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