feat: add dummy perception publisher

simulator/dummy_perception_publisher/CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 3.5)
+project(dummy_perception_publisher)
+
+### Compile options
+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()
+
+# Dependencies for messages
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(autoware_auto_perception_msgs REQUIRED)
+find_package(autoware_perception_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(unique_identifier_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/InitialState.msg"
+  "msg/Object.msg"
+  DEPENDENCIES autoware_auto_perception_msgs autoware_perception_msgs geometry_msgs std_msgs unique_identifier_msgs
+)
+
+# See ndt_omp package for documentation on why PCL is special
+find_package(PCL REQUIRED COMPONENTS common filters)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+set(${PROJECT_NAME}_DEPENDENCIES
+  autoware_auto_perception_msgs
+  autoware_perception_msgs
+  pcl_conversions
+  rclcpp
+  sensor_msgs
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
+
+ament_auto_add_executable(dummy_perception_publisher_node
+  src/main.cpp
+  src/node.cpp
+)
+
+ament_target_dependencies(dummy_perception_publisher_node ${${PROJECT_NAME}_DEPENDENCIES})
+
+target_include_directories(dummy_perception_publisher_node
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+# For using message definitions from the same package
+rosidl_target_interfaces(dummy_perception_publisher_node
+  ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# PCL dependencies – `ament_target_dependencies` doesn't respect the
+# components/modules selected above and only links in `common` ,so we need
+# to do this manually.
+target_compile_definitions(dummy_perception_publisher_node PRIVATE ${PCL_DEFINITIONS})
+target_include_directories(dummy_perception_publisher_node PRIVATE ${PCL_INCLUDE_DIRS})
+# Unfortunately, this one can't be PRIVATE because only the plain or only the
+# keyword (PRIVATE) signature of target_link_libraries can be used for one
+# target, not both. The plain signature is already used inside
+# `ament_target_dependencies` and possibly rosidl_target_interfaces.
+target_link_libraries(dummy_perception_publisher_node ${PCL_LIBRARIES})
+target_link_directories(dummy_perception_publisher_node PRIVATE ${PCL_LIBRARY_DIRS})
+
+
+ament_auto_add_executable(empty_objects_publisher
+  src/empty_objects_publisher.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

simulator/dummy_perception_publisher/README.md

@@ -0,0 +1,44 @@
+# dummy_perception_publisher
+
+## Purpose
+
+This node publishes the result of the dummy detection with the type of perception.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name           | Type                                      | Description             |
+| -------------- | ----------------------------------------- | ----------------------- |
+| `/tf`          | `tf2_msgs/TFMessage`                      | TF (self-pose)          |
+| `input/object` | `dummy_perception_publisher::msg::Object` | dummy detection objects |
+
+### Output
+
+| Name                    | Type                                                        | Description            |
+| ----------------------- | ----------------------------------------------------------- | ---------------------- |
+| `output/dynamic_object` | `autoware_perception_msgs::msg::DetectedObjectsWithFeature` | Publishes objects      |
+| `output/points_raw`     | `sensor_msgs::msg::PointCloud2`                             | point cloud of objects |
+
+## Parameters
+
+| Name                        | Type   | Default Value | Explanation                                 |
+| --------------------------- | ------ | ------------- | ------------------------------------------- |
+| `visible_range`             | double | 100.0         | sensor visible range [m]                    |
+| `detection_successful_rate` | double | 0.8           | sensor detection rate. (min) 0.0 - 1.0(max) |
+| `enable_ray_tracing`        | bool   | true          | if True, use ray tracking                   |
+| `use_object_recognition`    | bool   | true          | if True, publish objects topic              |
+
+### Node Parameters
+
+None.
+
+### Core Parameters
+
+None.
+
+## Assumptions / Known limits
+
+TBD.

simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp

@@ -0,0 +1,69 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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 DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_
+#define DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_
+
+#include "dummy_perception_publisher/msg/object.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/common/distances.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <random>
+#include <vector>
+
+class DummyPerceptionPublisherNode : public rclcpp::Node
+{
+private:
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
+  rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    detected_object_with_feature_pub_;
+  rclcpp::Subscription<dummy_perception_publisher::msg::Object>::SharedPtr object_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  std::vector<dummy_perception_publisher::msg::Object> objects_;
+  double visible_range_;
+  double detection_successful_rate_;
+  bool enable_ray_tracing_;
+  bool use_object_recognition_;
+  bool use_real_param_;
+  std::mt19937 random_generator_;
+  void timerCallback();
+  void createObjectPointcloud(
+    const double length, const double width, const double height, const double std_dev_x,
+    const double std_dev_y, const double std_dev_z,
+    const tf2::Transform & tf_base_link2moved_object,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud_ptr);
+  void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg);
+
+public:
+  DummyPerceptionPublisherNode();
+  ~DummyPerceptionPublisherNode() {}
+};
+
+#endif  // DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_

simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml

@@ -0,0 +1,97 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <!-- dummy detection and sensor data -->
+  <arg name="visible_range" default="300.0" />
+  <arg name="real" default="true" />
+  <arg name="use_object_recognition" default="true" />
+  <arg name="use_traffic_light_recognition" default="false" />
+
+  <group>
+    <push-ros-namespace namespace="simulation"/>
+    <group if="$(var real)">
+      <node pkg="dummy_perception_publisher" exec="dummy_perception_publisher_node" name="dummy_perception_publisher" output="screen">
+        <remap from="output/dynamic_object" to="/perception/object_recognition/detection/labeled_clusters" />
+        <remap from="output/objects_pose" to="debug/object_pose" />
+        <remap from="output/points_raw" to="/perception/obstacle_segmentation/pointcloud" />
+        <remap from="input/object" to="dummy_perception_publisher/object_info" />
+        <remap from="input/reset" to="input/reset" />
+        <param name="visible_range" value="$(var visible_range)" />
+        <param name="detection_successful_rate" value="0.999" />
+        <param name="enable_ray_tracing" value="true" />
+        <param name="use_object_recognition" value="$(var use_object_recognition)" />
+      </node>
+      <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+        <arg name="input/objects" value="/perception/object_recognition/detection/labeled_clusters" />
+        <arg name="output/objects" value="/perception/object_recognition/detection/objects_with_feature" />
+      </include>
+    </group>
+    <group unless="$(var real)">
+      <node pkg="dummy_perception_publisher" exec="dummy_perception_publisher_node" name="dummy_perception_publisher" output="screen">
+        <remap from="output/dynamic_object" to="/perception/object_recognition/detection/objects_with_feature" />
+        <remap from="output/objects_pose" to="debug/object_pose" />
+        <remap from="output/points_raw" to="/perception/obstacle_segmentation/pointcloud" />
+        <remap from="input/object" to="dummy_perception_publisher/object_info" />
+        <remap from="input/reset" to="input/reset" />
+        <param name="visible_range" value="$(var visible_range)" />
+        <param name="detection_successful_rate" value="1.0" />
+        <param name="enable_ray_tracing" value="false" />
+        <param name="use_object_recognition" value="$(var use_object_recognition)" />
+      </node>
+    </group>
+
+    <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
+    <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
+      <arg name="input" value="/perception/object_recognition/detection/objects_with_feature" />
+      <arg name="output" value="/perception/object_recognition/detection/objects" />
+    </include>
+
+  </group>
+
+  <!-- perception module -->
+  <group>
+    <push-ros-namespace namespace="perception"/>
+    <!-- object segmentation module -->
+    <group>
+      <push-ros-namespace namespace="obstacle_segmentation"/>
+      <!-- Occupancy Grid -->
+      <include file="$(find-pkg-share laserscan_to_occupancy_grid_map)/launch/laserscan_to_occupancy_grid_map.launch.py">
+        <arg name="input_obstacle_pointcloud" value="true" />
+        <arg name="input_obstacle_and_raw_pointcloud" value="false" />
+        <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud" />
+      </include>
+    </group>
+
+    <!-- object recognition module -->
+    <group if="$(var use_object_recognition)">
+      <push-ros-namespace namespace="object_recognition"/>
+      <!-- detection module -->
+      <!-- tracking module -->
+      <group>
+        <push-ros-namespace namespace="tracking"/>
+        <include file="$(find-pkg-share perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
+        </include>
+      </group>
+      <!-- prediction module -->
+      <group>
+        <push-ros-namespace namespace="prediction"/>
+        <include file="$(find-pkg-share perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
+          <arg name="use_vector_map" value="true" />
+        </include>
+      </group>
+    </group>
+
+    <group unless="$(var use_object_recognition)">
+      <push-ros-namespace namespace="object_recognition"/>
+      <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
+        <remap from="~/output/objects" to="/perception/object_recognition/objects" />
+      </node>
+    </group>
+
+    <!-- traffic light module -->
+    <group if="$(var use_traffic_light_recognition)">
+      <push-ros-namespace namespace="traffic_light_recognition"/>
+      <include file="$(find-pkg-share perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"></include>
+    </group>
+  </group>
+</launch>

simulator/dummy_perception_publisher/msg/InitialState.msg

@@ -0,0 +1,2 @@
+geometry_msgs/PoseWithCovariance pose_covariance
+geometry_msgs/TwistWithCovariance twist_covariance

simulator/dummy_perception_publisher/msg/Object.msg

@@ -0,0 +1,11 @@
+std_msgs/Header header
+unique_identifier_msgs/UUID id
+dummy_perception_publisher/InitialState initial_state
+autoware_auto_perception_msgs/ObjectClassification classification
+autoware_auto_perception_msgs/Shape shape
+
+uint8 ADD=0
+uint8 MODIFY=1
+uint8 DELETE=2
+uint8 DELETEALL=3
+int32 action

simulator/dummy_perception_publisher/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dummy_perception_publisher</name>
+  <version>0.1.0</version>
+  <description>The dummy_perception_publisher package</description>
+
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>unique_identifier_msgs</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>