feat: localization trigger and control mode

aichallenge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml

@@ -6,7 +6,7 @@
   <arg name="map_path" default="/aichallenge/mapfile"/>
   <arg name="vehicle_model" default="racing_kart"  description="vehicle model name"/>
   <arg name="sensor_model" description="sensor model name"/>
-  
+
   <!-- Optional parameters -->
   <!-- Map -->
   <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
@@ -200,7 +200,7 @@
     <param name="lookahead_gain" value="0.4"/>
     <param name="lookahead_min_distance" value="5.0"/>
     <param name="speed_proportional_gain" value="1.0"/>
-    
+
     <remap from="input/kinematics" to="/localization/kinematic_state"/>
     <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
     <remap from="output/control_cmd" to="/control/command/control_cmd"/>
@@ -236,7 +236,7 @@
         <param name="viewer_frame" value="viewer" />
         <extra_arg name="use_intra_process_comms" value="false" />
       </composable_node>
-     
+
     </node_container>
 
   </group> <!-- map -->
@@ -250,4 +250,4 @@
     <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
   </group>
 
-</launch>
+</launch>

aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml

@@ -9,9 +9,10 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
   <depend>autoware_adapi_v1_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp

@@ -1,76 +1,74 @@
-#include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "autoware_adapi_v1_msgs/msg/route_state.hpp"
+#include "goal_pose_setter_node.hpp"
 
-class GoalPosePublisher : public rclcpp::Node
+GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher")
 {
-    bool stop_streaming_goal_pose_ = false;
+    const auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
+    ekf_trigger_client_ = this->create_client<std_srvs::srv::SetBool>("/localization/trigger_node");
+    goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/planning/mission_planning/goal", qos);
+    route_state_subscriber_ = this->create_subscription<autoware_adapi_v1_msgs::msg::RouteState>(
+        "/planning/mission_planning/route_state",
+        rclcpp::QoS(rclcpp::KeepLast(10)).reliable().transient_local(),
+        std::bind(&GoalPosePublisher::route_state_callback, this, std::placeholders::_1));
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(300),
+        std::bind(&GoalPosePublisher::on_timer, this));
 
-public:
-    GoalPosePublisher() : Node("goal_pose_publisher")
-    {
-        auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
-        goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/planning/mission_planning/goal", qos);
-        route_state_subscriber_ = this->create_subscription<autoware_adapi_v1_msgs::msg::RouteState>(
-            "/planning/mission_planning/route_state",
-            rclcpp::QoS(rclcpp::KeepLast(10)).reliable().transient_local(),
-            std::bind(&GoalPosePublisher::route_state_callback, this, std::placeholders::_1));
-        timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(300),
-            std::bind(&GoalPosePublisher::publish_goal_pose, this));
-
-        this->declare_parameter("goal.position.x", 21920.2);
-        this->declare_parameter("goal.position.y", 51741.1);
-        this->declare_parameter("goal.position.z", 0.0);
-        this->declare_parameter("goal.orientation.x", 0.0);
-        this->declare_parameter("goal.orientation.y", 0.0);
-        this->declare_parameter("goal.orientation.z", 0.645336);
-        this->declare_parameter("goal.orientation.w", 0.763899);
+    this->declare_parameter("goal.position.x", 21920.2);
+    this->declare_parameter("goal.position.y", 51741.1);
+    this->declare_parameter("goal.position.z", 0.0);
+    this->declare_parameter("goal.orientation.x", 0.0);
+    this->declare_parameter("goal.orientation.y", 0.0);
+    this->declare_parameter("goal.orientation.z", 0.645336);
+    this->declare_parameter("goal.orientation.w", 0.763899);
+}
 
