Move nav_2d_utils to nav2_util (#5414)

* Move nav_2d_util to nav2_util

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Rename frame

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Replace OdomSubscriber with OdomSmoother

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Use transformPoseInTargetFrame in all controllers

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

nav2_controller/CMakeLists.txt

@@ -11,8 +11,6 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(nav_2d_msgs REQUIRED)
-find_package(nav_2d_utils REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -42,9 +40,6 @@ target_link_libraries(${library_name} PUBLIC
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
   nav2_ros_common::nav2_ros_common
-  ${nav_2d_msgs_TARGETS}
-  nav_2d_utils::conversions
-  nav_2d_utils::tf_help
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
@@ -86,7 +81,6 @@ target_link_libraries(simple_progress_checker PUBLIC
 )
 target_link_libraries(simple_progress_checker PRIVATE
   nav2_util::nav2_util_core
-  nav_2d_utils::conversions
   pluginlib::pluginlib
 )
 
@@ -107,7 +101,6 @@ target_link_libraries(pose_progress_checker PUBLIC
 target_link_libraries(pose_progress_checker PRIVATE
   angles::angles
   nav2_util::nav2_util_core
-  nav_2d_utils::conversions
   pluginlib::pluginlib
 )
 
@@ -218,8 +211,6 @@ ament_export_dependencies(
   nav2_costmap_2d
   nav2_msgs
   nav2_util
-  nav_2d_msgs
-  nav_2d_utils
   pluginlib
   rclcpp
   rclcpp_lifecycle

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -29,10 +29,10 @@
 #include "tf2_ros/transform_listener.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/msg/speed_limit.hpp"
-#include "nav_2d_utils/odom_subscriber.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -208,12 +208,12 @@ class ControllerServer : public nav2::LifecycleNode
    * @param Twist The current Twist from odometry
    * @return Twist Twist after thresholds applied
    */
-  nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist)
+  geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist)
   {
-    nav_2d_msgs::msg::Twist2D twist_thresh;
-    twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_);
-    twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_);
-    twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_);
+    geometry_msgs::msg::Twist twist_thresh;
+    twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, min_x_velocity_threshold_);
+    twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, min_y_velocity_threshold_);
+    twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, min_theta_velocity_threshold_);
     return twist_thresh;
   }
 
@@ -233,7 +233,7 @@ class ControllerServer : public nav2::LifecycleNode
   std::unique_ptr<nav2::NodeThread> costmap_thread_;
 
   // Publishers and subscribers
-  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   nav2::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
 

nav2_controller/package.xml

@@ -18,8 +18,6 @@
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
-  <depend>nav_2d_msgs</depend>
-  <depend>nav_2d_utils</depend>
   <depend>pluginlib</depend>
   <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>

nav2_controller/plugins/pose_progress_checker.cpp

@@ -18,7 +18,6 @@
 #include <memory>
 #include <vector>
 #include "angles/angles.h"
-#include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"

nav2_controller/plugins/simple_progress_checker.cpp

@@ -17,7 +17,6 @@
 #include <string>
 #include <memory>
 #include <vector>
-#include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"

nav2_controller/plugins/test/CMakeLists.txt

@@ -1,5 +1,5 @@
 ament_add_gtest(pctest progress_checker.cpp)
-target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions)
+target_link_libraries(pctest simple_progress_checker pose_progress_checker)
 
 ament_add_gtest(gctest goal_checker.cpp)
-target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions)
+target_link_libraries(gctest simple_goal_checker stopped_goal_checker)

nav2_controller/plugins/test/goal_checker.cpp

@@ -38,7 +38,6 @@
 #include "gtest/gtest.h"
 #include "nav2_controller/plugins/simple_goal_checker.hpp"
 #include "nav2_controller/plugins/stopped_goal_checker.hpp"
-#include "nav_2d_utils/conversions.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "eigen3/Eigen/Geometry"

nav2_controller/plugins/test/progress_checker.cpp

@@ -38,7 +38,6 @@
 #include "gtest/gtest.h"
 #include "nav2_controller/plugins/simple_progress_checker.hpp"
 #include "nav2_controller/plugins/pose_progress_checker.hpp"
-#include "nav_2d_utils/conversions.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_util/geometry_utils.hpp"
 

nav2_controller/src/controller_server.cpp

@@ -21,8 +21,6 @@
 
 #include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_core/controller_exceptions.hpp"
-#include "nav_2d_utils/conversions.hpp"
-#include "nav_2d_utils/tf_help.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_controller/controller_server.hpp"
@@ -65,6 +63,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));
   declare_parameter("costmap_update_timeout", 0.30);  // 300ms
 
+  declare_parameter("odom_topic", rclcpp::ParameterValue("odom"));
+  declare_parameter("odom_duration", rclcpp::ParameterValue(0.3));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
@@ -125,8 +126,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
-  std::string speed_limit_topic;
+  std::string speed_limit_topic, odom_topic;
   get_parameter("speed_limit_topic", speed_limit_topic);
+  get_parameter("odom_topic", odom_topic);
+  double odom_duration;
+  get_parameter("odom_duration", odom_duration);
   get_parameter("failure_tolerance", failure_tolerance_);
   get_parameter("use_realtime_priority", use_realtime_priority_);
   get_parameter("publish_zero_velocity", publish_zero_velocity_);
@@ -219,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     get_logger(),
     "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
 
-  odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
+  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
 
   double costmap_update_timeout_dbl;
@@ -624,15 +628,15 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::FailedToMakeProgress("Failed to make progress");
   }
 
-  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::TwistStamped cmd_vel_2d;
 
   try {
     cmd_vel_2d =
       controllers_[current_controller_]->computeVelocityCommands(
       pose,
-      nav_2d_utils::twist2Dto3D(twist),
+      twist,
       goal_checkers_[current_goal_checker_].get());
     last_valid_cmd_time_ = now();
     cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
@@ -665,10 +669,9 @@ void ControllerServer::computeAndPublishVelocity()
 
   // Find the closest pose to current pose on global path
   geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
-  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  if (!nav_2d_utils::transformPose(
-          costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose,
-          robot_pose_in_path_frame, tolerance))
+  if (!nav2_util::transformPoseInTargetFrame(
+          pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(),
+          current_path_.header.frame_id, costmap_ros_->getTransformTolerance()))
   {
     throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
   }
@@ -793,14 +796,12 @@ bool ControllerServer::isGoalReached()
     return false;
   }
 
-  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
-  geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
+  geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
-  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  nav_2d_utils::transformPose(
-    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
-    end_pose_, transformed_end_pose, tolerance);
+  nav2_util::transformPoseInTargetFrame(
+    end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(),
+    costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance());
 
   return goal_checkers_[current_goal_checker_]->isGoalReached(
     pose.pose, transformed_end_pose.pose,

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -27,7 +27,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/twist_publisher.hpp"
-#include "nav_2d_utils/odom_subscriber.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "opennav_docking/controller.hpp"
 #include "opennav_docking/utils.hpp"
 #include "opennav_docking/types.hpp"
@@ -264,7 +264,7 @@ class DockingServer : public nav2::LifecycleNode
   rclcpp::Time action_start_time_;
 
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
-  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   typename DockingActionServer::SharedPtr docking_action_server_;
   typename UndockingActionServer::SharedPtr undocking_action_server_;