diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d5d743c9..b0417909 100644 --- a/.github/CODEOWNERS +++ b/.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/goal_pose_setter/** @hrjp diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt new file mode 100644 index 00000000..d43b8ac3 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp new file mode 100644 index 00000000..6d588a1c --- /dev/null +++ b/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 + +#include +#include + +#include + +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::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr objects_pub_; + + std::string map_frame_id_; +}; +} // namespace booars_dummy_perception_publisher + +#endif // BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml new file mode 100644 index 00000000..9ab26623 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml @@ -0,0 +1,21 @@ + + + + booars_dummy_perception_publisher + 0.0.0 + The booars_dummy_perception_publisher package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + autoware_auto_perception_msgs + rclcpp_components + std_msgs + ament_lint_auto + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp new file mode 100644 index 00000000..0c639e3c --- /dev/null +++ b/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("map_frame_id", "map"); + + objects_sub_ = create_subscription( + "~/input/objects", 10, + std::bind(&DummyObjectsPublisher::objects_callback, this, std::placeholders::_1)); + objects_pub_ = create_publisher("~/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(booars_dummy_perception_publisher::DummyObjectsPublisher) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml index c2338819..a16e4d85 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml @@ -2,7 +2,8 @@ - + +