From 975ba9796fee7e3b34465ea380fc356e7707a4c1 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 1 Jul 2024 04:24:22 +0900 Subject: [PATCH] add navyu_utils --- .../navyu_global_planner.hpp | 3 +- navyu_path_planner/package.xml | 1 + .../src/navyu_global_planner.cpp | 23 +------ .../navyu_path_tracker/navyu_path_tracker.hpp | 26 +------- navyu_path_tracker/package.xml | 1 + navyu_path_tracker/src/navyu_path_tracker.cpp | 16 ++--- .../navyu_simple_simulator.hpp | 2 - navyu_simulator/package.xml | 1 + .../src/navyu_simple_simulator.cpp | 2 +- navyu_utils/CMakeLists.txt | 30 +++++++++ .../include/navyu_utils/navyu_utils.hpp | 43 +++++++++++++ .../include/navyu_utils/quaternion_utils.hpp | 42 +++++++++++++ navyu_utils/package.xml | 25 ++++++++ navyu_utils/src/navyu_utils.cpp | 61 +++++++++++++++++++ .../src/quaternion_utils.cpp | 16 +---- 15 files changed, 217 insertions(+), 75 deletions(-) create mode 100644 navyu_utils/CMakeLists.txt create mode 100644 navyu_utils/include/navyu_utils/navyu_utils.hpp create mode 100644 navyu_utils/include/navyu_utils/quaternion_utils.hpp create mode 100644 navyu_utils/package.xml create mode 100644 navyu_utils/src/navyu_utils.cpp rename navyu_simulator/include/navyu_simulator/quaternion_utils.hpp => navyu_utils/src/quaternion_utils.cpp (85%) diff --git a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp index 35aa81c..4460d49 100644 --- a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp +++ b/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 @@ -44,8 +45,6 @@ class NavyuGlobalPlanner : public rclcpp::Node void publish_path(std::vector path); - bool get_robot_pose(geometry_msgs::msg::Pose & robot_pose); - private: rclcpp::Subscription::SharedPtr goal_pose_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; diff --git a/navyu_path_planner/package.xml b/navyu_path_planner/package.xml index a6e92d2..5337fdb 100644 --- a/navyu_path_planner/package.xml +++ b/navyu_path_planner/package.xml @@ -17,6 +17,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + navyu_utils launch_ros ament_lint_auto diff --git a/navyu_path_planner/src/navyu_global_planner.cpp b/navyu_path_planner/src/navyu_global_planner.cpp index 4783d4d..0716e5f 100644 --- a/navyu_path_planner/src/navyu_global_planner.cpp +++ b/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 & 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; } diff --git a/navyu_path_tracker/include/navyu_path_tracker/navyu_path_tracker.hpp b/navyu_path_tracker/include/navyu_path_tracker/navyu_path_tracker.hpp index 8f063a2..56c000e 100644 --- a/navyu_path_tracker/include/navyu_path_tracker/navyu_path_tracker.hpp +++ b/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 #include @@ -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::SharedPtr path_subscriber_; diff --git a/navyu_path_tracker/package.xml b/navyu_path_tracker/package.xml index e741d5b..ec502c9 100644 --- a/navyu_path_tracker/package.xml +++ b/navyu_path_tracker/package.xml @@ -17,6 +17,7 @@ tf2_ros tf2_geometry_msgs tf2_eigen + navyu_utils launch_ros ament_lint_auto diff --git a/navyu_path_tracker/src/navyu_path_tracker.cpp b/navyu_path_tracker/src/navyu_path_tracker.cpp index e9e04df..14f34c0 100644 --- a/navyu_path_tracker/src/navyu_path_tracker.cpp +++ b/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 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; diff --git a/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp b/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp index 286aa76..bd950f2 100644 --- a/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp +++ b/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::SharedPtr twist_subscriber_; rclcpp::Subscription::SharedPtr diff --git a/navyu_simulator/package.xml b/navyu_simulator/package.xml index 9935b2e..639db61 100644 --- a/navyu_simulator/package.xml +++ b/navyu_simulator/package.xml @@ -21,6 +21,7 @@ tf2_geometry_msgs urdf xacro + navyu_utils ament_lint_auto ament_lint_common diff --git a/navyu_simulator/src/navyu_simple_simulator.cpp b/navyu_simulator/src/navyu_simple_simulator.cpp index 0a1af48..5beb35d 100644 --- a/navyu_simulator/src/navyu_simple_simulator.cpp +++ b/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 #include diff --git a/navyu_utils/CMakeLists.txt b/navyu_utils/CMakeLists.txt new file mode 100644 index 0000000..d91ea60 --- /dev/null +++ b/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() diff --git a/navyu_utils/include/navyu_utils/navyu_utils.hpp b/navyu_utils/include/navyu_utils/navyu_utils.hpp new file mode 100644 index 0000000..f918dad --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +#include +#include +#include + +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 diff --git a/navyu_utils/include/navyu_utils/quaternion_utils.hpp b/navyu_utils/include/navyu_utils/quaternion_utils.hpp new file mode 100644 index 0000000..7c6a2de --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#include + +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 diff --git a/navyu_utils/package.xml b/navyu_utils/package.xml new file mode 100644 index 0000000..62b60e4 --- /dev/null +++ b/navyu_utils/package.xml @@ -0,0 +1,25 @@ + + + navyu_utils + 0.1.0 + The navyu_utils package + RyuYamamoto + Apache License 2.0 + + ament_cmake_auto + + rclcpp + geometry_msgs + visualization_msgs + tf2 + tf2_ros + tf2_geometry_msgs + tf2_eigen + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/navyu_utils/src/navyu_utils.cpp b/navyu_utils/src/navyu_utils.cpp new file mode 100644 index 0000000..e6bf08c --- /dev/null +++ b/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 diff --git a/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp b/navyu_utils/src/quaternion_utils.cpp similarity index 85% rename from navyu_simulator/include/navyu_simulator/quaternion_utils.hpp rename to navyu_utils/src/quaternion_utils.cpp index 2e36931..39cdbb0 100644 --- a/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp +++ b/navyu_utils/src/quaternion_utils.cpp @@ -12,21 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License -#ifndef NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ -#define NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ - -#include -#include - -#include -#include -#include -#include - -#include +#include "navyu_utils/quaternion_utils.hpp" namespace quaternion_utils { + geometry_msgs::msg::Quaternion convert_euler_to_quaternion(const geometry_msgs::msg::Vector3 euler) { tf2::Quaternion tf2_quat; @@ -83,5 +73,3 @@ Eigen::Matrix3f get_rotation_matrix_from_quaternion(const geometry_msgs::msg::Qu } } // namespace quaternion_utils - -#endif // NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_