Public Member Functions | Private Member Functions | Static Private Member Functions | Private Attributes | Static Private Attributes | List of all members
AmclNode Class Reference

Public Member Functions

 AmclNode ()
 
int process ()
 
void runFromBag (const std::string &in_bag_fn, bool trigger_global_localization=false)
 Uses TF and LaserScan messages from bag file to drive AMCL instead. More...
 
void savePoseToServer ()
 
 ~AmclNode ()
 

Private Member Functions

void applyInitialPose ()
 
void checkLaserReceived (const ros::TimerEvent &event)
 
map_t * convertMap (const nav_msgs::OccupancyGrid &map_msg)
 
void freeMapDependentMemory ()
 
bool getOdomPose (geometry_msgs::PoseStamped &pose, double &x, double &y, double &yaw, const ros::Time &t, const std::string &f)
 
bool globalLocalizationCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
 
void handleInitialPoseMessage (const geometry_msgs::PoseWithCovarianceStamped &msg)
 
void handleMapMessage (const nav_msgs::OccupancyGrid &msg)
 
void initialPoseReceived (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
 
void laserReceived (const sensor_msgs::LaserScanConstPtr &laser_scan)
 
void mapReceived (const nav_msgs::OccupancyGridConstPtr &msg)
 
bool nomotionUpdateCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
 
void reconfigureCB (amcl::AMCLConfig &config, uint32_t level)
 
void requestMap ()
 
bool setMapCallback (nav_msgs::SetMap::Request &req, nav_msgs::SetMap::Response &res)
 
void standardDeviationDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status)
 
void updatePoseFromServer ()
 

Static Private Member Functions

static pf_vector_t uniformPoseGenerator (void *arg)
 

Private Attributes

double a_thresh_
 
double alpha1_
 
double alpha2_
 
double alpha3_
 
double alpha4_
 
double alpha5_
 
double alpha_fast_
 
double alpha_slow_
 
ros::WallDuration bag_scan_period_
 
std::string base_frame_id_
 
double beam_skip_distance_
 
double beam_skip_error_threshold_
 
double beam_skip_threshold_
 
ros::Timer check_laser_timer_
 
ros::Duration cloud_pub_interval
 
boost::recursive_mutex configuration_mutex_
 
double d_thresh_
 
amcl::AMCLConfig default_config_
 
diagnostic_updater::Updater diagnosic_updater_
 
bool do_beamskip_
 
dynamic_reconfigure::Server< amcl::AMCLConfig > * dsrv_
 
bool first_map_only_
 
bool first_map_received_
 
bool first_reconfigure_call_
 
bool force_update_after_initialpose_
 
bool force_update_after_set_map_
 
std::map< std::string, int > frame_to_laser_
 
std::string global_frame_id_
 
ros::ServiceServer global_loc_srv_
 
ros::Duration gui_publish_period
 
double init_cov_ [3]
 
double init_pose_ [3]
 
amcl_hyp_t * initial_pose_hyp_
 
ros::Subscriber initial_pose_sub_
 
ros::Subscriber initial_pose_sub_old_
 
double lambda_short_
 
AMCLLaser * laser_
 
ros::Duration laser_check_interval_
 
double laser_likelihood_max_dist_
 
double laser_max_range_
 
double laser_min_range_
 
laser_model_t laser_model_type_
 
tf2_ros::MessageFilter< sensor_msgs::LaserScan > * laser_scan_filter_
 
message_filters::Subscriber< sensor_msgs::LaserScan > * laser_scan_sub_
 
std::vector< AMCLLaser * > lasers_
 
std::vector< bool > lasers_update_
 
ros::Time last_cloud_pub_time
 
ros::Time last_laser_received_ts_
 
geometry_msgs::PoseWithCovarianceStamped last_published_pose
 
geometry_msgs::PoseStamped latest_odom_pose_
 
tf2::Transform latest_tf_
 
bool latest_tf_valid_
 
bool m_force_update
 
map_t * map_
 
ros::Subscriber map_sub_
 
char * mapdata
 
int max_beams_
 
int max_particles_
 
int min_particles_
 
ros::NodeHandle nh_
 
ros::ServiceServer nomotion_update_srv_
 
AMCLOdom * odom_
 
std::string odom_frame_id_
 
odom_model_t odom_model_type_
 
ros::Publisher particlecloud_pub_
 
pf_t * pf_
 
double pf_err_
 
bool pf_init_
 
pf_vector_t pf_odom_pose_
 
double pf_z_
 
ros::Publisher pose_pub_
 
ros::NodeHandle private_nh_
 
