add navyu_utils

navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp

@@ -17,6 +17,7 @@
 
 #include "navyu_path_planner/astar_planner.hpp"
 #include "navyu_path_planner/smoother.hpp"
+#include "navyu_utils/navyu_utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -44,8 +45,6 @@ class NavyuGlobalPlanner : public rclcpp::Node
 
   void publish_path(std::vector<Node2D *> path);
 
-  bool get_robot_pose(geometry_msgs::msg::Pose & robot_pose);
-
 private:
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_subscriber_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscriber_;

navyu_path_planner/package.xml

@@ -17,6 +17,7 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>navyu_utils</depend>
 
   <exec_depend>launch_ros</exec_depend>
   <test_depend>ament_lint_auto</test_depend>

navyu_path_planner/src/navyu_global_planner.cpp

@@ -45,27 +45,6 @@ NavyuGlobalPlanner::~NavyuGlobalPlanner()
 {
 }
 
-bool NavyuGlobalPlanner::get_robot_pose(geometry_msgs::msg::Pose & robot_pose)
-{
-  geometry_msgs::msg::TransformStamped robot_pose_frame;
-
-  try {
-    robot_pose_frame = tf_buffer_.lookupTransform(
-      map_frame_, base_frame_, tf2::TimePointZero, tf2::durationFromSec(0.5));
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR_STREAM(
-      get_logger(), "Can not get Transform " << map_frame_ << " to " << base_frame_);
-    return false;
-  }
-
-  robot_pose.position.x = robot_pose_frame.transform.translation.x;
-  robot_pose.position.y = robot_pose_frame.transform.translation.y;
-  robot_pose.position.z = robot_pose_frame.transform.translation.z;
-  robot_pose.orientation = robot_pose_frame.transform.rotation;
-
-  return true;
-}
-
 bool NavyuGlobalPlanner::plan(
   const geometry_msgs::msg::Pose start, const geometry_msgs::msg::Pose goal,
   std::vector<Node2D *> & path)
@@ -115,7 +94,7 @@ void NavyuGlobalPlanner::callback_goal_pose(const geometry_msgs::msg::PoseStampe
   }
 
   geometry_msgs::msg::Pose start;
-  if (!get_robot_pose(start)) {
+  if (!navyu_utils::get_robot_pose(map_frame_, base_frame_, tf_buffer_, start)) {
     RCLCPP_ERROR_STREAM(get_logger(), "Can not get Robot Pose.");
     return;
   }

navyu_path_tracker/include/navyu_path_tracker/navyu_path_tracker.hpp

@@ -15,6 +15,9 @@
 #ifndef NAVYU_PATH_TRACEKR__NAVYU_PATH_TRACKER_HPP_
 #define NAVYU_PATH_TRACEKR__NAVYU_PATH_TRACKER_HPP_
 
+#include "navyu_utils/navyu_utils.hpp"
+#include "navyu_utils/quaternion_utils.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
@@ -42,9 +45,6 @@ class NavyuPathTracker : public rclcpp::Node
 
   void publish_velocity_command(const double v, const double w);
 
-  double calculation_distance(
-    const geometry_msgs ::msg::Pose p1, const geometry_msgs::msg::Pose p2);
-
   visualization_msgs::msg::Marker create_marker(
     const geometry_msgs::msg::Pose pose, const geometry_msgs::msg::Vector3 scale,
     const std_msgs::msg::ColorRGBA color);
@@ -67,26 +67,6 @@ class NavyuPathTracker : public rclcpp::Node
     scale.z = z;
     return scale;
   }
