Feature/occulsion spot generator

planning/occlusion_spot_generator/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.5)
+project(occlusion_spot_generator)
+
+### 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(-Wextra)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_library(occlusion_spot_generator SHARED
+  src/occlusion_spot_generator_node.cpp
+  src/occlusion_spot_generator.cpp
+  src/grid_utils.cpp
+)
+
+rclcpp_components_register_node(occlusion_spot_generator
+  PLUGIN "occlusion_spot_generator::OcclusionSpotGeneratorNode"
+  EXECUTABLE occlusion_spot_generator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

planning/occlusion_spot_generator/Readme.md

@@ -0,0 +1,13 @@
+# Occlusion Spot Generator
+
+## Purpose
+
+This package is to get occlusion spot info.
+
+### Description
+
+TBD.
+
+## Assumptions / Known limits
+
+TBD.

planning/occlusion_spot_generator/config/occlusion_spot_generator.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    use_object_info: true                 # [-] whether to reflect object info to occupancy grid map or not
+    use_moving_object_ray_cast: true      # [-] whether to reflect moving object ray_cast to occupancy grid map or not
+    debug:                                # !Note: default should be false for performance
+      is_show_occlusion: false            # [-] whether to show occlusion point markers.
+      is_show_cv_window: false            # [-] whether to show open_cv debug window
+      is_show_processing_time: false      # [-] whether to show processing time
+    threshold:
+      detection_area_length: 100.0        # [m] the length of path to consider perception range
+      stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop
+    grid:
+      free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
+      occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
+      occlusion_size_of_pedestrian: 1.5     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
+      occlusion_size_of_bicycle: 3.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
+      occlusion_size_of_car: 7.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
+      occupancy_grid_resolusion: 0.5 # [m] TODO replace this with actual parameter set for occupancy grid

planning/occlusion_spot_generator/include/occlusion_spot_generator/grid_utils.hpp

@@ -0,0 +1,119 @@
+// Copyright 2021 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 OCCLUSION_SPOT_GENERATOR__GRID_UTILS_HPP_
+#define OCCLUSION_SPOT_GENERATOR__GRID_UTILS_HPP_
+
+#include <grid_map_core/GridMap.hpp>
+#include <grid_map_core/iterators/LineIterator.hpp>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include <grid_map_utils/polygon_iterator.hpp>
+#include <opencv2/opencv.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/map_meta_data.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/geometry/algorithms/area.hpp>
+#include <boost/geometry/algorithms/disjoint.hpp>
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/equals.hpp>
+#include <boost/geometry/algorithms/intersection.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/segment.hpp>
+#include <boost/geometry/strategies/strategies.hpp>
+
+#include <lanelet2_core/primitives/Polygon.h>
+#include <tf2/utils.h>
+
+#include <algorithm>
+#include <vector>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+// cppcheck-suppress unknownMacro
+BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z)
+
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using Point2d = boost::geometry::model::d2::point_xy<double>;
+using Polygon2d = boost::geometry::model::polygon<Point2d, false, false>;
+using Polygons2d = std::vector<Polygon2d>;
+
+namespace grid_utils
+{
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::TransformStamped;
+using nav_msgs::msg::MapMetaData;
+using nav_msgs::msg::OccupancyGrid;
+namespace occlusion_cost_value
+{
+static constexpr unsigned char FREE_SPACE = 0;
+static constexpr unsigned char UNKNOWN = 50;
+static constexpr unsigned char OCCUPIED = 100;
+static constexpr unsigned char UNKNOWN_IMAGE = 100;
+static constexpr unsigned char OCCUPIED_IMAGE = 255;
+}  // namespace occlusion_cost_value
+
+struct PolarCoordinates
+{
+  double radius;
+  double theta;
+};
+
+inline PolarCoordinates toPolarCoordinates(const Point2d & origin, const Point2d & point)
+{
+  const double dy = point.y() - origin.y();
+  const double dx = point.x() - origin.x();
+  const double radius = std::hypot(dy, dx);
+  const double theta = std::atan2(dy, dx);
+  return {radius, theta};
+}
+
+struct GridParam
+{
+  int free_space_max;  // maximum value of a freespace cell in the occupancy grid
+  int occupied_min;    // minimum value of an occupied cell in the occupancy grid
+};
+
+void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid);
+void toQuantizedImage(
+  const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * border_image,
+  cv::Mat * occlusion_image, const GridParam & param);
+void denoiseOccupancyGridCV(
+  const OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, grid_map::GridMap & grid_map,
+  const GridParam & param, const bool is_show_debug_window, const int num_iter,
+  const bool use_object_footprints, const bool use_object_ray_casts);
+void addObjectsToGridMap(const std::vector<PredictedObject> & objs, grid_map::GridMap & grid);
+}  // namespace grid_utils
+
+#endif  // OCCLUSION_SPOT_GENERATOR__GRID_UTILS_HPP_

