Merge pull request #2 from StrayedCats/feat/add_move2targetdeg

Feat/add move2targetdeg

auto_driver_bringup/launch/auto2024.launch.py

@@ -32,9 +32,9 @@ def generate_launch_description():
                     namespace='can_node/gm6020_0',
                     parameters=[
                         {'p': 80},
-                        {'i': 10},
-                        {'d': 10},
-                        {'max_spd': 8000},
+                        {'i': 0},
+                        {'d': 5},
+                        {'max_spd': 3000},
                         {'init_deg': 120},
                         {'min_limit': 20},
                         {'max_limit': 135}
@@ -48,9 +48,9 @@ def generate_launch_description():
                     namespace='can_node/gm6020_1',
                     parameters=[
                         {'p': 80},
-                        {'i': 10},
+                        {'i': 0},
                         {'d': 5},
-                        {'max_spd': 5000},
+                        {'max_spd': 3000},
                         {'init_deg': 180},
                         {'min_limit': 25},
                         {'max_limit': 315}
@@ -125,6 +125,20 @@ def generate_launch_description():
                 {"robot_description": robot_description_config.toxml()}],
             output="screen",
         ),
+        Node(
+            package="auto_driver_interface",
+            executable="move_to_target_deg_server_exec",
+            name="move_to_target_deg_server",
+            output="screen",
+        ),
+        Node(
+            package="auto_driver_interface",
+            executable="tf_to_position_server_exec",
+            name="tf_to_position_server",
+            output="screen",
+        ),
+
+
         Node(
             package="rviz2",
             executable="rviz2",

auto_driver_interface/CMakeLists.txt

@@ -44,5 +44,14 @@ rclcpp_components_register_node(
   PLUGIN "${PROJECT_NAME}::TfToPositionActionServer"
   EXECUTABLE ${TARGET}_exec)
 
+# auto_driver_interface::MoveToTargetDegServer
+set(TARGET move_to_target_deg_server)
+ament_auto_add_library(${TARGET} SHARED
+  src/${TARGET}.cpp)
+
+rclcpp_components_register_node(
+  ${TARGET}
+  PLUGIN "${PROJECT_NAME}::MoveToTargetDegServer"
+  EXECUTABLE ${TARGET}_exec)
 
 ament_auto_package()

auto_driver_interface/include/auto_driver_interface/move_to_target_deg_server.hpp

@@ -0,0 +1,57 @@
+// Copyright 2024 StrayedCats.
+//
+// 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 <memory>
+#include <thread>
+
+#include <std_msgs/msg/int64.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include "auto_driver_msgs/action/move_to_target_deg.hpp"
+
+namespace auto_driver_interface
+{
+
+class MoveToTargetDegServer : public rclcpp::Node
+{
+public:
+  using MoveToTargetDeg = auto_driver_msgs::action::MoveToTargetDeg;
+  using GoalHandleMoveToTargetDeg = rclcpp_action::ServerGoalHandle<MoveToTargetDeg>;
+
+  explicit MoveToTargetDegServer(const rclcpp::NodeOptions &);
+
+private:
+  double center_offset_yaw_ = -180.0;
+  double center_offset_pitch_ = -90.0;
+
+  double speed_multiplier_yaw_ = 0.5;
+  double speed_multiplier_pitch_ = 0.5;
+
+  rclcpp_action::Server<MoveToTargetDeg>::SharedPtr action_server_;
+
+  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &, std::shared_ptr<const MoveToTargetDeg::Goal>);
+  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMoveToTargetDeg>);
+
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveToTargetDeg>);
+
+  void execute(const std::shared_ptr<GoalHandleMoveToTargetDeg>);
+
+  rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr yaw_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr pitch_publisher_;
+};
+
+}  // namespace auto_driver_interface

auto_driver_interface/include/auto_driver_interface/tf_to_position_server.hpp

@@ -23,6 +23,7 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/buffer.h>
+#include <tf2/utils.h>
 
 #include "auto_driver_msgs/action/get_angles_from_tf.hpp"
 

auto_driver_interface/src/move_to_target_deg_server.cpp

