diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index bafb561ed6554..b29034eb17b8e 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -15,6 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include #include #include #include @@ -85,6 +87,8 @@ class PredictedPathCheckerNode : public rclcpp::Node explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options); private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + // Subscriber std::shared_ptr self_pose_listener_; std::shared_ptr transform_listener_; @@ -96,6 +100,12 @@ class PredictedPathCheckerNode : public rclcpp::Node sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; + component_interface_utils::Subscription::SharedPtr sub_is_paused_; + component_interface_utils::Subscription::SharedPtr + sub_is_start_requested_; + + // Service + component_interface_utils::Client::SharedPtr cli_set_pause_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; @@ -111,6 +121,9 @@ class PredictedPathCheckerNode : public rclcpp::Node // Variables State current_state_{State::DRIVE}; vehicle_info_util::VehicleInfo vehicle_info_; + std::optional is_paused_; + std::optional is_start_requested_; + bool is_calling_set_pause_{false}; // Callback void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); @@ -118,6 +131,9 @@ class PredictedPathCheckerNode : public rclcpp::Node void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void on_is_start_requested( + const control_interface::IsStartRequested::Message::ConstSharedPtr msg); + void on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg); // Publisher rclcpp::Publisher::SharedPtr virtual_wall_publisher_; @@ -137,6 +153,7 @@ class PredictedPathCheckerNode : public rclcpp::Node void publishVirtualWall(boost::optional & output); TrajectoryPoints trimTrajectoryFromSelfPose( const TrajectoryPoints & input, const Pose & self_pose); + void sendRequest(bool make_stop_vehicle); // Parameter collision_checker::collisionCheckerParam collision_checker_param_; diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 198bb053abe55..e0187181d0f3e 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -29,7 +29,10 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_control_msgs vehicle_info_util + component_interface_specs + component_interface_utils ament_cmake_ros ament_lint_auto diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index e32105a23ea10..57a5c550550e5 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -32,6 +32,11 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n : Node("predicted_path_checker_node", node_options), updater_(this) { using std::placeholders::_1; + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_set_pause_, group_cli_); + adaptor.init_sub(sub_is_paused_, this, &PredictedPathCheckerNode::on_is_paused); + adaptor.init_sub(sub_is_start_requested_, this, &PredictedPathCheckerNode::on_is_start_requested); // Node Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); @@ -167,6 +172,13 @@ bool PredictedPathCheckerNode::isDataReady() return false; } + if (!is_paused_ || !is_start_requested_ || !cli_set_pause_->service_is_ready()) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for pause interface service..."); + return false; + } + return true; } @@ -220,6 +232,18 @@ void PredictedPathCheckerNode::onTimer() auto collision_checker_output = collision_checker_->run(collision_checker_input); updateState(collision_checker_output); + + // send pause request + if (current_state_ == State::PAUSE) { + if (!is_paused_ || is_start_requested_) { + sendRequest(true); + } + } else if (current_state_ == State::DRIVE) { + if (is_paused_) { + sendRequest(false); + } + } + publishVirtualWall(collision_checker_output); } @@ -323,6 +347,27 @@ void PredictedPathCheckerNode::publishVirtualWall( virtual_wall_publisher_->publish(msg); } +void PredictedPathCheckerNode::on_is_paused( + const control_interface::IsPaused::Message::ConstSharedPtr msg) +{ + is_paused_ = msg->data; +} + +void PredictedPathCheckerNode::on_is_start_requested( + const control_interface::IsStartRequested::Message::ConstSharedPtr msg) +{ + is_start_requested_ = msg->data; +} + +void PredictedPathCheckerNode::sendRequest(bool pause) +{ + if (!is_calling_set_pause_ && cli_set_pause_->service_is_ready()) { + const auto req = std::make_shared(); + req->pause = pause; + is_calling_set_pause_ = true; + cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; }); + } +} } // namespace predicted_path_checker #include