Merge remote-tracking branch 'origin/main' into fix-devcontainer

nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp

@@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated(
 {
   return new_path != old_path && old_path.poses.size() != 0 &&
          new_path.poses.size() != 0 &&
-         old_path.poses.back() == new_path.poses.back();
+         old_path.poses.back().pose == new_path.poses.back().pose;
 }
 
 bool PathLongerOnApproach::isRobotInGoalProximity(

nav2_collision_monitor/src/polygon.cpp

@@ -238,7 +238,12 @@ double Polygon::getCollisionTime(
   Velocity vel = velocity;
 
   // Array of points transformed to the frame concerned with pose on each simulation step
-  std::vector<Point> points_transformed;
+  std::vector<Point> points_transformed = collision_points;
+
+  // Check static polygon
+  if (getPointsInside(points_transformed) >= min_points_) {
+    return 0.0;
+  }
 
   // Robot movement simulation
   for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -991,7 +991,8 @@ TEST_F(Tester, testProcessApproach)
   // 3. Obstacle is inside robot footprint
   publishScan(0.5, curr_time);
   ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
-  publishCmdVel(0.5, 0.2, 0.0);
+  // Publish impossible cmd_vel to ensure robot footprint is checked
+  publishCmdVel(1000000000.0, 0.2, 0.0);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -84,23 +84,6 @@ class LifecycleManagerClient
    */
   SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  // A couple convenience methods to facilitate scripting tests
-  /**
-   * @brief Set initial pose with covariance
-   * @param x X position
-   * @param y Y position
-   * @param theta orientation
-   */
-  void set_initial_pose(double x, double y, double theta);
-  /**
-   * @brief Send goal pose to NavigationToPose action server
-   * @param x X position
-   * @param y Y position
-   * @param theta orientation
-   * @return true or false
-   */
-  bool navigate_to_pose(double x, double y, double theta);
-
 protected:
   using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes;
 

nav2_smac_planner/src/analytic_expansion.cpp

@@ -210,7 +210,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   node_costs.reserve(num_intervals);
 
   // Check intermediary poses (non-goal, non-start)
-  for (float i = 1; i < num_intervals; i++) {
+  for (float i = 1; i <= num_intervals; i++) {
     state_space->interpolate(from(), to(), i / num_intervals, s());
     reals = s.reals();
     // Make sure in range [0, 2PI)

nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp

@@ -58,27 +58,27 @@ class SpinBehaviorTestFixture
 
 SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr;
 
-TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
-{
-  float target_yaw = std::get<0>(GetParam());
-  float tolerance = std::get<1>(GetParam());
-
-  bool success = false;
-  int num_tries = 3;
-  for (int i = 0; i != num_tries; i++) {
-    success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance);
-    if (success) {
-      break;
-    }
-  }
-  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
-    // if this variable is set, make a fake costmap
-    // in the fake spin test, we expect a collision for angles > M_PI_2
-    EXPECT_EQ(false, success);
-  } else {
-    EXPECT_EQ(true, success);
-  }
-}
+// TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
+// {
+//   float target_yaw = std::get<0>(GetParam());
+//   float tolerance = std::get<1>(GetParam());
+
+//   bool success = false;
+//   int num_tries = 3;
+//   for (int i = 0; i != num_tries; i++) {
+//     success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance);
+//     if (success) {
+//       break;
+//     }
+//   }
+//   if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
+//     // if this variable is set, make a fake costmap
+//     // in the fake spin test, we expect a collision for angles > M_PI_2
+//     EXPECT_EQ(false, success);
+//   } else {
+//     EXPECT_EQ(true, success);
+//   }
+// }
 
 TEST_F(SpinBehaviorTestFixture, testSpinPreemption)
 {
@@ -114,18 +114,18 @@ TEST_F(SpinBehaviorTestFixture, testSpinCancel)
   EXPECT_EQ(false, success);
 }
 
-INSTANTIATE_TEST_SUITE_P(
-  SpinRecoveryTests,
-  SpinBehaviorTestFixture,
-  ::testing::Values(
-    std::make_tuple(-M_PIf32 / 6.0, 0.1),
-    // std::make_tuple(M_PI_4f32, 0.1),
-    // std::make_tuple(-M_PI_2f32, 0.1),
-    std::make_tuple(M_PIf32, 0.1),
-    std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15),
-    std::make_tuple(-2.0 * M_PIf32, 0.1),
-    std::make_tuple(4.0 * M_PIf32, 0.15)),
-  testNameGenerator);
+// INSTANTIATE_TEST_SUITE_P(
+//   SpinRecoveryTests,
+//   SpinBehaviorTestFixture,
+//   ::testing::Values(
+//     std::make_tuple(-M_PIf32 / 6.0, 0.1),
+//     // std::make_tuple(M_PI_4f32, 0.1),
+//     // std::make_tuple(-M_PI_2f32, 0.1),
+//     std::make_tuple(M_PIf32, 0.1),
+//     std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15),
+//     std::make_tuple(-2.0 * M_PIf32, 0.1),
+//     std::make_tuple(4.0 * M_PIf32, 0.15)),
+//   testNameGenerator);
 
 int main(int argc, char ** argv)
 {