From 0869e61ad2d6b6b2329ecad2bf24dbbaeb71df57 Mon Sep 17 00:00:00 2001 From: Omer Spalter Date: Wed, 26 Oct 2022 21:29:30 +0300 Subject: [PATCH] Update to latest Navigation 2 --- nav_layer_from_points/src/costmap_layer.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/nav_layer_from_points/src/costmap_layer.cpp b/nav_layer_from_points/src/costmap_layer.cpp index 93b4365..cd68f54 100755 --- a/nav_layer_from_points/src/costmap_layer.cpp +++ b/nav_layer_from_points/src/costmap_layer.cpp @@ -32,12 +32,17 @@ void NavLayerFromPoints::onInitialize() current_ = true; first_time_ = true; - rclcpp_node_->declare_parameter(name_ + "." + "enabled", true); - rclcpp_node_->declare_parameter(name_ + "." + "keep_time", 0.75); - rclcpp_node_->declare_parameter(name_ + "." + "point_radius", 0.2); - rclcpp_node_->declare_parameter(name_ + "." + "robot_radius", 0.6); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->declare_parameter(name_ + "." + "enabled", true); + node->declare_parameter(name_ + "." + "keep_time", 0.75); + node->declare_parameter(name_ + "." + "point_radius", 0.2); + node->declare_parameter(name_ + "." + "robot_radius", 0.6); - sub_points_ = rclcpp_node_->create_subscription( + sub_points_ = node->create_subscription( "points", 1, std::bind(&NavLayerFromPoints::pointsCallback, this, std::placeholders::_1)); } @@ -100,15 +105,15 @@ void NavLayerFromPoints::updateBounds( transformed_points_.push_back(tpt); } catch(tf2::LookupException& ex) { - RCLCPP_ERROR(rclcpp_node_->get_logger(), "No Transform available Error: %s\n", ex.what()); + RCLCPP_ERROR(logger_, "No Transform available Error: %s\n", ex.what()); continue; } catch(tf2::ConnectivityException& ex) { - RCLCPP_ERROR(rclcpp_node_->get_logger(), "Connectivity Error: %s\n", ex.what()); + RCLCPP_ERROR(logger_, "Connectivity Error: %s\n", ex.what()); continue; } catch(tf2::ExtrapolationException& ex) { - RCLCPP_ERROR(rclcpp_node_->get_logger(), "Extrapolation Error: %s\n", ex.what()); + RCLCPP_ERROR(logger_, "Extrapolation Error: %s\n", ex.what()); continue; } }