-        delay_count_ = 0;
+void GoalPosePublisher::on_timer()
+{
+    if (++delay_count_ <= 10) {
+        return;
     }
 
-private:
-    void publish_goal_pose()
-    {
-        if (delay_count_ <= 10) {
-          ++delay_count_;
-          return;
+    if (!stop_initializing_pose_) {
+        if (ekf_trigger_client_->service_is_ready()) {
+            const auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+            req->data = true;
+            ekf_trigger_client_->async_send_request(req, [this](rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future)
+            {
+                stop_initializing_pose_ = future.get()->success;
+                delay_count_ = 0;
+                RCLCPP_INFO(this->get_logger(), "Complete localization trigger");
+            });
+            RCLCPP_INFO(this->get_logger(), "Call localization trigger");
         }
+        return;
+    }
 
-        if (!stop_streaming_goal_pose_)
-        {
-            auto msg = std::make_shared<geometry_msgs::msg::PoseStamped>();
-            msg->header.stamp = this->get_clock()->now();
-            msg->header.frame_id = "map";
-            msg->pose.position.x = this->get_parameter("goal.position.x").as_double();
-            msg->pose.position.y = this->get_parameter("goal.position.y").as_double();
-            msg->pose.position.z = this->get_parameter("goal.position.z").as_double();
-            msg->pose.orientation.x = this->get_parameter("goal.orientation.x").as_double();
-            msg->pose.orientation.y = this->get_parameter("goal.orientation.y").as_double();
-            msg->pose.orientation.z = this->get_parameter("goal.orientation.z").as_double();
-            msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double();
+    if (!stop_streaming_goal_pose_)
+    {
+        auto msg = std::make_shared<geometry_msgs::msg::PoseStamped>();
+        msg->header.stamp = this->get_clock()->now();
+        msg->header.frame_id = "map";
+        msg->pose.position.x = this->get_parameter("goal.position.x").as_double();
+        msg->pose.position.y = this->get_parameter("goal.position.y").as_double();
+        msg->pose.position.z = this->get_parameter("goal.position.z").as_double();
+        msg->pose.orientation.x = this->get_parameter("goal.orientation.x").as_double();
+        msg->pose.orientation.y = this->get_parameter("goal.orientation.y").as_double();
+        msg->pose.orientation.z = this->get_parameter("goal.orientation.z").as_double();
+        msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double();
 
-            goal_publisher_->publish(*msg);
-            RCLCPP_INFO(this->get_logger(), "Publishing goal pose");
-        }
+        goal_publisher_->publish(*msg);
+        RCLCPP_INFO(this->get_logger(), "Publishing goal pose");
     }
+}
 
-    void route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg)
+void GoalPosePublisher::route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg)
+{
+    if (msg->state >= autoware_adapi_v1_msgs::msg::RouteState::SET)
     {
-        if (msg->state >= autoware_adapi_v1_msgs::msg::RouteState::SET)
-        {
-            stop_streaming_goal_pose_ = true;
-            RCLCPP_INFO(this->get_logger(), "Stop streaming goal pose");
-        }
+        stop_streaming_goal_pose_ = true;
+        RCLCPP_INFO(this->get_logger(), "Stop streaming goal pose");
     }
-
-    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;
-    // subscribe route state
-    rclcpp::Subscription<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr route_state_subscriber_;
-    rclcpp::TimerBase::SharedPtr timer_;
-    int delay_count_;
-};
+}
 
 int main(int argc, char const *argv[])
 {

aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp

@@ -0,0 +1,42 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GOAL_POSE_SETTER_NODE_
+#define GOAL_POSE_SETTER_NODE_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+
+class GoalPosePublisher : public rclcpp::Node
+{
+public:
+    GoalPosePublisher();
+
+private:
+    void on_timer();
+    void route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg);
+
+    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr ekf_trigger_client_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;
+    rclcpp::Subscription<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr route_state_subscriber_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    bool stop_initializing_pose_ = false;
+    bool stop_streaming_goal_pose_ = false;
+    int delay_count_ = 0;
+};
+
+#endif  // GOAL_POSE_SETTER_NODE_

aichallenge/run_evaluation.bash

@@ -42,7 +42,7 @@ sleep 5
 
 # Start driving and wait for the simulation to finish
 echo "Waiting for the simulation"
-ros2 service call /localization/trigger_node std_srvs/srv/SetBool '{data: true}' >/dev/null
+ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}' >/dev/null
 wait $PID_AWSIM
 
 # Stop recording rviz2