// Copyright 2022 HarvestX 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. #include "p9n_example/display_node.hpp" #include "std_msgs/msg/string.hpp" namespace p9n_example { DisplayNode::DisplayNode(const rclcpp::NodeOptions & options) : rclcpp::Node("display_node", options) { const std::string hw_name = this->declare_parameter<std::string>( "hw_type", p9n_interface::HW_NAME::DUALSENSE); try { this->hw_type_ = p9n_interface::getHwType(hw_name); } catch (std::runtime_error & e) { RCLCPP_ERROR( this->get_logger(), e.what()); RCLCPP_ERROR( this->get_logger(), "Please select hardware from %s", p9n_interface::getAllHwName().c_str()); rclcpp::shutdown(); return; } this->p9n_if_ = std::make_unique<p9n_interface::PlayStationInterface>(this->hw_type_); this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>( "joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)), std::bind(&DisplayNode::onJoy, this, std::placeholders::_1)); // Define the publisher this->feedback_pub_ = this->create_publisher<std_msgs::msg::String>( "joy_feedback", 10); if (this->joy_sub_->get_publisher_count() == 0) { RCLCPP_WARN( this->get_logger(), "Joy node not launched"); } } void DisplayNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg) { this->p9n_if_->setJoyMsg(joy_msg); std::string feedback; // for handling multi button press if (this->p9n_if_->pressedCross()) { auto msg = std_msgs::msg::String(); msg.data = "Cross button pressed!"; RCLCPP_INFO(this->get_logger(), " ☓ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedCircle()) { auto msg = std_msgs::msg::String(); msg.data = "Circle button pressed!"; RCLCPP_INFO(this->get_logger(), " ○ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedTriangle()) { auto msg = std_msgs::msg::String(); msg.data = "Triangle button pressed!"; RCLCPP_INFO(this->get_logger(), " △ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedSquare()) { auto msg = std_msgs::msg::String(); msg.data = "Square button pressed!"; RCLCPP_INFO(this->get_logger(), " □ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedL1()) { auto msg = std_msgs::msg::String(); msg.data = "L1 button pressed!"; RCLCPP_INFO(this->get_logger(), " L1 Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedR1()) { auto msg = std_msgs::msg::String(); msg.data = "R1 button pressed!"; RCLCPP_INFO(this->get_logger(), " R1 Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedL2()) { auto msg = std_msgs::msg::String(); msg.data = "L2 button pressed with analog value: " + std::to_string(this->p9n_if_->pressedL2Analog()); RCLCPP_INFO_STREAM( this->get_logger(), " L2 Pressed! : " << std::right << std::setw(6) << std::fixed << std::setprecision(3) << this->p9n_if_->pressedL2Analog()); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedR2()) { auto msg = std_msgs::msg::String(); msg.data = "R2 button pressed with analog value: " + std::to_string(this->p9n_if_->pressedR2Analog()); RCLCPP_INFO_STREAM( this->get_logger(), " R2 Pressed! : " << std::right << std::setw(6) << std::fixed << std::setprecision(3) << this->p9n_if_->pressedR2Analog()); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedSelect()) { auto msg = std_msgs::msg::String(); msg.data = "Select button pressed!"; RCLCPP_INFO(this->get_logger(), " Select Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedStart()) { auto msg = std_msgs::msg::String(); msg.data = "Start button pressed!"; RCLCPP_INFO(this->get_logger(), " Start Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedPS()) { auto msg = std_msgs::msg::String(); msg.data = "PS button pressed!"; RCLCPP_INFO(this->get_logger(), " PS Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedDPadUp()) { auto msg = std_msgs::msg::String(); msg.data = "D-Pad Up pressed!"; RCLCPP_INFO(this->get_logger(), " ↑ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedDPadDown()) { auto msg = std_msgs::msg::String(); msg.data = "D-Pad Down pressed!"; RCLCPP_INFO(this->get_logger(), " ↓ Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedDPadRight()) { auto msg = std_msgs::msg::String(); msg.data = "D-Pad Right pressed!"; RCLCPP_INFO(this->get_logger(), " → Pressed!"); this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedDPadLeft()) { auto msg = std_msgs::msg::String(); msg.data = "D-Pad Left pressed!"; RCLCPP_INFO(this->get_logger(), " ← Pressed!"); this->feedback_pub_->publish(msg); } // L and R stick if ( std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 || std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2) { std::ostringstream oss; oss << "LStick Tilted! x: " << std::fixed << std::setw(6) << std::setprecision(3) << this->p9n_if_->tiltedStickLX() << " y: " << std::fixed << std::setw(6) << std::setprecision(3) << this->p9n_if_->tiltedStickLY(); std::string feedback = oss.str(); RCLCPP_INFO_STREAM(this->get_logger(), feedback); // Publish the feedback message auto msg = std_msgs::msg::String(); msg.data = feedback; this->feedback_pub_->publish(msg); } if ( std::abs(this->p9n_if_->tiltedStickRX()) > 1e-2 || std::abs(this->p9n_if_->tiltedStickRY()) > 1e-2) { std::ostringstream oss; oss << "RStick Tilted! x: " << std::fixed << std::setw(6) << std::setprecision(3) << this->p9n_if_->tiltedStickRX() << " y: " << std::fixed << std::setw(6) << std::setprecision(3) << this->p9n_if_->tiltedStickRY(); std::string feedback = oss.str(); RCLCPP_INFO_STREAM(this->get_logger(), feedback); // Publish the feedback message auto msg = std_msgs::msg::String(); msg.data = feedback; this->feedback_pub_->publish(msg); } if (this->p9n_if_->pressedAny()) { using namespace std::chrono_literals; // NOLINT rclcpp::sleep_for(100ms); } } } // namespace p9n_example #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(p9n_example::DisplayNode)