fixing collision monitor, velocity smoother unit tests

Signed-off-by: Steve Macenski <[email protected]>
ros-navigation · Dec 3, 2024 · 4a6c6a8 · 4a6c6a8
1 parent 2d2d4fd
commit 4a6c6a8
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 9 deletions.
diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -220,6 +220,7 @@ class Tester : public ::testing::Test
 Tester::Tester()
 {
   cm_ = std::make_shared<CollisionMonitorWrapper>();
+  cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
 
   footprint_pub_ = cm_->create_publisher<geometry_msgs::msg::PolygonStamped>(
     FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp
@@ -29,6 +29,7 @@ TEST(TwistPublisher, Unstamped)
   rclcpp::init(0, nullptr);
   auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
   pub_node->configure();
+  pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
   auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
   ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
   EXPECT_FALSE(vel_publisher->is_activated());

diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp
@@ -29,6 +29,7 @@ TEST(TwistSubscriber, Unstamped)
   auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
   sub_node->configure();
   sub_node->activate();
+  sub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
 
   geometry_msgs::msg::TwistStamped sub_msg {};
   auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(

diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
@@ -22,7 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_velocity_smoother/velocity_smoother.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav2_util/twist_subscriber.hpp"
 
 using namespace std::chrono_literals;
@@ -53,7 +53,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
   bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;}
   geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;}
 
-  void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);}
+  void sendCommandMsg(geometry_msgs::msg::TwistStamped::SharedPtr msg)
+  {
+    inputCommandStampedCallback(msg);
+  }
 };
 
 TEST(VelocitySmootherTest, openLoopTestTimer)
@@ -81,8 +84,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
     });
 
   // Send a velocity command
-  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
-  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  cmd->twist.linear.x = 1.0;  // Max is 0.5, so should threshold
   smoother->sendCommandMsg(cmd);
 
   // Process velocity smoothing and send updated odometry based on commands
@@ -147,8 +150,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
   }
 
   // Send a velocity command
-  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
-  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  cmd->twist.linear.x = 1.0;  // Max is 0.5, so should threshold
   smoother->sendCommandMsg(cmd);
 
   // Process velocity smoothing and send updated odometry based on commands
@@ -568,10 +571,10 @@ TEST(VelocitySmootherTest, testCommandCallback)
   smoother->configure(state);
   smoother->activate(state);
 
-  auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+  auto pub = smoother->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 1);
   pub->on_activate();
-  auto msg = std::make_unique<geometry_msgs::msg::Twist>();
-  msg->linear.x = 100.0;
+  auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  msg->twist.linear.x = 100.0;
   pub->publish(std::move(msg));
   rclcpp::spin_some(smoother->get_node_base_interface());