From 0eb34d0c6057497c15f4170c270478ce8e9e94d6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 24 May 2024 16:19:07 +0900 Subject: [PATCH 1/3] feat(goal_pose_setter): add localization trigger Signed-off-by: Takagi, Isamu --- .../launch/reference.launch.xml | 8 +- .../goal_pose_setter/package.xml | 5 +- .../src/goal_pose_setter_node.cpp | 118 +++++++++--------- .../src/goal_pose_setter_node.hpp | 42 +++++++ 4 files changed, 107 insertions(+), 66 deletions(-) create mode 100644 aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp diff --git a/aichallenge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index fa003500..f0ccb859 100644 --- a/aichallenge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -6,7 +6,7 @@ - + @@ -200,7 +200,7 @@ - + @@ -236,7 +236,7 @@ - + @@ -250,4 +250,4 @@ - \ No newline at end of file + diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml index 741a2962..cd056c27 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml +++ b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml @@ -9,9 +9,10 @@ ament_cmake_auto - rclcpp - geometry_msgs autoware_adapi_v1_msgs + geometry_msgs + rclcpp + std_srvs ament_cmake diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp index d186046a..1fbd37b6 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp +++ b/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("/localization/trigger_node"); + goal_publisher_ = this->create_publisher("/planning/mission_planning/goal", qos); + route_state_subscriber_ = this->create_subscription( + "/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("/planning/mission_planning/goal", qos); - route_state_subscriber_ = this->create_subscription( - "/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) { + ++delay_count_; + 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(); + req->data = true; + ekf_trigger_client_->async_send_request(req, [this](rclcpp::Client::SharedFuture future) + { + stop_initializing_pose_ = future.get()->success; + 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(); - 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(); + 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::SharedPtr goal_publisher_; - // subscribe route state - rclcpp::Subscription::SharedPtr route_state_subscriber_; - rclcpp::TimerBase::SharedPtr timer_; - int delay_count_; -}; +} int main(int argc, char const *argv[]) { diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp new file mode 100644 index 00000000..ad719221 --- /dev/null +++ b/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 + +#include +#include +#include + +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::SharedPtr ekf_trigger_client_; + rclcpp::Publisher::SharedPtr goal_publisher_; + rclcpp::Subscription::SharedPtr route_state_subscriber_; + rclcpp::TimerBase::SharedPtr timer_; + int delay_count_ = 0; + bool stop_initializing_pose_ = false; + bool stop_streaming_goal_pose_ = false; +}; + +#endif // GOAL_POSE_SETTER_NODE_ From 146644c16508b9b9e1db191018b5ca966ee1c29b Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 24 May 2024 16:36:53 +0900 Subject: [PATCH 2/3] feat(goal_pose_setter): add localization trigger Signed-off-by: Takagi, Isamu --- .../goal_pose_setter/src/goal_pose_setter_node.cpp | 5 ----- .../goal_pose_setter/src/goal_pose_setter_node.hpp | 1 - aichallenge/run_evaluation.bash | 3 ++- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp index 1fbd37b6..091a8d77 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp +++ b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp @@ -24,11 +24,6 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") void GoalPosePublisher::on_timer() { - if (delay_count_ <= 10) { - ++delay_count_; - return; - } - if (!stop_initializing_pose_) { if (ekf_trigger_client_->service_is_ready()) { const auto req = std::make_shared(); diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp index ad719221..0116dafe 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp +++ b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp @@ -34,7 +34,6 @@ class GoalPosePublisher : public rclcpp::Node rclcpp::Publisher::SharedPtr goal_publisher_; rclcpp::Subscription::SharedPtr route_state_subscriber_; rclcpp::TimerBase::SharedPtr timer_; - int delay_count_ = 0; bool stop_initializing_pose_ = false; bool stop_streaming_goal_pose_ = false; }; diff --git a/aichallenge/run_evaluation.bash b/aichallenge/run_evaluation.bash index f3a93860..cd253b76 100755 --- a/aichallenge/run_evaluation.bash +++ b/aichallenge/run_evaluation.bash @@ -42,7 +42,8 @@ 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 +# TODO +#ros2 service call /localization/trigger_node std_srvs/srv/SetBool '{data: true}' >/dev/null wait $PID_AWSIM # Stop recording rviz2 From 6edd53a1a592c1b0122be56892d99c33d5ddd4c3 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 24 May 2024 19:46:19 +0900 Subject: [PATCH 3/3] apply control mode Signed-off-by: Takagi, Isamu --- .../goal_pose_setter/src/goal_pose_setter_node.cpp | 5 +++++ .../goal_pose_setter/src/goal_pose_setter_node.hpp | 1 + aichallenge/run_evaluation.bash | 3 +-- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp index 091a8d77..9223fc5f 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp +++ b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp @@ -24,6 +24,10 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") void GoalPosePublisher::on_timer() { + if (++delay_count_ <= 10) { + return; + } + if (!stop_initializing_pose_) { if (ekf_trigger_client_->service_is_ready()) { const auto req = std::make_shared(); @@ -31,6 +35,7 @@ void GoalPosePublisher::on_timer() ekf_trigger_client_->async_send_request(req, [this](rclcpp::Client::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"); diff --git a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp index 0116dafe..536218b8 100644 --- a/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp +++ b/aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp @@ -36,6 +36,7 @@ class GoalPosePublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; bool stop_initializing_pose_ = false; bool stop_streaming_goal_pose_ = false; + int delay_count_ = 0; }; #endif // GOAL_POSE_SETTER_NODE_ diff --git a/aichallenge/run_evaluation.bash b/aichallenge/run_evaluation.bash index cd253b76..58c94134 100755 --- a/aichallenge/run_evaluation.bash +++ b/aichallenge/run_evaluation.bash @@ -42,8 +42,7 @@ sleep 5 # Start driving and wait for the simulation to finish echo "Waiting for the simulation" -# TODO -#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