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");
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();
119 for (
int c = 0;
c < 3;
c++)
120 for (
int r = 0;
r < 3;
r++)
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());
150 #if MRPT_VERSION >= 0x130 156 CObservationBearingRange::Ptr landmark = CObservationBearingRange::Create();
165 sf = CSensoryFrame::Create();
166 CObservationOdometry::Ptr odometry;
169 CObservation::Ptr
obs = CObservation::Ptr(landmark);
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>();
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];
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;
336 mapping.getLastDataAssociation();
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;
378 mrpt::opengl::CEllipsoid::Ptr landmark;
379 for (
size_t i = 0; i < objs_counter; i++) {
383 float quantiles = landmark->getQuantiles();
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);
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
GLenum GLenum GLenum GLenum GLenum scale
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
GLuint GLenum GLenum transform
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
GLuint const GLchar * name
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
GLenum GLenum GLenum GLenum mapping
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
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
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
const_iterator find(const KEY &key) const
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
GLdouble GLdouble GLdouble r
std::map< observation_index_t, prediction_index_t > associations
bool getParam(const std::string &key, std::string &s) const
size_t prediction_index_t
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.
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
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
TDataAssociationResults results
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f