From 8e8279ec556685a5a9fede6e5ebfb6bcf62f53b4 Mon Sep 17 00:00:00 2001
From: MrBearing <takumi1988okamoto@gmail.com>
Date: Tue, 24 Sep 2024 12:09:07 +0900
Subject: [PATCH] Add echoreply_twist

---
 mros2_echoreply_twist/CMakeLists.txt         | 50 +++++++++++++++++
 mros2_echoreply_twist/package.xml            | 23 ++++++++
 mros2_echoreply_twist/src/echoreply_node.cpp | 57 ++++++++++++++++++++
 3 files changed, 130 insertions(+)
 create mode 100644 mros2_echoreply_twist/CMakeLists.txt
 create mode 100644 mros2_echoreply_twist/package.xml
 create mode 100644 mros2_echoreply_twist/src/echoreply_node.cpp

diff --git a/mros2_echoreply_twist/CMakeLists.txt b/mros2_echoreply_twist/CMakeLists.txt
new file mode 100644
index 0000000..19ee090
--- /dev/null
+++ b/mros2_echoreply_twist/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5)
+project(mros2_echoreply_twist)
+
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(echoreply_node src/echoreply_node.cpp)
+ament_target_dependencies(echoreply_node rclcpp std_msgs geometry_msgs)
+
+target_include_directories(echoreply_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+install(TARGETS echoreply_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/mros2_echoreply_twist/package.xml b/mros2_echoreply_twist/package.xml
new file mode 100644
index 0000000..56b4cc7
--- /dev/null
+++ b/mros2_echoreply_twist/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>mros2_echoreply_twist</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="uden@todo.todo">uden</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</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>
diff --git a/mros2_echoreply_twist/src/echoreply_node.cpp b/mros2_echoreply_twist/src/echoreply_node.cpp
new file mode 100644
index 0000000..b2e4408
--- /dev/null
+++ b/mros2_echoreply_twist/src/echoreply_node.cpp
@@ -0,0 +1,57 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+using std::placeholders::_1;
+
+/* This example creates a subclass of Node and uses std::bind() to register a
+* member function as a callback from the timer. */
+
+class Echoreplyer : public rclcpp::Node
+{
+public:
+  Echoreplyer() : Node("mros2_echoreply_node")
+  {
+    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("to_stm", 10);
+    subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>("to_linux", rclcpp::QoS(10).best_effort(), std::bind(&Echoreplyer::topic_callback, this, _1));
+  }
+  
+private:
+  void topic_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
+  {
+    RCLCPP_INFO(this->get_logger(), "{ linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }",
+      msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
+
+    geometry_msgs::msg::Vector3 linear;
+    geometry_msgs::msg::Vector3 angular;
+    geometry_msgs::msg::Twist twist;
+
+    linear.x = msg->linear.x;
+    linear.y = msg->linear.y;
+    linear.z = msg->linear.z;
+    angular.x = msg->angular.x;
+    angular.y = msg->angular.y;
+    angular.z = msg->angular.z;
+    twist.linear = linear;
+    twist.angular = angular;
+
+    RCLCPP_INFO(this->get_logger(), "{ linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }",
+      linear.x, linear.y, linear.z, angular.x, angular.y, angular.z);
+    publisher_->publish(*msg);
+  }
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscriber_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Echoreplyer>());
+  rclcpp::shutdown();
+  return 0;
+}