diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index eac78cc..2ca624e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,12 @@ src/common/wheel_stuck_common_utils/** @Autumn60 @Bey9434 src/control/joy2twist/** @Bey9434 +src/launch/wheel_stuck_launcher/** @Autumn60 src/perception/velodyne_cloud_separator/** @Autumn60 src/planning/dwa_planner/** @Autumn60 src/planning/waypoint/wheel_stuck_waypoint_msgs/** @Autumn60 src/planning/waypoint/wheel_stuck_waypoint_utils/** @Autumn60 +src/robot/wheel_stuck_robot_utils/** @Autumn60 +src/sample/robot_info_user/** @Autumn60 src/sample/simple_int_publisher/** @Autumn60 src/sample/simple_int_subscriber/** @Autumn60 README.md @RyodoTanaka diff --git a/src/launch/wheel_stuck_launcher/CMakeLists.txt b/src/launch/wheel_stuck_launcher/CMakeLists.txt new file mode 100644 index 0000000..4381fd3 --- /dev/null +++ b/src/launch/wheel_stuck_launcher/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.5) +project(wheel_stuck_launcher) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/src/launch/wheel_stuck_launcher/launch/components/common_components/load_robot_info.launch.py b/src/launch/wheel_stuck_launcher/launch/components/common_components/load_robot_info.launch.py new file mode 100644 index 0000000..723c932 --- /dev/null +++ b/src/launch/wheel_stuck_launcher/launch/components/common_components/load_robot_info.launch.py @@ -0,0 +1,47 @@ +# Copyright 2024 Fool Stuck Engineers +# +# 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.actions import IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription + + +def launch_setup(context, *args, **kwargs): + robot_description_pkg = FindPackageShare( + [LaunchConfiguration("robot_model"), "_description"] + ).perform(context) + + load_robot_info = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("wheel_stuck_robot_utils"), "/launch/load_robot_info.launch.py"] + ), + launch_arguments={ + "robot_info_param_file": [robot_description_pkg, "/config/robot_info.param.yaml"] + }.items(), + ) + + return [ + load_robot_info, + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/src/launch/wheel_stuck_launcher/launch/components/wheel_stuck_common.launch.xml b/src/launch/wheel_stuck_launcher/launch/components/wheel_stuck_common.launch.xml new file mode 100644 index 0000000..c0b5a0e --- /dev/null +++ b/src/launch/wheel_stuck_launcher/launch/components/wheel_stuck_common.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/launch/wheel_stuck_launcher/launch/wheel_stuck.launch.xml b/src/launch/wheel_stuck_launcher/launch/wheel_stuck.launch.xml new file mode 100644 index 0000000..2913f8f --- /dev/null +++ b/src/launch/wheel_stuck_launcher/launch/wheel_stuck.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/launch/wheel_stuck_launcher/package.xml b/src/launch/wheel_stuck_launcher/package.xml new file mode 100644 index 0000000..84c2d82 --- /dev/null +++ b/src/launch/wheel_stuck_launcher/package.xml @@ -0,0 +1,20 @@ + + + + wheel_stuck_launcher + 0.1.0 + The wheel_stuck_launcher package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + wheel_stuck_robot_utils + + ament_lint_auto + + + ament_cmake + + diff --git a/src/robot/wheel_stuck_robot_utils/CMakeLists.txt b/src/robot/wheel_stuck_robot_utils/CMakeLists.txt new file mode 100644 index 0000000..4b71c1b --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(wheel_stuck_robot_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() + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() + +# ament_auto_add_gtest(test_wheel_stuck_robot_utils test/test_wheel_stuck_robot_utils.cpp) +# file(GLOB TEST_SOURCES "test/**/*.cpp") +# target_sources(test_wheel_stuck_robot_utils PRIVATE ${TEST_SOURCES}) +# endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/src/robot/wheel_stuck_robot_utils/config/robot_info.param.yaml b/src/robot/wheel_stuck_robot_utils/config/robot_info.param.yaml new file mode 100644 index 0000000..10c374e --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/config/robot_info.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + wheel_radius: 0.0 # [m] + wheel_tread: 0.0 # [m] + front_overhang: 0.0 # [m] + rear_overhang: 0.0 # [m] + left_overhang: 0.0 # [m] + right_overhang: 0.0 # [m] + robot_height: 0.0 # [m] diff --git a/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info.hpp b/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info.hpp new file mode 100644 index 0000000..7ae3365 --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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 WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_HPP_ +#define WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_HPP_ + +namespace wheel_stuck_robot_utils +{ +struct RobotRawInfo +{ + double wheel_radius_m; + double wheel_tread_m; + double front_overhang_m; + double rear_overhang_m; + double left_overhang_m; + double right_overhang_m; + double robot_height_m; +}; + +struct RobotInfo +{ + RobotInfo() = default; + explicit RobotInfo(RobotRawInfo raw_info) + { + this->raw_info = raw_info; + robot_length = this->raw_info.front_overhang_m + this->raw_info.rear_overhang_m; + robot_width = this->raw_info.wheel_tread_m + this->raw_info.left_overhang_m + + this->raw_info.right_overhang_m; + } + + RobotRawInfo raw_info; + double robot_length; + double robot_width; +}; +} // namespace wheel_stuck_robot_utils + +#endif // WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_HPP_ diff --git a/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info_utils.hpp b/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info_utils.hpp new file mode 100644 index 0000000..53995ae --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/include/wheel_stuck_robot_utils/robot_info_utils.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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 WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_UTILS_HPP_ +#define WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_UTILS_HPP_ + +#include "wheel_stuck_robot_utils/robot_info.hpp" + +#include + +#include + +namespace wheel_stuck_robot_utils +{ +class RobotInfoUtils +{ +public: + explicit RobotInfoUtils(rclcpp::Node & node) + { + RobotRawInfo raw_info; + raw_info.wheel_radius_m = get_parameter(node, "wheel_radius"); + raw_info.wheel_tread_m = get_parameter(node, "wheel_tread"); + raw_info.front_overhang_m = get_parameter(node, "front_overhang"); + raw_info.rear_overhang_m = get_parameter(node, "rear_overhang"); + raw_info.left_overhang_m = get_parameter(node, "left_overhang"); + raw_info.right_overhang_m = get_parameter(node, "right_overhang"); + raw_info.robot_height_m = get_parameter(node, "robot_height"); + + robot_info_ = RobotInfo(raw_info); + } + RobotInfo get_info() const { return robot_info_; } + +private: + template + T get_parameter(rclcpp::Node & node, const std::string & name) + { + if (node.has_parameter(name)) return node.get_parameter(name).get_value(); + try { + return node.declare_parameter(name); + } catch (const rclcpp::ParameterTypeException & e) { + RCLCPP_ERROR(node.get_logger(), "Failed to get parameter: %s", e.what()); + throw e; + } + } + + RobotInfo robot_info_; +}; +} // namespace wheel_stuck_robot_utils + +#endif // WHEEL_STUCK_ROBOT_UTILS__ROBOT_INFO_UTILS_HPP_ diff --git a/src/robot/wheel_stuck_robot_utils/launch/load_robot_info.launch.py b/src/robot/wheel_stuck_robot_utils/launch/load_robot_info.launch.py new file mode 100644 index 0000000..7ce93c7 --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/launch/load_robot_info.launch.py @@ -0,0 +1,36 @@ +# Copyright 2024 Fool Stuck Engineers +# +# 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. + +import yaml +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import SetParameter + +from launch import LaunchDescription + + +def launch_setup(context, *args, **kwargs): + param_file = LaunchConfiguration("robot_info_param_file").perform(context) + with open(param_file, "r") as f: + robot_info = yaml.safe_load(f)["/**"]["ros__parameters"] + return [SetParameter(name=n, value=v) for (n, v) in robot_info.items()] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("robot_info_param_file"), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/src/robot/wheel_stuck_robot_utils/package.xml b/src/robot/wheel_stuck_robot_utils/package.xml new file mode 100644 index 0000000..598b811 --- /dev/null +++ b/src/robot/wheel_stuck_robot_utils/package.xml @@ -0,0 +1,20 @@ + + + + wheel_stuck_robot_utils + 0.1.0 + The wheel_stuck_robot_utils package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + + + ament_cmake + + diff --git a/src/sample/robot_info_user/CMakeLists.txt b/src/sample/robot_info_user/CMakeLists.txt new file mode 100644 index 0000000..d90a4ae --- /dev/null +++ b/src/sample/robot_info_user/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(robot_info_user) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + 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(robot_info_user SHARED + src/robot_info_user.cpp +) + +rclcpp_components_register_node(robot_info_user + PLUGIN "robot_info_user::RobotInfoUser" + EXECUTABLE robot_info_user_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/src/sample/robot_info_user/include/robot_info_user/robot_info_user.hpp b/src/sample/robot_info_user/include/robot_info_user/robot_info_user.hpp new file mode 100644 index 0000000..6e5002c --- /dev/null +++ b/src/sample/robot_info_user/include/robot_info_user/robot_info_user.hpp @@ -0,0 +1,29 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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 ROBOT_INFO_USER__ROBOT_INFO_USER_HPP_ +#define ROBOT_INFO_USER__ROBOT_INFO_USER_HPP_ + +#include + +namespace robot_info_user +{ +class RobotInfoUser : public rclcpp::Node +{ +public: + explicit RobotInfoUser(const rclcpp::NodeOptions & options); +}; +} // namespace robot_info_user + +#endif // ROBOT_INFO_USER__ROBOT_INFO_USER_HPP_ diff --git a/src/sample/robot_info_user/launch/robot_info_user.launch.xml b/src/sample/robot_info_user/launch/robot_info_user.launch.xml new file mode 100644 index 0000000..bccffac --- /dev/null +++ b/src/sample/robot_info_user/launch/robot_info_user.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/sample/robot_info_user/package.xml b/src/sample/robot_info_user/package.xml new file mode 100644 index 0000000..0e95388 --- /dev/null +++ b/src/sample/robot_info_user/package.xml @@ -0,0 +1,21 @@ + + + + robot_info_user + 0.0.0 + The robot_info_user package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + rclcpp + rclcpp_components + wheel_stuck_robot_utils + ament_lint_auto + + + ament_cmake + + diff --git a/src/sample/robot_info_user/src/robot_info_user.cpp b/src/sample/robot_info_user/src/robot_info_user.cpp new file mode 100644 index 0000000..711f520 --- /dev/null +++ b/src/sample/robot_info_user/src/robot_info_user.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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 "robot_info_user/robot_info_user.hpp" + +#include + +namespace robot_info_user +{ + +using RobotInfoUtils = wheel_stuck_robot_utils::RobotInfoUtils; + +RobotInfoUser::RobotInfoUser(const rclcpp::NodeOptions & options) +: rclcpp::Node("robot_info_user", options) +{ + auto robot_info = RobotInfoUtils(*this).get_info(); + RCLCPP_INFO( + this->get_logger(), + "\nRobotInfo: \n wheel_radius: %f\n wheel_tread: %f\n front_overhang: %f\n rear_overhang: " + "%f\n left_overhang: %f\n right_overhang: %f\n robot_height: %f", + robot_info.raw_info.wheel_radius_m, robot_info.raw_info.wheel_tread_m, + robot_info.raw_info.front_overhang_m, robot_info.raw_info.rear_overhang_m, + robot_info.raw_info.left_overhang_m, robot_info.raw_info.right_overhang_m, + robot_info.raw_info.robot_height_m); +} +} // namespace robot_info_user + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(robot_info_user::RobotInfoUser)