Merge remote-tracking branch 'origin/main' into feature/add-pit-in-co…

…ntrol

.cspell.json

@@ -18,6 +18,7 @@
     "**/.vscode/**",
     "**/build/**",
     "**/CHANGELOG.rst",
+    "**/.clang-format",
     "**/CPPLINT.cfg",
     "**/Doxyfile",
     "**/install/**",
@@ -53,6 +54,7 @@
     "automotiveaichallenge",
     "autoware",
     "awsim",
+    "booars",
     "buildtool",
     "colcon",
     "costmap",
@@ -70,6 +72,7 @@
     "lanechange",
     "lanefollowing",
     "lanelet",
+    "lanelets",
     "libgazebo",
     "libgl",
     "linalg",
@@ -101,6 +104,28 @@
     "wpedantic",
     "xacro",
     "xyzrpy",
-    "zcvf"
+    "zcvf",
+    "mppi",
+    "Qdin",
+    "Kohei",
+    "MPPI",
+    "Savitzky",
+    "satitzky",
+    "Golay",
+    "Vandermonde",
+    "michikuni",
+    "eguchi",
+    "vander",
+    "pinv",
+    "dtheta",
+    "cind",
+    "dind",
+    "ncourse",
+    "argmin",
+    "reparameting",
+    "tempature",
+    "rsample",
+    "coeffs",
+    "softplus"
   ]
 }

.github/CODEOWNERS

@@ -0,0 +1,15 @@
+aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/costmap/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp
+aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/imu_corrector/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara
+aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117

aichallenge/workspace/.clang-format

@@ -0,0 +1,47 @@
+# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AllowShortFunctionsOnASingleLine: InlineOnly
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: true
+IncludeCategories:
+  # C++ system headers
+  - Regex: <[a-z_]*>
+    Priority: 6
+    CaseSensitive: true
+  # C system headers
+  - Regex: <.*\.h>
+    Priority: 5
+    CaseSensitive: true
+  # Boost headers
+  - Regex: boost/.*
+    Priority: 4
+    CaseSensitive: true
+  # Message headers
+  - Regex: .*_msgs/.*
+    Priority: 3
+    CaseSensitive: true
+  - Regex: .*_srvs/.*
+    Priority: 3
+    CaseSensitive: true
+  # Other Package headers
+  - Regex: <.*>
+    Priority: 2
+    CaseSensitive: true
+  # Local package headers
+  - Regex: '".*"'
+    Priority: 1
+    CaseSensitive: true

aichallenge/workspace/.gitignore

@@ -1,4 +1,7 @@
 /build
 /install
 /log
-result-details.json
+result-details.json
+
+.vscode
+*.code-workspace

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml

@@ -1,11 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <log message="This is aichallenge_submit_launch."/>
-  <include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
-    <arg name="vehicle_model" value="racing_kart"/>
-    <arg name="sensor_model" value="racing_kart_sensor_kit"/>
-    <arg name="map_path" value="$(find-pkg-share aichallenge_submit_launch)/map"/>
-  </include>
+  <include file="$(find-pkg-share booars_launch)/launch/booars.launch.xml" />
 
   <!-- place a goal pose anywhere you like-->
   <node pkg="goal_pose_setter" exec="goal_pose_setter_node" name="goal_pose_setter" output="screen">

aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(booars_dummy_perception_publisher)
+
+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()
+
+include_directories(include)
+
+ament_auto_add_library(booars_dummy_perception_publisher SHARED
+  src/dummy_objects_publisher.cpp
+)
+
+rclcpp_components_register_node(booars_dummy_perception_publisher
+  PLUGIN "booars_dummy_perception_publisher::DummyObjectsPublisher"
+  EXECUTABLE dummy_objects_publisher
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp

@@ -0,0 +1,45 @@
+// Copyright 2024 Booars
+//
+// 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 BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_
+#define BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+
+#include <string>
+
+namespace booars_dummy_perception_publisher
+{
+using Float64MultiArray = std_msgs::msg::Float64MultiArray;
+using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects;
+
+class DummyObjectsPublisher : public rclcpp::Node
+{
+public:
+  explicit DummyObjectsPublisher(const rclcpp::NodeOptions & options);
+
+private:
+  void objects_callback(const Float64MultiArray::SharedPtr msg);
+
+  rclcpp::Subscription<Float64MultiArray>::SharedPtr objects_sub_;
+  rclcpp::Publisher<PredictedObjects>::SharedPtr objects_pub_;
+
+  std::string map_frame_id_;
+};
+}  // namespace booars_dummy_perception_publisher
+
+#endif  // BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_

aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml

@@ -0,0 +1,21 @@
+<?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>booars_dummy_perception_publisher</name>
+  <version>0.0.0</version>
+  <description>The booars_dummy_perception_publisher package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp

@@ -0,0 +1,66 @@
+// Copyright 2024 Booars
+//
+// 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 "booars_dummy_perception_publisher/dummy_objects_publisher.hpp"
+
+namespace booars_dummy_perception_publisher
+{
+DummyObjectsPublisher::DummyObjectsPublisher(const rclcpp::NodeOptions & options)
+: Node("dummy_objects_publisher", options)
+{
+  map_frame_id_ = declare_parameter<std::string>("map_frame_id", "map");
+
+  objects_sub_ = create_subscription<Float64MultiArray>(
+    "~/input/objects", 10,
+    std::bind(&DummyObjectsPublisher::objects_callback, this, std::placeholders::_1));
+  objects_pub_ = create_publisher<PredictedObjects>("~/output/objects", 10);
+}
+
+void DummyObjectsPublisher::objects_callback(const Float64MultiArray::SharedPtr msg)
+{
+  if (msg->data.size() % 4 != 0) {
+    RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, "Invalid message size");
+    return;
+  }
+  int object_count = msg->data.size() / 4;
+
+  PredictedObjects objects_msg;
+
+  for (int i = 0; i < object_count; i++) {
+    autoware_auto_perception_msgs::msg::PredictedObject object;
+
+    object.object_id.uuid[0] = i;
+
+    object.kinematics.initial_pose_with_covariance.pose.position.x = msg->data[i * 4 + 0];
+    object.kinematics.initial_pose_with_covariance.pose.position.y = msg->data[i * 4 + 1];
+    object.kinematics.initial_pose_with_covariance.pose.position.z = msg->data[i * 4 + 2];
+    object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0;
+
+    object.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER;
+    const double size = msg->data[i * 4 + 3] * 2.0;
+    object.shape.dimensions.x = size;
+    object.shape.dimensions.y = size;
+    object.shape.dimensions.z = 1.0;
+
+    objects_msg.objects.push_back(object);
+  }
+
+  objects_msg.header.frame_id = map_frame_id_;
+  objects_msg.header.stamp = now();
+  objects_pub_->publish(objects_msg);
+}
+}  // namespace booars_dummy_perception_publisher
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(booars_dummy_perception_publisher::DummyObjectsPublisher)

aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt

@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 3.5)
+project(booars_launch)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+ament_auto_package(INSTALL_TO_SHARE launch map config)

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    map_frame_id: "map"
+    costmap_center_frame_id: "base_link"
+    costmap_width: 50.0
+    costmap_resolution: 0.2
+    multi_layered_costmap:
+      layers:
+        - "lanelet_layer"
+        - "predicted_object_layer"
+      lanelet_layer:
+        type: "lanelet"
+        map_topic: "/map/vector_map"
+      predicted_object_layer:
+        type: "predicted_object"
+        predicted_objects_topic: "/perception/object_recognition/objects"
+        distance_threshold: 1.2

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/imu_corrector.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    angular_velocity_offset_x: 0.0    # [rad/s]
+    angular_velocity_offset_y: 0.0    # [rad/s]
+    angular_velocity_offset_z: 0.0    # [rad/s]
+    angular_velocity_stddev_xx: 0.03  # [rad/s]
+    angular_velocity_stddev_yy: 0.03  # [rad/s]
+    angular_velocity_stddev_zz: 0.03  # [rad/s]