-  inline geometry_msgs::msg::Vector3 convert_quaternion_to_euler(
-    const geometry_msgs::msg::Quaternion quaternion)
-  {
-    geometry_msgs::msg::Vector3 euler;
-
-    tf2::Quaternion quat(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
-    tf2::Matrix3x3 matrix(quat);
-    matrix.getRPY(euler.x, euler.y, euler.z);
-
-    return euler;
-  }
-  inline double normalized(const double radian)
-  {
-    double normalized_radian = radian;
-    if (M_PI < radian)
-      normalized_radian -= 2.0 * M_PI;
-    else if (radian < -M_PI)
-      normalized_radian += 2.0 * M_PI;
-    return normalized_radian;
-  }
 
 private:
   rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_subscriber_;

navyu_path_tracker/package.xml

@@ -17,6 +17,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_eigen</depend>
+  <depend>navyu_utils</depend>
 
   <exec_depend>launch_ros</exec_depend>
   <test_depend>ament_lint_auto</test_depend>

navyu_path_tracker/src/navyu_path_tracker.cpp

@@ -49,14 +49,6 @@ NavyuPathTracker::~NavyuPathTracker()
 {
 }
 
-double NavyuPathTracker::calculation_distance(
-  const geometry_msgs::msg::Pose p1, const geometry_msgs::msg::Pose p2)
-{
-  const double dx = p1.position.x - p2.position.x;
-  const double dy = p1.position.y - p2.position.y;
-  return std::hypot(dx, dy);
-}
-
 void NavyuPathTracker::publish_velocity_command(const double v, const double w)
 {
   geometry_msgs::msg::Twist twist;
@@ -95,7 +87,8 @@ void NavyuPathTracker::process()
   // nearest path point from robot pose
   std::vector<double> distance_list;
   for (std::size_t idx = 0; idx < path_.poses.size(); idx++) {
-    distance_list.emplace_back(calculation_distance(robot_pose, path_.poses[idx].pose));
+    distance_list.emplace_back(
+      navyu_utils::calculation_distance(robot_pose, path_.poses[idx].pose));
   }
   auto itr = std::min_element(distance_list.begin(), distance_list.end());
 
@@ -120,10 +113,11 @@ void NavyuPathTracker::process()
   auto target_point = path_.poses[target_point_index].pose.position;
 
   // update steer
-  geometry_msgs::msg::Vector3 euler = convert_quaternion_to_euler(robot_pose.orientation);
+  geometry_msgs::msg::Vector3 euler =
+    quaternion_utils::convert_quaternion_to_euler(robot_pose.orientation);
   const double dx = target_point.x - robot_pose.position.x;
   const double dy = target_point.y - robot_pose.position.y;
-  double alpha = normalized(std::atan2(dy, dx) - euler.z);
+  double alpha = navyu_utils::normalized_radian(std::atan2(dy, dx) - euler.z);
 
   if (!adjust_yaw_angle_) {
     if (std::fabs(alpha) < yaw_tolerance_) adjust_yaw_angle_ = true;

navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp

@@ -50,8 +50,6 @@ class NavyuSimpleSimulator : public rclcpp::Node
     const geometry_msgs::msg::Pose pose, const std::string base_frame,
     const std::string child_frame, const rclcpp::Time stamp);
 
-  void get_transform();
-
 private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_subscriber_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr

navyu_simulator/package.xml

@@ -21,6 +21,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>urdf</depend>
   <depend>xacro</depend>
+  <depend>navyu_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

navyu_simulator/src/navyu_simple_simulator.cpp

@@ -14,7 +14,7 @@
 
 #include "navyu_simulator/navyu_simple_simulator.hpp"
 
-#include "navyu_simulator/quaternion_utils.hpp"
+#include "navyu_utils/quaternion_utils.hpp"
 
 #include <kdl/frames.hpp>
 #include <rclcpp/clock.hpp>

navyu_utils/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5)
+project(navyu_utils)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+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()
+
+ament_auto_add_library(navyu_utils SHARED
+  src/navyu_utils.cpp
+)
+
+ament_auto_add_library(quaternion_utils SHARED
+  src/quaternion_utils.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

navyu_utils/include/navyu_utils/navyu_utils.hpp

@@ -0,0 +1,43 @@
+// Copyright 2024 RyuYamamoto.
+//
+// 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
+
+#ifndef NAVYU_UTIULS__NAVYU_UTILS_HPP_
+#define NAVYU_UTIULS__NAVYU_UTILS_HPP_
+
+#include <Eigen/Core>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace navyu_utils
+{
+
+bool get_robot_pose(
+  const std::string global_frame, const std::string robot_frame, tf2_ros::Buffer & tf_buffer,
+  geometry_msgs::msg::Pose & robot_pose);
+
+double calculation_distance(const geometry_msgs::msg::Pose p1, const geometry_msgs::msg::Pose p2);
+
+double normalized_radian(const double radian);
+
+}  // namespace navyu_utils
+
+#endif

navyu_utils/include/navyu_utils/quaternion_utils.hpp

@@ -0,0 +1,42 @@
+// Copyright 2024 RyuYamamoto.
+//
+// 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
+
+#ifndef NAVYU_UTILS__QUATERNION_UTILS_HPP_
+#define NAVYU_UTILS__QUATERNION_UTILS_HPP_
+
+#include <Eigen/Core>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+
+namespace quaternion_utils
+{
+
+geometry_msgs::msg::Quaternion convert_euler_to_quaternion(const geometry_msgs::msg::Vector3 euler);
+
+geometry_msgs::msg::Vector3 convert_quaternion_to_euler(
+  const geometry_msgs::msg::Quaternion quaternion);
+
+geometry_msgs::msg::Quaternion rotation(
+  const geometry_msgs::msg::Quaternion quat1, const geometry_msgs::msg::Quaternion quat2);
+
+Eigen::Matrix3f get_rotation_matrix_from_euler(const geometry_msgs::msg::Vector3 euler);
+
+}  // namespace quaternion_utils
+
+#endif

navyu_utils/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>navyu_utils</name>
+  <version>0.1.0</version>
+  <description>The navyu_utils package</description>
+  <maintainer email="[email protected]">RyuYamamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_eigen</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navyu_utils/src/navyu_utils.cpp

@@ -0,0 +1,61 @@
+// Copyright 2024 RyuYamamoto.
+//
+// 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 "navyu_utils/navyu_utils.hpp"
+
+namespace navyu_utils
+{
+
+bool get_robot_pose(
+  const std::string global_frame, const std::string robot_frame, tf2_ros::Buffer & tf_buffer,
+  geometry_msgs::msg::Pose & robot_pose)
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("get_robot_pose");
+
+  geometry_msgs::msg::TransformStamped robot_pose_frame;
+
+  try {
+    robot_pose_frame = tf_buffer.lookupTransform(
+      global_frame, robot_frame, tf2::TimePointZero, tf2::durationFromSec(0.5));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(logger, "Can not get Transform " << global_frame << " to " << robot_frame);
+    return false;
+  }
+
+  robot_pose.position.x = robot_pose_frame.transform.translation.x;
+  robot_pose.position.y = robot_pose_frame.transform.translation.y;
+  robot_pose.position.z = robot_pose_frame.transform.translation.z;
+  robot_pose.orientation = robot_pose_frame.transform.rotation;
+
+  return true;
+}
+
+double calculation_distance(const geometry_msgs::msg::Pose p1, const geometry_msgs::msg::Pose p2)
+{
+  const double dx = p1.position.x - p2.position.x;
+  const double dy = p1.position.y - p2.position.y;
+  return std::hypot(dx, dy);
+}
+
+double normalized_radian(const double radian)
+{
+  double normalized_radian = radian;
+  if (M_PI < radian)
+    normalized_radian -= 2.0 * M_PI;
+  else if (radian < -M_PI)
+    normalized_radian += 2.0 * M_PI;
+  return normalized_radian;
+}
+
+}  // namespace navyu_utils