@@ -35,7 +35,7 @@ void ReactionAnalyzerNode::routeStateCallback(RouteState::ConstSharedPtr msg_ptr
3535void ReactionAnalyzerNode::vehiclePoseCallback (Odometry::ConstSharedPtr msg_ptr)
3636{
3737 std::lock_guard<std::mutex> lock (mutex_);
38- odometry_ = msg_ptr;
38+ odometry_ptr_ = msg_ptr;
3939}
4040
4141void ReactionAnalyzerNode::initializationStateCallback (
@@ -45,6 +45,12 @@ void ReactionAnalyzerNode::initializationStateCallback(
4545 initialization_state_ptr_ = std::move (msg_ptr);
4646}
4747
48+ void ReactionAnalyzerNode::groundTruthPoseCallback (PoseStamped::ConstSharedPtr msg_ptr)
49+ {
50+ std::lock_guard<std::mutex> lock (mutex_);
51+ ground_truth_pose_ptr_ = std::move (msg_ptr);
52+ }
53+
4854ReactionAnalyzerNode::ReactionAnalyzerNode (rclcpp::NodeOptions options)
4955: Node(" reaction_analyzer_node" true ))
5056{
@@ -130,6 +136,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
130136 // init dummy perception publisher
131137 const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
132138 std::chrono::duration<double >(node_params_.dummy_perception_publisher_period ));
139+
133140 dummy_perception_timer_ = rclcpp::create_timer (
134141 this , get_clock (), period_ns,
135142 std::bind (&ReactionAnalyzerNode::dummyPerceptionPublisher, this ));
@@ -138,12 +145,19 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
138145 // Create topic publishers
139146 topic_publisher_ptr_ =
140147 std::make_shared<topic_publisher::TopicPublisher>(this , spawn_object_cmd_, spawn_cmd_time_);
148+
149+ // Subscribe to the ground truth position
150+ sub_ground_truth_pose_ = create_subscription<PoseStamped>(
151+ " input/ground_truth_pose" 1 },
152+ std::bind (&ReactionAnalyzerNode::groundTruthPoseCallback, this , _1),
153+ createSubscriptionOptions (this ));
141154 }
142155
143156 // Load the subscriber to listen the topics for reactions
144- odometry_ = std::make_shared<Odometry>(); // initialize the odometry before init the subscriber
145- subscriber_ptr_ =
146- std::make_unique<subscriber::SubscriberBase>(this , odometry_, entity_pose_, spawn_object_cmd_);
157+ odometry_ptr_ =
158+ std::make_shared<Odometry>(); // initialize the odometry before init the subscriber
159+ subscriber_ptr_ = std::make_unique<subscriber::SubscriberBase>(
160+ this , odometry_ptr_, entity_pose_, spawn_object_cmd_);
147161
148162 const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
149163 std::chrono::duration<double >(node_params_.timer_period ));
@@ -154,16 +168,19 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
154168void ReactionAnalyzerNode::onTimer ()
155169{
156170 mutex_.lock ();
157- const auto current_odometry_ptr = odometry_ ;
171+ const auto current_odometry_ptr = odometry_ptr_ ;
158172 const auto initialization_state_ptr = initialization_state_ptr_;
159173 const auto route_state_ptr = current_route_state_ptr_;
160174 const auto operation_mode_ptr = operation_mode_ptr_;
175+ const auto ground_truth_pose_ptr = ground_truth_pose_ptr_;
161176 const auto spawn_cmd_time = spawn_cmd_time_;
162177 mutex_.unlock ();
163178
164179 // Init the test environment
165180 if (!test_environment_init_time_) {
166- initEgoForTest (initialization_state_ptr, route_state_ptr, operation_mode_ptr);
181+ initEgoForTest (
182+ initialization_state_ptr, route_state_ptr, operation_mode_ptr, ground_truth_pose_ptr,
183+ current_odometry_ptr);
167184 return ;
168185 }
169186
@@ -437,11 +454,13 @@ void ReactionAnalyzerNode::initPredictedObjects()
437454void ReactionAnalyzerNode::initEgoForTest (
438455 const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
439456 const RouteState::ConstSharedPtr & route_state_ptr,
440- const OperationModeState::ConstSharedPtr & operation_mode_ptr)
457+ const OperationModeState::ConstSharedPtr & operation_mode_ptr,
458+ const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
459+ const Odometry::ConstSharedPtr & odometry_ptr)
441460{
442461 const auto current_time = this ->now ();
443462
444- // Initialize the vehicle
463+ // Initialize the test environment
445464 constexpr double initialize_call_period = 1.0 ; // sec
446465
447466 if (
@@ -450,6 +469,7 @@ void ReactionAnalyzerNode::initEgoForTest(
450469 initialize_call_period) {
451470 last_test_environment_init_request_time_ = current_time;
452471
472+ // Pose initialization of the ego
453473 if (
454474 initialization_state_ptr &&
455475 (initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED ||
@@ -466,6 +486,35 @@ void ReactionAnalyzerNode::initEgoForTest(
466486 return ;
467487 }
468488
489+ // Wait until odometry_ptr is initialized
490+ if (!odometry_ptr) {
491+ RCLCPP_WARN_ONCE (get_logger (), " Odometry is not received. Waiting for odometry..."
492+ return ;
493+ }
494+
495+ // Check is position initialized accurately, if node is running PerceptionPlanning mode
496+ if (node_running_mode_ == RunningMode::PerceptionPlanning) {
497+ if (!ground_truth_pose_ptr) {
498+ RCLCPP_WARN (
499+ get_logger (), " Ground truth pose is not received. Waiting for Ground truth pose..."
500+ return ;
501+ } else {
502+ constexpr double deviation_threshold = 0.1 ;
503+ const auto deviation = tier4_autoware_utils::calcPoseDeviation (
504+ ground_truth_pose_ptr->pose , odometry_ptr->pose .pose );
505+ if (
506+ deviation.longitudinal > deviation_threshold || deviation.lateral > deviation_threshold ||
507+ deviation.yaw > deviation_threshold) {
508+ RCLCPP_ERROR (
509+ get_logger (),
510+ " Deviation between ground truth position and ego position is high. Node is shutting "
511+ " down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f"
512+ deviation.longitudinal , deviation.lateral , deviation.yaw );
513+ rclcpp::shutdown ();
514+ }
515+ }
516+ }
517+
469518 if (route_state_ptr && (route_state_ptr->state != RouteState::SET || !is_route_set_)) {
470519 if (route_state_ptr->state == RouteState::SET) {
471520 is_route_set_ = true ;
@@ -477,6 +526,7 @@ void ReactionAnalyzerNode::initEgoForTest(
477526 return ;
478527 }
479528
529+ // if node is running PlanningControl mode, change ego to Autonomous mode.
480530 if (node_running_mode_ == RunningMode::PlanningControl) {
481531 // change to autonomous
482532 if (operation_mode_ptr && operation_mode_ptr->mode != OperationModeState::AUTONOMOUS) {
0 commit comments