yjyoon / ros2_플레이스테이션_컨트롤러 star
WATANABE Aoi 2022-07-12
Merge pull request #11 from HarvestX/feature/p9n_teleop_twist 
add teleop_twist_joy
@1a9ef34961215be304d293f00f4cd6f7f7434fa7
dfe82b5
1a9ef34
.vscode/c_cpp_properties.json
--- .vscode/c_cpp_properties.json
+++ .vscode/c_cpp_properties.json
@@ -1,23 +1,24 @@
 
 {
 
-  "configurations": [
 
-    {
 
-      "browse": {
 
-        "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
 
-        "limitSymbolsToIncludedHeaders": false
 
-      },
 
-      "includePath": [
 
-        "/opt/ros/galactic/include/**",
 
-        "/usr/include/**",
 
-        "{workspaceFolder}/p9n_interface/include/**",
 
-        "{workspaceFolder}/p9n_example/include/**",
 
-        "{workspaceFolder}/p9n_test/include/**"
 
-      ],
 
-      "name": "ROS",
 
-      "intelliSenseMode": "clang-x64",
 
-      "compilerPath": "/usr/bin/gcc",
 
-      "cStandard": "c99",
 
-      "cppStandard": "c++17"
 
-    }
 
-  ],
 
-  "version": 4
 
-}
 
+    "configurations": [
 
+        {
 
+            "browse": {
 
+                "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
 
+                "limitSymbolsToIncludedHeaders": false
 
+            },
 
+            "includePath": [
 
+                "/opt/ros/galactic/include/**",
 
+                "/usr/include/**",
 
+                "{workspaceFolder}/p9n_interface/include/**",
 
+                "{workspaceFolder}/p9n_example/include/**",
 
+                "{workspaceFolder}/p9n_test/include/**",
 
+                "${workspaceFolder}/p9n_node/include/**"
 
+            ],
 
+            "name": "ROS",
 
+            "intelliSenseMode": "clang-x64",
 
+            "compilerPath": "/usr/bin/gcc",
 
+            "cStandard": "c99",
 
+            "cppStandard": "c++17"
 
+        }
 
+    ],
 
+    "version": 4
 
+}
(파일 끝에 줄바꿈 문자 없음)
 
1a9ef34
p9n_node/CMakeLists.txt (added)
+++ p9n_node/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(p9n_node)
+
+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 teleop_twist_joy_node)
+ament_auto_add_library(
+  ${TARGET}
+    SHARED
+      src/${TARGET}.cpp)
+rclcpp_components_register_node(
+  ${TARGET}
+    PLUGIN "p9n_node::TeleopTwistJoyNode"
+    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)
 
1a9ef34
p9n_node/include/p9n_node/teleop_twist_joy_node.hpp (added)
+++ p9n_node/include/p9n_node/teleop_twist_joy_node.hpp
@@ -0,0 +1,44 @@
+// 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 <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <p9n_interface/p9n_interface.hpp>
+
+
+namespace p9n_node
+{
+class TeleopTwistJoyNode : 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_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+
+public:
+  TeleopTwistJoyNode(const rclcpp::NodeOptions & options);
+  void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg);
+};
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(p9n_node::TeleopTwistJoyNode)
 
1a9ef34
p9n_node/launch/teleop.launch.py (added)
+++ p9n_node/launch/teleop.launch.py
@@ -0,0 +1,60 @@
+# 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.actions import DeclareLaunchArgument
+from launch.substitutions import (
+    LaunchConfiguration,
+    TextSubstitution,
+)
+
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Generate launch description."""
+    launch_args = [
+        DeclareLaunchArgument(
+            'hw_type',
+            default_value=TextSubstitution(text='DualSense')
+        )
+    ]
+    nodes = [
+        ComposableNodeContainer(
+            name='joy_container',
+            package='rclcpp_components',
+            executable='component_container',
+            namespace='',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='joy',
+                    plugin='joy::Joy',
+                    name='joy',
+                    namespace='',
+                ),
+                ComposableNode(
+                    package='p9n_node',
+                    plugin='p9n_node::TeleopTwistJoyNode',
+                    name='teleop_twist_joy_node',
+                    namespace='',
+                    parameters=[{
+                        'hw_type': LaunchConfiguration('hw_type')
+                    }],
+                )
+            ],
+        ),
+    ]
+
+    return LaunchDescription(launch_args + nodes)
 
1a9ef34
p9n_node/package.xml (added)
+++ p9n_node/package.xml
@@ -0,0 +1,23 @@
+<?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_node</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>
+  <depend>geometry_msgs</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>(파일 끝에 줄바꿈 문자 없음)
 
1a9ef34
p9n_node/src/teleop_twist_joy_node.cpp (added)
+++ p9n_node/src/teleop_twist_joy_node.cpp
@@ -0,0 +1,79 @@
+// 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_node/teleop_twist_joy_node.hpp"
+
+namespace p9n_node
+{
+
+TeleopTwistJoyNode::TeleopTwistJoyNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("teleop_twist_joy_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(&TeleopTwistJoyNode::onJoy, this, std::placeholders::_1));
+
+  this->twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
+    "cmd_vel", rclcpp::QoS(10));
+
+
+  if (this->joy_sub_->get_publisher_count() == 0) {
+    RCLCPP_WARN(
+      this->get_logger(),
+      "Joy node not launched");
+  }
+}
+
+void TeleopTwistJoyNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
+{
+  this->p9n_if_->setJoyMsg(joy_msg);
+  if (
+    std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 ||
+    std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2)
+  {
+    float l_x = this->p9n_if_->tiltedStickLX();
+    float l_y = this->p9n_if_->tiltedStickLY();
+
+    auto twist = geometry_msgs::msg::Twist();
+    twist.linear.x = l_y;
+    twist.angular.z = l_x;
+    this->twist_pub_->publish(twist);
+  } else {
+    using namespace std::chrono_literals;
+    rclcpp::sleep_for(100ms);
+  }
+}
+} // namespace p9n_node
Add a comment
List