int resample_count_
 
int resample_interval_
 
double resolution
 
ros::Time save_pose_last_time
 
ros::Duration save_pose_period
 
bool selective_resampling_
 
bool sent_first_transform_
 
ros::ServiceServer set_map_srv_
 
double sigma_hit_
 
double std_warn_level_x_
 
double std_warn_level_y_
 
double std_warn_level_yaw_
 
int sx
 
int sy
 
std::shared_ptr< tf2_ros::Buffer > tf_
 
bool tf_broadcast_
 
std::shared_ptr< tf2_ros::TransformBroadcaster > tfb_
 
std::shared_ptr< tf2_ros::TransformListener > tfl_
 
ros::Duration transform_tolerance_
 
bool use_map_topic_
 
double z_hit_
 
double z_max_
 
double z_rand_
 
double z_short_
 

Static Private Attributes

static std::vector< std::pair< int, int > > free_space_indices
 

Detailed Description

Definition at line 132 of file amcl_node.cpp.

Constructor & Destructor Documentation

◆ AmclNode()

AmclNode::AmclNode ( )

Definition at line 349 of file amcl_node.cpp.

◆ ~AmclNode()

AmclNode::~AmclNode ( )

Definition at line 1014 of file amcl_node.cpp.

Member Function Documentation

◆ applyInitialPose()

void AmclNode::applyInitialPose ( )
private

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.

Definition at line 1622 of file amcl_node.cpp.

◆ checkLaserReceived()

void AmclNode::checkLaserReceived ( const ros::TimerEvent &  event)
private

Definition at line 836 of file amcl_node.cpp.

◆ convertMap()

map_t * AmclNode::convertMap ( const nav_msgs::OccupancyGrid &  map_msg)
private

Convert an OccupancyGrid map message into the internal representation. This allocates a map_t and returns it.

Definition at line 988 of file amcl_node.cpp.

◆ freeMapDependentMemory()

void AmclNode::freeMapDependentMemory ( )
private

Definition at line 967 of file amcl_node.cpp.

◆ getOdomPose()

bool AmclNode::getOdomPose ( geometry_msgs::PoseStamped &  pose,
double &  x,
double &  y,
double &  yaw,
const ros::Time &  t,
const std::string &  f 
)
private

Definition at line 1024 of file amcl_node.cpp.

◆ globalLocalizationCallback()

bool AmclNode::globalLocalizationCallback ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
)
private

Definition at line 1089 of file amcl_node.cpp.

◆ handleInitialPoseMessage()

void AmclNode::handleInitialPoseMessage ( const geometry_msgs::PoseWithCovarianceStamped &  msg)
private

Definition at line 1542 of file amcl_node.cpp.

◆ handleMapMessage()

void AmclNode::handleMapMessage ( const nav_msgs::OccupancyGrid &  msg)
private

Definition at line 878 of file amcl_node.cpp.

◆ initialPoseReceived()

void AmclNode::initialPoseReceived ( const geometry_msgs::PoseWithCovarianceStampedConstPtr &  msg)
private

Definition at line 1532 of file amcl_node.cpp.

◆ laserReceived()

void AmclNode::laserReceived ( const sensor_msgs::LaserScanConstPtr &  laser_scan)
private

Definition at line 1129 of file amcl_node.cpp.

◆ mapReceived()

void AmclNode::mapReceived ( const nav_msgs::OccupancyGridConstPtr &  msg)
private

Definition at line 866 of file amcl_node.cpp.

◆ nomotionUpdateCallback()

bool AmclNode::nomotionUpdateCallback ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
)
private

Definition at line 1106 of file amcl_node.cpp.

◆ process()

int AmclNode::process ( )

◆ reconfigureCB()

void AmclNode::reconfigureCB ( amcl::AMCLConfig &  config,
uint32_t  level 
)
private

Definition at line 517 of file amcl_node.cpp.

◆ requestMap()

void AmclNode::requestMap ( )
private

Definition at line 848 of file amcl_node.cpp.

◆ runFromBag()

void AmclNode::runFromBag ( const std::string &  in_bag_fn,
bool  trigger_global_localization = false 
)

Uses TF and LaserScan messages from bag file to drive AMCL instead.

Parameters
in_bag_fninput bagfile
trigger_global_localizationwhether to trigger global localization before starting to process the bagfile

Definition at line 676 of file amcl_node.cpp.

◆ savePoseToServer()

void AmclNode::savePoseToServer ( )

Definition at line 769 of file amcl_node.cpp.

◆ setMapCallback()

