8 #include <mrpt/version.h> 9 #if MRPT_VERSION >= 0x150 13 #if MRPT_VERSION >= 0x199 14 #include <mrpt/serialization/CArchive.h> 17 #include <mrpt/maps/COccupancyGridMap2D.h> 18 #include <mrpt/maps/CSimplePointsMap.h> 34 std::string sOutMap =
"mrpt_icpslam_";
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,
41 sOutMap +=
".simplemap";
44 ROS_INFO(
"Saving built map to `%s`", sOutMap.c_str());
46 }
catch (std::exception &e) {
51 std::ifstream
f(name.c_str());
62 #if MRPT_VERSION < 0x150 65 log4cxx::LoggerPtr ros_logger =
69 mapBuilder.logging_enable_console_output =
false;
70 #if MRPT_VERSION < 0x199 71 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
74 mapBuilder.logRegisterCallback(static_cast<output_logger_callback_t>(
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 129 "pf-localization - The MRPT project", 1000, 600);
130 win3D_->setCameraZoom(20);
131 win3D_->setCameraAzimuthDeg(-45);
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);
167 mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
169 groundPlane->setColor(0.4, 0.4, 0.4);
170 view->insert(groundPlane);
171 view_map->insert(CRenderizable::Ptr(groundPlane));
175 scene->enableFollowCamera(
true);
185 opengl::CSetOfObjects::Ptr
obj = opengl::CSetOfObjects::Create();
193 #if MRPT_VERSION >= 0x199 201 opengl::CSetOfObjects::Ptr ptsMap = opengl::CSetOfObjects::Create();
204 view_map->insert(ptsMap);
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 MRPT_VERSION >= 0x199 250 for (
size_t i = 0;; i++) {
251 CObservation2DRangeScan::Ptr new_obs =
252 observations->getObservationByClass<CObservation2DRangeScan>(i);
265 opengl::CPlanarLaserScan::Ptr
obj = opengl::CPlanarLaserScan::Create();
267 obj->setPose(curRobotPose);
268 obj->setSurfaceColor(1.0
f, 0.0
f, 0.0
f, 0.5
f);
294 n_.
advertise<sensor_msgs::PointCloud>(
"PointCloudMap", 1,
true);
310 std::vector<std::string> lstSources;
313 "*Fatal*: At least one sensor source must be provided in " 314 "~sensor_sources (e.g. " 315 "\"scan\" or \"beacon\")");
319 for (
size_t i = 0; i < lstSources.size(); i++) {
320 if (lstSources[i].
find(
"scan") != std::string::npos) {
324 std::cout <<
"Sensor topics should be 2d laser scans which inlude in the " 325 "name the word scan " 334 #if MRPT_VERSION >= 0x130 340 CObservation2DRangeScan::Ptr laser = CObservation2DRangeScan::Create();
366 #if MRPT_VERSION >= 0x199 386 sensor_msgs::PointCloud _msg;
403 pose.pose.position.x = robotPose.
x();
404 pose.pose.position.y = robotPose.
y();
405 pose.pose.position.z = 0.0;
419 pose.
x() = translation.
x();
420 pose.
y() = translation.
y();
421 pose.z() = translation.
z();
424 for (
int c = 0;
c < 3;
c++)
425 for (
int r = 0;
r < 3;
r++)
439 #if MRPT_VERSION >= 0x199 441 auto rawlogFile = mrpt::serialization::archiveFrom(rawlog_stream);
445 CActionCollection::Ptr
action;
449 if (!CRawlog::getActionObservationPairOrObservation(
474 #if MRPT_VERSION >= 0x199 496 sensor_msgs::PointCloud _msg;
511 pose.pose.position.x = robotPose.
x();
512 pose.pose.position.y = robotPose.
y();
513 pose.pose.position.z = 0.0;
547 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
void BASE_IMPEXP timestampToParts(TTimeStamp t, TTimeParts &p, bool localTime=false)
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
void setOrthogonal(bool v=true)
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
mrpt::gui::CDisplayWindow3D::Ptr win3D_
MRPT window.
void updateTrajectoryTimerCallback(const ros::TimerEvent &event)
the callback for update trajectory
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const MRPT_OVERRIDE
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())
static CDisplayWindow3DPtr Create(const std::string &windowCaption, unsigned int initialWindowWidth=400, unsigned int initialWindowHeight=300)
bool SHOW_PROGRESS_3D_REAL_TIME
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void init()
initialize publishers subscribers and icp slam
mrpt::system::TTimeStamp now()
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
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
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
void setAzimuthDegrees(float ang)
double rawlog_play_delay
delay of replay from rawlog file
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
mrpt::utils::CListOfClasses alwaysInsertByClass
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
GLuint GLenum GLenum transform
ros::Publisher pub_point_cloud_
publishers for map and pose particles
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
CICP::TConfigParams ICP_params
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const MRPT_OVERRIDE
bool mrpt2ros(const mrpt::maps::CSimplePointsMap &obj, const std_msgs::Header &msg_header, sensor_msgs::PointCloud &msg)
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
tf::TransformListener listenerTF_
transform listener
CMetricMapBuilderICP mapBuilder
icp slam class
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
TFSIMD_FORCE_INLINE const tfScalar & z() const
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) MRPT_OVERRIDE
CTicTac tictac
timer for SLAM performance evaluation
GLuint const GLchar * name
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
VerbosityLevel rosLoggerLvlToMRPTLoggerLvl(log4cxx::LevelPtr lvl)
static CGridPlaneXYPtr Create(float xMin, float xMax, float yMin, float yMax, float z=0, float frequency=1, float lineWidth=1.3f, bool antiAliasing=true)
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.
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars= '_')
void setPointingAt(float x, float y, float z)
#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
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
const_iterator find(const KEY &key) const
ros::Timer publish_trajectory_timer
timer for publish trajectory
void run3Dwindow()
run 3D window update from mrpt lib
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL) MRPT_OVERRIDE
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
void mrptToROSLoggerCallback(const std::string &msg, const VerbosityLevel level, const std::string &loggerName, const mrpt::system::TTimeStamp timestamp)
void fromString(const std::string &s)
GLdouble GLdouble GLdouble r
bool getParam(const std::string &key, std::string &s) const
ros::Publisher trajectory_pub_
trajectory publisher
void dumpToConsole() const
void read_iniFile(std::string ini_filename)
read ini file
~ICPslamWrapper()
destructor
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
double trajectory_update_rate
trajectory update rate(Hz)
TConfigParams ICP_options
#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
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
#define IS_CLASS(ptrObj, class_name)
ROSCPP_DECL void spinOnce()
std::map< std::string, mrpt::poses::CPose3D > laser_poses_
laser scan poses with respect to the map
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
void mrptToROSLoggerCallback_mrpt_15(const std::string &msg, const VerbosityLevel level, const std::string &loggerName, const mrpt::system::TTimeStamp timestamp, void *userParam)
double trajectory_publish_rate
trajectory publish rate(Hz)
#define ROSCONSOLE_DEFAULT_NAME
ros::Timer update_trajector_timer
timer for update trajectory
void setZoomDistance(float z)
void processObservation(const mrpt::obs::CObservationPtr &obs)
void publishTF()
publis tf tree
std::string sensor_source
2D laser scans
std::vector< CObservation2DRangeScan::Ptr > lst_current_laser_scans
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
ros::Publisher pub_metadata_
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
void setElevationDegrees(float ang)