add sending goal pose in perception planning mode

autowarefoundation · Jan 31, 2024 · cb628e2 · cb628e2
1 parent 7e15c2a
commit cb628e2
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 19 deletions.
diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
@@ -31,25 +31,25 @@
         pitch: 0.0
         yaw: 90.0
       goal_pose:
-        x: 1.5
-        y: 400.0
-        z: 101.25
+        x: 81462.78125
+        y: 49978.484375
+        z: 0.0
         roll: 0.0
         pitch: 0.0
-        yaw: 90.0
+        yaw: 35.0
     chain:
-#      planning_validator:
-#        topic_name: /planning/scenario_planning/trajectory
-#        message_type: autoware_auto_planning_msgs::msg::Trajectory
-#      scenario_selector:
-#        topic_name: /planning/scenario_planning/scenario_selector/trajectory
-#        message_type: autoware_auto_planning_msgs::msg::Trajectory
-#      motion_velocity_smoother:
-#        topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
-#        message_type: autoware_auto_planning_msgs::msg::Trajectory
-#      obstacle_stop_planner:
-#        topic_name: /planning/scenario_planning/lane_driving/trajectory
-#        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      planning_validator:
+        topic_name: /planning/scenario_planning/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      scenario_selector:
+        topic_name: /planning/scenario_planning/scenario_selector/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      motion_velocity_smoother:
+        topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      obstacle_stop_planner:
+        topic_name: /planning/scenario_planning/lane_driving/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
 #      trajectory_follower:
 #        topic_name: /control/trajectory_follower/control_cmd
 #        message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
@@ -62,9 +62,9 @@
       occupancy_grid_map_outlier:
         topic_name: /perception/obstacle_segmentation/pointcloud
         message_type: sensor_msgs::msg::PointCloud2
-      voxel_based_compare_map_filter:
-        topic_name: /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud
-        message_type: sensor_msgs::msg::PointCloud2
+#      voxel_based_compare_map_filter:
+#        topic_name: /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud
+#        message_type: sensor_msgs::msg::PointCloud2
       lidar_centerpoint:
         topic_name: /perception/object_recognition/detection/centerpoint/objects
         message_type: autoware_auto_perception_msgs::msg::DetectedObjects

diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
@@ -1063,6 +1063,15 @@ void ReactionAnalyzerNode::onTimer()
     if (
       initialization_state_ptr &&
       initialization_state_ptr->state == LocalizationInitializationState::INITIALIZED) {
+
+      if (route_state_ptr && route_state_ptr->state != RouteState::SET && !is_route_set_) {
+        // publish goal pose
+        is_route_set_ = true;
+        goal_pose_.header.stamp = this->now();
+        goal_pose_.header.frame_id = "map";
+        pub_goal_pose_->publish(goal_pose_);
+        return;
+      }
       if (!last_test_environment_init_time_) {
         last_test_environment_init_time_ = this->now();
       }