bool AmclNode::setMapCallback ( nav_msgs::SetMap::Request &  req,
nav_msgs::SetMap::Response &  res 
)
private

Definition at line 1115 of file amcl_node.cpp.

◆ standardDeviationDiagnostics()

void AmclNode::standardDeviationDiagnostics ( diagnostic_updater::DiagnosticStatusWrapper &  diagnostic_status)
private

Definition at line 1635 of file amcl_node.cpp.

◆ uniformPoseGenerator()

pf_vector_t AmclNode::uniformPoseGenerator ( void *  arg)
staticprivate

Definition at line 1051 of file amcl_node.cpp.

◆ updatePoseFromServer()

void AmclNode::updatePoseFromServer ( )
private

Definition at line 793 of file amcl_node.cpp.

Member Data Documentation

◆ a_thresh_

double AmclNode::a_thresh_
private

Definition at line 220 of file amcl_node.cpp.

◆ alpha1_

double AmclNode::alpha1_
private

Definition at line 275 of file amcl_node.cpp.

◆ alpha2_

double AmclNode::alpha2_
private

Definition at line 275 of file amcl_node.cpp.

◆ alpha3_

double AmclNode::alpha3_
private

Definition at line 275 of file amcl_node.cpp.

◆ alpha4_

double AmclNode::alpha4_
private

Definition at line 275 of file amcl_node.cpp.

◆ alpha5_

double AmclNode::alpha5_
private

Definition at line 275 of file amcl_node.cpp.

◆ alpha_fast_

double AmclNode::alpha_fast_
private

Definition at line 276 of file amcl_node.cpp.

◆ alpha_slow_

double AmclNode::alpha_slow_
private

Definition at line 276 of file amcl_node.cpp.

◆ bag_scan_period_

ros::WallDuration AmclNode::bag_scan_period_
private

Definition at line 236 of file amcl_node.cpp.

◆ base_frame_id_

std::string AmclNode::base_frame_id_
private

Definition at line 191 of file amcl_node.cpp.

◆ beam_skip_distance_

double AmclNode::beam_skip_distance_
private

Definition at line 280 of file amcl_node.cpp.

◆ beam_skip_error_threshold_

double AmclNode::beam_skip_error_threshold_
private

Definition at line 280 of file amcl_node.cpp.

◆ beam_skip_threshold_

double AmclNode::beam_skip_threshold_
private

Definition at line 280 of file amcl_node.cpp.

◆ check_laser_timer_

ros::Timer AmclNode::check_laser_timer_
private

Definition at line 272 of file amcl_node.cpp.

◆ cloud_pub_interval

ros::Duration AmclNode::cloud_pub_interval
private

Definition at line 232 of file amcl_node.cpp.

◆ configuration_mutex_

boost::recursive_mutex AmclNode::configuration_mutex_
private

Definition at line 269 of file amcl_node.cpp.

◆ d_thresh_

double AmclNode::d_thresh_
private

Definition at line 220 of file amcl_node.cpp.

◆ default_config_

amcl::AMCLConfig AmclNode::default_config_
private

Definition at line 271 of file amcl_node.cpp.

◆ diagnosic_updater_

diagnostic_updater::Updater AmclNode::diagnosic_updater_
private

Definition at line 259 of file amcl_node.cpp.

◆ do_beamskip_

bool AmclNode::do_beamskip_
private

Definition at line 279 of file amcl_node.cpp.

◆ dsrv_

dynamic_reconfigure::Server<amcl::AMCLConfig>* AmclNode::dsrv_
private

Definition at line 270 of file amcl_node.cpp.

◆ first_map_only_

bool AmclNode::first_map_only_
private

Definition at line 195 of file amcl_node.cpp.

◆ first_map_received_

bool AmclNode::first_map_received_
private

Definition at line 266 of file amcl_node.cpp.

◆ first_reconfigure_call_

bool AmclNode::first_reconfigure_call_
private

Definition at line 267 of file amcl_node.cpp.

◆ force_update_after_initialpose_

bool AmclNode::force_update_after_initialpose_
private

Definition at line 287 of file amcl_node.cpp.

◆ force_update_after_set_map_

bool AmclNode::force_update_after_set_map_
private

Definition at line 288 of file amcl_node.cpp.

◆ frame_to_laser_

std::map< std::string, int > AmclNode::frame_to_laser_
private

Definition at line 213 of file amcl_node.cpp.

◆ free_space_indices

std::vector< std::pair< int, int > > AmclNode::free_space_indices
staticprivate

Definition at line 163 of file amcl_node.cpp.

◆ global_frame_id_

std::string AmclNode::global_frame_id_
private

