yjyoon / ros2_플레이스테이션_컨트롤러 star
m12watanabe1a 2022-05-31
add example node
@5a396954d643392de0d0a3e2ec2ec7cbd4c70d7f
 
5a39695
p9n_example/.vscode/c_cpp_properties.json (added)
+++ p9n_example/.vscode/c_cpp_properties.json
@@ -0,0 +1,22 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/galactic/include/**",
+        "/usr/include/**",
+        "${workspaceFolder}/include/**",
+        "${workspaceFolder}/../p9n_interface/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "clang-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "c99",
+      "cppStandard": "c++17"
+    }
+  ],
+  "version": 4
+}(파일 끝에 줄바꿈 문자 없음)
 
5a39695
p9n_example/.vscode/cspell.json (added)
+++ p9n_example/.vscode/cspell.json
@@ -0,0 +1,1 @@
+../../.vscode/cspell.json(파일 끝에 줄바꿈 문자 없음)
 
5a39695
p9n_example/.vscode/settings.json (added)
+++ p9n_example/.vscode/settings.json
@@ -0,0 +1,1 @@
+../../.vscode/settings.json(파일 끝에 줄바꿈 문자 없음)
 
5a39695
p9n_example/.vscode/uncrustify.cfg (added)
+++ p9n_example/.vscode/uncrustify.cfg
@@ -0,0 +1,1 @@
+../../.vscode/uncrustify.cfg(파일 끝에 줄바꿈 문자 없음)
 
5a39695
p9n_example/CMakeLists.txt (added)
+++ p9n_example/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(p9n_example)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Display Node ======================================================
+set(TARGET display_node)
+ament_auto_add_library(
+  ${TARGET}
+    SHARED
+      src/${TARGET}.cpp)
+rclcpp_components_register_node(
+  ${TARGET}
+    PLUGIN "p9n_example::DisplayNode"
+    EXECUTABLE ${TARGET}_exec)
+# End Display Node ==================================================
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch)
 
5a39695
p9n_example/include/p9n_example/display_node.hpp (added)
+++ p9n_example/include/p9n_example/display_node.hpp
@@ -0,0 +1,42 @@
+// 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.
+
+#pragma once
+
+#include <string>
+#include <memory>
+#include <sensor_msgs/msg/joy.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <p9n_interface/p9n_interface.hpp>
+
+
+namespace p9n_example
+{
+class DisplayNode : public rclcpp::Node
+{
+private:
+  p9n_interface::HW_TYPE hw_type_;
+  std::unique_ptr<p9n_interface::PlayStationInterface> p9n_if_;
+
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+
+public:
+  explicit DisplayNode(const rclcpp::NodeOptions &);
+  void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr);
+};
+}  // namespace p9n_example
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(p9n_example::DisplayNode)
 
5a39695
p9n_example/launch/display.launch.py (added)
+++ p9n_example/launch/display.launch.py
@@ -0,0 +1,41 @@
+# 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.
+
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Generate launch description."""
+    nodes = [
+        ComposableNodeContainer(
+            name='joy_container',
+            namespace='p9n',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='joy',
+                    plugin='joy::Joy',
+                    name='joy',
+                ),
+            ]),
+        Node(
+            name='display_node',
+            package='p9n_example',
+            executable='display_node_exec'),
+    ]
+
+    return LaunchDescription(nodes)
 
5a39695
p9n_example/package.xml (added)
+++ p9n_example/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>p9n_example</name>
+  <version>0.0.0</version>
+  <description>PlayStation JoyInterface example package.</description>
+  <maintainer email="m12watanabe1a@gmail.com">m12watanabe1a</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp_components</depend>
+  <depend>p9n_interface</depend>
+  <exec_depend>joy</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>(파일 끝에 줄바꿈 문자 없음)
 
5a39695
p9n_example/src/display_node.cpp (added)
+++ p9n_example/src/display_node.cpp
@@ -0,0 +1,169 @@
+// 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"
+
+
+namespace p9n_example
+{
+DisplayNode::DisplayNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("display_node", options)
+{
+
+  const std::string hw_name = this->declare_parameter<std::string>(
+    "controller_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));
+
+
+  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);
+
+  if (this->p9n_if_->pressedCross()) {
+    RCLCPP_INFO(this->get_logger(), " ☓ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedCircle()) {
+    RCLCPP_INFO(this->get_logger(), " ○ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedTriangle()) {
+    RCLCPP_INFO(this->get_logger(), " △ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedSquare()) {
+    RCLCPP_INFO(this->get_logger(), " □ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedL1()) {
+    RCLCPP_INFO(this->get_logger(), " L1 Pressed!");
+  }
+
+  if (this->p9n_if_->pressedR1()) {
+    RCLCPP_INFO(this->get_logger(), " R1 Pressed!");
+  }
+
+  if (this->p9n_if_->pressedL2()) {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      " L2 Pressed! : " <<
+        std::right << std::setw(6) <<
+        std::fixed << std::setprecision(3) <<
+        this->p9n_if_->pressedL2Analog());
+  }
+
+  if (this->p9n_if_->pressedR2()) {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      " R2 Pressed! : " <<
+        std::right << std::setw(6) <<
+        std::fixed << std::setprecision(3) <<
+        this->p9n_if_->pressedR2Analog());
+  }
+
+  if (this->p9n_if_->pressedSelect()) {
+    RCLCPP_INFO(
+      this->get_logger(), " Select Pressed!");
+  }
+
+  if (this->p9n_if_->pressedStart()) {
+    RCLCPP_INFO(
+      this->get_logger(), " Start Pressed!");
+  }
+
+  if (this->p9n_if_->pressedPS()) {
+    RCLCPP_INFO(
+      this->get_logger(), " PS Pressed!");
+  }
+
+  if (this->p9n_if_->pressedDPadUp()) {
+    RCLCPP_INFO(
+      this->get_logger(), " ↑ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedDPadDown()) {
+    RCLCPP_INFO(
+      this->get_logger(), " ↓ Pressed!");
+  }
+
+  if (this->p9n_if_->pressedDPadRight()) {
+    RCLCPP_INFO(
+      this->get_logger(), " → Pressed!");
+  }
+
+  if (this->p9n_if_->pressedDPadLeft()) {
+    RCLCPP_INFO(
+      this->get_logger(), " ← Pressed!");
+  }
+
+  if (
+    std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 ||
+    std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2)
+  {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      " 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());
+  }
+
+  if (
+    std::abs(this->p9n_if_->tiltedStickRX()) > 1e-2 ||
+    std::abs(this->p9n_if_->tiltedStickRY()) > 1e-2)
+  {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      " 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());
+  }
+
+  if (this->p9n_if_->pressedAny()) {
+    using namespace std::chrono_literals;
+    rclcpp::sleep_for(200ms);
+  }
+}
+}  // namespace p9n_example
0fe8548
5a39695
p9n_interface/package.xml
--- p9n_interface/package.xml
+++ p9n_interface/package.xml
@@ -10,6 +10,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
 
   <depend>rclcpp</depend>
 
+  <depend>sensor_msgs</depend>
 
 
   <test_depend>ament_lint_auto</test_depend>
 
   <test_depend>ament_lint_common</test_depend>
0fe8548
5a39695
p9n_test/CMakeLists.txt
--- p9n_test/CMakeLists.txt
+++ p9n_test/CMakeLists.txt
@@ -18,7 +18,7 @@
 
       src/workflow_handler.cpp)
 
 rclcpp_components_register_node(
 
   ${TARGET}
 
-    PLUGIN "p9n_test::PlayStationTest"
 
+    PLUGIN "p9n_test::PlayStationTestNode"
 
     EXECUTABLE ${TARGET}_exec)
 
 # End Playstation Test ==============================================
0fe8548
5a39695
p9n_test/include/p9n_test/p9n_test_node.hpp
--- p9n_test/include/p9n_test/p9n_test_node.hpp
+++ p9n_test/include/p9n_test/p9n_test_node.hpp
@@ -24,7 +24,7 @@
 
 
 namespace p9n_test
 
 {
 
-class PlayStationTest : public rclcpp::Node
 
+class PlayStationTestNode : public rclcpp::Node
 
 {
 
 private:
 
   p9n_interface::HW_TYPE hw_type_;
@@ -37,11 +37,11 @@
 
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
 
 
 public:
 
-  explicit PlayStationTest(const rclcpp::NodeOptions &);
 
+  explicit PlayStationTestNode(const rclcpp::NodeOptions &);
 
   void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr);
 
 
 };
 
 }  // namespace p9n_test
 
 
 #include "rclcpp_components/register_node_macro.hpp"
 
-RCLCPP_COMPONENTS_REGISTER_NODE(p9n_test::PlayStationTest)
 
+RCLCPP_COMPONENTS_REGISTER_NODE(p9n_test::PlayStationTestNode)
0fe8548
5a39695
p9n_test/launch/test.launch.py
--- p9n_test/launch/test.launch.py
+++ p9n_test/launch/test.launch.py
@@ -33,7 +33,7 @@
 
                 ),
 
                 ComposableNode(
 
                     package='p9n_test',
 
-                    plugin='p9n_test::PlayStationTest',
 
+                    plugin='p9n_test::PlayStationTestNode',
 
                     name='p9n_test'
 
                 ),
 
             ])]
0fe8548
5a39695
p9n_test/package.xml
--- p9n_test/package.xml
+++ p9n_test/package.xml
@@ -12,7 +12,6 @@
 
   <depend>rclcpp</depend>
 
   <depend>rclcpp_components</depend>
 
   <depend>p9n_interface</depend>
 
-  <depend>sensor_msgs</depend>
 
   <exec_depend>joy</exec_depend>
 
 
   <test_depend>ament_lint_auto</test_depend>
0fe8548
5a39695
p9n_test/src/p9n_test_node.cpp
--- p9n_test/src/p9n_test_node.cpp
+++ p9n_test/src/p9n_test_node.cpp
@@ -16,7 +16,7 @@
 
 
 namespace p9n_test
 
 {
 
-PlayStationTest::PlayStationTest(const rclcpp::NodeOptions & options)
 
+PlayStationTestNode::PlayStationTestNode(const rclcpp::NodeOptions & options)
 
 : rclcpp::Node("p9n_test_node", options)
 
 {
 
   const std::string hw_name = this->declare_parameter<std::string>(
@@ -48,7 +48,7 @@
 
 
   this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
 
     "joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)),
 
-    std::bind(&PlayStationTest::onJoy, this, std::placeholders::_1));
 
+    std::bind(&PlayStationTestNode::onJoy, this, std::placeholders::_1));
 
 
   this->wf_handler_ = std::make_unique<WorkflowHandler>();
 
   this->p9n_interface_ =
@@ -64,7 +64,7 @@
 
   }
 
 }
 
 
-void PlayStationTest::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
 
+void PlayStationTestNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
 
 {
 
   this->p9n_interface_->setJoyMsg(joy_msg);
 
   this->wf_handler_->explainNextTask(true);
Add a comment
List