00001
00002
00003
00004
00005
00006
00007 #include "mrpt_icp_slam_2d/mrpt_icp_slam_2d_wrapper.h"
00008 #include <mrpt/version.h>
00009 #if MRPT_VERSION >= 0x150
00010 #include <mrpt_bridge/utils.h>
00011 #endif
00012
00013 #if MRPT_VERSION >= 0x199
00014 #include <mrpt/serialization/CArchive.h>
00015 #endif
00016
00017 #include <mrpt/maps/COccupancyGridMap2D.h>
00018 #include <mrpt/maps/CSimplePointsMap.h>
00019 using mrpt::maps::COccupancyGridMap2D;
00020 using mrpt::maps::CSimplePointsMap;
00021
00022 ICPslamWrapper::ICPslamWrapper() {
00023 rawlog_play_ = false;
00024 stamp = ros::Time(0);
00025
00026 SHOW_PROGRESS_3D_REAL_TIME = false;
00027 SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
00028 SHOW_LASER_SCANS_3D = true;
00029 CAMERA_3DSCENE_FOLLOWS_ROBOT = true;
00030 isObsBasedRawlog = true;
00031 }
00032 ICPslamWrapper::~ICPslamWrapper() {
00033 try {
00034 std::string sOutMap = "mrpt_icpslam_";
00035 mrpt::system::TTimeParts parts;
00036 mrpt::system::timestampToParts(now(), parts, true);
00037 sOutMap += format("%04u-%02u-%02u_%02uh%02um%02us",
00038 (unsigned int)parts.year, (unsigned int)parts.month,
00039 (unsigned int)parts.day, (unsigned int)parts.hour,
00040 (unsigned int)parts.minute, (unsigned int)parts.second);
00041 sOutMap += ".simplemap";
00042
00043 sOutMap = mrpt::system::fileNameStripInvalidChars(sOutMap);
00044 ROS_INFO("Saving built map to `%s`", sOutMap.c_str());
00045 mapBuilder.saveCurrentMapToFile(sOutMap);
00046 } catch (std::exception &e) {
00047 ROS_ERROR("Exception: %s", e.what());
00048 }
00049 }
00050 bool ICPslamWrapper::is_file_exists(const std::string &name) {
00051 std::ifstream f(name.c_str());
00052 return f.good();
00053 }
00054
00055 void ICPslamWrapper::read_iniFile(std::string ini_filename) {
00056 CConfigFile iniFile(ini_filename);
00057
00058 mapBuilder.ICP_options.loadFromConfigFile(iniFile, "MappingApplication");
00059 mapBuilder.ICP_params.loadFromConfigFile(iniFile, "ICP");
00060 mapBuilder.initialize();
00061
00062 #if MRPT_VERSION < 0x150
00063 mapBuilder.options.verbose = true;
00064 #else
00065 log4cxx::LoggerPtr ros_logger =
00066 log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00067 mapBuilder.setVerbosityLevel(
00068 mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel()));
00069 mapBuilder.logging_enable_console_output = false;
00070 #if MRPT_VERSION < 0x199
00071 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
00072 &mrpt_bridge::mrptToROSLoggerCallback_mrpt_15));
00073 #else
00074 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
00075 &mrpt_bridge::mrptToROSLoggerCallback));
00076 #endif
00077 #endif
00078 mapBuilder.options.alwaysInsertByClass.fromString(
00079 iniFile.read_string("MappingApplication", "alwaysInsertByClass", ""));
00080
00081 mapBuilder.ICP_params.dumpToConsole();
00082 mapBuilder.ICP_options.dumpToConsole();
00083
00084
00085 CAMERA_3DSCENE_FOLLOWS_ROBOT =
00086 iniFile.read_bool("MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT",
00087 true, true);
00088 MRPT_LOAD_CONFIG_VAR(SHOW_PROGRESS_3D_REAL_TIME, bool, iniFile,
00089 "MappingApplication");
00090 MRPT_LOAD_CONFIG_VAR(SHOW_LASER_SCANS_3D, bool, iniFile,
00091 "MappingApplication");
00092 MRPT_LOAD_CONFIG_VAR(SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile,
00093 "MappingApplication");
00094 }
00095
00096 void ICPslamWrapper::get_param() {
00097 ROS_INFO("READ PARAM FROM LAUNCH FILE");
00098 n_.param<double>("rawlog_play_delay", rawlog_play_delay, 0.1);
00099 ROS_INFO("rawlog_play_delay: %f", rawlog_play_delay);
00100
00101 n_.getParam("rawlog_filename", rawlog_filename);
00102 ROS_INFO("rawlog_filename: %s", rawlog_filename.c_str());
00103
00104 n_.getParam("ini_filename", ini_filename);
00105 ROS_INFO("ini_filename: %s", ini_filename.c_str());
00106
00107 n_.param<std::string>("global_frame_id", global_frame_id, "map");
00108 ROS_INFO("global_frame_id: %s", global_frame_id.c_str());
00109
00110 n_.param<std::string>("odom_frame_id", odom_frame_id, "odom");
00111 ROS_INFO("odom_frame_id: %s", odom_frame_id.c_str());
00112
00113 n_.param<std::string>("base_frame_id", base_frame_id, "base_link");
00114 ROS_INFO("base_frame_id: %s", base_frame_id.c_str());
00115
00116 n_.param<std::string>("sensor_source", sensor_source, "scan");
00117 ROS_INFO("sensor_source: %s", sensor_source.c_str());
00118
00119 n_.param("trajectory_update_rate", trajectory_update_rate, 10.0);
00120 ROS_INFO("trajectory_update_rate: %f", trajectory_update_rate);
00121
00122 n_.param("trajectory_publish_rate", trajectory_publish_rate, 5.0);
00123 ROS_INFO("trajectory_publish_rate: %f", trajectory_publish_rate);
00124 }
00125 void ICPslamWrapper::init3Dwindow() {
00126 #if MRPT_HAS_WXWIDGETS
00127 if (SHOW_PROGRESS_3D_REAL_TIME) {
00128 win3D_ = mrpt::gui::CDisplayWindow3D::Create(
00129 "pf-localization - The MRPT project", 1000, 600);
00130 win3D_->setCameraZoom(20);
00131 win3D_->setCameraAzimuthDeg(-45);
00132 }
00133
00134 #endif
00135 }
00136
00137 void ICPslamWrapper::run3Dwindow() {
00138
00139
00140 if (SHOW_PROGRESS_3D_REAL_TIME && win3D_) {
00141
00142 metric_map_ = mapBuilder.getCurrentlyBuiltMetricMap();
00143
00144 lst_current_laser_scans.clear();
00145
00146 CPose3D robotPose;
00147 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
00148 COpenGLScene::Ptr scene = COpenGLScene::Create();
00149
00150 COpenGLViewport::Ptr view = scene->getViewport("main");
00151
00152 COpenGLViewport::Ptr view_map = scene->createViewport("mini-map");
00153 view_map->setBorderSize(2);
00154 view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
00155 view_map->setTransparent(false);
00156
00157 {
00158 mrpt::opengl::CCamera &cam = view_map->getCamera();
00159 cam.setAzimuthDegrees(-90);
00160 cam.setElevationDegrees(90);
00161 cam.setPointingAt(robotPose);
00162 cam.setZoomDistance(20);
00163 cam.setOrthogonal();
00164 }
00165
00166
00167 mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
00168 mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
00169 groundPlane->setColor(0.4, 0.4, 0.4);
00170 view->insert(groundPlane);
00171 view_map->insert(CRenderizable::Ptr(groundPlane));
00172
00173
00174 if (CAMERA_3DSCENE_FOLLOWS_ROBOT) {
00175 scene->enableFollowCamera(true);
00176
00177 mrpt::opengl::CCamera &cam = view_map->getCamera();
00178 cam.setAzimuthDegrees(-45);
00179 cam.setElevationDegrees(45);
00180 cam.setPointingAt(robotPose);
00181 }
00182
00183
00184 {
00185 opengl::CSetOfObjects::Ptr obj = opengl::CSetOfObjects::Create();
00186 metric_map_->getAs3DObject(obj);
00187 view->insert(obj);
00188
00189
00190
00191
00192 CSimplePointsMap *pm = nullptr;
00193 #if MRPT_VERSION >= 0x199
00194 if (metric_map_->countMapsByClass<CSimplePointsMap>())
00195 pm = metric_map_->mapByClass<CSimplePointsMap>().get();
00196 #else
00197 if (metric_map_->m_pointsMaps.size())
00198 pm = metric_map_->m_pointsMaps[0].get();
00199 #endif
00200
00201 opengl::CSetOfObjects::Ptr ptsMap = opengl::CSetOfObjects::Create();
00202 if (pm) {
00203 pm->getAs3DObject(ptsMap);
00204 view_map->insert(ptsMap);
00205 }
00206 }
00207
00208
00209 CPose3DPDF::Ptr posePDF = mapBuilder.getCurrentPoseEstimation();
00210 CPose3D curRobotPose;
00211 posePDF->getMean(curRobotPose);
00212 {
00213 opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
00214 obj->setPose(curRobotPose);
00215 view->insert(obj);
00216 }
00217 {
00218 opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
00219 obj->setPose(curRobotPose);
00220 view_map->insert(obj);
00221 }
00222
00223 opengl::COpenGLScene::Ptr &ptrScene = win3D_->get3DSceneAndLock();
00224 ptrScene = scene;
00225
00226 win3D_->unlockAccess3DScene();
00227
00228
00229 win3D_->setCameraPointingToPoint(curRobotPose.x(), curRobotPose.y(),
00230 curRobotPose.z());
00231
00232
00233 win3D_->forceRepaint();
00234
00235
00236 if (SHOW_LASER_SCANS_3D) {
00237
00238 if (isObsBasedRawlog) {
00239 if (IS_CLASS(observation, CObservation2DRangeScan)) {
00240 lst_current_laser_scans.push_back(
00241 mrpt::ptr_cast<CObservation2DRangeScan>::from(observation));
00242 }
00243 } else {
00244
00245 for (size_t i = 0;; i++) {
00246 CObservation2DRangeScan::Ptr new_obs =
00247 observations->getObservationByClass<CObservation2DRangeScan>(i);
00248 if (!new_obs)
00249 break;
00250 else
00251 lst_current_laser_scans.push_back(new_obs);
00252 }
00253 }
00254 }
00255
00256
00257 if (SHOW_LASER_SCANS_3D) {
00258 for (size_t i = 0; i < lst_current_laser_scans.size(); i++) {
00259
00260 opengl::CPlanarLaserScan::Ptr obj = opengl::CPlanarLaserScan::Create();
00261 obj->setScan(*lst_current_laser_scans[i]);
00262 obj->setPose(curRobotPose);
00263 obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
00264
00265 view->insert(obj);
00266 }
00267 }
00268 }
00269 }
00270 void ICPslamWrapper::init() {
00271
00272 if (!is_file_exists(ini_filename)) {
00273 ROS_ERROR_STREAM("CAN'T READ INI FILE");
00274 return;
00275 }
00276 read_iniFile(ini_filename);
00277
00278 if (is_file_exists(rawlog_filename)) {
00279 ROS_WARN_STREAM("PLAY FROM RAWLOG FILE: " << rawlog_filename.c_str());
00280 rawlog_play_ = true;
00281 }
00282
00284
00285 pub_map_ = n_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00286 pub_metadata_ = n_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00287
00288 pub_point_cloud_ =
00289 n_.advertise<sensor_msgs::PointCloud>("PointCloudMap", 1, true);
00290
00291 trajectory_pub_ = n_.advertise<nav_msgs::Path>("trajectory", 1, true);
00292
00293
00294 pub_pose_ = n_.advertise<geometry_msgs::PoseStamped>("robot_pose", 1);
00295
00296 update_trajector_timer = n_.createTimer(
00297 ros::Duration(1.0 / trajectory_update_rate),
00298 &ICPslamWrapper::updateTrajectoryTimerCallback, this, false);
00299
00300 publish_trajectory_timer = n_.createTimer(
00301 ros::Duration(1.0 / trajectory_publish_rate),
00302 &ICPslamWrapper::publishTrajectoryTimerCallback, this, false);
00303
00304
00305 std::vector<std::string> lstSources;
00306 mrpt::system::tokenize(sensor_source, " ,\t\n", lstSources);
00307 ROS_ASSERT_MSG(!lstSources.empty(),
00308 "*Fatal*: At least one sensor source must be provided in "
00309 "~sensor_sources (e.g. "
00310 "\"scan\" or \"beacon\")");
00311
00313 sensorSub_.resize(lstSources.size());
00314 for (size_t i = 0; i < lstSources.size(); i++) {
00315 if (lstSources[i].find("scan") != std::string::npos) {
00316 sensorSub_[i] =
00317 n_.subscribe(lstSources[i], 1, &ICPslamWrapper::laserCallback, this);
00318 } else {
00319 std::cout << "Sensor topics should be 2d laser scans which inlude in the "
00320 "name the word scan "
00321 << "\n";
00322 }
00323 }
00324
00325 init3Dwindow();
00326 }
00327
00328 void ICPslamWrapper::laserCallback(const sensor_msgs::LaserScan &_msg) {
00329 #if MRPT_VERSION >= 0x130
00330 using namespace mrpt::maps;
00331 using namespace mrpt::obs;
00332 #else
00333 using namespace mrpt::slam;
00334 #endif
00335 CObservation2DRangeScan::Ptr laser = CObservation2DRangeScan::Create();
00336 if (laser_poses_.find(_msg.header.frame_id) == laser_poses_.end()) {
00337 updateSensorPose(_msg.header.frame_id);
00338 } else {
00339 mrpt::poses::CPose3D pose = laser_poses_[_msg.header.frame_id];
00340 mrpt_bridge::convert(_msg, laser_poses_[_msg.header.frame_id], *laser);
00341
00342 observation = CObservation::Ptr(laser);
00343 stamp = ros::Time(0);
00344 tictac.Tic();
00345 mapBuilder.processObservation(observation);
00346 t_exec = tictac.Tac();
00347 ROS_INFO("Map building executed in %.03fms", 1000.0f * t_exec);
00348
00349 run3Dwindow();
00350 publishTF();
00351 publishMapPose();
00352 }
00353 }
00354 void ICPslamWrapper::publishMapPose() {
00355
00356 metric_map_ = mapBuilder.getCurrentlyBuiltMetricMap();
00357
00358
00359 COccupancyGridMap2D *grid = nullptr;
00360 CSimplePointsMap *pm = nullptr;
00361 #if MRPT_VERSION >= 0x199
00362 if (metric_map_->countMapsByClass<COccupancyGridMap2D>())
00363 grid = metric_map_->mapByClass<COccupancyGridMap2D>().get();
00364 if (metric_map_->countMapsByClass<CSimplePointsMap>())
00365 pm = metric_map_->mapByClass<CSimplePointsMap>().get();
00366 #else
00367 if (metric_map_->m_gridMaps.size())
00368 grid = metric_map_->m_gridMaps[0].get();
00369 if (metric_map_->m_pointsMaps.size())
00370 pm = metric_map_->m_pointsMaps[0].get();
00371 #endif
00372
00373 if (grid) {
00374 nav_msgs::OccupancyGrid _msg;
00375
00376 mrpt_bridge::convert(*grid, _msg);
00377 pub_map_.publish(_msg);
00378 pub_metadata_.publish(_msg.info);
00379 }
00380 if (pm) {
00381 sensor_msgs::PointCloud _msg;
00382 std_msgs::Header header;
00383 header.stamp = ros::Time(0);
00384 header.frame_id = global_frame_id;
00385
00386 mrpt_bridge::point_cloud::mrpt2ros(*pm, header, _msg);
00387 pub_point_cloud_.publish(_msg);
00388 }
00389
00390 CPose3D robotPose;
00391 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
00392
00393
00394
00395 pose.header.frame_id = global_frame_id;
00396
00397
00398 pose.pose.position.x = robotPose.x();
00399 pose.pose.position.y = robotPose.y();
00400 pose.pose.position.z = 0.0;
00401 pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotPose.yaw());
00402
00403 pub_pose_.publish(pose);
00404 }
00405 void ICPslamWrapper::updateSensorPose(std::string _frame_id) {
00406 CPose3D pose;
00407 tf::StampedTransform transform;
00408 try {
00409 listenerTF_.lookupTransform(base_frame_id, _frame_id, ros::Time(0),
00410 transform);
00411
00412 tf::Vector3 translation = transform.getOrigin();
00413 tf::Quaternion quat = transform.getRotation();
00414 pose.x() = translation.x();
00415 pose.y() = translation.y();
00416 pose.z() = translation.z();
00417 tf::Matrix3x3 Rsrc(quat);
00418 CMatrixDouble33 Rdes;
00419 for (int c = 0; c < 3; c++)
00420 for (int r = 0; r < 3; r++)
00421 Rdes(r, c) = Rsrc.getRow(r)[c];
00422 pose.setRotationMatrix(Rdes);
00423 laser_poses_[_frame_id] = pose;
00424 } catch (tf::TransformException ex) {
00425 ROS_ERROR("%s", ex.what());
00426 ros::Duration(1.0).sleep();
00427 }
00428 }
00429 bool ICPslamWrapper::rawlogPlay() {
00430 if (rawlog_play_ == false) {
00431 return false;
00432 } else {
00433 size_t rawlogEntry = 0;
00434 #if MRPT_VERSION >= 0x199
00435 CFileGZInputStream rawlog_stream(rawlog_filename);
00436 auto rawlogFile = mrpt::serialization::archiveFrom(rawlog_stream);
00437 #else
00438 CFileGZInputStream rawlogFile(rawlog_filename);
00439 #endif
00440 CActionCollection::Ptr action;
00441
00442 for (;;) {
00443 if (ros::ok()) {
00444 if (!CRawlog::getActionObservationPairOrObservation(
00445 rawlogFile, action, observations, observation, rawlogEntry)) {
00446 break;
00447 }
00448 isObsBasedRawlog = (bool)observation;
00449
00450
00451 tictac.Tic();
00452 if (isObsBasedRawlog)
00453 mapBuilder.processObservation(observation);
00454 else
00455 mapBuilder.processActionObservation(*action, *observations);
00456 t_exec = tictac.Tac();
00457 ROS_INFO("Map building executed in %.03fms", 1000.0f * t_exec);
00458
00459 ros::Duration(rawlog_play_delay).sleep();
00460
00461 metric_map_ = mapBuilder.getCurrentlyBuiltMetricMap();
00462
00463 CPose3D robotPose;
00464 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
00465
00466
00467 COccupancyGridMap2D *grid = nullptr;
00468 CSimplePointsMap *pm = nullptr;
00469 #if MRPT_VERSION >= 0x199
00470 if (metric_map_->countMapsByClass<COccupancyGridMap2D>())
00471 grid = metric_map_->mapByClass<COccupancyGridMap2D>().get();
00472 if (metric_map_->countMapsByClass<CSimplePointsMap>())
00473 pm = metric_map_->mapByClass<CSimplePointsMap>().get();
00474 #else
00475 if (metric_map_->m_gridMaps.size())
00476 grid = metric_map_->m_gridMaps[0].get();
00477 if (metric_map_->m_pointsMaps.size())
00478 pm = metric_map_->m_pointsMaps[0].get();
00479 #endif
00480
00481 if (grid) {
00482 nav_msgs::OccupancyGrid _msg;
00483
00484
00485 mrpt_bridge::convert(*grid, _msg);
00486 pub_map_.publish(_msg);
00487 pub_metadata_.publish(_msg.info);
00488 }
00489
00490 if (pm) {
00491 sensor_msgs::PointCloud _msg;
00492 std_msgs::Header header;
00493 header.stamp = ros::Time(0);
00494 header.frame_id = global_frame_id;
00495
00496 mrpt_bridge::point_cloud::mrpt2ros(*pm, header, _msg);
00497 pub_point_cloud_.publish(_msg);
00498
00499 }
00500
00501
00502
00503 pose.header.frame_id = global_frame_id;
00504
00505
00506 pose.pose.position.x = robotPose.x();
00507 pose.pose.position.y = robotPose.y();
00508 pose.pose.position.z = 0.0;
00509 pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotPose.yaw());
00510
00511 pub_pose_.publish(pose);
00512 }
00513
00514 run3Dwindow();
00515 ros::spinOnce();
00516 }
00517
00518
00519
00520 if (win3D_)
00521 win3D_->waitForKey();
00522
00523 return true;
00524 }
00525 }
00526
00527 void ICPslamWrapper::publishTF() {
00528
00529 mrpt::poses::CPose3D robotPoseTF;
00530 mapBuilder.getCurrentPoseEstimation()->getMean(robotPoseTF);
00531
00532 stamp = ros::Time(0);
00533 tf::Stamped<tf::Pose> odom_to_map;
00534 tf::Transform tmp_tf;
00535 mrpt_bridge::convert(robotPoseTF, tmp_tf);
00536
00537 try {
00538 tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(), stamp,
00539 base_frame_id);
00540 listenerTF_.transformPose(odom_frame_id, tmp_tf_stamped, odom_to_map);
00541 } catch (tf::TransformException) {
00542 ROS_INFO("Failed to subtract global_frame (%s) from odom_frame (%s)",
00543 global_frame_id.c_str(), odom_frame_id.c_str());
00544 return;
00545 }
00546
00547 tf::Transform latest_tf_ =
00548 tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
00549 tf::Point(odom_to_map.getOrigin()));
00550
00551 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), stamp,
00552 global_frame_id, odom_frame_id);
00553
00554 tf_broadcaster_.sendTransform(tmp_tf_stamped);
00555 }
00556
00557 void ICPslamWrapper::updateTrajectoryTimerCallback(
00558 const ros::TimerEvent &event) {
00559 ROS_DEBUG("update trajectory");
00560 path.header.frame_id = global_frame_id;
00561 path.header.stamp = ros::Time(0);
00562 path.poses.push_back(pose);
00563 }
00564
00565 void ICPslamWrapper::publishTrajectoryTimerCallback(
00566 const ros::TimerEvent &event) {
00567 ROS_DEBUG("publish trajectory");
00568 trajectory_pub_.publish(path);
00569 }