Definition at line 192 of file amcl_node.cpp.

◆ global_loc_srv_

ros::ServiceServer AmclNode::global_loc_srv_
private

Definition at line 253 of file amcl_node.cpp.

◆ gui_publish_period

ros::Duration AmclNode::gui_publish_period
private

Definition at line 197 of file amcl_node.cpp.

◆ init_cov_

double AmclNode::init_cov_[3]
private

Definition at line 284 of file amcl_node.cpp.

◆ init_pose_

double AmclNode::init_pose_[3]
private

Definition at line 283 of file amcl_node.cpp.

◆ initial_pose_hyp_

amcl_hyp_t* AmclNode::initial_pose_hyp_
private

Definition at line 265 of file amcl_node.cpp.

◆ initial_pose_sub_

ros::Subscriber AmclNode::initial_pose_sub_
private

Definition at line 210 of file amcl_node.cpp.

◆ initial_pose_sub_old_

ros::Subscriber AmclNode::initial_pose_sub_old_
private

Definition at line 256 of file amcl_node.cpp.

◆ lambda_short_

double AmclNode::lambda_short_
private

Definition at line 277 of file amcl_node.cpp.

◆ laser_

AMCLLaser* AmclNode::laser_
private

Definition at line 230 of file amcl_node.cpp.

◆ laser_check_interval_

ros::Duration AmclNode::laser_check_interval_
private

Definition at line 294 of file amcl_node.cpp.

◆ laser_likelihood_max_dist_

double AmclNode::laser_likelihood_max_dist_
private

Definition at line 281 of file amcl_node.cpp.

◆ laser_max_range_

double AmclNode::laser_max_range_
private

Definition at line 224 of file amcl_node.cpp.

◆ laser_min_range_

double AmclNode::laser_min_range_
private

Definition at line 223 of file amcl_node.cpp.

◆ laser_model_type_

laser_model_t AmclNode::laser_model_type_
private

Definition at line 285 of file amcl_node.cpp.

◆ laser_scan_filter_

tf2_ros::MessageFilter<sensor_msgs::LaserScan>* AmclNode::laser_scan_filter_
private

Definition at line 209 of file amcl_node.cpp.

◆ laser_scan_sub_

message_filters::Subscriber<sensor_msgs::LaserScan>* AmclNode::laser_scan_sub_
private

Definition at line 208 of file amcl_node.cpp.

◆ lasers_

std::vector< AMCLLaser* > AmclNode::lasers_
private

Definition at line 211 of file amcl_node.cpp.

◆ lasers_update_

std::vector< bool > AmclNode::lasers_update_
private

Definition at line 212 of file amcl_node.cpp.

◆ last_cloud_pub_time

ros::Time AmclNode::last_cloud_pub_time
private

Definition at line 233 of file amcl_node.cpp.

◆ last_laser_received_ts_

ros::Time AmclNode::last_laser_received_ts_
private

Definition at line 293 of file amcl_node.cpp.

◆ last_published_pose

geometry_msgs::PoseWithCovarianceStamped AmclNode::last_published_pose
private

Definition at line 201 of file amcl_node.cpp.

◆ latest_odom_pose_

geometry_msgs::PoseStamped AmclNode::latest_odom_pose_
private

Definition at line 188 of file amcl_node.cpp.

◆ latest_tf_

tf2::Transform AmclNode::latest_tf_
private

Definition at line 156 of file amcl_node.cpp.

◆ latest_tf_valid_

bool AmclNode::latest_tf_valid_
private

Definition at line 157 of file amcl_node.cpp.

◆ m_force_update

bool AmclNode::m_force_update
private

Definition at line 227 of file amcl_node.cpp.

◆ map_

map_t* AmclNode::map_
private

Definition at line 203 of file amcl_node.cpp.

◆ map_sub_

ros::Subscriber AmclNode::map_sub_
private

Definition at line 257 of file amcl_node.cpp.

◆ mapdata

char* AmclNode::mapdata
private

Definition at line 204 of file amcl_node.cpp.

◆ max_beams_

int AmclNode::max_beams_
private

Definition at line 274 of file amcl_node.cpp.

◆ max_particles_

int AmclNode::max_particles_
private

Definition at line 274 of file amcl_node.cpp.

◆ min_particles_

int AmclNode::min_particles_
private

Definition at line 274 of file amcl_node.cpp.

◆ nh_

ros::NodeHandle AmclNode::nh_
private

Definition at line 249 of file amcl_node.cpp.

◆ nomotion_update_srv_

ros::ServiceServer AmclNode::nomotion_update_srv_
private

