feat: create wheel_stuck_robot_utils pkg (#109)

* create wheel_stuck_robot_utils

Signed-off-by: Autumn60 <[email protected]>

* create sample package for wheel_stuck_vehicle_utils

Signed-off-by: Autumn60 <[email protected]>

* create wheel_stuck_launcher pkg

Signed-off-by: Autumn60 <[email protected]>

* add missing license notation

Signed-off-by: Autumn60 <[email protected]>

* update CODEOWNERS for new pkg

Signed-off-by: Autumn60 <[email protected]>

---------

Signed-off-by: Autumn60 <[email protected]>

.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

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
+)

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),
+        ]
+    )

src/launch/wheel_stuck_launcher/launch/components/wheel_stuck_common.launch.xml

@@ -0,0 +1,8 @@
+<launch>
+  <!-- Parameters -->
+  <arg name="robot_model" default="wheel_stuck"/>
+
+  <include file="$(find-pkg-share wheel_stuck_launcher)/launch/components/common_components/load_robot_info.launch.py">
+    <arg name="robot_model" value="$(var robot_model)"/>
+  </include>
+</launch>

src/launch/wheel_stuck_launcher/launch/wheel_stuck.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <!-- Essential parameters -->
+  <arg name="robot_model" default="wheel_stuck"/>
+
+  <!-- Optional parameters -->
+  <arg name="launch_common" default="true"/>
+
+  <!-- Common -->
+  <group if="$(var launch_common)" scoped="false">
+    <include file="$(find-pkg-share wheel_stuck_launcher)/launch/components/wheel_stuck_common.launch.xml">
+      <arg name="robot_model" value="$(var robot_model)"/>
+    </include>
+  </group>
+</launch>

src/launch/wheel_stuck_launcher/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wheel_stuck_launcher</name>
+  <version>0.1.0</version>
+  <description>The wheel_stuck_launcher package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <exec_depend>wheel_stuck_robot_utils</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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
+)

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]

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_

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 <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace wheel_stuck_robot_utils
+{
+class RobotInfoUtils
+{
+public:
+  explicit RobotInfoUtils(rclcpp::Node & node)
+  {
+    RobotRawInfo raw_info;
+    raw_info.wheel_radius_m = get_parameter<double>(node, "wheel_radius");
+    raw_info.wheel_tread_m = get_parameter<double>(node, "wheel_tread");
+    raw_info.front_overhang_m = get_parameter<double>(node, "front_overhang");
+    raw_info.rear_overhang_m = get_parameter<double>(node, "rear_overhang");
+    raw_info.left_overhang_m = get_parameter<double>(node, "left_overhang");
+    raw_info.right_overhang_m = get_parameter<double>(node, "right_overhang");
+    raw_info.robot_height_m = get_parameter<double>(node, "robot_height");
+
+    robot_info_ = RobotInfo(raw_info);
+  }
+  RobotInfo get_info() const { return robot_info_; }
+
+private:
+  template <typename T>
+  T get_parameter(rclcpp::Node & node, const std::string & name)
+  {
+    if (node.has_parameter(name)) return node.get_parameter(name).get_value<T>();
+    try {
+      return node.declare_parameter<T>(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_

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),
+        ]
+    )

src/robot/wheel_stuck_robot_utils/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wheel_stuck_robot_utils</name>
+  <version>0.1.0</version>
+  <description>The wheel_stuck_robot_utils package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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
+)

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 <rclcpp/rclcpp.hpp>
+
+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_

src/sample/robot_info_user/launch/robot_info_user.launch.xml

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="robot_info_user" name="robot_info_user" exec="robot_info_user_node" output="screen"/>
+</launch>