00001
00002
00003
00004
00005
00006
00007 #include "mrpt_ekf_slam_3d/mrpt_ekf_slam_3d_wrapper.h"
00008
00009 EKFslamWrapper::EKFslamWrapper()
00010 {
00011 rawlog_play_ = false;
00012 mrpt_bridge::convert(ros::Time(0), timeLastUpdate_);
00013 }
00014 EKFslamWrapper::~EKFslamWrapper()
00015 {
00016 }
00017 bool EKFslamWrapper::is_file_exists(const std::string& name)
00018 {
00019 std::ifstream f(name.c_str());
00020 return f.good();
00021 }
00022
00023 void EKFslamWrapper::get_param()
00024 {
00025 ROS_INFO("READ PARAM FROM LAUNCH FILE");
00026 n_.param<double>("ellipse_scale", ellipse_scale_, 1);
00027 ROS_INFO("ellipse_scale: %f", ellipse_scale_);
00028
00029 n_.param<double>("rawlog_play_delay", rawlog_play_delay, 0.1);
00030 ROS_INFO("rawlog_play_delay: %f", rawlog_play_delay);
00031
00032 n_.getParam("rawlog_filename", rawlog_filename);
00033 ROS_INFO("rawlog_filename: %s", rawlog_filename.c_str());
00034
00035 n_.getParam("ini_filename", ini_filename);
00036 ROS_INFO("ini_filename: %s", ini_filename.c_str());
00037
00038 n_.param<std::string>("global_frame_id", global_frame_id, "map");
00039 ROS_INFO("global_frame_id: %s", global_frame_id.c_str());
00040
00041 n_.param<std::string>("odom_frame_id", odom_frame_id, "odom");
00042 ROS_INFO("odom_frame_id: %s", odom_frame_id.c_str());
00043
00044 n_.param<std::string>("base_frame_id", base_frame_id, "base_link");
00045 ROS_INFO("base_frame_id: %s", base_frame_id.c_str());
00046
00047 n_.param<std::string>("sensor_source", sensor_source, "scan");
00048 ROS_INFO("sensor_source: %s", sensor_source.c_str());
00049 }
00050 void EKFslamWrapper::init()
00051 {
00052
00053 if (!is_file_exists(ini_filename))
00054 {
00055 ROS_ERROR_STREAM("CAN'T READ INI FILE");
00056 return;
00057 }
00058
00059 EKFslam::read_iniFile(ini_filename);
00060
00061 if (is_file_exists(rawlog_filename))
00062 {
00063 ROS_WARN_STREAM("PLAY FROM RAWLOG FILE: " << rawlog_filename.c_str());
00064 rawlog_play_ = true;
00065 }
00066
00067 state_viz_pub_ = n_.advertise<visualization_msgs::MarkerArray>("/state_viz", 1);
00068 data_association_viz_pub_ =
00069 n_.advertise<visualization_msgs::MarkerArray>("/data_association_viz", 1);
00070
00071
00072 std::vector<std::string> lstSources;
00073 mrpt::system::tokenize(sensor_source, " ,\t\n", lstSources);
00074 ROS_ASSERT_MSG(!lstSources.empty(), "*Fatal*: At least one sensor source must be provided in ~sensor_sources (e.g. "
00075 "\"scan\" or \"beacon\")");
00076
00078 sensorSub_.resize(lstSources.size());
00079 for (size_t i = 0; i < lstSources.size(); i++)
00080 {
00081 if (lstSources[i].find("landmark") != std::string::npos)
00082 {
00083 sensorSub_[i] = n_.subscribe(lstSources[i], 1, &EKFslamWrapper::landmarkCallback, this);
00084 }
00085 else
00086 {
00087 ROS_ERROR("Can't find the sensor topics. The sensor topics should contain the word \"landmark\" in the name");
00088 }
00089 }
00090
00091 init3Dwindow();
00092 }
00093
00094 void EKFslamWrapper::odometryForCallback(CObservationOdometryPtr& _odometry, const std_msgs::Header& _msg_header)
00095 {
00096 mrpt::poses::CPose3D poseOdom;
00097 if (this->waitForTransform(poseOdom, odom_frame_id, base_frame_id, _msg_header.stamp, ros::Duration(1)))
00098 {
00099 _odometry = CObservationOdometry::Create();
00100 _odometry->sensorLabel = odom_frame_id;
00101 _odometry->hasEncodersInfo = false;
00102 _odometry->hasVelocities = false;
00103 _odometry->odometry.x() = poseOdom.x();
00104 _odometry->odometry.y() = poseOdom.y();
00105 _odometry->odometry.phi() = poseOdom.yaw();
00106 }
00107 }
00108
00109 void EKFslamWrapper::updateSensorPose(std::string _frame_id)
00110 {
00111 CPose3D pose;
00112 tf::StampedTransform transform;
00113 try
00114 {
00115 listenerTF_.lookupTransform(base_frame_id, _frame_id, ros::Time(0), transform);
00116
00117 tf::Vector3 translation = transform.getOrigin();
00118 tf::Quaternion quat = transform.getRotation();
00119 pose.x() = translation.x();
00120 pose.y() = translation.y();
00121 pose.z() = translation.z();
00122 double roll, pitch, yaw;
00123 tf::Matrix3x3 Rsrc(quat);
00124 CMatrixDouble33 Rdes;
00125 for (int c = 0; c < 3; c++)
00126 for (int r = 0; r < 3; r++)
00127 Rdes(r, c) = Rsrc.getRow(r)[c];
00128 pose.setRotationMatrix(Rdes);
00129 landmark_poses_[_frame_id] = pose;
00130 }
00131 catch (tf::TransformException ex)
00132 {
00133 ROS_ERROR("%s", ex.what());
00134 ros::Duration(1.0).sleep();
00135 }
00136 }
00137
00138 bool EKFslamWrapper::waitForTransform(mrpt::poses::CPose3D& des, const std::string& target_frame,
00139 const std::string& source_frame, const ros::Time& time,
00140 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration)
00141 {
00142 tf::StampedTransform transform;
00143 try
00144 {
00145 listenerTF_.waitForTransform(target_frame, source_frame, time, polling_sleep_duration);
00146 listenerTF_.lookupTransform(target_frame, source_frame, time, transform);
00147 }
00148 catch (tf::TransformException)
00149 {
00150 ROS_INFO("Failed to get transform target_frame (%s) to source_frame (%s)", target_frame.c_str(),
00151 source_frame.c_str());
00152 return false;
00153 }
00154 mrpt_bridge::convert(transform, des);
00155 return true;
00156 }
00157
00158 void EKFslamWrapper::landmarkCallback(const mrpt_msgs::ObservationRangeBearing& _msg)
00159 {
00160 #if MRPT_VERSION >= 0x130
00161 using namespace mrpt::maps;
00162 using namespace mrpt::obs;
00163 #else
00164 using namespace mrpt::slam;
00165 #endif
00166 CObservationBearingRangePtr landmark = CObservationBearingRange::Create();
00167
00168 if (landmark_poses_.find(_msg.header.frame_id) == landmark_poses_.end())
00169 {
00170 updateSensorPose(_msg.header.frame_id);
00171 }
00172 else
00173 {
00174 mrpt::poses::CPose3D pose = landmark_poses_[_msg.header.frame_id];
00175 mrpt_bridge::convert(_msg, landmark_poses_[_msg.header.frame_id], *landmark);
00176
00177 sf = CSensoryFrame::Create();
00178 CObservationOdometryPtr odometry;
00179 odometryForCallback(odometry, _msg.header);
00180
00181 CObservationPtr obs = CObservationPtr(landmark);
00182 sf->insert(obs);
00183 observation(sf, odometry);
00184 timeLastUpdate_ = sf->getObservationByIndex(0)->timestamp;
00185
00186 tictac.Tic();
00187 mapping.processActionObservation(action, sf);
00188 t_exec = tictac.Tac();
00189 ROS_INFO("Map building executed in %.03fms", 1000.0f * t_exec);
00190 ros::Duration(rawlog_play_delay).sleep();
00191 mapping.getCurrentState(robotPose_, LMs_, LM_IDs_, fullState_, fullCov_);
00192 viz_state();
00193 viz_dataAssociation();
00194 run3Dwindow();
00195 publishTF();
00196 }
00197 }
00198
00199 bool EKFslamWrapper::rawlogPlay()
00200 {
00201 if (rawlog_play_ == false)
00202 {
00203 return false;
00204 }
00205 else
00206 {
00207 size_t rawlogEntry = 0;
00208 CFileGZInputStream rawlogFile(rawlog_filename);
00209 CActionCollectionPtr action;
00210 CSensoryFramePtr observations;
00211
00212 for (;;)
00213 {
00214 if (ros::ok())
00215 {
00216 if (!CRawlog::readActionObservationPair(rawlogFile, action, observations, rawlogEntry))
00217 {
00218 break;
00219 }
00220 tictac.Tic();
00221 mapping.processActionObservation(action, observations);
00222 t_exec = tictac.Tac();
00223 ROS_INFO("Map building executed in %.03fms", 1000.0f * t_exec);
00224 ros::Duration(rawlog_play_delay).sleep();
00225 mapping.getCurrentState(robotPose_, LMs_, LM_IDs_, fullState_, fullCov_);
00226
00227 viz_state();
00228 viz_dataAssociation();
00229 run3Dwindow();
00230 }
00231 }
00232 if (win3d)
00233 {
00234 cout << "\n Close the 3D window to quit the application.\n";
00235 win3d->waitForKey();
00236 }
00237 return true;
00238 }
00239 }
00240
00241
00242 void EKFslamWrapper::makeRightHanded(Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues)
00243 {
00244
00245
00246 Eigen::Vector3d c0 = eigenvectors.block<3, 1>(0, 0);
00247 c0.normalize();
00248 Eigen::Vector3d c1 = eigenvectors.block<3, 1>(0, 1);
00249 c1.normalize();
00250 Eigen::Vector3d c2 = eigenvectors.block<3, 1>(0, 2);
00251 c2.normalize();
00252 Eigen::Vector3d cc = c0.cross(c1);
00253 if (cc.dot(c2) < 0)
00254 {
00255 eigenvectors << c1, c0, c2;
00256 double e = eigenvalues[0];
00257 eigenvalues[0] = eigenvalues[1];
00258 eigenvalues[1] = e;
00259 }
00260 else
00261 {
00262 eigenvectors << c0, c1, c2;
00263 }
00264 }
00265
00266 void EKFslamWrapper::computeEllipseOrientationScale(tf::Quaternion& orientation, Eigen::Vector3d& scale,
00267 const mrpt::math::CMatrixDouble covariance)
00268 {
00269
00270 tf::Matrix3x3 tf3d;
00271 Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity());
00272 Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero());
00273
00274
00275 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance);
00276
00277 if (eigensolver.info() == Eigen::Success)
00278 {
00279 eigenvalues = eigensolver.eigenvalues();
00280 eigenvectors = eigensolver.eigenvectors();
00281 }
00282 else
00283 {
00284 ROS_ERROR_STREAM("failed to compute eigen vectors/values for position. Is the covariance matrix correct?");
00285 eigenvalues = Eigen::Vector3d::Zero();
00286 eigenvectors = Eigen::Matrix3d::Identity();
00287 }
00288
00289
00290 makeRightHanded(eigenvectors, eigenvalues);
00291
00292
00293 tf3d.setValue(eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), eigenvectors(1, 0), eigenvectors(1, 1),
00294 eigenvectors(1, 2), eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2));
00295
00296
00297 tf3d.getRotation(orientation);
00298
00299 scale[0] = eigenvalues[0];
00300 scale[1] = eigenvalues[1];
00301 scale[2] = eigenvalues[2];
00302 }
00303 void EKFslamWrapper::viz_dataAssociation()
00304 {
00305
00306 mrpt::poses::CPose3D robotPose;
00307 robotPose = CPose3D(robotPose_.mean);
00308 geometry_msgs::Point pointRobotPose;
00309 pointRobotPose.z = robotPose.z();
00310 pointRobotPose.x = robotPose.x();
00311 pointRobotPose.y = robotPose.y();
00312
00313
00314 visualization_msgs::MarkerArray ma;
00315 visualization_msgs::Marker line_strip;
00316
00317 line_strip.header.frame_id = "/map";
00318 line_strip.header.stamp = ros::Time::now();
00319
00320 line_strip.id = 0;
00321 line_strip.type = visualization_msgs::Marker::LINE_STRIP;
00322 line_strip.action = visualization_msgs::Marker::ADD;
00323
00324 line_strip.lifetime = ros::Duration(0.1);
00325 line_strip.pose.position.x = 0;
00326 line_strip.pose.position.y = 0;
00327 line_strip.pose.position.z = 0;
00328 line_strip.pose.orientation.x = 0.0;
00329 line_strip.pose.orientation.y = 0.0;
00330 line_strip.pose.orientation.z = 0.0;
00331 line_strip.pose.orientation.w = 1.0;
00332 line_strip.scale.x = 0.02;
00333 line_strip.scale.y = 0.0;
00334 line_strip.scale.z = 0.0;
00335 line_strip.color.a = 1.0;
00336 line_strip.color.r = 1.0;
00337 line_strip.color.g = 1.0;
00338 line_strip.color.b = 1.0;
00339
00340
00341 const CRangeBearingKFSLAM::TDataAssocInfo& da = mapping.getLastDataAssociation();
00342
00343 for (auto it = da.results.associations.begin(); it != da.results.associations.end(); ++it)
00344 {
00345 const prediction_index_t idxPred = it->second;
00346
00347 CRangeBearingKFSLAM::KFArray_FEAT featMean;
00348 mapping.getLandmarkMean(idxPred, featMean);
00349
00350 line_strip.points.clear();
00351 line_strip.points.push_back(pointRobotPose);
00352 geometry_msgs::Point pointLm;
00353 pointLm.z = featMean[2];
00354 pointLm.x = featMean[0];
00355 pointLm.y = featMean[1];
00356 line_strip.points.push_back(pointLm);
00357 ma.markers.push_back(line_strip);
00358 line_strip.id++;
00359 }
00360
00361 data_association_viz_pub_.publish(ma);
00362 }
00363
00364 void EKFslamWrapper::viz_state()
00365 {
00366 visualization_msgs::MarkerArray ma;
00367 visualization_msgs::Marker marker;
00368 marker.header.frame_id = "/map";
00369 marker.id = 0;
00370 marker.type = visualization_msgs::Marker::SPHERE;
00371 marker.action = visualization_msgs::Marker::ADD;
00372 marker.lifetime = ros::Duration(0);
00373
00374
00375 mrpt::opengl::CSetOfObjectsPtr objs;
00376 objs = mrpt::opengl::CSetOfObjects::Create();
00377 mapping.getAs3DObject(objs);
00378
00379
00380 int objs_counter = 0;
00381 while (objs->getByClass<mrpt::opengl::CEllipsoid>(objs_counter))
00382 {
00383 objs_counter++;
00384 }
00385
00386 mrpt::opengl::CEllipsoidPtr landmark;
00387
00388 for (size_t i = 0; i < objs_counter; i++)
00389 {
00390 landmark = objs->getByClass<mrpt::opengl::CEllipsoid>(i);
00391
00392 float quantiles = landmark->getQuantiles();
00393 mrpt::math::CMatrixDouble covariance = landmark->getCovMatrix();
00394
00395
00396 CPose3D pose = mrpt::poses::CPose3D(landmark->getPose());
00397
00398
00399 Eigen::Vector3d scale;
00400 tf::Quaternion orientation;
00401 computeEllipseOrientationScale(orientation, scale, covariance);
00402
00403 marker.id++;
00404 marker.color.a = 1.0;
00405 if (i == 0)
00406 {
00407 marker.color.r = 1.0;
00408 marker.color.g = 0.0;
00409 marker.color.b = 0.0;
00410 }
00411 else
00412 {
00413 marker.color.r = 0.0;
00414 marker.color.g = 0.0;
00415 marker.color.b = 1.0;
00416 }
00417 marker.type = visualization_msgs::Marker::SPHERE;
00418 marker.pose.position.x = pose.x();
00419 marker.pose.position.y = pose.y();
00420 marker.pose.position.z = pose.z();
00421 marker.pose.orientation.x = orientation.x();
00422 marker.pose.orientation.y = orientation.y();
00423 marker.pose.orientation.z = orientation.z();
00424 marker.pose.orientation.w = orientation.w();
00425 marker.scale.x = ellipse_scale_ * quantiles * std::sqrt(scale[0]);
00426 marker.scale.y = ellipse_scale_ * quantiles * std::sqrt(scale[1]);
00427 marker.scale.z = ellipse_scale_ * quantiles * std::sqrt(scale[2]);
00428 ma.markers.push_back(marker);
00429
00430 marker.id++;
00431 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00432 if (i == 0)
00433 {
00434 marker.text = "robot";
00435 }
00436 else
00437 {
00438 marker.text = std::to_string(LM_IDs_[i]);
00439 }
00440 marker.pose.position.x = pose.x();
00441 marker.pose.position.y = pose.y();
00442 marker.pose.position.z = pose.z() + marker.scale.z + 0.1;
00443 marker.color.r = 1.0;
00444 marker.color.g = 1.0;
00445 marker.color.b = 1.0;
00446 marker.scale.x = 0.3;
00447 marker.scale.y = 0.3;
00448 marker.scale.z = 0.3;
00449 ma.markers.push_back(marker);
00450 }
00451
00452 state_viz_pub_.publish(ma);
00453 }
00454
00455 void EKFslamWrapper::publishTF()
00456 {
00457 mapping.getCurrentState(robotPose_, LMs_, LM_IDs_, fullState_, fullCov_);
00458
00459 mrpt::poses::CPose3D robotPose;
00460
00461 robotPose = CPose3D(robotPose_.mean);
00462
00463
00464
00465
00466 tf::Stamped<tf::Pose> odom_to_map;
00467 tf::Transform tmp_tf;
00468 ros::Time stamp;
00469 mrpt_bridge::convert(timeLastUpdate_, stamp);
00470 mrpt_bridge::convert(robotPose, tmp_tf);
00471
00472 try
00473 {
00474 tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(), stamp, base_frame_id);
00475 listenerTF_.transformPose(odom_frame_id, tmp_tf_stamped, odom_to_map);
00476 }
00477 catch (tf::TransformException)
00478 {
00479 ROS_INFO("Failed to subtract global_frame (%s) from odom_frame (%s)", global_frame_id.c_str(),
00480 odom_frame_id.c_str());
00481 return;
00482 }
00483
00484 tf::Transform latest_tf_ =
00485 tf::Transform(tf::Quaternion(odom_to_map.getRotation()), tf::Point(odom_to_map.getOrigin()));
00486
00487
00488
00489
00490 ros::Duration transform_tolerance_(0.1);
00491 ros::Time transform_expiration = (stamp + transform_tolerance_);
00492 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), transform_expiration, global_frame_id, odom_frame_id);
00493 tf_broadcaster_.sendTransform(tmp_tf_stamped);
00494 }