Definition at line 254 of file amcl_node.cpp.

◆ odom_

AMCLOdom* AmclNode::odom_
private

Definition at line 229 of file amcl_node.cpp.

◆ odom_frame_id_

std::string AmclNode::odom_frame_id_
private

Definition at line 185 of file amcl_node.cpp.

◆ odom_model_type_

odom_model_t AmclNode::odom_model_type_
private

Definition at line 282 of file amcl_node.cpp.

◆ particlecloud_pub_

ros::Publisher AmclNode::particlecloud_pub_
private

Definition at line 252 of file amcl_node.cpp.

◆ pf_

pf_t* AmclNode::pf_
private

Definition at line 216 of file amcl_node.cpp.

◆ pf_err_

double AmclNode::pf_err_
private

Definition at line 217 of file amcl_node.cpp.

◆ pf_init_

bool AmclNode::pf_init_
private

Definition at line 218 of file amcl_node.cpp.

◆ pf_odom_pose_

pf_vector_t AmclNode::pf_odom_pose_
private

Definition at line 219 of file amcl_node.cpp.

◆ pf_z_

double AmclNode::pf_z_
private

Definition at line 217 of file amcl_node.cpp.

◆ pose_pub_

ros::Publisher AmclNode::pose_pub_
private

Definition at line 251 of file amcl_node.cpp.

◆ private_nh_

ros::NodeHandle AmclNode::private_nh_
private

Definition at line 250 of file amcl_node.cpp.

◆ resample_count_

int AmclNode::resample_count_
private

Definition at line 222 of file amcl_node.cpp.

◆ resample_interval_

int AmclNode::resample_interval_
private

Definition at line 221 of file amcl_node.cpp.

◆ resolution

double AmclNode::resolution
private

Definition at line 206 of file amcl_node.cpp.

◆ save_pose_last_time

ros::Time AmclNode::save_pose_last_time
private

Definition at line 198 of file amcl_node.cpp.

◆ save_pose_period

ros::Duration AmclNode::save_pose_period
private

Definition at line 199 of file amcl_node.cpp.

◆ selective_resampling_

bool AmclNode::selective_resampling_
private

Definition at line 289 of file amcl_node.cpp.

◆ sent_first_transform_

bool AmclNode::sent_first_transform_
private

Definition at line 154 of file amcl_node.cpp.

◆ set_map_srv_

ros::ServiceServer AmclNode::set_map_srv_
private

Definition at line 255 of file amcl_node.cpp.

◆ sigma_hit_

double AmclNode::sigma_hit_
private

Definition at line 277 of file amcl_node.cpp.

◆ std_warn_level_x_

double AmclNode::std_warn_level_x_
private

Definition at line 261 of file amcl_node.cpp.

◆ std_warn_level_y_

double AmclNode::std_warn_level_y_
private

Definition at line 262 of file amcl_node.cpp.

◆ std_warn_level_yaw_

double AmclNode::std_warn_level_yaw_
private

Definition at line 263 of file amcl_node.cpp.

◆ sx

int AmclNode::sx
private

Definition at line 205 of file amcl_node.cpp.

◆ sy

int AmclNode::sy
private

Definition at line 205 of file amcl_node.cpp.

◆ tf_

std::shared_ptr<tf2_ros::Buffer> AmclNode::tf_
private

Definition at line 152 of file amcl_node.cpp.

◆ tf_broadcast_

bool AmclNode::tf_broadcast_
private

Definition at line 286 of file amcl_node.cpp.

◆ tfb_

std::shared_ptr<tf2_ros::TransformBroadcaster> AmclNode::tfb_
private

Definition at line 150 of file amcl_node.cpp.

◆ tfl_

std::shared_ptr<tf2_ros::TransformListener> AmclNode::tfl_
private

Definition at line 151 of file amcl_node.cpp.

◆ transform_tolerance_

ros::Duration AmclNode::transform_tolerance_
private

Definition at line 247 of file amcl_node.cpp.

◆ use_map_topic_

bool AmclNode::use_map_topic_
private

Definition at line 194 of file amcl_node.cpp.

◆ z_hit_

double AmclNode::z_hit_
private

Definition at line 277 of file amcl_node.cpp.

◆ z_max_

double AmclNode::z_max_
private

Definition at line 277 of file amcl_node.cpp.

◆ z_rand_

double AmclNode::z_rand_
private

Definition at line 277 of file amcl_node.cpp.

◆ z_short_

double AmclNode::z_short_
private

Definition at line 277 of file amcl_node.cpp.

The documentation for this class was generated from the following file:

amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:13