diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index e88f7bcb3904e..3ea279774a299 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -18,6 +18,7 @@ #include "joy_controller/joy_converter/joy_converter_base.hpp" #include +#include #include #include @@ -66,19 +67,18 @@ class AutowareJoyControllerNode : public rclcpp::Node double backward_accel_ratio_; // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - rclcpp::Subscription::SharedPtr sub_joy_; - rclcpp::Subscription::SharedPtr sub_odom_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_joy_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_; rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; - void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onJoy(); + void onOdometry(); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index f7a5ed805b8ad..f160d561057e8 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -26,6 +26,7 @@ sensor_msgs std_srvs tier4_api_utils + tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 39198825f9c4e..c62eea6d969d9 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -150,6 +150,7 @@ namespace joy_controller { void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) { + const auto msg = sub_joy_.takeData(); last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); @@ -190,8 +191,13 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt } } -void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void AutowareJoyControllerNode::onOdometry() { + if (raw_control_) { + return; + } + + const auto msg = sub_odom_.takeData(); auto twist = std::make_shared(); twist->header = msg->header; twist->twist = msg->twist.twist; @@ -243,6 +249,9 @@ bool AutowareJoyControllerNode::isDataReady() void AutowareJoyControllerNode::onTimer() { + onOdometry(); + onJoy(); + if (!isDataReady()) { return; } @@ -470,23 +479,11 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_services_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; // Subscriber - sub_joy_ = this->create_subscription( - "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), - subscriber_option); - if (!raw_control_) { - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); - } else { + if (raw_control_) { twist_ = std::make_shared(); }