From f55a87fc940541ab1660a06edf3d11acd26f981d Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Wed, 7 Aug 2019 15:21:41 -0700
Subject: [PATCH] removing unnecessary smart pointers that dont effect
lifecycle
---
nav2_amcl/include/nav2_amcl/amcl_node.hpp | 2 +-
nav2_amcl/src/amcl_node.cpp | 18 ++++++++----------
.../include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +-
nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +-
.../include/dwb_controller/dwb_controller.hpp | 2 +-
.../dwb_controller/src/dwb_controller.cpp | 11 ++++-------
6 files changed, 16 insertions(+), 21 deletions(-)
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 13338fb8..5c4dfadc 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -198,7 +198,7 @@ protected:
const std::vector<amcl_hyp_t> & hyps,
const int & max_weight_hyp);
void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
- void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+ void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg);
bool init_pose_received_on_inactive{false};
bool initial_pose_is_known_{false};
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 5c036859..d9b331d5 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -156,8 +156,7 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
active_ = true;
if (init_pose_received_on_inactive) {
- handleInitialPose(
- std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(last_published_pose_));
+ handleInitialPose(last_published_pose_);
}
return nav2_util::CallbackReturn::SUCCESS;
@@ -392,11 +391,11 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
"but AMCL is not yet in the active state");
return;
}
- handleInitialPose(msg);
+ handleInitialPose(*msg);
}
void
-AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
{
// In case the client sent us a pose estimate in the past, integrate the
// intervening odometric change.
@@ -406,7 +405,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::Share
tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds()));
// Check if the transform is available
- tx_odom = tf_buffer_->lookupTransform(base_frame_id_, tf2_ros::fromMsg(msg->header.stamp),
+ tx_odom = tf_buffer_->lookupTransform(base_frame_id_, tf2_ros::fromMsg(msg.header.stamp),
base_frame_id_, tf2_time, odom_frame_id_);
} catch (tf2::TransformException & e) {
// If we've never sent a transform, then this is normal, because the
@@ -423,7 +422,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::Share
tf2::impl::Converter<true, false>::convert(tx_odom.transform, tx_odom_tf2);
tf2::Transform pose_old;
- tf2::impl::Converter<true, false>::convert(msg->pose.pose, pose_old);
+ tf2::impl::Converter<true, false>::convert(msg.pose.pose, pose_old);
tf2::Transform pose_new = pose_old * tx_odom_tf2;
@@ -445,11 +444,11 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::Share
// 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[i][j] = msg.pose.covariance[6 * i + j];
}
}
- pf_init_pose_cov.m[2][2] = msg->pose.covariance[6 * 5 + 5];
+ pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;
@@ -988,8 +987,7 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
// Laser
lasers_.clear();
- handleInitialPose(
- std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(last_published_pose_));
+ handleInitialPose(last_published_pose_);
}
void
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index e737cc86..b2c4e9fc 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -290,7 +290,7 @@ protected:
std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
std::vector<geometry_msgs::msg::Point> padded_footprint_;
- std::shared_ptr<ClearCostmapService> clear_costmap_service_;
+ std::unique_ptr<ClearCostmapService> clear_costmap_service_;
};
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 3deeb856..9d84c1d5 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -152,7 +152,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
}
// Add cleaning service
- clear_costmap_service_ = std::make_shared<ClearCostmapService>(shared_from_this(), *this);
+ clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
return nav2_util::CallbackReturn::SUCCESS;
}
diff --git a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
index 5d8f4004..25fa4901 100644
--- a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
+++ b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
@@ -76,7 +76,7 @@ protected:
std::unique_ptr<dwb_core::DWBLocalPlanner> planner_;
// An executor used to spin the costmap node
- std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> costmap_executor_;
+ rclcpp::executors::SingleThreadedExecutor costmap_executor_;
std::unique_ptr<ProgressChecker> progress_checker_;
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 507e84ab..ed267d5d 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -38,25 +38,22 @@ DwbController::DwbController()
// The costmap node is used in the implementation of the DWB controller
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("local_costmap");
- // Create an executor that will be used to spin the costmap node
- costmap_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
-
// Launch a thread to run the costmap node
costmap_thread_ = std::make_unique<std::thread>(
[&](rclcpp_lifecycle::LifecycleNode::SharedPtr node)
{
// TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll
// be able to provide our own executor to spin(), reducing this to a single line
- costmap_executor_->add_node(node->get_node_base_interface());
- costmap_executor_->spin();
- costmap_executor_->remove_node(node->get_node_base_interface());
+ costmap_executor_.add_node(node->get_node_base_interface());
+ costmap_executor_.spin();
+ costmap_executor_.remove_node(node->get_node_base_interface());
}, costmap_ros_);
}
DwbController::~DwbController()
{
RCLCPP_INFO(get_logger(), "Destroying");
- costmap_executor_->cancel();
+ costmap_executor_.cancel();
costmap_thread_->join();
}
--
GitLab