00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <boost/interprocess/sync/scoped_lock.hpp>
00035 #include <geometry_msgs/PoseArray.h>
00036 #include <pose_cov_ops/pose_cov_ops.h>
00037
00038 #include <mrpt_bridge/pose.h>
00039 #include <mrpt_bridge/laser_scan.h>
00040 #include <mrpt_bridge/time.h>
00041 #include <mrpt_bridge/map.h>
00042 #include <mrpt_bridge/beacon.h>
00043
00044 #include <mrpt/version.h>
00045 #include <mrpt/obs/CObservationBeaconRanges.h>
00046 using namespace mrpt::obs;
00047
00048 #include <mrpt/obs/CObservationRobotPose.h>
00049
00050 #include "mrpt_localization_node.h"
00051
00052 int main(int argc, char** argv)
00053 {
00054 ros::init(argc, argv, "localization");
00055 ros::NodeHandle nh;
00056 PFLocalizationNode my_node(nh);
00057 my_node.init();
00058 my_node.loop();
00059 return 0;
00060 }
00061
00062 PFLocalizationNode::~PFLocalizationNode() {}
00063 PFLocalizationNode::PFLocalizationNode(ros::NodeHandle& n)
00064 : PFLocalization(new PFLocalizationNode::Parameters(this)),
00065 nh_(n),
00066 first_map_received_(false),
00067 loop_count_(0)
00068 {
00069 }
00070
00071 PFLocalizationNode::Parameters* PFLocalizationNode::param()
00072 {
00073 return (PFLocalizationNode::Parameters*)param_;
00074 }
00075
00076 void PFLocalizationNode::init()
00077 {
00078
00079
00080 useROSLogLevel();
00081
00082 PFLocalization::init();
00083 sub_init_pose_ = nh_.subscribe(
00084 "initialpose", 1, &PFLocalizationNode::callbackInitialpose, this);
00085
00086 sub_odometry_ =
00087 nh_.subscribe("odom", 1, &PFLocalizationNode::callbackOdometry, this);
00088
00089
00090 std::vector<std::string> sources;
00091 mrpt::system::tokenize(param()->sensor_sources, " ,\t\n", sources);
00092 ROS_ASSERT_MSG(
00093 !sources.empty(),
00094 "*Fatal*: At least one sensor source must be provided in "
00095 "~sensor_sources (e.g. \"scan\" or \"beacon\")");
00096 sub_sensors_.resize(sources.size());
00097 for (size_t i = 0; i < sources.size(); i++)
00098 {
00099 if (sources[i].find("scan") != std::string::npos)
00100 {
00101 sub_sensors_[i] = nh_.subscribe(
00102 sources[i], 1, &PFLocalizationNode::callbackLaser, this);
00103 }
00104 else if (sources[i].find("beacon") != std::string::npos)
00105 {
00106 sub_sensors_[i] = nh_.subscribe(
00107 sources[i], 1, &PFLocalizationNode::callbackBeacon, this);
00108 }
00109 else
00110 {
00111 sub_sensors_[i] = nh_.subscribe(
00112 sources[i], 1, &PFLocalizationNode::callbackRobotPose, this);
00113 }
00114 }
00115
00116 if (!param()->map_file.empty())
00117 {
00118 if (metric_map_.m_gridMaps.size())
00119 {
00120 mrpt_bridge::convert(*metric_map_.m_gridMaps[0], resp_.map);
00121 }
00122 pub_map_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00123 pub_metadata_ =
00124 nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00125 service_map_ = nh_.advertiseService(
00126 "static_map", &PFLocalizationNode::mapCallback, this);
00127 }
00128 pub_particles_ =
00129 nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 1, true);
00130
00131 pub_pose_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
00132 "mrpt_pose", 2, true);
00133 }
00134
00135 void PFLocalizationNode::loop()
00136 {
00137 ROS_INFO("loop");
00138 for (ros::Rate rate(param()->rate); ros::ok(); loop_count_++)
00139 {
00140 param()->update(loop_count_);
00141 if ((loop_count_ % param()->map_update_skip == 0) &&
00142 (metric_map_.m_gridMaps.size()))
00143 publishMap();
00144 if (loop_count_ % param()->particlecloud_update_skip == 0)
00145 publishParticles();
00146 if (param()->tf_broadcast) publishTF();
00147 if (param()->pose_broadcast) publishPose();
00148
00149 ros::spinOnce();
00150 rate.sleep();
00151 }
00152 }
00153
00154 bool PFLocalizationNode::waitForTransform(
00155 mrpt::poses::CPose3D& des, const std::string& target_frame,
00156 const std::string& source_frame, const ros::Time& time,
00157 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration)
00158 {
00159 tf::StampedTransform transform;
00160 try
00161 {
00162 tf_listener_.waitForTransform(
00163 target_frame, source_frame, time, timeout, polling_sleep_duration);
00164 tf_listener_.lookupTransform(
00165 target_frame, source_frame, time, transform);
00166 }
00167 catch (tf::TransformException& e)
00168 {
00169 ROS_WARN(
00170 "Failed to get transform target_frame (%s) to source_frame (%s): "
00171 "%s",
00172 target_frame.c_str(), source_frame.c_str(), e.what());
00173 return false;
00174 }
00175 mrpt_bridge::convert(transform, des);
00176 return true;
00177 }
00178
00179 void PFLocalizationNode::callbackLaser(const sensor_msgs::LaserScan& _msg)
00180 {
00181 using namespace mrpt::maps;
00182 using namespace mrpt::obs;
00183
00184 time_last_input_ = ros::Time::now();
00185
00186
00187 CObservation2DRangeScan::Ptr laser =
00188 mrpt::make_aligned_shared<CObservation2DRangeScan>();
00189
00190
00191 if (laser_poses_.find(_msg.header.frame_id) == laser_poses_.end())
00192 {
00193 updateSensorPose(_msg.header.frame_id);
00194 }
00195 else if (state_ != IDLE)
00196
00197 {
00198
00199
00200
00201 mrpt_bridge::convert(_msg, laser_poses_[_msg.header.frame_id], *laser);
00202
00203 CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
00204 CObservationOdometry::Ptr odometry;
00205 odometryForCallback(odometry, _msg.header);
00206
00207 CObservation::Ptr obs = CObservation::Ptr(laser);
00208 sf->insert(obs);
00209 observation(sf, odometry);
00210 if (param()->gui_mrpt) show3DDebug(sf);
00211 }
00212 }
00213
00214 void PFLocalizationNode::callbackBeacon(
00215 const mrpt_msgs::ObservationRangeBeacon& _msg)
00216 {
00217 using namespace mrpt::maps;
00218 using namespace mrpt::obs;
00219
00220 time_last_input_ = ros::Time::now();
00221
00222
00223 CObservationBeaconRanges::Ptr beacon =
00224 mrpt::make_aligned_shared<CObservationBeaconRanges>();
00225
00226 if (beacon_poses_.find(_msg.header.frame_id) == beacon_poses_.end())
00227 {
00228 updateSensorPose(_msg.header.frame_id);
00229 }
00230 else if (state_ != IDLE)
00231
00232 {
00233
00234
00235
00236 mrpt_bridge::convert(
00237 _msg, beacon_poses_[_msg.header.frame_id], *beacon);
00238
00239 CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
00240 CObservationOdometry::Ptr odometry;
00241 odometryForCallback(odometry, _msg.header);
00242
00243 CObservation::Ptr obs = CObservation::Ptr(beacon);
00244 sf->insert(obs);
00245 observation(sf, odometry);
00246 if (param()->gui_mrpt) show3DDebug(sf);
00247 }
00248 }
00249
00250 void PFLocalizationNode::callbackRobotPose(
00251 const geometry_msgs::PoseWithCovarianceStamped& _msg)
00252 {
00253 using namespace mrpt::maps;
00254 using namespace mrpt::obs;
00255
00256 time_last_input_ = ros::Time::now();
00257
00258
00259
00260
00261
00262
00263
00264
00265 static std::string base_frame_id =
00266 tf::resolve(param()->tf_prefix, param()->base_frame_id);
00267 static std::string global_frame_id =
00268 tf::resolve(param()->tf_prefix, param()->global_frame_id);
00269
00270 tf::StampedTransform map_to_obs_tf;
00271 try
00272 {
00273 tf_listener_.waitForTransform(
00274 global_frame_id, _msg.header.frame_id, ros::Time(0.0),
00275 ros::Duration(0.5));
00276 tf_listener_.lookupTransform(
00277 global_frame_id, _msg.header.frame_id, ros::Time(0.0),
00278 map_to_obs_tf);
00279 }
00280 catch (tf::TransformException& ex)
00281 {
00282 ROS_ERROR("%s", ex.what());
00283 return;
00284 }
00285
00286
00287
00288
00289
00290 geometry_msgs::Pose map_to_obs_pose;
00291 tf::pointTFToMsg(map_to_obs_tf.getOrigin(), map_to_obs_pose.position);
00292 tf::quaternionTFToMsg(
00293 map_to_obs_tf.getRotation(), map_to_obs_pose.orientation);
00294 geometry_msgs::PoseWithCovarianceStamped obs_pose_world;
00295 obs_pose_world.header.stamp = _msg.header.stamp;
00296 obs_pose_world.header.frame_id = global_frame_id;
00297 pose_cov_ops::compose(map_to_obs_pose, _msg.pose, obs_pose_world.pose);
00298
00299
00300 for (int i = 0; i < obs_pose_world.pose.covariance.size(); ++i)
00301 {
00302 if (i / 6 == i % 6 && obs_pose_world.pose.covariance[i] <= 0.0)
00303 obs_pose_world.pose.covariance[i] =
00304 std::numeric_limits<double>().infinity();
00305 }
00306
00307
00308 CObservationRobotPose::Ptr feature =
00309 mrpt::make_aligned_shared<CObservationRobotPose>();
00310
00311 feature->sensorLabel = _msg.header.frame_id;
00312 mrpt_bridge::convert(_msg.header.stamp, feature->timestamp);
00313 mrpt_bridge::convert(obs_pose_world.pose, feature->pose);
00314
00315 CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
00316 CObservationOdometry::Ptr odometry;
00317 odometryForCallback(odometry, _msg.header);
00318
00319 CObservation::Ptr obs = CObservation::Ptr(feature);
00320 sf->insert(obs);
00321 observation(sf, odometry);
00322 if (param()->gui_mrpt) show3DDebug(sf);
00323 }
00324
00325 void PFLocalizationNode::odometryForCallback(
00326 CObservationOdometry::Ptr& _odometry, const std_msgs::Header& _msg_header)
00327 {
00328 std::string base_frame_id =
00329 tf::resolve(param()->tf_prefix, param()->base_frame_id);
00330 std::string odom_frame_id =
00331 tf::resolve(param()->tf_prefix, param()->odom_frame_id);
00332 mrpt::poses::CPose3D poseOdom;
00333 if (this->waitForTransform(
00334 poseOdom, odom_frame_id, base_frame_id, _msg_header.stamp,
00335 ros::Duration(1.0)))
00336 {
00337 _odometry = mrpt::make_aligned_shared<CObservationOdometry>();
00338 _odometry->sensorLabel = odom_frame_id;
00339 _odometry->hasEncodersInfo = false;
00340 _odometry->hasVelocities = false;
00341 _odometry->odometry.x() = poseOdom.x();
00342 _odometry->odometry.y() = poseOdom.y();
00343 _odometry->odometry.phi() = poseOdom.yaw();
00344 }
00345 }
00346
00347 bool PFLocalizationNode::waitForMap()
00348 {
00349 int wait_counter = 0;
00350 int wait_limit = 10;
00351
00352 if (param()->use_map_topic)
00353 {
00354 sub_map_ =
00355 nh_.subscribe("map", 1, &PFLocalizationNode::callbackMap, this);
00356 ROS_INFO("Subscribed to map topic.");
00357
00358 while (!first_map_received_ && ros::ok() && wait_counter < wait_limit)
00359 {
00360 ROS_INFO("waiting for map callback..");
00361 ros::Duration(0.5).sleep();
00362 ros::spinOnce();
00363 wait_counter++;
00364 }
00365 if (wait_counter != wait_limit)
00366 {
00367 return true;
00368 }
00369 }
00370 else
00371 {
00372 client_map_ = nh_.serviceClient<nav_msgs::GetMap>("static_map");
00373 nav_msgs::GetMap srv;
00374 while (!client_map_.call(srv) && ros::ok() && wait_counter < wait_limit)
00375 {
00376 ROS_INFO("waiting for map service!");
00377 ros::Duration(0.5).sleep();
00378 wait_counter++;
00379 }
00380 client_map_.shutdown();
00381 if (wait_counter != wait_limit)
00382 {
00383 ROS_INFO_STREAM("Map service complete.");
00384 updateMap(srv.response.map);
00385 return true;
00386 }
00387 }
00388
00389 ROS_WARN_STREAM("No map received.");
00390 return false;
00391 }
00392
00393 void PFLocalizationNode::callbackMap(const nav_msgs::OccupancyGrid& msg)
00394 {
00395 if (param()->first_map_only && first_map_received_)
00396 {
00397 return;
00398 }
00399
00400 ROS_INFO_STREAM("Map received.");
00401 updateMap(msg);
00402
00403 first_map_received_ = true;
00404 }
00405
00406 void PFLocalizationNode::updateSensorPose(std::string _frame_id)
00407 {
00408 mrpt::poses::CPose3D pose;
00409 tf::StampedTransform transform;
00410 try
00411 {
00412 std::string base_frame_id =
00413 tf::resolve(param()->tf_prefix, param()->base_frame_id);
00414 tf_listener_.lookupTransform(
00415 base_frame_id, _frame_id, ros::Time(0), transform);
00416 tf::Vector3 translation = transform.getOrigin();
00417 tf::Quaternion quat = transform.getRotation();
00418 pose.x() = translation.x();
00419 pose.y() = translation.y();
00420 pose.z() = translation.z();
00421 double roll, pitch, yaw;
00422 tf::Matrix3x3 Rsrc(quat);
00423 mrpt::math::CMatrixDouble33 Rdes;
00424 for (int c = 0; c < 3; c++)
00425 for (int r = 0; r < 3; r++) Rdes(r, c) = Rsrc.getRow(r)[c];
00426 pose.setRotationMatrix(Rdes);
00427 laser_poses_[_frame_id] = pose;
00428 beacon_poses_[_frame_id] = pose;
00429 }
00430 catch (tf::TransformException& ex)
00431 {
00432 ROS_ERROR("%s", ex.what());
00433 ros::Duration(1.0).sleep();
00434 }
00435 }
00436
00437 void PFLocalizationNode::callbackInitialpose(
00438 const geometry_msgs::PoseWithCovarianceStamped& _msg)
00439 {
00440 const geometry_msgs::PoseWithCovariance& pose = _msg.pose;
00441 mrpt_bridge::convert(pose, initial_pose_);
00442 update_counter_ = 0;
00443 state_ = INIT;
00444 }
00445
00446 void PFLocalizationNode::callbackOdometry(const nav_msgs::Odometry& _msg)
00447 {
00448
00449
00450
00451
00452 bool moving = std::abs(_msg.twist.twist.linear.x) > 1e-3 ||
00453 std::abs(_msg.twist.twist.linear.y) > 1e-3 ||
00454 std::abs(_msg.twist.twist.linear.z) > 1e-3 ||
00455 std::abs(_msg.twist.twist.angular.x) > 1e-3 ||
00456 std::abs(_msg.twist.twist.angular.y) > 1e-3 ||
00457 std::abs(_msg.twist.twist.angular.z) > 1e-3;
00458 if (param()->update_while_stopped || moving)
00459 {
00460 if (state_ == IDLE)
00461 {
00462 state_ = RUN;
00463 }
00464 }
00465 else if (state_ == RUN && update_counter_ >= 100)
00466 {
00467 state_ = IDLE;
00468 }
00469 }
00470
00471 void PFLocalizationNode::updateMap(const nav_msgs::OccupancyGrid& _msg)
00472 {
00473 ASSERT_(metric_map_.m_gridMaps.size() == 1);
00474 mrpt_bridge::convert(_msg, *metric_map_.m_gridMaps[0]);
00475 }
00476
00477 bool PFLocalizationNode::mapCallback(
00478 nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
00479 {
00480 ROS_INFO("mapCallback: service requested!\n");
00481 res = resp_;
00482 return true;
00483 }
00484
00485 void PFLocalizationNode::publishMap()
00486 {
00487 resp_.map.header.stamp = ros::Time::now();
00488 resp_.map.header.frame_id =
00489 tf::resolve(param()->tf_prefix, param()->global_frame_id);
00490 resp_.map.header.seq = loop_count_;
00491 if (pub_map_.getNumSubscribers() > 0)
00492 {
00493 pub_map_.publish(resp_.map);
00494 }
00495 if (pub_metadata_.getNumSubscribers() > 0)
00496 {
00497 pub_metadata_.publish(resp_.map.info);
00498 }
00499 }
00500
00501 void PFLocalizationNode::publishParticles()
00502 {
00503 if (pub_particles_.getNumSubscribers() > 0)
00504 {
00505 geometry_msgs::PoseArray poseArray;
00506 poseArray.header.frame_id =
00507 tf::resolve(param()->tf_prefix, param()->global_frame_id);
00508 poseArray.header.stamp = ros::Time::now();
00509 poseArray.header.seq = loop_count_;
00510 poseArray.poses.resize(pdf_.particlesCount());
00511 for (size_t i = 0; i < pdf_.particlesCount(); i++)
00512 {
00513 mrpt::poses::CPose2D p = pdf_.getParticlePose(i);
00514 mrpt_bridge::convert(p, poseArray.poses[i]);
00515 }
00516 mrpt::poses::CPose2D p;
00517 pub_particles_.publish(poseArray);
00518 }
00519 }
00520
00525 void PFLocalizationNode::publishTF()
00526 {
00527 static std::string base_frame_id =
00528 tf::resolve(param()->tf_prefix, param()->base_frame_id);
00529 static std::string odom_frame_id =
00530 tf::resolve(param()->tf_prefix, param()->odom_frame_id);
00531 static std::string global_frame_id =
00532 tf::resolve(param()->tf_prefix, param()->global_frame_id);
00533
00534 mrpt::poses::CPose2D robot_pose;
00535 pdf_.getMean(robot_pose);
00536 tf::StampedTransform base_on_map_tf, odom_on_base_tf;
00537 mrpt_bridge::convert(robot_pose, base_on_map_tf);
00538 ros::Time time_last_update(0.0);
00539 if (state_ == RUN)
00540 {
00541 mrpt_bridge::convert(time_last_update_, time_last_update);
00542
00543
00544
00545
00546
00547
00548
00549 if ((ros::Time::now() - time_last_update).toSec() >
00550 param()->no_update_tolerance)
00551 {
00552 if ((ros::Time::now() - time_last_input_).toSec() >
00553 param()->no_inputs_tolerance)
00554 {
00555 ROS_WARN_THROTTLE(
00556 2.0,
00557 "No observations received for %.2fs (tolerance %.2fs); are "
00558 "robot sensors working?",
00559 (ros::Time::now() - time_last_input_).toSec(),
00560 param()->no_inputs_tolerance);
00561 }
00562 else
00563 {
00564 ROS_DEBUG_THROTTLE(
00565 2.0,
00566 "No filter updates for %.2fs (tolerance %.2fs); probably "
00567 "robot stopped for a while",
00568 (ros::Time::now() - time_last_update).toSec(),
00569 param()->no_update_tolerance);
00570 }
00571
00572 time_last_update = ros::Time(0.0);
00573 }
00574 }
00575
00576 try
00577 {
00578
00579 tf_listener_.waitForTransform(
00580 base_frame_id, odom_frame_id, time_last_update, ros::Duration(0.1));
00581 tf_listener_.lookupTransform(
00582 base_frame_id, odom_frame_id, time_last_update, odom_on_base_tf);
00583 }
00584 catch (tf::TransformException& e)
00585 {
00586 ROS_WARN_THROTTLE(
00587 2.0, "Transform from base frame (%s) to odom frame (%s) failed: %s",
00588 base_frame_id.c_str(), odom_frame_id.c_str(), e.what());
00589 ROS_WARN_THROTTLE(
00590 2.0,
00591 "Ensure that your mobile base driver is broadcasting %s -> %s tf",
00592 odom_frame_id.c_str(), base_frame_id.c_str());
00593 return;
00594 }
00595
00596
00597
00598 ros::Time transform_expiration =
00599 (time_last_update.isZero() ? ros::Time::now() : time_last_update) +
00600 ros::Duration(param()->transform_tolerance);
00601 tf::StampedTransform tmp_tf_stamped(
00602 base_on_map_tf * odom_on_base_tf, transform_expiration, global_frame_id,
00603 odom_frame_id);
00604 tf_broadcaster_.sendTransform(tmp_tf_stamped);
00605 }
00606
00610 void PFLocalizationNode::publishPose()
00611 {
00612 mrpt::math::CMatrixDouble33
00613 cov;
00614 mrpt::poses::CPose2D mean;
00615
00616 pdf_.getCovarianceAndMean(cov, mean);
00617
00618 geometry_msgs::PoseWithCovarianceStamped p;
00619
00620
00621 p.header.frame_id =
00622 tf::resolve(param()->tf_prefix, param()->global_frame_id);
00623 if (loop_count_ < 10 || state_ == IDLE)
00624 p.header.stamp = ros::Time::now();
00625
00626 else
00627 mrpt_bridge::convert(time_last_update_, p.header.stamp);
00628
00629
00630 mrpt_bridge::convert(mean, p.pose.pose);
00631
00632
00633 for (int i = 0; i < 3; i++)
00634 {
00635 for (int j = 0; j < 3; j++)
00636 {
00637 int ros_i = i;
00638 int ros_j = j;
00639 if (i == 2 || j == 2)
00640 {
00641 ros_i = i == 2 ? 5 : i;
00642 ros_j = j == 2 ? 5 : j;
00643 }
00644 p.pose.covariance[ros_i * 6 + ros_j] = cov(i, j);
00645 }
00646 }
00647
00648 pub_pose_.publish(p);
00649 }
00650
00651 void PFLocalizationNode::useROSLogLevel()
00652 {
00653
00654
00655 std::map<std::string, ros::console::levels::Level> loggers;
00656 ros::console::get_loggers(loggers);
00657 if (loggers.find("ros.roscpp") != loggers.end())
00658 pdf_.setVerbosityLevel(
00659 static_cast<mrpt::utils::VerbosityLevel>(loggers["ros.roscpp"]));
00660 if (loggers.find("ros.mrpt_localization") != loggers.end())
00661 pdf_.setVerbosityLevel(
00662 static_cast<mrpt::utils::VerbosityLevel>(
00663 loggers["ros.mrpt_localization"]));
00664 }