Merge branch 'main' into feat/planning/multi_layered_costmap

.github/CODEOWNERS

@@ -1,4 +1,5 @@
 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/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/launch/components/perception.launch.xml

@@ -2,7 +2,8 @@
 <launch>
   <group>
     <push-ros-namespace namespace="perception"/>
-    <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
+    <node pkg="booars_dummy_perception_publisher" exec="dummy_objects_publisher" name="dummy_objects_publisher" output="screen">
+      <remap from="~/input/objects" to="/aichallenge/objects"/>
       <remap from="~/output/objects" to="/perception/object_recognition/objects"/>
     </node>
   </group>