9 #include <mrpt/version.h> 10 #if MRPT_VERSION >= 0x199 11 #include <mrpt/serialization/CArchive.h> 20 std::ifstream
f(name.c_str());
25 ROS_INFO(
"READ PARAM FROM LAUNCH FILE");
27 ROS_INFO(
"ellipse_scale: %f", ellipse_scale_);
30 ROS_INFO(
"rawlog_play_delay: %f", rawlog_play_delay);
39 ROS_INFO(
"global_frame_id: %s", global_frame_id.c_str());
42 ROS_INFO(
"odom_frame_id: %s", odom_frame_id.c_str());
45 ROS_INFO(
"base_frame_id: %s", base_frame_id.c_str());
48 ROS_INFO(
"sensor_source: %s", sensor_source.c_str());
65 n_.
advertise<visualization_msgs::MarkerArray>(
"/state_viz", 1);
67 "/data_association_viz", 1);
70 std::vector<std::string> lstSources;
73 "*Fatal*: At least one sensor source must be provided in " 74 "~sensor_sources (e.g. " 75 "\"scan\" or \"beacon\")");
79 for (
size_t i = 0; i < lstSources.size(); i++) {
80 if (lstSources[i].
find(
"landmark") != std::string::npos) {
84 ROS_ERROR(
"Can't find the sensor topics. The sensor topics should " 85 "contain the word \"landmark\" in the name");
97 _odometry = CObservationOdometry::Create();
99 _odometry->hasEncodersInfo =
false;
100 _odometry->hasVelocities =
false;
101 _odometry->odometry.x() = poseOdom.
x();
102 _odometry->odometry.y() = poseOdom.
y();
103 _odometry->odometry.phi() = poseOdom.
yaw();
116 pose.
x() = translation.
x();
117 pose.
y() = translation.
y();
118 pose.z() = translation.
z();
121 for (
int c = 0;
c < 3;
c++)
122 for (
int r = 0;
r < 3;
r++)
134 const std::string &source_frame,
const ros::Time &time,
139 polling_sleep_duration);
142 ROS_INFO(
"Failed to get transform target_frame (%s) to source_frame (%s)",
143 target_frame.c_str(), source_frame.c_str());
152 #if MRPT_VERSION >= 0x130 158 CObservationBearingRange::Ptr landmark = CObservationBearingRange::Create();
167 sf = CSensoryFrame::Create();
168 CObservationOdometry::Ptr odometry;
171 CObservation::Ptr
obs = CObservation::Ptr(landmark);
194 #if MRPT_VERSION >= 0x199 196 auto rawlogFile = mrpt::serialization::archiveFrom(f);
201 CActionCollection::Ptr
action;
202 CSensoryFrame::Ptr observations;
206 if (!CRawlog::readActionObservationPair(rawlogFile, action,
207 observations, rawlogEntry)) {
211 mapping.processActionObservation(action, observations);
224 cout <<
"\n Close the 3D window to quit the application.\n";
234 Eigen::Vector3d &eigenvalues) {
238 Eigen::Vector3d c0 = eigenvectors.block<3, 1>(0, 0);
240 Eigen::Vector3d c1 = eigenvectors.block<3, 1>(0, 1);
242 Eigen::Vector3d c2 = eigenvectors.block<3, 1>(0, 2);
244 Eigen::Vector3d cc = c0.cross(c1);
245 if (cc.dot(c2) < 0) {
246 eigenvectors << c1, c0, c2;
247 double e = eigenvalues[0];
248 eigenvalues[0] = eigenvalues[1];
251 eigenvectors << c0, c1, c2;
260 Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity());
261 Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero());
264 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> 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::Vector3d::Zero();
281 eigenvectors = Eigen::Matrix3d::Identity();
288 tf3d.
setValue(eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2),
289 eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2),
290 eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2));
295 scale[0] = eigenvalues[0];
296 scale[1] = eigenvalues[1];
297 scale[2] = eigenvalues[2];
303 geometry_msgs::Point pointRobotPose;
304 pointRobotPose.z = robotPose.z();
305 pointRobotPose.
x = robotPose.
x();
306 pointRobotPose.y = robotPose.
y();
309 visualization_msgs::MarkerArray ma;
310 visualization_msgs::Marker line_strip;
312 line_strip.header.frame_id =
"/map";
316 line_strip.type = visualization_msgs::Marker::LINE_STRIP;
317 line_strip.action = visualization_msgs::Marker::ADD;
320 line_strip.pose.position.x = 0;
321 line_strip.pose.position.y = 0;
322 line_strip.pose.position.z = 0;
323 line_strip.pose.orientation.x = 0.0;
324 line_strip.pose.orientation.y = 0.0;
325 line_strip.pose.orientation.z = 0.0;
326 line_strip.pose.orientation.w = 1.0;
327 line_strip.scale.x = 0.02;
328 line_strip.scale.y = 0.0;
329 line_strip.scale.z = 0.0;
330 line_strip.color.a = 1.0;
331 line_strip.color.r = 1.0;
332 line_strip.color.g = 1.0;
333 line_strip.color.b = 1.0;
337 mapping.getLastDataAssociation();
343 CRangeBearingKFSLAM::KFArray_FEAT featMean;
344 mapping.getLandmarkMean(idxPred, featMean);
346 line_strip.points.
clear();
347 line_strip.points.push_back(pointRobotPose);
348 geometry_msgs::Point pointLm;
349 pointLm.z = featMean[2];
350 pointLm.x = featMean[0];
351 pointLm.y = featMean[1];
352 line_strip.points.push_back(pointLm);
353 ma.markers.push_back(line_strip);
361 visualization_msgs::MarkerArray ma;
362 visualization_msgs::Marker
marker;
363 marker.header.frame_id =
"/map";
365 marker.type = visualization_msgs::Marker::SPHERE;
366 marker.action = visualization_msgs::Marker::ADD;
370 mrpt::opengl::CSetOfObjects::Ptr objs;
371 objs = mrpt::opengl::CSetOfObjects::Create();
375 unsigned int objs_counter = 0;
380 mrpt::opengl::CEllipsoid::Ptr landmark;
382 for (
size_t i = 0; i < objs_counter; i++) {
395 Eigen::Vector3d
scale;
400 marker.color.a = 1.0;
402 marker.color.r = 1.0;
403 marker.color.g = 0.0;
404 marker.color.b = 0.0;
406 marker.color.r = 0.0;
407 marker.color.g = 0.0;
408 marker.color.b = 1.0;
410 marker.type = visualization_msgs::Marker::SPHERE;
411 marker.pose.position.x = pose.
x();
412 marker.pose.position.y = pose.
y();
413 marker.pose.position.z = pose.z();
414 marker.pose.orientation.
x = orientation.x();
415 marker.pose.orientation.y = orientation.y();
416 marker.pose.orientation.z = orientation.z();
417 marker.pose.orientation.w = orientation.w();
418 marker.scale.x =
ellipse_scale_ * quantiles * std::sqrt(scale[0]);
419 marker.scale.y =
ellipse_scale_ * quantiles * std::sqrt(scale[1]);
420 marker.scale.z =
ellipse_scale_ * quantiles * std::sqrt(scale[2]);
421 ma.markers.push_back(marker);
424 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
426 marker.text =
"robot";
428 marker.text = std::to_string(
LM_IDs_[i]);
430 marker.pose.position.x = pose.
x();
431 marker.pose.position.y = pose.
y();
432 marker.pose.position.z = pose.z() + marker.scale.z + 0.1;
433 marker.color.r = 1.0;
434 marker.color.g = 1.0;
435 marker.color.b = 1.0;
436 marker.scale.
x = 0.3;
437 marker.scale.y = 0.3;
438 marker.scale.z = 0.3;
439 ma.markers.push_back(marker);
466 ROS_INFO(
"Failed to subtract global_frame (%s) from odom_frame (%s)",
479 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
TDataAssociationResults results
std::string base_frame_id
robot frame
std::vector< mrpt::math::TPoint3D > LMs_
vector of the landmarks
CPose3DQuatPDFGaussian 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
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
#define ROS_WARN_STREAM(args)
std::string rawlog_filename
name of rawlog file
void init()
initialize publishers subscribers and EKF 3d 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 transfor between odometry frame and the robot frame
void publishTF()
publis tf tree
const_iterator find(const KEY &key) const
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
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)
void computeEllipseOrientationScale(tf::Quaternion &orientation, Eigen::Vector3d &scale, const mrpt::math::CMatrixDouble covariance)
compute the orientation and scale of covariance ellipsoids
void makeRightHanded(Eigen::Matrix3d &eigenvectors, Eigen::Vector3d &eigenvalues)
compute the correct orientation and scale of covariance ellipsoids (make sure that we output covarian...
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_
void read_iniFile(std::string ini_filename)
read ini file
CActionCollection::Ptr action
actions
CSensoryFrame::Ptr sf
observations
float getQuantiles() const
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f