Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[AMCL] Add option to force nomotion update after initialpose #1226

Merged
merged 4 commits into from
Nov 29, 2022
Merged

[AMCL] Add option to force nomotion update after initialpose #1226

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3599fc5
Add param to force nomotion update after initialpose
stwirth Nov 28, 2022
319829e
Fix warnings
stwirth Nov 28, 2022
0f08b6d
Add param to force localization update after /set_map
stwirth Nov 28, 2022
49553a6
Shorter variable names
stwirth Nov 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions amcl/cfg/AMCL.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point befo
gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1)

gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True)
gen.add("force_nomotion_update_after_initialpose", bool_t, 0, "When true, force a pose update after a new /initialpose is received.", False)
gen.add("force_nomotion_update_after_set_map", bool_t, 0, "When true, force a pose update after a new map (and pose) has been set via the /set_map service.", False)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is there a reason to include "nomotion" in the names? These are pretty long variable names, and "force_update_after_X" seems to convey exactly what is going on.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds good, I'll rename.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mikeferguson done

gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100)
gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10)

Expand Down
25 changes: 19 additions & 6 deletions amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,8 @@ class AmclNode
double init_cov_[3];
laser_model_t laser_model_type_;
bool tf_broadcast_;
bool force_nomotion_update_after_initialpose_;
bool force_nomotion_update_after_set_map_;
bool selective_resampling_;

void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);
Expand Down Expand Up @@ -445,6 +447,8 @@ AmclNode::AmclNode() :
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);
private_nh_.param("force_nomotion_update_after_initialpose", force_nomotion_update_after_initialpose_, false);
private_nh_.param("force_nomotion_update_after_set_map", force_nomotion_update_after_set_map_, false);

// For diagnostics
private_nh_.param("std_warn_level_x", std_warn_level_x_, 0.2);
Expand Down Expand Up @@ -584,6 +588,8 @@ void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
alpha_slow_ = config.recovery_alpha_slow;
alpha_fast_ = config.recovery_alpha_fast;
tf_broadcast_ = config.tf_broadcast;
force_nomotion_update_after_initialpose_ = config.force_nomotion_update_after_initialpose;
force_nomotion_update_after_set_map_ = config.force_nomotion_update_after_set_map;

do_beamskip_= config.do_beamskip;
beam_skip_distance_ = config.beam_skip_distance;
Expand Down Expand Up @@ -1028,7 +1034,7 @@ AmclNode::getOdomPose(geometry_msgs::PoseStamped& odom_pose,
{
this->tf_->transform(ident, odom_pose, odom_frame_id_);
}
catch(tf2::TransformException e)
catch(const tf2::TransformException& e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
Expand Down Expand Up @@ -1111,6 +1117,10 @@ AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
{
handleMapMessage(req.map);
handleInitialPoseMessage(req.initial_pose);
if (force_nomotion_update_after_set_map_)
{
m_force_update = true;
}
res.success = true;
return true;
}
Expand Down Expand Up @@ -1144,7 +1154,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
this->tf_->transform(ident, laser_pose, base_frame_id_);
}
catch(tf2::TransformException& e)
catch(const tf2::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
Expand Down Expand Up @@ -1267,7 +1277,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
tf_->transform(min_q, min_q, base_frame_id_);
tf_->transform(inc_q, inc_q, base_frame_id_);
}
catch(tf2::TransformException& e)
catch(const tf2::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
Expand Down Expand Up @@ -1459,7 +1469,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)

this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
}
catch(tf2::TransformException)
catch(const tf2::TransformException&)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
Expand Down Expand Up @@ -1522,6 +1532,10 @@ void
AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
handleInitialPoseMessage(*msg);
if (force_nomotion_update_after_initialpose_)
{
m_force_update = true;
}
}

void
Expand All @@ -1547,13 +1561,12 @@ AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStampe
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)
catch(const 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
Expand Down