/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with this library; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * */ /* Author: Brian Gerkey */ #include <algorithm> #include <vector> #include <map> #include <cmath> #include <memory> #include <boost/bind.hpp> #include <boost/thread/mutex.hpp> // Signal handling #include <signal.h> #include "amcl/map/map.h" #include "amcl/pf/pf.h" #include "amcl/sensors/amcl_odom.h" #include "amcl/sensors/amcl_laser.h" #include "portable_utils.hpp" #include "ros/assert.h" // roscpp #include "ros/ros.h" // Messages that I need #include "sensor_msgs/LaserScan.h" #include "geometry_msgs/PoseWithCovarianceStamped.h" #include "geometry_msgs/PoseArray.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/PoseStamped.h" #include "nav_msgs/GetMap.h" #include "nav_msgs/SetMap.h" #include "std_srvs/Empty.h" // For transform support #include "tf2/LinearMath/Transform.h" #include "tf2/convert.h" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "tf2_ros/buffer.h" #include "tf2_ros/message_filter.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" #include "message_filters/subscriber.h" // Dynamic_reconfigure #include "dynamic_reconfigure/server.h" #include "amcl/AMCLConfig.h" // Allows AMCL to run from bag file #include <rosbag/bag.h> #include <rosbag/view.h> #include <boost/foreach.hpp> // For monitoring the estimator #include <diagnostic_updater/diagnostic_updater.h> #define NEW_UNIFORM_SAMPLING 1 using namespace amcl; // Pose hypothesis typedef struct { // Total weight (weights sum to 1) double weight; // Mean of pose esimate pf_vector_t pf_pose_mean; // Covariance of pose estimate pf_matrix_t pf_pose_cov; } amcl_hyp_t; static double normalize(double z) { return atan2(sin(z),cos(z)); } static double angle_diff(double a, double b) { double d1, d2; a = normalize(a); b = normalize(b); d1 = a-b; d2 = 2*M_PI - fabs(d1); if(d1 > 0) d2 *= -1.0; if(fabs(d1) < fabs(d2)) return(d1); else return(d2); } static const std::string scan_topic_ = "scan"; /* This function is only useful to have the whole code work * with old rosbags that have trailing slashes for their frames */ inline std::string stripSlash(const std::string& in) { std::string out = in; if ( ( !in.empty() ) && (in[0] == '/') ) out.erase(0,1); return out; } class AmclNode { public: AmclNode(); ~AmclNode(); /** * @brief Uses TF and LaserScan messages from bag file to drive AMCL instead * @param in_bag_fn input bagfile * @param trigger_global_localization whether to trigger global localization * before starting to process the bagfile */ void runFromBag(const std::string &in_bag_fn, bool trigger_global_localization = false); int process(); void savePoseToServer(); private: std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_; std::shared_ptr<tf2_ros::TransformListener> tfl_; std::shared_ptr<tf2_ros::Buffer> tf_; bool sent_first_transform_; tf2::Transform latest_tf_; bool latest_tf_valid_; // Pose-generating function used to uniformly distribute particles over // the map static pf_vector_t uniformPoseGenerator(void* arg); #if NEW_UNIFORM_SAMPLING static std::vector<std::pair<int,int> > free_space_indices; #endif // Callbacks bool globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); bool nomotionUpdateCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); bool setMapCallback(nav_msgs::SetMap::Request& req, nav_msgs::SetMap::Response& res); void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan); void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg); void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg); void handleMapMessage(const nav_msgs::OccupancyGrid& msg); void freeMapDependentMemory(); map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg ); void updatePoseFromServer(); void applyInitialPose(); //parameter for which odom to use std::string odom_frame_id_; //paramater to store latest odom pose geometry_msgs::PoseStamped latest_odom_pose_; //parameter for which base to use std::string base_frame_id_; std::string global_frame_id_; bool use_map_topic_; bool first_map_only_; ros::Duration gui_publish_period; ros::Time save_pose_last_time; ros::Duration save_pose_period; geometry_msgs::PoseWithCovarianceStamped last_published_pose; map_t* map_; char* mapdata; int sx, sy; double resolution; message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_; tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_; ros::Subscriber initial_pose_sub_; std::vector< AMCLLaser* > lasers_; std::vector< bool > lasers_update_; std::map< std::string, int > frame_to_laser_; // Particle filter pf_t *pf_; double pf_err_, pf_z_; bool pf_init_; pf_vector_t pf_odom_pose_; double d_thresh_, a_thresh_; int resample_interval_; int resample_count_; double laser_min_range_; double laser_max_range_; //Nomotion update control bool m_force_update; // used to temporarily let amcl update samples even when no motion occurs... AMCLOdom* odom_; AMCLLaser* laser_; ros::Duration cloud_pub_interval; ros::Time last_cloud_pub_time; // For slowing play-back when reading directly from a bag file ros::WallDuration bag_scan_period_; void requestMap(); // Helper to get odometric pose from transform system bool getOdomPose(geometry_msgs::PoseStamped& pose, double& x, double& y, double& yaw, const ros::Time& t, const std::string& f); //time for tolerance on the published transform, //basically defines how long a map->odom transform is good for ros::Duration transform_tolerance_; ros::NodeHandle nh_; ros::NodeHandle private_nh_; ros::Publisher pose_pub_; ros::Publisher particlecloud_pub_; ros::ServiceServer global_loc_srv_; ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion ros::ServiceServer set_map_srv_; ros::Subscriber initial_pose_sub_old_; ros::Subscriber map_sub_; diagnostic_updater::Updater diagnosic_updater_; void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status); double std_warn_level_x_; double std_warn_level_y_; double std_warn_level_yaw_; amcl_hyp_t* initial_pose_hyp_; bool first_map_received_; bool first_reconfigure_call_; boost::recursive_mutex configuration_mutex_; dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_; amcl::AMCLConfig default_config_; ros::Timer check_laser_timer_; int max_beams_, min_particles_, max_particles_; double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; double alpha_slow_, alpha_fast_; double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_; //beam skip related params bool do_beamskip_; double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_; double laser_likelihood_max_dist_; odom_model_t odom_model_type_; double init_pose_[3]; double init_cov_[3]; laser_model_t laser_model_type_; bool tf_broadcast_; bool selective_resampling_; void reconfigureCB(amcl::AMCLConfig &config, uint32_t level); ros::Time last_laser_received_ts_; ros::Duration laser_check_interval_; void checkLaserReceived(const ros::TimerEvent& event); }; #if NEW_UNIFORM_SAMPLING std::vector<std::pair<int,int> > AmclNode::free_space_indices; #endif #define USAGE "USAGE: amcl" boost::shared_ptr<AmclNode> amcl_node_ptr; void sigintHandler(int sig) { // Save latest pose as we're shutting down. amcl_node_ptr->savePoseToServer(); ros::shutdown(); } int main(int argc, char** argv) { ros::init(argc, argv, "amcl"); ros::NodeHandle nh; // Override default sigint handler signal(SIGINT, sigintHandler); // Make our node available to sigintHandler amcl_node_ptr.reset(new AmclNode()); if (argc == 1) { // run using ROS input ros::spin(); } else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag")) { if (argc == 3) { amcl_node_ptr->runFromBag(argv[2]); } else if ((argc == 4) && (std::string(argv[3]) == "--global-localization")) { amcl_node_ptr->runFromBag(argv[2], true); } } // Without this, our boost locks are not shut down nicely amcl_node_ptr.reset(); // To quote Morgan, Hooray! return(0); } AmclNode::AmclNode() : sent_first_transform_(false), latest_tf_valid_(false), map_(NULL), pf_(NULL), resample_count_(0), odom_(NULL), laser_(NULL), private_nh_("~"), initial_pose_hyp_(NULL), first_map_received_(false), first_reconfigure_call_(true) { boost::recursive_mutex::scoped_lock l(configuration_mutex_); // Grab params off the param server private_nh_.param("use_map_topic", use_map_topic_, false); private_nh_.param("first_map_only", first_map_only_, false); double tmp; private_nh_.param("gui_publish_rate", tmp, -1.0); gui_publish_period = ros::Duration(1.0/tmp); private_nh_.param("save_pose_rate", tmp, 0.5); save_pose_period = ros::Duration(1.0/tmp); private_nh_.param("laser_min_range", laser_min_range_, -1.0); private_nh_.param("laser_max_range", laser_max_range_, -1.0); private_nh_.param("laser_max_beams", max_beams_, 30); private_nh_.param("min_particles", min_particles_, 100); private_nh_.param("max_particles", max_particles_, 5000); private_nh_.param("kld_err", pf_err_, 0.01); private_nh_.param("kld_z", pf_z_, 0.99); private_nh_.param("odom_alpha1", alpha1_, 0.2); private_nh_.param("odom_alpha2", alpha2_, 0.2); private_nh_.param("odom_alpha3", alpha3_, 0.2); private_nh_.param("odom_alpha4", alpha4_, 0.2); private_nh_.param("odom_alpha5", alpha5_, 0.2); private_nh_.param("do_beamskip", do_beamskip_, false); private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5); private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3); if (private_nh_.hasParam("beam_skip_error_threshold_")) { private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_); } else { private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9); } private_nh_.param("laser_z_hit", z_hit_, 0.95); private_nh_.param("laser_z_short", z_short_, 0.1); private_nh_.param("laser_z_max", z_max_, 0.05); private_nh_.param("laser_z_rand", z_rand_, 0.05); private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2); private_nh_.param("laser_lambda_short", lambda_short_, 0.1); private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0); std::string tmp_model_type; private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field")); if(tmp_model_type == "beam") laser_model_type_ = LASER_MODEL_BEAM; else if(tmp_model_type == "likelihood_field") laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; else if(tmp_model_type == "likelihood_field_prob"){ laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB; } else { ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model", tmp_model_type.c_str()); laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; } private_nh_.param("odom_model_type", tmp_model_type, std::string("diff")); if(tmp_model_type == "diff") odom_model_type_ = ODOM_MODEL_DIFF; else if(tmp_model_type == "omni") odom_model_type_ = ODOM_MODEL_OMNI; else if(tmp_model_type == "diff-corrected") odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED; else if(tmp_model_type == "omni-corrected") odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED; else { ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model", tmp_model_type.c_str()); odom_model_type_ = ODOM_MODEL_DIFF; } private_nh_.param("update_min_d", d_thresh_, 0.2); private_nh_.param("update_min_a", a_thresh_, M_PI/6.0); private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom")); private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link")); private_nh_.param("global_frame_id", global_frame_id_, std::string("map")); private_nh_.param("resample_interval", resample_interval_, 2); private_nh_.param("selective_resampling", selective_resampling_, false); double tmp_tol; private_nh_.param("transform_tolerance", tmp_tol, 0.1); private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001); private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1); private_nh_.param("tf_broadcast", tf_broadcast_, true); // For diagnostics private_nh_.param("std_warn_level_x", std_warn_level_x_, 0.2); private_nh_.param("std_warn_level_y", std_warn_level_y_, 0.2); private_nh_.param("std_warn_level_yaw", std_warn_level_yaw_, 0.1); transform_tolerance_.fromSec(tmp_tol); { double bag_scan_period; private_nh_.param("bag_scan_period", bag_scan_period, -1.0); bag_scan_period_.fromSec(bag_scan_period); } odom_frame_id_ = stripSlash(odom_frame_id_); base_frame_id_ = stripSlash(base_frame_id_); global_frame_id_ = stripSlash(global_frame_id_); updatePoseFromServer(); cloud_pub_interval.fromSec(1.0); tfb_.reset(new tf2_ros::TransformBroadcaster()); tf_.reset(new tf2_ros::Buffer()); tfl_.reset(new tf2_ros::TransformListener(*tf_)); pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true); particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true); global_loc_srv_ = nh_.advertiseService("global_localization", &AmclNode::globalLocalizationCallback, this); nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this); set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this); laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100); laser_scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, *tf_, odom_frame_id_, 100, nh_); laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1)); initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this); if(use_map_topic_) { map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this); ROS_INFO("Subscribed to map topic."); } else { requestMap(); } m_force_update = false; dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~")); dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); // 15s timer to warn on lack of receipt of laser scans, #5209 laser_check_interval_ = ros::Duration(15.0); check_laser_timer_ = nh_.createTimer(laser_check_interval_, boost::bind(&AmclNode::checkLaserReceived, this, _1)); diagnosic_updater_.setHardwareID("None"); diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics); } void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level) { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); //we don't want to do anything on the first call //which corresponds to startup if(first_reconfigure_call_) { first_reconfigure_call_ = false; default_config_ = config; return; } if(config.restore_defaults) { config = default_config_; //avoid looping config.restore_defaults = false; } d_thresh_ = config.update_min_d; a_thresh_ = config.update_min_a; resample_interval_ = config.resample_interval; laser_min_range_ = config.laser_min_range; laser_max_range_ = config.laser_max_range; gui_publish_period = ros::Duration(1.0/config.gui_publish_rate); save_pose_period = ros::Duration(1.0/config.save_pose_rate); transform_tolerance_.fromSec(config.transform_tolerance); max_beams_ = config.laser_max_beams; alpha1_ = config.odom_alpha1; alpha2_ = config.odom_alpha2; alpha3_ = config.odom_alpha3; alpha4_ = config.odom_alpha4; alpha5_ = config.odom_alpha5; z_hit_ = config.laser_z_hit; z_short_ = config.laser_z_short; z_max_ = config.laser_z_max; z_rand_ = config.laser_z_rand; sigma_hit_ = config.laser_sigma_hit; lambda_short_ = config.laser_lambda_short; laser_likelihood_max_dist_ = config.laser_likelihood_max_dist; if(config.laser_model_type == "beam") laser_model_type_ = LASER_MODEL_BEAM; else if(config.laser_model_type == "likelihood_field") laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; else if(config.laser_model_type == "likelihood_field_prob") laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB; if(config.odom_model_type == "diff") odom_model_type_ = ODOM_MODEL_DIFF; else if(config.odom_model_type == "omni") odom_model_type_ = ODOM_MODEL_OMNI; else if(config.odom_model_type == "diff-corrected") odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED; else if(config.odom_model_type == "omni-corrected") odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED; if(config.min_particles > config.max_particles) { ROS_WARN("You've set min_particles to be greater than max particles, this isn't allowed so they'll be set to be equal."); config.max_particles = config.min_particles; } min_particles_ = config.min_particles; max_particles_ = config.max_particles; alpha_slow_ = config.recovery_alpha_slow; alpha_fast_ = config.recovery_alpha_fast; tf_broadcast_ = config.tf_broadcast; do_beamskip_= config.do_beamskip; beam_skip_distance_ = config.beam_skip_distance; beam_skip_threshold_ = config.beam_skip_threshold; // Clear queued laser objects so that their parameters get updated lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear(); if( pf_ != NULL ) { pf_free( pf_ ); pf_ = NULL; } pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, (void *)map_); pf_set_selective_resampling(pf_, selective_resampling_); pf_err_ = config.kld_err; pf_z_ = config.kld_z; pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; // Initialize the filter pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x; pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y; pf_init_pose_mean.v[2] = tf2::getYaw(last_published_pose.pose.pose.orientation); pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0]; pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1]; pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5]; pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); pf_init_ = false; // Instantiate the sensor objects // Odometry delete odom_; odom_ = new AMCLOdom(); ROS_ASSERT(odom_); odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ ); // Laser delete laser_; laser_ = new AMCLLaser(max_beams_, map_); ROS_ASSERT(laser_); if(laser_model_type_ == LASER_MODEL_BEAM) laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, 0.0); else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){ ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_); ROS_INFO("Done initializing likelihood field model with probabilities."); } else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD){ ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_); ROS_INFO("Done initializing likelihood field model."); } odom_frame_id_ = stripSlash(config.odom_frame_id); base_frame_id_ = stripSlash(config.base_frame_id); global_frame_id_ = stripSlash(config.global_frame_id); delete laser_scan_filter_; laser_scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, *tf_, odom_frame_id_, 100, nh_); laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1)); initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this); } void AmclNode::runFromBag(const std::string &in_bag_fn, bool trigger_global_localization) { rosbag::Bag bag; bag.open(in_bag_fn, rosbag::bagmode::Read); std::vector<std::string> topics; topics.push_back(std::string("tf")); std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS topics.push_back(scan_topic_name); rosbag::View view(bag, rosbag::TopicQuery(topics)); ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100); ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100); // Sleep for a second to let all subscribers connect ros::WallDuration(1.0).sleep(); ros::WallTime start(ros::WallTime::now()); // Wait for map while (ros::ok()) { { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); if (map_) { ROS_INFO("Map is ready"); break; } } ROS_INFO("Waiting for the map..."); ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0)); } if (trigger_global_localization) { std_srvs::Empty empty_srv; globalLocalizationCallback(empty_srv.request, empty_srv.response); } BOOST_FOREACH(rosbag::MessageInstance const msg, view) { if (!ros::ok()) { break; } // Process any ros messages or callbacks at this point ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration()); tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>(); if (tf_msg != NULL) { tf_pub.publish(msg); for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii) { tf_->setTransform(tf_msg->transforms[ii], "rosbag_authority"); } continue; } sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>(); if (base_scan != NULL) { laser_pub.publish(msg); laser_scan_filter_->add(base_scan); if (bag_scan_period_ > ros::WallDuration(0)) { bag_scan_period_.sleep(); } continue; } ROS_WARN_STREAM("Unsupported message type" << msg.getTopic()); } bag.close(); double runtime = (ros::WallTime::now() - start).toSec(); ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime); const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation); double yaw, pitch, roll; tf2::Matrix3x3(tf2::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll); ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f", last_published_pose.pose.pose.position.x, last_published_pose.pose.pose.position.y, yaw, last_published_pose.header.stamp.toSec() ); ros::shutdown(); } void AmclNode::savePoseToServer() { // We need to apply the last transform to the latest odom pose to get // the latest map pose to store. We'll take the covariance from // last_published_pose. tf2::Transform odom_pose_tf2; tf2::convert(latest_odom_pose_.pose, odom_pose_tf2); tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2; double yaw = tf2::getYaw(map_pose.getRotation()); ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() ); private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x()); private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y()); private_nh_.setParam("initial_pose_a", yaw); private_nh_.setParam("initial_cov_xx", last_published_pose.pose.covariance[6*0+0]); private_nh_.setParam("initial_cov_yy", last_published_pose.pose.covariance[6*1+1]); private_nh_.setParam("initial_cov_aa", last_published_pose.pose.covariance[6*5+5]); } void AmclNode::updatePoseFromServer() { init_pose_[0] = 0.0; init_pose_[1] = 0.0; init_pose_[2] = 0.0; init_cov_[0] = 0.5 * 0.5; init_cov_[1] = 0.5 * 0.5; init_cov_[2] = (M_PI/12.0) * (M_PI/12.0); // Check for NAN on input from param server, #5239 double tmp_pos; private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]); if(!std::isnan(tmp_pos)) init_pose_[0] = tmp_pos; else ROS_WARN("ignoring NAN in initial pose X position"); private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]); if(!std::isnan(tmp_pos)) init_pose_[1] = tmp_pos; else ROS_WARN("ignoring NAN in initial pose Y position"); private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]); if(!std::isnan(tmp_pos)) init_pose_[2] = tmp_pos; else ROS_WARN("ignoring NAN in initial pose Yaw"); private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]); if(!std::isnan(tmp_pos)) init_cov_[0] =tmp_pos; else ROS_WARN("ignoring NAN in initial covariance XX"); private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]); if(!std::isnan(tmp_pos)) init_cov_[1] = tmp_pos; else ROS_WARN("ignoring NAN in initial covariance YY"); private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]); if(!std::isnan(tmp_pos)) init_cov_[2] = tmp_pos; else ROS_WARN("ignoring NAN in initial covariance AA"); } void AmclNode::checkLaserReceived(const ros::TimerEvent& event) { ros::Duration d = ros::Time::now() - last_laser_received_ts_; if(d > laser_check_interval_) { ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds. Verify that data is being published on the %s topic.", d.toSec(), ros::names::resolve(scan_topic_).c_str()); } } void AmclNode::requestMap() { boost::recursive_mutex::scoped_lock ml(configuration_mutex_); // get map via RPC nav_msgs::GetMap::Request req; nav_msgs::GetMap::Response resp; ROS_INFO("Requesting the map..."); while(!ros::service::call("static_map", req, resp)) { ROS_WARN("Request for map failed; trying again..."); ros::Duration d(0.5); d.sleep(); } handleMapMessage( resp.map ); } void AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg) { if( first_map_only_ && first_map_received_ ) { return; } handleMapMessage( *msg ); first_map_received_ = true; } void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); ROS_INFO("Received a %d X %d map @ %.3f m/pix\n", msg.info.width, msg.info.height, msg.info.resolution); if(msg.header.frame_id != global_frame_id_) ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics", msg.header.frame_id.c_str(), global_frame_id_.c_str()); freeMapDependentMemory(); // Clear queued laser objects because they hold pointers to the existing // map, #5202. lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear(); map_ = convertMap(msg); #if NEW_UNIFORM_SAMPLING // Index of free space free_space_indices.resize(0); for(int i = 0; i < map_->size_x; i++) for(int j = 0; j < map_->size_y; j++) if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1) free_space_indices.push_back(std::make_pair(i,j)); #endif // Create the particle filter pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, (void *)map_); pf_set_selective_resampling(pf_, selective_resampling_); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; // Initialize the filter updatePoseFromServer(); pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = init_pose_[0]; pf_init_pose_mean.v[1] = init_pose_[1]; pf_init_pose_mean.v[2] = init_pose_[2]; pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); pf_init_pose_cov.m[0][0] = init_cov_[0]; pf_init_pose_cov.m[1][1] = init_cov_[1]; pf_init_pose_cov.m[2][2] = init_cov_[2]; pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); pf_init_ = false; // Instantiate the sensor objects // Odometry delete odom_; odom_ = new AMCLOdom(); ROS_ASSERT(odom_); odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ ); // Laser delete laser_; laser_ = new AMCLLaser(max_beams_, map_); ROS_ASSERT(laser_); if(laser_model_type_ == LASER_MODEL_BEAM) laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, 0.0); else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){ ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_); ROS_INFO("Done initializing likelihood field model."); } else { ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_); ROS_INFO("Done initializing likelihood field model."); } // In case the initial pose message arrived before the first map, // try to apply the initial pose now that the map has arrived. applyInitialPose(); } void AmclNode::freeMapDependentMemory() { if( map_ != NULL ) { map_free( map_ ); map_ = NULL; } if( pf_ != NULL ) { pf_free( pf_ ); pf_ = NULL; } delete odom_; odom_ = NULL; delete laser_; laser_ = NULL; } /** * Convert an OccupancyGrid map message into the internal * representation. This allocates a map_t and returns it. */ map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg ) { map_t* map = map_alloc(); ROS_ASSERT(map); map->size_x = map_msg.info.width; map->size_y = map_msg.info.height; map->scale = map_msg.info.resolution; map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale; map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale; // Convert to player format map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y); ROS_ASSERT(map->cells); for(int i=0;i<map->size_x * map->size_y;i++) { if(map_msg.data[i] == 0) map->cells[i].occ_state = -1; else if(map_msg.data[i] == 100) map->cells[i].occ_state = +1; else map->cells[i].occ_state = 0; } return map; } AmclNode::~AmclNode() { delete dsrv_; freeMapDependentMemory(); delete laser_scan_filter_; delete laser_scan_sub_; // TODO: delete everything allocated in constructor } bool AmclNode::getOdomPose(geometry_msgs::PoseStamped& odom_pose, double& x, double& y, double& yaw, const ros::Time& t, const std::string& f) { // Get the robot's pose geometry_msgs::PoseStamped ident; ident.header.frame_id = stripSlash(f); ident.header.stamp = t; tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); try { this->tf_->transform(ident, odom_pose, odom_frame_id_); } catch(tf2::TransformException e) { ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); return false; } x = odom_pose.pose.position.x; y = odom_pose.pose.position.y; yaw = tf2::getYaw(odom_pose.pose.orientation); return true; } pf_vector_t AmclNode::uniformPoseGenerator(void* arg) { map_t* map = (map_t*)arg; #if NEW_UNIFORM_SAMPLING unsigned int rand_index = drand48() * free_space_indices.size(); std::pair<int,int> free_point = free_space_indices[rand_index]; pf_vector_t p; p.v[0] = MAP_WXGX(map, free_point.first); p.v[1] = MAP_WYGY(map, free_point.second); p.v[2] = drand48() * 2 * M_PI - M_PI; #else double min_x, max_x, min_y, max_y; min_x = (map->size_x * map->scale)/2.0 - map->origin_x; max_x = (map->size_x * map->scale)/2.0 + map->origin_x; min_y = (map->size_y * map->scale)/2.0 - map->origin_y; max_y = (map->size_y * map->scale)/2.0 + map->origin_y; pf_vector_t p; ROS_DEBUG("Generating new uniform sample"); for(;;) { p.v[0] = min_x + drand48() * (max_x - min_x); p.v[1] = min_y + drand48() * (max_y - min_y); p.v[2] = drand48() * 2 * M_PI - M_PI; // Check that it's a free cell int i,j; i = MAP_GXWX(map, p.v[0]); j = MAP_GYWY(map, p.v[1]); if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1)) break; } #endif return p; } bool AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { if( map_ == NULL ) { return true; } boost::recursive_mutex::scoped_lock gl(configuration_mutex_); ROS_INFO("Initializing with uniform distribution"); pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, (void *)map_); ROS_INFO("Global initialisation done!"); pf_init_ = false; return true; } // force nomotion updates (amcl updating without requiring motion) bool AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { m_force_update = true; //ROS_INFO("Requesting no-motion update"); return true; } bool AmclNode::setMapCallback(nav_msgs::SetMap::Request& req, nav_msgs::SetMap::Response& res) { handleMapMessage(req.map); handleInitialPoseMessage(req.initial_pose); res.success = true; return true; } void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) { std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id); last_laser_received_ts_ = ros::Time::now(); if( map_ == NULL ) { return; } boost::recursive_mutex::scoped_lock lr(configuration_mutex_); int laser_index = -1; // Do we have the base->base_laser Tx yet? if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end()) { ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str()); lasers_.push_back(new AMCLLaser(*laser_)); lasers_update_.push_back(true); laser_index = frame_to_laser_.size(); geometry_msgs::PoseStamped ident; ident.header.frame_id = laser_scan_frame_id; ident.header.stamp = ros::Time(); tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); geometry_msgs::PoseStamped laser_pose; try { this->tf_->transform(ident, laser_pose, base_frame_id_); } catch(tf2::TransformException& e) { ROS_ERROR("Couldn't transform from %s to %s, " "even though the message notifier is in use", laser_scan_frame_id.c_str(), base_frame_id_.c_str()); return; } pf_vector_t laser_pose_v; laser_pose_v.v[0] = laser_pose.pose.position.x; laser_pose_v.v[1] = laser_pose.pose.position.y; // laser mounting angle gets computed later -> set to 0 here! laser_pose_v.v[2] = 0; lasers_[laser_index]->SetLaserPose(laser_pose_v); ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f", laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]); frame_to_laser_[laser_scan_frame_id] = laser_index; } else { // we have the laser pose, retrieve laser index laser_index = frame_to_laser_[laser_scan_frame_id]; } // Where was the robot when this scan was taken? pf_vector_t pose; if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2], laser_scan->header.stamp, base_frame_id_)) { ROS_ERROR("Couldn't determine robot's pose associated with laser scan"); return; } pf_vector_t delta = pf_vector_zero(); if(pf_init_) { // Compute change in pose //delta = pf_vector_coord_sub(pose, pf_odom_pose_); delta.v[0] = pose.v[0] - pf_odom_pose_.v[0]; delta.v[1] = pose.v[1] - pf_odom_pose_.v[1]; delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]); // See if we should update the filter bool update = fabs(delta.v[0]) > d_thresh_ || fabs(delta.v[1]) > d_thresh_ || fabs(delta.v[2]) > a_thresh_; update = update || m_force_update; m_force_update=false; // Set the laser update flags if(update) for(unsigned int i=0; i < lasers_update_.size(); i++) lasers_update_[i] = true; } bool force_publication = false; if(!pf_init_) { // Pose at last filter update pf_odom_pose_ = pose; // Filter is now initialized pf_init_ = true; // Should update sensor data for(unsigned int i=0; i < lasers_update_.size(); i++) lasers_update_[i] = true; force_publication = true; resample_count_ = 0; } // If the robot has moved, update the filter else if(pf_init_ && lasers_update_[laser_index]) { //printf("pose\n"); //pf_vector_fprintf(pose, stdout, "%.3f"); AMCLOdomData odata; odata.pose = pose; // HACK // Modify the delta in the action data so the filter gets // updated correctly odata.delta = delta; // Use the action data to update the filter odom_->UpdateAction(pf_, (AMCLSensorData*)&odata); // Pose at last filter update //this->pf_odom_pose = pose; } bool resampled = false; // If the robot has moved, update the filter if(lasers_update_[laser_index]) { AMCLLaserData ldata; ldata.sensor = lasers_[laser_index]; ldata.range_count = laser_scan->ranges.size(); // To account for lasers that are mounted upside-down, we determine the // min, max, and increment angles of the laser in the base frame. // // Construct min and max angles of laser, in the base_link frame. tf2::Quaternion q; q.setRPY(0.0, 0.0, laser_scan->angle_min); geometry_msgs::QuaternionStamped min_q, inc_q; min_q.header.stamp = laser_scan->header.stamp; min_q.header.frame_id = stripSlash(laser_scan->header.frame_id); tf2::convert(q, min_q.quaternion); q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); inc_q.header = min_q.header; tf2::convert(q, inc_q.quaternion); try { tf_->transform(min_q, min_q, base_frame_id_); tf_->transform(inc_q, inc_q, base_frame_id_); } catch(tf2::TransformException& e) { ROS_WARN("Unable to transform min/max laser angles into base frame: %s", e.what()); return; } double angle_min = tf2::getYaw(min_q.quaternion); double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min; // wrapping angle to [-pi .. pi] angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI; ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); // Apply range min/max thresholds, if the user supplied them if(laser_max_range_ > 0.0) ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_); else ldata.range_max = laser_scan->range_max; double range_min; if(laser_min_range_ > 0.0) range_min = std::max(laser_scan->range_min, (float)laser_min_range_); else range_min = laser_scan->range_min; if(ldata.range_max <= 0.0 || range_min < 0.0) { ROS_ERROR("range_max or range_min from laser is negative! ignore this message."); return; // ignore this. } // The AMCLLaserData destructor will free this memory ldata.ranges = new double[ldata.range_count][2]; ROS_ASSERT(ldata.ranges); for(int i=0;i<ldata.range_count;i++) { // amcl doesn't (yet) have a concept of min range. So we'll map short // readings to max range. if(laser_scan->ranges[i] <= range_min) ldata.ranges[i][0] = ldata.range_max; else if(laser_scan->ranges[i] > ldata.range_max) ldata.ranges[i][0] = std::numeric_limits<decltype(ldata.range_max)>::max(); else ldata.ranges[i][0] = laser_scan->ranges[i]; // Compute bearing ldata.ranges[i][1] = angle_min + (i * angle_increment); } lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata); lasers_update_[laser_index] = false; pf_odom_pose_ = pose; // Resample the particles if(!(++resample_count_ % resample_interval_)) { pf_update_resample(pf_); resampled = true; } pf_sample_set_t* set = pf_->sets + pf_->current_set; ROS_DEBUG("Num samples: %d\n", set->sample_count); // Publish the resulting cloud // TODO: set maximum rate for publishing if (!m_force_update) { geometry_msgs::PoseArray cloud_msg; cloud_msg.header.stamp = ros::Time::now(); cloud_msg.header.frame_id = global_frame_id_; cloud_msg.poses.resize(set->sample_count); for(int i=0;i<set->sample_count;i++) { cloud_msg.poses[i].position.x = set->samples[i].pose.v[0]; cloud_msg.poses[i].position.y = set->samples[i].pose.v[1]; cloud_msg.poses[i].position.z = 0; tf2::Quaternion q; q.setRPY(0, 0, set->samples[i].pose.v[2]); tf2::convert(q, cloud_msg.poses[i].orientation); } particlecloud_pub_.publish(cloud_msg); } } if(resampled || force_publication) { // Read out the current hypotheses double max_weight = 0.0; int max_weight_hyp = -1; std::vector<amcl_hyp_t> hyps; hyps.resize(pf_->sets[pf_->current_set].cluster_count); for(int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) { double weight; pf_vector_t pose_mean; pf_matrix_t pose_cov; if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) { ROS_ERROR("Couldn't get stats on cluster %d", hyp_count); break; } hyps[hyp_count].weight = weight; hyps[hyp_count].pf_pose_mean = pose_mean; hyps[hyp_count].pf_pose_cov = pose_cov; if(hyps[hyp_count].weight > max_weight) { max_weight = hyps[hyp_count].weight; max_weight_hyp = hyp_count; } } if(max_weight > 0.0) { ROS_DEBUG("Max weight pose: %.3f %.3f %.3f", hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1], hyps[max_weight_hyp].pf_pose_mean.v[2]); /* puts(""); pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f"); puts(""); */ geometry_msgs::PoseWithCovarianceStamped p; // Fill in the header p.header.frame_id = global_frame_id_; p.header.stamp = laser_scan->header.stamp; // Copy in the pose p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; tf2::Quaternion q; q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]); tf2::convert(q, p.pose.pose.orientation); // Copy in the covariance, converting from 3-D to 6-D pf_sample_set_t* set = pf_->sets + pf_->current_set; for(int i=0; i<2; i++) { for(int j=0; j<2; j++) { // Report the overall filter covariance, rather than the // covariance for the highest-weight cluster //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; p.pose.covariance[6*i+j] = set->cov.m[i][j]; } } // Report the overall filter covariance, rather than the // covariance for the highest-weight cluster //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2]; p.pose.covariance[6*5+5] = set->cov.m[2][2]; /* printf("cov:\n"); for(int i=0; i<6; i++) { for(int j=0; j<6; j++) printf("%6.3f ", p.covariance[6*i+j]); puts(""); } */ pose_pub_.publish(p); last_published_pose = p; ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1], hyps[max_weight_hyp].pf_pose_mean.v[2]); // subtracting base to odom from map to base and send map to odom instead geometry_msgs::PoseStamped odom_to_map; try { tf2::Quaternion q; q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]); tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0)); geometry_msgs::PoseStamped tmp_tf_stamped; tmp_tf_stamped.header.frame_id = base_frame_id_; tmp_tf_stamped.header.stamp = laser_scan->header.stamp; tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose); this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_); } catch(tf2::TransformException) { ROS_DEBUG("Failed to subtract base to odom transform"); return; } tf2::convert(odom_to_map.pose, latest_tf_); latest_tf_valid_ = true; if (tf_broadcast_ == true) { // We want to send a transform that is good up until a // tolerance time so that odom can be used ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_); geometry_msgs::TransformStamped tmp_tf_stamped; tmp_tf_stamped.header.frame_id = global_frame_id_; tmp_tf_stamped.header.stamp = transform_expiration; tmp_tf_stamped.child_frame_id = odom_frame_id_; tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); this->tfb_->sendTransform(tmp_tf_stamped); sent_first_transform_ = true; } } else { ROS_ERROR("No pose!"); } } else if(latest_tf_valid_) { if (tf_broadcast_ == true) { // Nothing changed, so we'll just republish the last transform, to keep // everybody happy. ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_); geometry_msgs::TransformStamped tmp_tf_stamped; tmp_tf_stamped.header.frame_id = global_frame_id_; tmp_tf_stamped.header.stamp = transform_expiration; tmp_tf_stamped.child_frame_id = odom_frame_id_; tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); this->tfb_->sendTransform(tmp_tf_stamped); } // Is it time to save our last pose to the param server ros::Time now = ros::Time::now(); if((save_pose_period.toSec() > 0.0) && (now - save_pose_last_time) >= save_pose_period) { this->savePoseToServer(); save_pose_last_time = now; } } diagnosic_updater_.update(); } void AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { handleInitialPoseMessage(*msg); } void AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg) { boost::recursive_mutex::scoped_lock prl(configuration_mutex_); if(msg.header.frame_id == "") { // This should be removed at some point ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id."); } // We only accept initial pose estimates in the global frame, #5148. else if(stripSlash(msg.header.frame_id) != global_frame_id_) { ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"", stripSlash(msg.header.frame_id).c_str(), global_frame_id_.c_str()); return; } // In case the client sent us a pose estimate in the past, integrate the // intervening odometric change. geometry_msgs::TransformStamped tx_odom; try { ros::Time now = ros::Time::now(); // wait a little for the latest tf to become available tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp, base_frame_id_, ros::Time::now(), odom_frame_id_, ros::Duration(0.5)); } catch(tf2::TransformException e) { // If we've never sent a transform, then this is normal, because the // global_frame_id_ frame doesn't exist. We only care about in-time // transformation for on-the-move pose-setting, so ignoring this // startup condition doesn't really cost us anything. if(sent_first_transform_) ROS_WARN("Failed to transform initial pose in time (%s)", e.what()); tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform); } tf2::Transform tx_odom_tf2; tf2::convert(tx_odom.transform, tx_odom_tf2); tf2::Transform pose_old, pose_new; tf2::convert(msg.pose.pose, pose_old); pose_new = pose_old * tx_odom_tf2; // Transform into the global frame ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f", ros::Time::now().toSec(), pose_new.getOrigin().x(), pose_new.getOrigin().y(), tf2::getYaw(pose_new.getRotation())); // Re-initialize the filter pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = pose_new.getOrigin().x(); pf_init_pose_mean.v[1] = pose_new.getOrigin().y(); pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation()); pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); // Copy in the covariance, converting from 6-D to 3-D for(int i=0; i<2; i++) { for(int j=0; j<2; j++) { pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j]; } } pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5]; delete initial_pose_hyp_; initial_pose_hyp_ = new amcl_hyp_t(); initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; applyInitialPose(); } /** * If initial_pose_hyp_ and map_ are both non-null, apply the initial * pose to the particle filter state. initial_pose_hyp_ is deleted * and set to NULL after it is used. */ void AmclNode::applyInitialPose() { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); if( initial_pose_hyp_ != NULL && map_ != NULL ) { pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov); pf_init_ = false; delete initial_pose_hyp_; initial_pose_hyp_ = NULL; } } void AmclNode::standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status) { double std_x = sqrt(last_published_pose.pose.covariance[6*0+0]); double std_y = sqrt(last_published_pose.pose.covariance[6*1+1]); double std_yaw = sqrt(last_published_pose.pose.covariance[6*5+5]); diagnostic_status.add("std_x", std_x); diagnostic_status.add("std_y", std_y); diagnostic_status.add("std_yaw", std_yaw); diagnostic_status.add("std_warn_level_x", std_warn_level_x_); diagnostic_status.add("std_warn_level_y", std_warn_level_y_); diagnostic_status.add("std_warn_level_yaw", std_warn_level_yaw_); if (std_x > std_warn_level_x_ || std_y > std_warn_level_y_ || std_yaw > std_warn_level_yaw_) { diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too large"); } else { diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK"); } }