@@ -0,0 +1,84 @@
+// Copyright 2024 StrayedCats.
+//
+// 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 "auto_driver_interface/move_to_target_deg_server.hpp"
+
+namespace auto_driver_interface
+{
+
+  MoveToTargetDegServer::MoveToTargetDegServer(const rclcpp::NodeOptions & options)
+    : Node("move_to_target_deg_server", options)
+  {
+    this->yaw_publisher_ = this->create_publisher<std_msgs::msg::Int64>("can_node/gm6020_1/target_deg", 1);
+    this->pitch_publisher_ = this->create_publisher<std_msgs::msg::Int64>("can_node/gm6020_0/target_deg", 1);
+
+    using namespace std::placeholders;
+    action_server_ = rclcpp_action::create_server<MoveToTargetDeg>(
+      this,
+      "move_to_target_deg",
+      std::bind(&MoveToTargetDegServer::handle_goal, this, _1, _2),
+      std::bind(&MoveToTargetDegServer::handle_cancel, this, _1),
+      std::bind(&MoveToTargetDegServer::handle_accepted, this, _1));
+  }
+
+  rclcpp_action::GoalResponse MoveToTargetDegServer::handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveToTargetDeg::Goal> goal)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received goal request");
+    (void)uuid;
+    (void)goal;
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse MoveToTargetDegServer::handle_cancel(
+    const std::shared_ptr<GoalHandleMoveToTargetDeg> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void MoveToTargetDegServer::handle_accepted(const std::shared_ptr<GoalHandleMoveToTargetDeg> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Goal has been accepted");
+    using namespace std::placeholders;
+    std::thread{std::bind(&MoveToTargetDegServer::execute, this, _1), goal_handle}.detach();
+  }
+
+  void MoveToTargetDegServer::execute(const std::shared_ptr<GoalHandleMoveToTargetDeg> goal_handle)
+  {
+    auto result = std::make_shared<MoveToTargetDeg::Result>();
+    auto goal = goal_handle->get_goal();
+    auto msec = goal->msec;
+
+    auto yaw_msg = std_msgs::msg::Int64();
+    auto pitch_msg = std_msgs::msg::Int64();
+    yaw_msg.data = goal->yaw_deg + (goal->yaw_deg + center_offset_yaw_) * speed_multiplier_yaw_;
+    pitch_msg.data = goal->pitch_deg + (goal->pitch_deg + center_offset_pitch_) * speed_multiplier_pitch_;
+    this->yaw_publisher_->publish(yaw_msg);
+    this->pitch_publisher_->publish(pitch_msg);
+
+    RCLCPP_INFO(this->get_logger(), "Publishing target degrees: yaw: %d, pitch: %d", yaw_msg.data, pitch_msg.data);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(msec));
+
+    result->succeed = true;
+    goal_handle->succeed(result);
+  }
+}  // namespace auto_driver_interface
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(auto_driver_interface::MoveToTargetDegServer)

auto_driver_interface/src/pid_node.cpp

@@ -18,7 +18,7 @@
 #include <std_msgs/msg/int64.hpp>
 #include <std_msgs/msg/bool.hpp>
 
-#include "pid.hpp"
+#include "auto_driver_interface/pid.hpp"
 
 namespace auto_driver_interface
 {

auto_driver_interface/src/tf_to_position_server.cpp

@@ -97,7 +97,17 @@ namespace auto_driver_interface
       auto x = transform2.transform.translation.x - transform.transform.translation.x;
       auto y = transform2.transform.translation.y - transform.transform.translation.y;
       auto z = transform2.transform.translation.z - transform.transform.translation.z;
+      auto current_rotation = transform.transform.rotation;
 
+      double yaw_gap, pitch_gap, current_roll, yaw_gap_deg, pitch_gap_deg;
+      tf2::Quaternion q(current_rotation.x, current_rotation.y, current_rotation.z, current_rotation.w);
+      tf2::Matrix3x3 m(q);
+      m.getRPY(current_roll, pitch_gap, yaw_gap);
+      yaw_gap_deg = yaw_gap * 180 / M_PI;
+      pitch_gap_deg = pitch_gap * 180 / M_PI;
+
+
+      RCLCPP_INFO(this->get_logger(), "current yaw: %f, pitch: %f", yaw_gap_deg, pitch_gap_deg);
       auto yaw = atan2(y, x);
       yaw = (x < 0) ? -yaw : yaw;
       auto degree = yaw * 180 / M_PI;
@@ -107,6 +117,8 @@ namespace auto_driver_interface
 
       result->yaw_deg = static_cast<int32_t>(degree);
       result->pitch_deg = static_cast<int32_t>(pitch);
+      result->yaw_gap_deg = static_cast<int32_t>(yaw_gap_deg);
+      result->pitch_gap_deg = static_cast<int32_t>(pitch_gap_deg);
       result->succeed = true;
 
       RCLCPP_INFO(this->get_logger(), "yaw: %f", degree);

auto_driver_msgs/action/GetAnglesFromTf.action

@@ -7,5 +7,7 @@ string target_tf_frame_id
 bool succeed
 int32 yaw_deg
 int32 pitch_deg
+int32 yaw_gap_deg
+int32 pitch_gap_deg
 ---
 #feedback definition

auto_driver_msgs/action/MoveToTargetDeg.action

@@ -0,0 +1,9 @@
+#goal definition
+int32 yaw_deg
+int32 pitch_deg
+int32 msec
+---
+#result definition
+bool succeed
+---
+#feedback definition