9 #include <mrpt/version.h> 17 std::ifstream
f(name.c_str());
22 ROS_INFO(
"READ PARAM FROM LAUNCH FILE");
24 ROS_INFO(
"ellipse_scale: %f", ellipse_scale_);
27 ROS_INFO(
"rawlog_play_delay: %f", rawlog_play_delay);
36 ROS_INFO(
"global_frame_id: %s", global_frame_id.c_str());
39 ROS_INFO(
"odom_frame_id: %s", odom_frame_id.c_str());
42 ROS_INFO(
"base_frame_id: %s", base_frame_id.c_str());
45 ROS_INFO(
"sensor_source: %s", sensor_source.c_str());
63 n_.
advertise<visualization_msgs::MarkerArray>(
"/state_viz", 1);
65 "/data_association_viz", 1);
68 std::vector<std::string> lstSources;
71 "*Fatal*: At least one sensor source must be provided in " 72 "~sensor_sources (e.g. " 73 "\"scan\" or \"beacon\")");
77 for (
size_t i = 0; i < lstSources.size(); i++) {
78 if (lstSources[i].find(
"landmark") != std::string::npos) {
82 ROS_ERROR(
"Can't find the sensor topics. The sensor topics should " 83 "contain the word \"landmark\" in the name");
91 const std_msgs::Header &_msg_header) {
92 mrpt::poses::CPose3D poseOdom;
95 _odometry = CObservationOdometry::Create();
97 _odometry->hasEncodersInfo =
false;
98 _odometry->hasVelocities =
false;
99 _odometry->odometry.x() = poseOdom.x();
100 _odometry->odometry.y() = poseOdom.y();
101 _odometry->odometry.phi() = poseOdom.yaw();
114 pose.x() = translation.
x();
115 pose.y() = translation.
y();
116 pose.z() = translation.
z();
118 CMatrixDouble33 Rdes;
119 for (
int c = 0; c < 3; c++)
120 for (
int r = 0;
r < 3;
r++)
122 pose.setRotationMatrix(Rdes);
131 mrpt::poses::CPose3D &des,
const std::string &target_frame,
132 const std::string &source_frame,
const ros::Time &time,
137 polling_sleep_duration);
140 ROS_INFO(
"Failed to get transform target_frame (%s) to source_frame (%s)",
141 target_frame.c_str(), source_frame.c_str());
144 mrpt_bridge::convert(transform, des);
149 const mrpt_msgs::ObservationRangeBearing &_msg) {
150 #if MRPT_VERSION >= 0x130 151 using namespace mrpt::maps;
152 using namespace mrpt::obs;
156 CObservationBearingRange::Ptr landmark = CObservationBearingRange::Create();
165 sf = CSensoryFrame::Create();
166 CObservationOdometry::Ptr odometry;
169 CObservation::Ptr obs = CObservation::Ptr(landmark);
191 size_t rawlogEntry = 0;
192 #if MRPT_VERSION >= 0x199 193 #include <mrpt/serialization/CArchive.h> 195 auto rawlogFile = mrpt::serialization::archiveFrom(rawlog_stream);
199 CActionCollection::Ptr
action;
200 CSensoryFrame::Ptr observations;
204 if (!CRawlog::readActionObservationPair(rawlogFile, action,
205 observations, rawlogEntry)) {
209 mapping.processActionObservation(action, observations);
222 cout <<
"\n Close the 3D window to quit the application.\n";
232 Eigen::Vector2d &eigenvalues) {
238 c0.head<2>() = eigenvectors.col(0);
242 c1.head<2>() = eigenvectors.col(1);
244 Eigen::Vector3d cc = c0.cross(c1);
246 eigenvectors << c1.head<2>(), c0.head<2>();
247 double e = eigenvalues[0];
248 eigenvalues[0] = eigenvalues[1];
251 eigenvectors << c0.head<2>(), c1.head<2>();
257 const mrpt::math::CMatrixDouble covariance) {
259 Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity());
260 Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero());
264 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(
265 #
if MRPT_VERSION >= 0x199
273 if (eigensolver.info() == Eigen::Success) {
274 eigenvalues = eigensolver.eigenvalues();
275 eigenvectors = eigensolver.eigenvectors();
278 "the covariance matrix correct?");
279 eigenvalues = Eigen::Vector2d::Zero();
281 eigenvectors = Eigen::Matrix2d::Identity();
288 tf3d.
setValue(eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0),
289 eigenvectors(1, 1), 0, 0, 0, 1);
294 scale[0] = eigenvalues[0];
295 scale[1] = eigenvalues[1];
300 mrpt::poses::CPose3D robotPose;
302 geometry_msgs::Point pointRobotPose;
303 pointRobotPose.z = 0;
304 pointRobotPose.x = robotPose.x();
305 pointRobotPose.y = robotPose.y();
308 visualization_msgs::MarkerArray ma;
309 visualization_msgs::Marker line_strip;
311 line_strip.header.frame_id =
"/map";
315 line_strip.type = visualization_msgs::Marker::LINE_STRIP;
316 line_strip.action = visualization_msgs::Marker::ADD;
319 line_strip.pose.position.x = 0;
320 line_strip.pose.position.y = 0;
321 line_strip.pose.position.z = 0;
322 line_strip.pose.orientation.x = 0.0;
323 line_strip.pose.orientation.y = 0.0;
324 line_strip.pose.orientation.z = 0.0;
325 line_strip.pose.orientation.w = 1.0;
326 line_strip.scale.x = 0.02;
327 line_strip.scale.y = 0.0;
328 line_strip.scale.z = 0.0;
329 line_strip.color.a = 1.0;
330 line_strip.color.r = 1.0;
331 line_strip.color.g = 1.0;
332 line_strip.color.b = 1.0;
335 const CRangeBearingKFSLAM2D::TDataAssocInfo &da =
336 mapping.getLastDataAssociation();
338 for (
auto it = da.results.associations.begin();
339 it != da.results.associations.end(); ++it) {
340 const prediction_index_t idxPred = it->second;
342 CRangeBearingKFSLAM2D::KFArray_FEAT featMean;
343 mapping.getLandmarkMean(idxPred, featMean);
345 line_strip.points.clear();
346 line_strip.points.push_back(pointRobotPose);
347 geometry_msgs::Point pointLm;
349 pointLm.x = featMean[0];
350 pointLm.y = featMean[1];
351 line_strip.points.push_back(pointLm);
352 ma.markers.push_back(line_strip);
359 visualization_msgs::MarkerArray ma;
360 visualization_msgs::Marker marker;
361 marker.header.frame_id =
"/map";
363 marker.type = visualization_msgs::Marker::SPHERE;
364 marker.action = visualization_msgs::Marker::ADD;
368 mrpt::opengl::CSetOfObjects::Ptr objs;
369 objs = mrpt::opengl::CSetOfObjects::Create();
373 unsigned int objs_counter = 0;
374 while (objs->getByClass<mrpt::opengl::CEllipsoid>(objs_counter)) {
378 mrpt::opengl::CEllipsoid::Ptr landmark;
379 for (
size_t i = 0; i < objs_counter; i++) {
380 landmark = objs->getByClass<mrpt::opengl::CEllipsoid>(i);
382 mrpt::math::CMatrixDouble covariance = landmark->getCovMatrix();
383 float quantiles = landmark->getQuantiles();
386 CPose3D pose = mrpt::poses::CPose3D(
387 landmark->getPose());
391 Eigen::Vector2d scale;
396 marker.color.a = 1.0;
398 marker.color.r = 1.0;
399 marker.color.g = 0.0;
400 marker.color.b = 0.0;
402 marker.color.r = 0.0;
403 marker.color.g = 0.0;
404 marker.color.b = 1.0;
406 marker.type = visualization_msgs::Marker::SPHERE;
407 marker.pose.position.x = pose.x();
408 marker.pose.position.y = pose.y();
409 marker.pose.position.z = 0;
410 marker.pose.orientation.x = orientation.x();
411 marker.pose.orientation.y = orientation.y();
412 marker.pose.orientation.z = orientation.z();
413 marker.pose.orientation.w = orientation.w();
416 marker.scale.z = 0.00001;
417 ma.markers.push_back(marker);
420 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
422 marker.text =
"robot";
424 marker.text = std::to_string(
LM_IDs_[i]);
426 marker.pose.position.x = pose.x();
427 marker.pose.position.y = pose.y();
428 marker.pose.position.z = 0.1;
429 marker.color.r = 1.0;
430 marker.color.g = 1.0;
431 marker.color.b = 1.0;
432 marker.scale.x = 0.3;
433 marker.scale.y = 0.3;
434 marker.scale.z = 0.3;
435 ma.markers.push_back(marker);
444 mrpt::poses::CPose3D robotPose;
451 mrpt_bridge::convert(robotPose, tmp_tf);
458 ROS_INFO(
"Failed to subtract global_frame (%s) from odom_frame (%s)",
470 ros::Time transform_expiration = (stamp + transform_tolerance_);
EKFslamWrapper()
constructor
float t_exec
the time which take one SLAM update execution
void viz_dataAssociation()
visualize the data associations for the landmarks observed by robot at the each step ...
CTicTac tictac
timer for SLAM performance evaluation
void updateSensorPose(std::string _frame_id)
update the pose of the sensor with respect to the robot
void publish(const boost::shared_ptr< M > &message) const
void odometryForCallback(CObservationOdometry::Ptr &_odometry, const std_msgs::Header &_msg_header)
get the odometry for received observation
std::string odom_frame_id
/odom frame
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Publisher data_association_viz_pub_
CVectorDouble fullState_
full state vector
double rawlog_play_delay
delay of replay from rawlog file
bool rawlogPlay()
play rawlog file
CMatrixDouble fullCov_
full covariance matrix
std::string global_frame_id
/map frame
std::string base_frame_id
robot frame
CPosePDFGaussian robotPose_
current robot pose
void setValue(const tfScalar &xx, const tfScalar &xy, const tfScalar &xz, const tfScalar &yx, const tfScalar &yy, const tfScalar &yz, const tfScalar &zx, const tfScalar &zy, const tfScalar &zz)
void run3Dwindow()
run 3D window update from mrpt lib
void getRotation(Quaternion &q) const
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
ros::NodeHandle n_
Node handler.
void landmarkCallback(const mrpt_msgs::ObservationRangeBearing &_msg)
callback function for the landmarks
bool is_file_exists(const std::string &name)
check the existance of the file
std::vector< mrpt::math::TPoint2D > LMs_
vector of the landmarks
TFSIMD_FORCE_INLINE const tfScalar & x() const
void init3Dwindow()
init 3D window from mrpt lib
void get_param()
read the parameters from launch file
bool rawlog_play_
true if rawlog file exists
TFSIMD_FORCE_INLINE const tfScalar & z() const
std::map< unsigned int, CLandmark::TLandmarkID > LM_IDs_
vector of the landmarks ID
~EKFslamWrapper()
destructor
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
tf::TransformListener listenerTF_
transform listener
#define ROS_ASSERT_MSG(cond,...)
TFSIMD_FORCE_INLINE const tfScalar & y() const
CRangeBearingKFSLAM2D mapping
EKF slam 2d class.
std::string sensor_source
2D laser scans
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
mrpt::system::TTimeStamp timeLastUpdate_
last update of the pose and map
void makeRightHanded(Eigen::Matrix2d &eigenvectors, Eigen::Vector2d &eigenvalues)
compute the correct orientation and scale of covariance ellipsoids (make sure that we output covarian...
#define ROS_WARN_STREAM(args)
std::string rawlog_filename
name of rawlog file
void init()
initialize publishers subscribers and EKF 2d slam
bool waitForTransform(mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
wait for transform between odometry frame and the robot frame
void publishTF()
publish tf tree
double ellipse_scale_
Scale of covariance ellipses.
std::string ini_filename
name of ini file
void viz_state()
visualize the covariance ellipsoids for robot and landmarks
bool getParam(const std::string &key, std::string &s) const
void computeEllipseOrientationScale2D(tf::Quaternion &orientation, Eigen::Vector2d &scale, const mrpt::math::CMatrixDouble covariance)
compute the orientation and scale of covariance ellipsoids
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
calculate the actions from odometry model for current observation
#define ROS_ERROR_STREAM(args)
mrpt::gui::CDisplayWindow3D::Ptr win3d
MRPT window.
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
std::map< std::string, mrpt::poses::CPose3D > landmark_poses_
landmark poses with respect to the map
ros::Publisher state_viz_pub_
publishers
void read_iniFile(std::string ini_filename)
read ini file
CActionCollection::Ptr action
actions
CSensoryFrame::Ptr sf
observations