35 #include <sensor_msgs/LaserScan.h> 36 #include <sensor_msgs/PointCloud.h> 37 #include <nav_msgs/Odometry.h> 47 #include <mrpt/version.h> 48 #include <mrpt/obs/CSensoryFrame.h> 49 #include <mrpt/maps/CSimplePointsMap.h> 50 #include <mrpt/maps/COccupancyGridMap2D.h> 51 #include <mrpt/obs/CObservation2DRangeScan.h> 61 #include <mrpt/version.h> 62 #if MRPT_VERSION>=0x199 63 #include <mrpt/system/CTimeLogger.h> 64 #include <mrpt/config/CConfigFile.h> 66 using namespace mrpt::config;
67 using namespace mrpt::img;
83 ros::init(argc, argv,
"mrpt_local_obstacles_node");
99 double m_publish_period;
113 boost::mutex m_hist_obs_mtx;
125 std::vector<ros::Subscriber>
136 template <
typename CALLBACK_METHOD_TYPE>
138 const std::string& lstTopics, std::vector<ros::Subscriber>& subs,
139 CALLBACK_METHOD_TYPE cb)
141 std::vector<std::string> lstSources;
143 subs.resize(lstSources.size());
144 for (
size_t i = 0; i < lstSources.size(); i++)
145 subs[i] = m_nh.
subscribe(lstSources[i], 1, cb,
this);
146 return lstSources.size();
160 m_profiler,
"onNewSensor_Laser2D.lookupTransform_sensor");
162 m_frameid_robot, scan->header.frame_id,
ros::Time(0),
176 auto obsScan = CObservation2DRangeScan::Create();
180 "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s",
181 static_cast<unsigned int>(obsScan->getScanSize()),
182 sensorOnRobot_mrpt.
asString().c_str());
185 const double timestamp = scan->header.stamp.toSec();
193 m_profiler,
"onNewSensor_Laser2D.lookupTransform_robot");
199 m_frameid_reference, m_frameid_robot, scan->header.stamp,
207 m_frameid_reference, m_frameid_robot,
ros::Time(0), tx);
211 "[onNewSensor_Laser2D] robot pose %s",
225 m_hist_obs_mtx.lock();
227 m_hist_obs.end(), TListObservations::value_type(timestamp, ipt));
228 m_hist_obs_mtx.unlock();
238 TListObservations
obs;
241 m_profiler,
"onDoPublish.removingOld");
242 m_hist_obs_mtx.lock();
245 if (!m_hist_obs.empty())
247 const double last_time = m_hist_obs.rbegin()->first;
248 TListObservations::iterator it_first_valid =
249 m_hist_obs.lower_bound(last_time - m_time_window);
250 const size_t nToRemove =
253 "[onDoPublish] Removing %u old entries, last_time=%lf",
254 static_cast<unsigned int>(nToRemove), last_time);
255 m_hist_obs.erase(m_hist_obs.begin(), it_first_valid);
259 m_hist_obs_mtx.unlock();
263 "Building local map with %u observations.",
264 static_cast<unsigned int>(obs.size()));
265 if (obs.empty())
return;
269 m_localmap_pts.
clear();
273 m_profiler,
"onDoPublish.buildLocalMap");
282 m_frameid_reference, m_frameid_robot,
ros::Time(0), tx);
285 "[onDoPublish] Building local map relative to latest robot " 297 for (TListObservations::const_iterator it = obs.begin();
298 it != obs.end(); ++it)
315 sensor_msgs::PointCloudPtr msg_pts =
316 sensor_msgs::PointCloudPtr(
new sensor_msgs::PointCloud);
317 msg_pts->header.frame_id = m_frameid_robot;
318 msg_pts->header.stamp =
ros::Time(obs.rbegin()->first);
320 m_localmap_pts, msg_pts->header, *msg_pts);
321 m_pub_local_map_pointcloud.
publish(msg_pts);
330 "LocalObstaclesNode", 800, 600);
331 mrpt::opengl::COpenGLScene::Ptr&
scene =
332 m_gui_win->get3DSceneAndLock();
337 auto gl_obs = mrpt::opengl::CSetOfObjects::Create();
338 gl_obs->setName(
"obstacles");
339 scene->insert(gl_obs);
341 auto gl_pts = mrpt::opengl::CPointCloud::Create();
342 gl_pts->setName(
"points");
343 gl_pts->setPointSize(2.0);
344 gl_pts->setColor_u8(
TColor(0x0000ff));
345 scene->insert(gl_pts);
347 m_gui_win->unlockAccess3DScene();
350 auto&
scene = m_gui_win->get3DSceneAndLock();
352 scene->getByName(
"obstacles"));
357 scene->getByName(
"points"));
359 for (
const auto &o :obs)
366 mrpt::opengl::CSetOfObjects::Ptr gl_axis =
368 gl_axis->setPose(relPose);
369 gl_obs->insert(gl_axis);
372 gl_pts->loadFromPointsMap(&m_localmap_pts);
374 m_gui_win->unlockAccess3DScene();
375 m_gui_win->repaint();
383 : m_auxinit(argc, argv),
387 m_frameid_reference(
"odom"),
388 m_frameid_robot(
"base_link"),
389 m_topic_local_map_pointcloud(
"local_map_pointcloud"),
390 m_source_topics_2dscan(
"scan,laser1"),
392 m_publish_period(0.05)
395 m_localn.
param(
"show_gui", m_show_gui, m_show_gui);
397 "frameid_reference", m_frameid_reference, m_frameid_reference);
398 m_localn.
param(
"frameid_robot", m_frameid_robot, m_frameid_robot);
400 "topic_local_map_pointcloud", m_topic_local_map_pointcloud,
401 m_topic_local_map_pointcloud);
403 "source_topics_2dscan", m_source_topics_2dscan,
404 m_source_topics_2dscan);
405 m_localn.
param(
"time_window", m_time_window, m_time_window);
406 m_localn.
param(
"publish_period", m_publish_period, m_publish_period);
412 m_pub_local_map_pointcloud = m_nh.
advertise<sensor_msgs::PointCloud>(
413 m_topic_local_map_pointcloud, 10);
417 size_t nSubsTotal = 0;
418 nSubsTotal += this->subscribeToMultipleTopics(
419 m_source_topics_2dscan, m_subs_2dlaser,
423 "Total number of sensor subscriptions: %u\n",
424 static_cast<unsigned int>(nSubsTotal));
427 "*Error* It is mandatory to set at least one source topic for " 428 "sensory information!");
443 int main(
int argc,
char** argv)
CSimplePointsMap m_localmap_pts
TAuxInitializer m_auxinit
Just to make sure ROS is init first.
std::string m_topic_local_map_pointcloud
Default: "local_map_pointcloud".
std::vector< ros::Subscriber > m_subs_2dlaser
Subscriber to 2D laser scans.
void publish(const boost::shared_ptr< M > &message) const
std::string m_frameid_reference
typ: "odom"
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)
TInsertionOptions insertionOptions
LocalObstaclesNode(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string m_frameid_robot
typ: "base_link"
int main(int argc, char **argv)
float minDistBetweenLaserPoints
TAuxInitializer(int argc, char **argv)
ros::NodeHandle m_localn
"~"
ROSCPP_DECL void spin(Spinner &spinner)
std::string m_source_topics_2dscan
Default: "scan,laser1".
bool mrpt2ros(const mrpt::maps::CSimplePointsMap &obj, const std_msgs::Header &msg_header, sensor_msgs::PointCloud &msg)
tf::TransformListener m_tf_listener
Use to retrieve TF data.
TListObservations m_hist_obs
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
mrpt::gui::CDisplayWindow3D::Ptr m_gui_win
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
static CGridPlaneXYPtr Create(float xMin, float xMax, float yMin, float yMax, float z=0, float frequency=1, float lineWidth=1.3f, bool antiAliasing=true)
CObservation::Ptr observation
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void onDoPublish(const ros::TimerEvent &)
size_t subscribeToMultipleTopics(const std::string &lstTopics, std::vector< ros::Subscriber > &subs, CALLBACK_METHOD_TYPE cb)
Subscribe to a variable number of topics.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
ros::Publisher m_pub_local_map_pointcloud
void asString(std::string &s) const
uint32_t getNumSubscribers() const
mrpt::poses::CPose3D robot_pose
static CAST_TO::Ptr from(const CAST_FROM_PTR &ptr)
ros::NodeHandle m_nh
The node handle.
void inverseComposeFrom(const CPose3D &A, const CPose3D &B)
void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr &scan)
ros::Timer m_timer_publish
than m_time_window
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
bool insertObservationPtr(const mrpt::obs::CObservationPtr &obs, const mrpt::poses::CPose3D *robotPose=NULL)
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
std::multimap< double, TInfoPerTimeStep > TListObservations