planning/occlusion_spot_generator/include/occlusion_spot_generator/occlusion_spot_generator.hpp

@@ -0,0 +1,79 @@
+// Copyright 2021 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 OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_HPP_
+#define OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_HPP_
+
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+// boost
+#include <boost/assign/list_of.hpp>
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/intersection.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/segment.hpp>
+#include <boost/geometry/strategies/strategies.hpp>
+
+// tf
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <boost/optional.hpp>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <utility>
+#include <vector>
+
+using geometry_msgs::msg::Pose;
+using Point2d = boost::geometry::model::d2::point_xy<double>;
+using Polygon2d = boost::geometry::model::polygon<Point2d, false, false>;
+using Polygons2d = std::vector<Polygon2d>;
+
+namespace occlusion_spot_generator
+{
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::Shape;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+
+bool isStuckVehicle(const PredictedObject & obj, const double min_vel);
+bool isMovingVehicle(const PredictedObject & obj, const double min_vel);
+std::vector<PredictedObject> extractVehicles(
+  const PredictedObjects::ConstSharedPtr objects_ptr, const Point ego_position,
+  const double distance);
+std::vector<PredictedObject> filterVehiclesByDetectionArea(
+  const std::vector<PredictedObject> & objs, const Polygons2d & polys);
+bool isVehicle(const ObjectClassification & obj_class);
+void vehiclesToFootprint(
+  const std::vector<PredictedObject> & vehicles, Polygons2d & stuck_vehicle_foot_prints,
+  Polygons2d & moving_vehicle_foot_prints, const double stuck_vehicle_vel);
+
+}  // namespace occlusion_spot_generator
+
+#endif  // OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_HPP_

planning/occlusion_spot_generator/include/occlusion_spot_generator/occlusion_spot_generator_node.hpp

@@ -0,0 +1,94 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// 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 OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_NODE_HPP_
+#define OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_NODE_HPP_
+
+#include "occlusion_spot_generator/occlusion_spot_generator.hpp"
+#include "occlusion_spot_generator/planner_data.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace occlusion_spot_generator
+{
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::PoseStamped;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::OccupancyGrid;
+using nav_msgs::msg::Odometry;
+
+/// This is a convenience class for saving you from declaring all parameters
+/// manually and calculating derived parameters.
+/// This class supposes that necessary parameters are set when the node is launched.
+class OcclusionSpotGeneratorNode : public rclcpp::Node
+{
+public:
+  /// Constructor
+  explicit OcclusionSpotGeneratorNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // tf
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  // subscriber
+  rclcpp::Subscription<PathWithLaneId>::SharedPtr trigger_sub_path_with_lane_id_;
+  rclcpp::Subscription<PredictedObjects>::SharedPtr sub_predicted_objects_;
+
+  rclcpp::Subscription<Odometry>::SharedPtr sub_vehicle_odometry_;
+  rclcpp::Subscription<OccupancyGrid>::SharedPtr sub_occupancy_grid_;
+  void onPathWithLaneId(const PathWithLaneId::ConstSharedPtr input_path_msg);
+  void onPredictedObjects(const PredictedObjects::ConstSharedPtr msg);
+  void onVehicleVelocity(const Odometry::ConstSharedPtr msg);
+  void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
+  bool isDataReady(const PerceptionData planner_data) const;
+  void onTimer();
+  void generateOcclusionSpot();
+
+  // param callback function
+  rcl_interfaces::msg::SetParametersResult paramCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_callback_;
+  PerceptionData perception_data_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  struct Param
+  {
+    int free_space_max;  // maximum value of a freespace cell in the occupancy grid
+    int occupied_min;    // minimum value of an occupied cell in the occupancy grid
+    double occlusion_size_of_pedestrian = {};
+    double occlusion_size_of_bicycle = {};
+    double occlusion_size_of_car = {};
+    double occupancy_grid_resolusion = {};
+  };
+  Param occlusion_param_;
+};
+
+}  // namespace occlusion_spot_generator
+
+#endif  // OCCLUSION_SPOT_GENERATOR__OCCLUSION_SPOT_GENERATOR_NODE_HPP_