8 #include <mrpt/version.h> 9 #if MRPT_VERSION >= 0x150 10 #include <mrpt_bridge/utils.h> 13 #if MRPT_VERSION >= 0x199 14 #include <mrpt/serialization/CArchive.h> 17 #include <mrpt/maps/COccupancyGridMap2D.h> 18 #include <mrpt/maps/CSimplePointsMap.h> 19 using mrpt::maps::COccupancyGridMap2D;
20 using mrpt::maps::CSimplePointsMap;
34 std::string sOutMap =
"mrpt_icpslam_";
35 mrpt::system::TTimeParts parts;
36 mrpt::system::timestampToParts(
now(), parts,
true);
37 sOutMap += format(
"%04u-%02u-%02u_%02uh%02um%02us",
38 (
unsigned int)parts.year, (
unsigned int)parts.month,
39 (
unsigned int)parts.day, (
unsigned int)parts.hour,
40 (
unsigned int)parts.minute, (
unsigned int)parts.second);
41 sOutMap +=
".simplemap";
43 sOutMap = mrpt::system::fileNameStripInvalidChars(sOutMap);
44 ROS_INFO(
"Saving built map to `%s`", sOutMap.c_str());
46 }
catch (std::exception &e) {
51 std::ifstream
f(name.c_str());
56 CConfigFile iniFile(ini_filename);
58 mapBuilder.ICP_options.loadFromConfigFile(iniFile,
"MappingApplication");
59 mapBuilder.ICP_params.loadFromConfigFile(iniFile,
"ICP");
62 #if MRPT_VERSION < 0x150 65 log4cxx::LoggerPtr ros_logger =
68 mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel()));
69 mapBuilder.logging_enable_console_output =
false;
70 #if MRPT_VERSION < 0x199 71 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
72 &mrpt_bridge::mrptToROSLoggerCallback_mrpt_15));
74 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
75 &mrpt_bridge::mrptToROSLoggerCallback));
78 mapBuilder.options.alwaysInsertByClass.fromString(
79 iniFile.read_string(
"MappingApplication",
"alwaysInsertByClass",
""));
86 iniFile.read_bool(
"MappingApplication",
"CAMERA_3DSCENE_FOLLOWS_ROBOT",
89 "MappingApplication");
91 "MappingApplication");
93 "MappingApplication");
97 ROS_INFO(
"READ PARAM FROM LAUNCH FILE");
99 ROS_INFO(
"rawlog_play_delay: %f", rawlog_play_delay);
108 ROS_INFO(
"global_frame_id: %s", global_frame_id.c_str());
111 ROS_INFO(
"odom_frame_id: %s", odom_frame_id.c_str());
114 ROS_INFO(
"base_frame_id: %s", base_frame_id.c_str());
117 ROS_INFO(
"sensor_source: %s", sensor_source.c_str());
126 #if MRPT_HAS_WXWIDGETS 128 win3D_ = mrpt::gui::CDisplayWindow3D::Create(
129 "pf-localization - The MRPT project", 1000, 600);
130 win3D_->setCameraZoom(20);
131 win3D_->setCameraAzimuthDeg(-45);
147 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
148 COpenGLScene::Ptr scene = COpenGLScene::Create();
150 COpenGLViewport::Ptr view = scene->getViewport(
"main");
152 COpenGLViewport::Ptr view_map = scene->createViewport(
"mini-map");
153 view_map->setBorderSize(2);
154 view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
155 view_map->setTransparent(
false);
158 mrpt::opengl::CCamera &cam = view_map->getCamera();
159 cam.setAzimuthDegrees(-90);
160 cam.setElevationDegrees(90);
161 cam.setPointingAt(robotPose);
162 cam.setZoomDistance(20);
167 mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
168 mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
169 groundPlane->setColor(0.4, 0.4, 0.4);
170 view->insert(groundPlane);
171 view_map->insert(CRenderizable::Ptr(groundPlane));
175 scene->enableFollowCamera(
true);
177 mrpt::opengl::CCamera &cam = view_map->getCamera();
178 cam.setAzimuthDegrees(-45);
179 cam.setElevationDegrees(45);
180 cam.setPointingAt(robotPose);
185 opengl::CSetOfObjects::Ptr obj = opengl::CSetOfObjects::Create();
192 CSimplePointsMap *pm =
nullptr;
193 #if MRPT_VERSION >= 0x199 194 if (
metric_map_->countMapsByClass<CSimplePointsMap>())
195 pm =
metric_map_->mapByClass<CSimplePointsMap>().
get();
201 opengl::CSetOfObjects::Ptr ptsMap = opengl::CSetOfObjects::Create();
203 pm->getAs3DObject(ptsMap);
204 view_map->insert(ptsMap);
209 CPose3DPDF::Ptr posePDF =
mapBuilder.getCurrentPoseEstimation();
210 CPose3D curRobotPose;
211 posePDF->getMean(curRobotPose);
213 opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
214 obj->setPose(curRobotPose);
218 opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
219 obj->setPose(curRobotPose);
220 view_map->insert(obj);
223 opengl::COpenGLScene::Ptr &ptrScene =
win3D_->get3DSceneAndLock();
226 win3D_->unlockAccess3DScene();
229 win3D_->setCameraPointingToPoint(curRobotPose.x(), curRobotPose.y(),
239 if (IS_CLASS(
observation, CObservation2DRangeScan)) {
241 mrpt::ptr_cast<CObservation2DRangeScan>::from(
observation));
245 for (
size_t i = 0;; i++) {
246 CObservation2DRangeScan::Ptr new_obs =
247 observations->getObservationByClass<CObservation2DRangeScan>(i);
260 opengl::CPlanarLaserScan::Ptr obj = opengl::CPlanarLaserScan::Create();
262 obj->setPose(curRobotPose);
263 obj->setSurfaceColor(1.0
f, 0.0
f, 0.0
f, 0.5
f);
289 n_.
advertise<sensor_msgs::PointCloud>(
"PointCloudMap", 1,
true);
305 std::vector<std::string> lstSources;
308 "*Fatal*: At least one sensor source must be provided in " 309 "~sensor_sources (e.g. " 310 "\"scan\" or \"beacon\")");
314 for (
size_t i = 0; i < lstSources.size(); i++) {
315 if (lstSources[i].find(
"scan") != std::string::npos) {
319 std::cout <<
"Sensor topics should be 2d laser scans which inlude in the " 320 "name the word scan " 329 #if MRPT_VERSION >= 0x130 330 using namespace mrpt::maps;
331 using namespace mrpt::obs;
335 CObservation2DRangeScan::Ptr laser = CObservation2DRangeScan::Create();
340 mrpt_bridge::convert(_msg,
laser_poses_[_msg.header.frame_id], *laser);
359 COccupancyGridMap2D *grid =
nullptr;
360 CSimplePointsMap *pm =
nullptr;
361 #if MRPT_VERSION >= 0x199 362 if (
metric_map_->countMapsByClass<COccupancyGridMap2D>())
363 grid =
metric_map_->mapByClass<COccupancyGridMap2D>().
get();
364 if (
metric_map_->countMapsByClass<CSimplePointsMap>())
365 pm =
metric_map_->mapByClass<CSimplePointsMap>().
get();
374 nav_msgs::OccupancyGrid _msg;
376 mrpt_bridge::convert(*grid, _msg);
381 sensor_msgs::PointCloud _msg;
386 mrpt_bridge::point_cloud::mrpt2ros(*pm, header, _msg);
391 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
398 pose.pose.position.x = robotPose.x();
399 pose.pose.position.y = robotPose.y();
400 pose.pose.position.z = 0.0;
414 pose.x() = translation.
x();
415 pose.y() = translation.
y();
416 pose.z() = translation.
z();
418 CMatrixDouble33 Rdes;
419 for (
int c = 0; c < 3; c++)
420 for (
int r = 0;
r < 3;
r++)
422 pose.setRotationMatrix(Rdes);
433 size_t rawlogEntry = 0;
434 #if MRPT_VERSION >= 0x199 436 auto rawlogFile = mrpt::serialization::archiveFrom(rawlog_stream);
440 CActionCollection::Ptr
action;
444 if (!CRawlog::getActionObservationPairOrObservation(
464 mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
467 COccupancyGridMap2D *grid =
nullptr;
468 CSimplePointsMap *pm =
nullptr;
469 #if MRPT_VERSION >= 0x199 470 if (
metric_map_->countMapsByClass<COccupancyGridMap2D>())
471 grid =
metric_map_->mapByClass<COccupancyGridMap2D>().
get();
472 if (
metric_map_->countMapsByClass<CSimplePointsMap>())
473 pm =
metric_map_->mapByClass<CSimplePointsMap>().
get();
482 nav_msgs::OccupancyGrid _msg;
485 mrpt_bridge::convert(*grid, _msg);
491 sensor_msgs::PointCloud _msg;
496 mrpt_bridge::point_cloud::mrpt2ros(*pm, header, _msg);
506 pose.pose.position.x = robotPose.x();
507 pose.pose.position.y = robotPose.y();
508 pose.pose.position.z = 0.0;
529 mrpt::poses::CPose3D robotPoseTF;
530 mapBuilder.getCurrentPoseEstimation()->getMean(robotPoseTF);
535 mrpt_bridge::convert(robotPoseTF, tmp_tf);
542 ROS_INFO(
"Failed to subtract global_frame (%s) from odom_frame (%s)",
ICPslamWrapper()
constructor
void publishTrajectoryTimerCallback(const ros::TimerEvent &event)
the callback for publish trajectory
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
mrpt::gui::CDisplayWindow3D::Ptr win3D_
MRPT window.
void updateTrajectoryTimerCallback(const ros::TimerEvent &event)
the callback for update trajectory
void publish(const boost::shared_ptr< M > &message) const
const CMultiMetricMap * metric_map_
receive map after iteration of SLAM to metric map
void get_param()
read the parameters from launch file
nav_msgs::Path path
trajectory path
std::string odom_frame_id
/odom frame
CSensoryFrame::Ptr observations
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool SHOW_PROGRESS_3D_REAL_TIME
void init()
initialize publishers subscribers and icp slam
bool is_file_exists(const std::string &name)
check the existance of the file
void laserCallback(const sensor_msgs::LaserScan &_msg)
callback function for the laser scans
bool rawlog_play_
true if rawlog file exists
float t_exec
the time which take one SLAM update execution
geometry_msgs::PoseStamped pose
the robot pose
void publishMapPose()
publish point and/or grid map and robot pose
double rawlog_play_delay
delay of replay from rawlog file
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
void updateSensorPose(std::string _frame_id)
update the pose of the sensor with respect to the robot
int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS
ros::Publisher pub_point_cloud_
publishers for map and pose particles
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
TFSIMD_FORCE_INLINE const tfScalar & x() const
tf::TransformListener listenerTF_
transform listener
CMetricMapBuilderICP mapBuilder
icp slam class
TFSIMD_FORCE_INLINE const tfScalar & z() const
CTicTac tictac
timer for SLAM performance evaluation
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
std::string ini_filename
name of ini file
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle n_
Node Handle.
#define ROS_WARN_STREAM(args)
bool rawlogPlay()
play rawlog file
CObservation::Ptr observation
std::string global_frame_id
/map frame
ros::Time stamp
timestamp for observations
ros::Timer publish_trajectory_timer
timer for publish trajectory
void run3Dwindow()
run 3D window update from mrpt lib
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
bool getParam(const std::string &key, std::string &s) const
ros::Publisher trajectory_pub_
trajectory publisher
void read_iniFile(std::string ini_filename)
read ini file
~ICPslamWrapper()
destructor
double trajectory_update_rate
trajectory update rate(Hz)
#define ROS_ERROR_STREAM(args)
std::string rawlog_filename
name of rawlog file
std::string base_frame_id
robot frame
void init3Dwindow()
init 3D window from mrpt lib
ROSCPP_DECL void spinOnce()
std::map< std::string, mrpt::poses::CPose3D > laser_poses_
laser scan poses with respect to the map
double trajectory_publish_rate
trajectory publish rate(Hz)
#define ROSCONSOLE_DEFAULT_NAME
ros::Timer update_trajector_timer
timer for update trajectory
void publishTF()
publis tf tree
std::string sensor_source
2D laser scans
std::vector< CObservation2DRangeScan::Ptr > lst_current_laser_scans
ros::Publisher pub_metadata_