mrpt_localization_node.cpp
Go to the documentation of this file.
00001 /***********************************************************************************
00002  * Revised BSD License *
00003  * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at> *
00004  * All rights reserved. *
00005  *                                                                                 *
00006  * Redistribution and use in source and binary forms, with or without *
00007  * modification, are permitted provided that the following conditions are met: *
00008  *     * Redistributions of source code must retain the above copyright *
00009  *       notice, this list of conditions and the following disclaimer. *
00010  *     * Redistributions in binary form must reproduce the above copyright *
00011  *       notice, this list of conditions and the following disclaimer in the *
00012  *       documentation and/or other materials provided with the distribution. *
00013  *     * Neither the name of the Vienna University of Technology nor the *
00014  *       names of its contributors may be used to endorse or promote products *
00015  *       derived from this software without specific prior written permission. *
00016  *                                                                                 *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  *AND *
00019  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020  **
00021  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
00022  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY *
00023  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
00024  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  **
00026  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
00027  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
00028  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00029  **
00030  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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         // Use MRPT library the same log level as on ROS nodes (only for
00079         // MRPT_VERSION >= 0x150)
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         // Subscribe to one or more laser sources:
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         // ROS_INFO("callbackLaser");
00187         CObservation2DRangeScan::Ptr laser =
00188                 mrpt::make_aligned_shared<CObservation2DRangeScan>();
00189 
00190         // printf("callbackLaser %s\n", _msg.header.frame_id.c_str());
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)  // updating filter; we must be moving or
00196         // update_while_stopped set to true
00197         {
00198                 // mrpt::poses::CPose3D pose = laser_poses_[_msg.header.frame_id];
00199                 // ROS_INFO("LASER POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
00200                 // pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
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         // ROS_INFO("callbackBeacon");
00223         CObservationBeaconRanges::Ptr beacon =
00224                 mrpt::make_aligned_shared<CObservationBeaconRanges>();
00225         // printf("callbackBeacon %s\n", _msg.header.frame_id.c_str());
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)  // updating filter; we must be moving or
00231         // update_while_stopped set to true
00232         {
00233                 // mrpt::poses::CPose3D pose = beacon_poses_[_msg.header.frame_id];
00234                 // ROS_INFO("BEACON POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
00235                 // pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
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         // Robot pose externally provided; we update filter regardless state_
00259         // attribute's value, as these
00260         // corrections are typically independent from robot motion (e.g. inputs from
00261         // GPS or tracking system)
00262         // XXX admittedly an arbitrary choice; feel free to open an issue if you
00263         // think it doesn't make sense
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         // Transform observation into global frame, including covariance. For that,
00287         // we must first obtain
00288         // the global frame -> observation frame tf as a Pose msg, as required by
00289         // pose_cov_ops::compose
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         // Ensure the covariance matrix can be inverted (no zeros in the diagonal)
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         // Covert the received pose into an observation the filter can integrate
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         // We always update the filter if update_while_stopped is true, regardless
00449         // robot is moving or
00450         // not; otherwise, update filter if we are moving or at initialization (100
00451         // first iterations)
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                 // Last update time can be too far in the past if we where not updating
00544                 // filter, due to robot stopped or no
00545                 // observations for a while (we optionally show a warning in the second
00546                 // case)
00547                 // We use time zero if so when getting base -> odom tf to prevent an
00548                 // extrapolation into the past exception
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                 // Get base -> odom transform
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         // We want to send a transform that is good up until a tolerance time so
00597         // that odom can be used
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;  // cov for x, y, phi (meter, meter, radian)
00614         mrpt::poses::CPose2D mean;
00615 
00616         pdf_.getCovarianceAndMean(cov, mean);
00617 
00618         geometry_msgs::PoseWithCovarianceStamped p;
00619 
00620         // Fill in the header
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();  // on first iterations timestamp
00625         // differs a lot from ROS time
00626         else
00627                 mrpt_bridge::convert(time_last_update_, p.header.stamp);
00628 
00629         // Copy in the pose
00630         mrpt_bridge::convert(mean, p.pose.pose);
00631 
00632         // Copy in the covariance, converting from 3-D to 6-D
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         // Set ROS log level also on MRPT internal log system; level enums are fully
00654         // compatible
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 }


mrpt_localization
Author(s): Markus Bader, Raphael Zack
autogenerated on Mon Sep 18 2017 03:12:10