00001 #include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>
00002
00003 #include <pluginlib/class_list_macros.h>
00004 #include <tf/tf.h>
00005
00006 PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)
00007
00008 namespace costmap_converter
00009 {
00010
00011 CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
00012 {
00013 ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
00014 costmap_ = nullptr;
00015 dynamic_recfg_ = nullptr;
00016 }
00017
00018 CostmapToDynamicObstacles::~CostmapToDynamicObstacles()
00019 {
00020 if(dynamic_recfg_ != nullptr)
00021 delete dynamic_recfg_;
00022 }
00023
00024 void CostmapToDynamicObstacles::initialize(ros::NodeHandle nh)
00025 {
00026 costmap_ = nullptr;
00027
00028
00029 ros::NodeHandle gn;
00030 odom_sub_ = gn.subscribe(odom_topic_, 1, &CostmapToDynamicObstacles::odomCallback, this);
00031
00032 nh.param("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
00033
00035
00036 BackgroundSubtractor::Params bg_sub_params;
00037
00038 bg_sub_params.alpha_slow = 0.3;
00039 nh.param("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
00040
00041 bg_sub_params.alpha_fast = 0.85;
00042 nh.param("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
00043
00044 bg_sub_params.beta = 0.85;
00045 nh.param("beta", bg_sub_params.beta, bg_sub_params.beta);
00046
00047 bg_sub_params.min_occupancy_probability = 180;
00048 nh.param("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
00049
00050 bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
00051 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);
00052
00053 bg_sub_params.max_occupancy_neighbors = 100;
00054 nh.param("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
00055
00056 bg_sub_params.morph_size = 1;
00057 nh.param("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
00058
00059 bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
00060
00062
00063 BlobDetector::Params blob_det_params;
00064
00065 blob_det_params.filterByColor = true;
00066 blob_det_params.blobColor = 255;
00067 blob_det_params.thresholdStep = 256;
00068 blob_det_params.minThreshold = 127;
00069 blob_det_params.maxThreshold = 255;
00070 blob_det_params.minRepeatability = 1;
00071
00072 blob_det_params.minDistBetweenBlobs = 10;
00073 nh.param("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
00074
00075 blob_det_params.filterByArea = true;
00076 nh.param("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
00077
00078 blob_det_params.minArea = 3;
00079 nh.param("min_area", blob_det_params.minArea, blob_det_params.minArea);
00080
00081 blob_det_params.maxArea = 300;
00082 nh.param("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
00083
00084 blob_det_params.filterByCircularity = true;
00085 nh.param("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
00086
00087 blob_det_params.minCircularity = 0.2;
00088 nh.param("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
00089
00090 blob_det_params.maxCircularity = 1;
00091 nh.param("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
00092
00093 blob_det_params.filterByInertia = true;
00094 nh.param("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
00095
00096 blob_det_params.minInertiaRatio = 0.2;
00097 nh.param("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
00098
00099 blob_det_params.maxInertiaRatio = 1;
00100 nh.param("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
00101
00102 blob_det_params.filterByConvexity = false;
00103 nh.param("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
00104
00105 blob_det_params.minConvexity = 0;
00106 nh.param("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
00107
00108 blob_det_params.maxConvexity = 1;
00109 nh.param("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
00110
00111 blob_det_ = BlobDetector::create(blob_det_params);
00112
00114
00115 CTracker::Params tracker_params;
00116 tracker_params.dt = 0.2;
00117 nh.param("dt", tracker_params.dt, tracker_params.dt);
00118
00119 tracker_params.dist_thresh = 60.0;
00120 nh.param("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
00121
00122 tracker_params.max_allowed_skipped_frames = 3;
00123 nh.param("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
00124
00125 tracker_params.max_trace_length = 10;
00126 nh.param("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
00127
00128 tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
00129
00130
00132
00133 std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
00134 nh.param("static_converter_plugin", static_converter_plugin, static_converter_plugin);
00135 loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
00136
00137
00138
00139 dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
00140 dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
00141 dynamic_recfg_->setCallback(cb);
00142 }
00143
00144 void CostmapToDynamicObstacles::compute()
00145 {
00146 if (costmap_mat_.empty())
00147 return;
00148
00150
00151 int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
00152 int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
00153
00154
00155
00156 bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
00157
00158
00159 if (fg_mask_.empty())
00160 return;
00161
00162 cv::Mat bg_mat;
00163 if (publish_static_obstacles_)
00164 {
00165
00166 bg_mat = costmap_mat_ - fg_mask_;
00167
00168 }
00169
00170
00172
00173 blob_det_->detect(fg_mask_, keypoints_);
00174 std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
00175
00176
00178
00179
00180 std::vector<Point_t> detected_centers(keypoints_.size());
00181 for (int i = 0; i < keypoints_.size(); i++)
00182 {
00183 detected_centers.at(i).x = keypoints_.at(i).pt.x;
00184 detected_centers.at(i).y = keypoints_.at(i).pt.y;
00185 detected_centers.at(i).z = 0;
00186 }
00187
00188 tracker_->Update(detected_centers, contours);
00189
00190
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00206 ObstacleArrayPtr obstacles(new ObstacleArrayMsg);
00207
00208 obstacles->header.stamp = ros::Time::now();
00209 obstacles->header.frame_id = "/map";
00210
00211
00212 for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
00213 {
00214 geometry_msgs::Polygon polygon;
00215
00216
00217 std::vector<Point_t> contour;
00218 getContour(i, contour);
00219
00220
00221 for (const Point_t& pt : contour)
00222 {
00223 polygon.points.emplace_back();
00224 polygon.points.back().x = pt.x;
00225 polygon.points.back().y = pt.y;
00226 polygon.points.back().z = 0;
00227 }
00228
00229 obstacles->obstacles.emplace_back();
00230 obstacles->obstacles.back().polygon = polygon;
00231
00232
00233 obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
00234
00235
00236 geometry_msgs::QuaternionStamped orientation;
00237
00238 Point_t vel = getEstimatedVelocityOfObject(i);
00239 double yaw = std::atan2(vel.y, vel.x);
00240
00241 obstacles->obstacles.back().orientation = tf::createQuaternionMsgFromYaw(yaw);
00242
00243
00244 geometry_msgs::TwistWithCovariance velocities;
00245
00246
00247 velocities.twist.linear.x = vel.x;
00248 velocities.twist.linear.y = vel.y;
00249 velocities.twist.linear.z = 0;
00250 velocities.twist.angular.x = 0;
00251 velocities.twist.angular.y = 0;
00252 velocities.twist.angular.z = 0;
00253
00254
00255 velocities.covariance = {1, 0, 0, 0, 0, 0,
00256 0, 1, 0, 0, 0, 0,
00257 0, 0, 1, 0, 0, 0,
00258 0, 0, 0, 1, 0, 0,
00259 0, 0, 0, 0, 1, 0,
00260 0, 0, 0, 0, 0, 1};
00261
00262 obstacles->obstacles.back().velocities = velocities;
00263 }
00264
00266 if (publish_static_obstacles_)
00267 {
00268 uchar* img_data = bg_mat.data;
00269 int width = bg_mat.cols;
00270 int height = bg_mat.rows;
00271 int stride = bg_mat.step;
00272
00273 if (stackedCostmapConversion())
00274 {
00275
00276 boost::shared_ptr<costmap_2d::Costmap2D> static_costmap(new costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),
00277 costmap_->getSizeInCellsY(),
00278 costmap_->getResolution(),
00279 costmap_->getOriginX(),
00280 costmap_->getOriginY()));
00281 for(int i = 0; i < height; i++)
00282 {
00283 for(int j = 0; j < width; j++)
00284 {
00285 static_costmap->setCost(j, i, img_data[i * stride + j]);
00286 }
00287 }
00288
00289
00290 setStaticCostmap(static_costmap);
00291 convertStaticObstacles();
00292
00293
00294 auto static_polygons = getStaticPolygons();
00295 for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
00296 {
00297 obstacles->obstacles.emplace_back();
00298 obstacles->obstacles.back().polygon = *it;
00299 obstacles->obstacles.back().velocities.twist.linear.x = 0;
00300 obstacles->obstacles.back().velocities.twist.linear.y = 0;
00301 obstacles->obstacles.back().id = -1;
00302 }
00303 }
00304
00305 else
00306 {
00307 for(int i = 0; i < height; i++)
00308 {
00309 for(int j = 0; j < width; j++)
00310 {
00311 uchar value = img_data[i * stride + j];
00312 if (value > 0)
00313 {
00314
00315 obstacles->obstacles.emplace_back();
00316 geometry_msgs::Point32 pt;
00317 pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
00318 pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
00319 obstacles->obstacles.back().polygon.points.push_back(pt);
00320 obstacles->obstacles.back().velocities.twist.linear.x = 0;
00321 obstacles->obstacles.back().velocities.twist.linear.y = 0;
00322 obstacles->obstacles.back().id = -1;
00323 }
00324 }
00325 }
00326 }
00327 }
00328
00329 updateObstacleContainer(obstacles);
00330 }
00331
00332 void CostmapToDynamicObstacles::setCostmap2D(costmap_2d::Costmap2D* costmap)
00333 {
00334 if (!costmap)
00335 return;
00336
00337 costmap_ = costmap;
00338
00339 updateCostmap2D();
00340 }
00341
00342 void CostmapToDynamicObstacles::updateCostmap2D()
00343 {
00344 if (!costmap_->getMutex())
00345 {
00346 ROS_ERROR("Cannot update costmap since the mutex pointer is null");
00347 return;
00348 }
00349 costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
00350
00351
00352
00353
00354 costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
00355 costmap_->getCharMap());
00356 }
00357
00358 ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
00359 {
00360 boost::mutex::scoped_lock lock(mutex_);
00361 return obstacles_;
00362 }
00363
00364 void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)
00365 {
00366 boost::mutex::scoped_lock lock(mutex_);
00367 obstacles_ = obstacles;
00368 }
00369
00370 Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)
00371 {
00372
00373 Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
00374
00375
00376
00377 return vel;
00378 }
00379
00380 void CostmapToDynamicObstacles::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
00381 {
00382 ROS_INFO_ONCE("CostmapToDynamicObstacles: odom received.");
00383
00384 tf::Quaternion pose;
00385 tf::quaternionMsgToTF(msg->pose.pose.orientation, pose);
00386
00387 tf::Vector3 twistLinear;
00388 tf::vector3MsgToTF(msg->twist.twist.linear, twistLinear);
00389
00390
00391 tf::Vector3 vel = tf::quatRotate(pose, twistLinear);
00392 ego_vel_.x = vel.x();
00393 ego_vel_.y = vel.y();
00394 ego_vel_.z = vel.z();
00395 }
00396
00397 void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
00398 {
00399 publish_static_obstacles_ = config.publish_static_obstacles;
00400
00401
00402 BackgroundSubtractor::Params bg_sub_params;
00403 bg_sub_params.alpha_slow = config.alpha_slow;
00404 bg_sub_params.alpha_fast = config.alpha_fast;
00405 bg_sub_params.beta = config.beta;
00406 bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
00407 bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
00408 bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
00409 bg_sub_params.morph_size = config.morph_size;
00410 bg_sub_->updateParameters(bg_sub_params);
00411
00412
00413 BlobDetector::Params blob_det_params;
00414
00415 blob_det_params.filterByColor = true;
00416 blob_det_params.blobColor = 255;
00417 blob_det_params.thresholdStep = 256;
00418 blob_det_params.minThreshold = 127;
00419 blob_det_params.maxThreshold = 255;
00420 blob_det_params.minRepeatability = 1;
00421 blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs;
00422 blob_det_params.filterByArea = config.filter_by_area;
00423 blob_det_params.minArea = config.min_area;
00424 blob_det_params.maxArea = config.max_area;
00425 blob_det_params.filterByCircularity = config.filter_by_circularity;
00426 blob_det_params.minCircularity = config.min_circularity;
00427 blob_det_params.maxCircularity = config.max_circularity;
00428 blob_det_params.filterByInertia = config.filter_by_inertia;
00429 blob_det_params.minInertiaRatio = config.min_inertia_ratio;
00430 blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
00431 blob_det_params.filterByConvexity = config.filter_by_convexity;
00432 blob_det_params.minConvexity = config.min_convexity;
00433 blob_det_params.maxConvexity = config.max_convexity;
00434 blob_det_->updateParameters(blob_det_params);
00435
00436
00437 CTracker::Params tracking_params;
00438 tracking_params.dt = config.dt;
00439 tracking_params.dist_thresh = config.dist_thresh;
00440 tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
00441 tracking_params.max_trace_length = config.max_trace_length;
00442 tracker_->updateParameters(tracking_params);
00443 }
00444
00445 void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
00446 {
00447 assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
00448
00449 contour.clear();
00450
00451
00452 std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
00453
00454 contour.reserve(contour2i.size());
00455
00456 Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
00457
00458 for (std::size_t i = 0; i < contour2i.size(); ++i)
00459 {
00460 contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
00461 + costmap_origin);
00462 }
00463
00464 }
00465
00466 void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
00467 {
00468 if (!image.empty())
00469 {
00470 cv::Mat im = image.clone();
00471 cv::flip(im, im, 0);
00472 cv::imshow(name, im);
00473 cv::waitKey(1);
00474 }
00475 }
00476 }