From 6be3a0826bc9eca716a40f3062c05a71dc59e601 Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Thu, 13 Jun 2019 08:43:28 -0700
Subject: [PATCH] Call now on the main lifecycle nodes instead of rclcpp_nodes
(#845)
Now that lifecycle nodes properly support use_sim_time, we can get the
current time from the node itself rather than relying on the rclcpp_node,
which is a temporary bridging solution to interface to ROS2 functionality
that doesn't yet support lifecycle nodes. This PR is one of several steps
that we'll take in order to remove the rclcpp_nodes from the navigation
code, thereby reducing the overall number of nodes in the system and
therefore the overall number of topics and services.
---
nav2_amcl/src/amcl_node.cpp | 10 +++++-----
.../include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +-
nav2_costmap_2d/src/costmap_2d_ros.cpp | 8 ++++----
3 files changed, 10 insertions(+), 10 deletions(-)
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index a0f4c3f1..75e5b07b 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -252,7 +252,7 @@ AmclNode::checkLaserReceived()
return;
}
- rclcpp::Duration d = rclcpp_node_->now() - last_laser_received_ts_;
+ rclcpp::Duration d = now() - last_laser_received_ts_;
if (d.nanoseconds() * 1e-9 > laser_check_interval_.count()) {
RCLCPP_WARN(
get_logger(), "No laser scan received (and thus no pose updates have been published) for %f"
@@ -264,7 +264,7 @@ AmclNode::checkLaserReceived()
bool
AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time)
{
- rclcpp::Duration elapsed_time = rclcpp_node_->now() - last_time;
+ rclcpp::Duration elapsed_time = now() - last_time;
if (elapsed_time.nanoseconds() * 1e-9 > check_interval.count()) {
return true;
}
@@ -411,7 +411,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::Share
// intervening odometric change.
geometry_msgs::msg::TransformStamped tx_odom;
try {
- rclcpp::Time rclcpp_time = rclcpp_node_->now();
+ rclcpp::Time rclcpp_time = now();
tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds()));
// Check if the transform is available
@@ -439,7 +439,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::Share
// Transform into the global frame
RCLCPP_INFO(get_logger(), "Setting pose (%.6f): %.3f %.3f %.3f",
- rclcpp_node_->now().nanoseconds() * 1e-9,
+ now().nanoseconds() * 1e-9,
pose_new.getOrigin().x(),
pose_new.getOrigin().y(),
tf2::getYaw(pose_new.getRotation()));
@@ -474,7 +474,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
if (!active_) {return;}
std::string laser_scan_frame_id = nav2_util::strip_leading_slash(laser_scan->header.frame_id);
- last_laser_received_ts_ = rclcpp_node_->now();
+ last_laser_received_ts_ = now();
int laser_index = -1;
geometry_msgs::msg::PoseStamped laser_pose;
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 7f30e118..e737cc86 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
@@ -127,7 +127,7 @@ public:
* @param global_pose Will be set to the pose of the robot in the global frame of the costmap
* @return True if the pose was set successfully, false otherwise
*/
- bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const;
+ bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose);
/** @brief Returns costmap name */
std::string getName() const
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 3c4e2d60..0d57196a 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -365,7 +365,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
costmap_publisher_->updateBounds(x0, xn, y0, yn);
- auto current_time = rclcpp_node_->now();
+ auto current_time = now();
if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due
(current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT
{
@@ -406,7 +406,7 @@ Costmap2DROS::updateMap()
geometry_msgs::msg::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
- footprint.header.stamp = rclcpp_node_->now();
+ footprint.header.stamp = now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
@@ -495,7 +495,7 @@ Costmap2DROS::resetLayers()
}
bool
-Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const
+Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
{
geometry_msgs::msg::PoseStamped robot_pose;
@@ -506,7 +506,7 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const
robot_pose.header.stamp = rclcpp::Time();
// Save time for checking tf delay later
- rclcpp::Time current_time = rclcpp_node_->now();
+ rclcpp::Time current_time = now();
// Get the global pose of the robot
try {
--
GitLab