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

Tf timeout #1724

Merged
merged 7 commits into from
May 13, 2020
Merged

Tf timeout #1724

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4008b02
Parameterized tf_timeout in amcl
Marwan99 May 12, 2020
64b9bbd
Added tf_timeout to static_layer and observation_buffer
Marwan99 May 12, 2020
b3ec76c
declared transform_tolerance parameter
Marwan99 May 13, 2020
a324e1c
change transform_tolerance default value
Marwan99 May 13, 2020
19ca42c
Removed transform_publish_shift param
Marwan99 May 13, 2020
d19b81e
Merge branch 'tf_timeout' of https://github.com/Marwan99/navigation2 …
Marwan99 May 13, 2020
cc56a13
Fixed linting errors
Marwan99 May 13, 2020
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
4 changes: 2 additions & 2 deletions nav2_amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,7 @@ AmclNode::waitForTransforms()
while (rclcpp::ok() &&
!tf_buffer_->canTransform(
global_frame_id_, odom_frame_id_, tf2::TimePointZero,
tf2::durationFromSec(1.0), &tf_error))
transform_tolerance_, &tf_error))
{
RCLCPP_INFO(
get_logger(), "Timed out waiting for transform from %s to %s"
Expand Down Expand Up @@ -749,7 +749,7 @@ bool AmclNode::addNewScanner(
ident.header.stamp = rclcpp::Time();
tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
try {
tf_buffer_->transform(ident, laser_pose, base_frame_id_);
tf_buffer_->transform(ident, laser_pose, base_frame_id_, transform_tolerance_);
} catch (tf2::TransformException & e) {
RCLCPP_ERROR(
get_logger(), "Couldn't transform from %s to %s, "
Expand Down
1 change: 1 addition & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ class StaticLayer : public CostmapLayer
unsigned char unknown_cost_value_;
bool trinary_costmap_;
bool map_received_{false};
tf2::Duration transform_tolerance_;
};

} // namespace nav2_costmap_2d
Expand Down
9 changes: 8 additions & 1 deletion nav2_costmap_2d/plugins/static_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,10 +117,12 @@ void
StaticLayer::getParameters()
{
int temp_lethal_threshold = 0;
double temp_tf_tol;

declareParameter("enabled", rclcpp::ParameterValue(true));
declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));

node_->get_parameter(name_ + "." + "enabled", enabled_);
node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
Expand All @@ -133,10 +135,13 @@ StaticLayer::getParameters()
node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
node_->get_parameter("unknown_cost_value", unknown_cost_value_);
node_->get_parameter("trinary_costmap", trinary_costmap_);
node_->get_parameter("transform_tolerance", temp_tf_tol);
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

// Enforce bounds
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
map_received_ = false;

transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
}

void
Expand Down Expand Up @@ -359,7 +364,9 @@ StaticLayer::updateCosts(
// Might even be in a different frame
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_->lookupTransform(map_frame_, global_frame_, tf2::TimePointZero);
transform = tf_->lookupTransform(
map_frame_, global_frame_, tf2::TimePointZero,
transform_tolerance_);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what());
return;
Expand Down
5 changes: 3 additions & 2 deletions nav2_costmap_2d/src/observation_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,11 +95,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_;

// we need to transform the origin of the observation to the new global frame
tf2_buffer_.transform(origin, origin, new_global_frame);
tf2_buffer_.transform(origin, origin, new_global_frame, tf2::durationFromSec(tf_tolerance_));
obs.origin_ = origin.point;

// we also need to transform the cloud of the observation to the new global frame
tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
tf2_buffer_.transform(
*(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_));
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(
rclcpp::get_logger(
Expand Down
4 changes: 2 additions & 2 deletions nav2_system_tests/src/system/test_multi_robot_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,9 @@ def generate_launch_description():

bringup_dir = get_package_share_directory('nav2_bringup')
robot1_params_file = os.path.join(bringup_dir, # noqa: F841
'params/nav2_multirobot_params_1.yaml')
'params/nav2_multirobot_params_1.yaml')
robot2_params_file = os.path.join(bringup_dir, # noqa: F841
'params/nav2_multirobot_params_2.yaml')
'params/nav2_multirobot_params_2.yaml')

# Names and poses of the robots
robots = [
Expand Down