add source_timeout tests

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -953,6 +953,88 @@ TEST_F(Tester, testCrossOver)
   cm_->stop();
 }
 
+TEST_F(Tester, testSourceTimeout)
+{
+  rclcpp::Time curr_time = cm_->now();
+
+  // Set Collision Monitor parameters.
+  // Making two polygons: outer polygon for slowdown and inner circle
+  // as robot footprint for approach.
+  setCommonParameters();
+  addPolygon("SlowDown", POLYGON, 2.0, "slowdown");
+  addPolygon("Approach", CIRCLE, 1.0, "approach");
+  addSource(POINTCLOUD_NAME, POINTCLOUD);
+  addSource(RANGE_NAME, RANGE);
+  setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME});
+
+  // Start Collision Monitor node
+  cm_->start();
+
+  // Share TF
+  sendTransforms(curr_time);
+
+  // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m).
+  // Robot should approach the obstacle.
+  publishPointCloud(2.5, curr_time);
+  ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time));
+  publishCmdVel(3.0, 3.0, 3.0);
+  ASSERT_TRUE(waitCmdVel(500ms));
+  // Range configured but not published, range source should be considered invalid
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, STOP);
+  ASSERT_EQ(action_state_->polygon_name, "invalid source");
+
+  // Stop Collision Monitor node
+  cm_->stop();
+}
+
+TEST_F(Tester, testSourceTimeoutOverride)
+{
+  rclcpp::Time curr_time = cm_->now();
+
+  // Set Collision Monitor parameters.
+  // Making two polygons: outer polygon for slowdown and inner circle
+  // as robot footprint for approach.
+  setCommonParameters();
+  addPolygon("SlowDown", POLYGON, 2.0, "slowdown");
+  addPolygon("Approach", CIRCLE, 1.0, "approach");
+  addSource(POINTCLOUD_NAME, POINTCLOUD);
+  addSource(RANGE_NAME, RANGE);
+  cm_->set_parameter(
+    rclcpp::Parameter(std::string(RANGE_NAME) + ".source_timeout", SOURCE_TIMEOUT));
+  setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME});
+
+  // Start Collision Monitor node
+  cm_->start();
+
+  // Share TF
+  sendTransforms(curr_time);
+
+  // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m).
+  // Robot should approach the obstacle.
+  publishPointCloud(2.5, curr_time);
+  ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time));
+  publishCmdVel(3.0, 3.0, 3.0);
+  ASSERT_TRUE(waitCmdVel(500ms));
+  // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s
+  double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION;
+  // Range configured but not published, range source should be considered invalid
+  // but as we set the source_timeout of the Range source to 0.0, its validity check is overidden
+  ASSERT_NEAR(
+    cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION);
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, APPROACH);
+  ASSERT_EQ(action_state_->polygon_name, "Approach");
+
+  // Stop Collision Monitor node
+  cm_->stop();
+}
+
 TEST_F(Tester, testCeasePublishZeroVel)
 {
   rclcpp::Time curr_time = cm_->now();