Merge pull request autowarefoundation#305 from tier4/sync-upstream

chore: sync upstream

common/tier4_autoware_utils/CMakeLists.txt

@@ -9,6 +9,8 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/geometry/boost_polygon_utils.cpp
+  src/math/sin_table.cpp
+  src/math/trigonometry.cpp
   src/ros/msg_operation.cpp
 )
 

common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp

@@ -0,0 +1,30 @@
+// Copyright 2023 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 TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
+#define TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
+
+#include <cstddef>
+
+namespace tier4_autoware_utils
+{
+
+constexpr size_t sin_table_size = 32769;
+constexpr size_t discrete_arcs_num_90 = 32768;
+constexpr size_t discrete_arcs_num_360 = 131072;
+extern const float g_sin_table[sin_table_size];
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -0,0 +1,27 @@
+// Copyright 2023 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 TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
+#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
+
+namespace tier4_autoware_utils
+{
+
+float sin(float radian);
+
+float cos(float radian);
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -23,6 +23,8 @@
 #include "tier4_autoware_utils/math/constants.hpp"
 #include "tier4_autoware_utils/math/normalization.hpp"
 #include "tier4_autoware_utils/math/range.hpp"
+#include "tier4_autoware_utils/math/sin_table.hpp"
+#include "tier4_autoware_utils/math/trigonometry.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/ros/debug_traits.hpp"

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -0,0 +1,49 @@
+// Copyright 2023 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.
+
+#include "tier4_autoware_utils/math/trigonometry.hpp"
+
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/math/sin_table.hpp"
+
+#include <cmath>
+
+namespace tier4_autoware_utils
+{
+
+float sin(float radian)
+{
+  float degree = radian * (180.f / static_cast<float>(tier4_autoware_utils::pi)) *
+                 (discrete_arcs_num_360 / 360.f);
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  float mul = 1.f;
+  if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    mul = -1.f;
+    idx = idx - 2 * discrete_arcs_num_90;
+  } else if (3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90) {
+    mul = -1.f;
+    idx = 4 * discrete_arcs_num_90 - idx;
+  }
+
+  return mul * g_sin_table[idx];
+}
+
+float cos(float radian) { return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f); }
+
+}  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -0,0 +1,42 @@
+// Copyright 2023 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.
+
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/math/trigonometry.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+TEST(trigonometry, sin)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    EXPECT_TRUE(
+      std::abs(
+        std::sin(x * static_cast<float>(i)) -
+        tier4_autoware_utils::sin(x * static_cast<float>(i))) < 10e-5);
+  }
+}
+
+TEST(trigonometry, cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    EXPECT_TRUE(
+      std::abs(
+        std::cos(x * static_cast<float>(i)) -
+        tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
+  }
+}

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -488,7 +488,7 @@ void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg)
   {
     QString text = "";
     QString style_sheet = "";
-    switch (msg->state) {
+    switch (msg->behavior) {
       case MRMState::NONE:
         text = "NONE";
         style_sheet = "background-color: #00FF00;";  // green

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -126,25 +126,25 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   using std::placeholders::_1;
 
   // Node Parameter
-  node_param_.update_rate = declare_parameter("update_rate", 10.0);
-  node_param_.visualize_lanelet = declare_parameter("visualize_lanelet", false);
-  node_param_.include_right_lanes = declare_parameter("include_right_lanes", false);
-  node_param_.include_left_lanes = declare_parameter("include_left_lanes", false);
-  node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes", false);
-  node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes", false);
+  node_param_.update_rate = declare_parameter<double>("update_rate");
+  node_param_.visualize_lanelet = declare_parameter<bool>("visualize_lanelet");
+  node_param_.include_right_lanes = declare_parameter<bool>("include_right_lanes");
+  node_param_.include_left_lanes = declare_parameter<bool>("include_left_lanes");
+  node_param_.include_opposite_lanes = declare_parameter<bool>("include_opposite_lanes");
+  node_param_.include_conflicting_lanes = declare_parameter<bool>("include_conflicting_lanes");
 
   // Vehicle Info
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
   vehicle_length_m_ = vehicle_info.vehicle_length_m;
 
   // Core Parameter
-  param_.footprint_margin_scale = declare_parameter("footprint_margin_scale", 1.0);
-  param_.resample_interval = declare_parameter("resample_interval", 0.3);
-  param_.max_deceleration = declare_parameter("max_deceleration", 3.0);
-  param_.delay_time = declare_parameter("delay_time", 0.3);
-  param_.max_lateral_deviation = declare_parameter("max_lateral_deviation", 1.0);
-  param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation", 1.0);
-  param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 30.0);
+  param_.footprint_margin_scale = declare_parameter<double>("footprint_margin_scale");
+  param_.resample_interval = declare_parameter<double>("resample_interval");
+  param_.max_deceleration = declare_parameter<double>("max_deceleration");
+  param_.delay_time = declare_parameter<double>("delay_time");
+  param_.max_lateral_deviation = declare_parameter<double>("max_lateral_deviation");
+  param_.max_longitudinal_deviation = declare_parameter<double>("max_longitudinal_deviation");
+  param_.max_yaw_deviation_deg = declare_parameter<double>("max_yaw_deviation_deg");
   param_.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
   param_.min_braking_distance = declare_parameter<double>("min_braking_distance");

control/trajectory_follower_node/package.xml

@@ -42,6 +42,7 @@
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>autoware_testing</test_depend>
   <test_depend>fake_test_node</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

@@ -23,7 +23,7 @@
   <!-- RTC controller -->
   <group>
     <push-ros-namespace namespace="autoware_api/external/rtc_controller"/>
-    <node_container pkg="rclcpp_components" exec="component_container_mt" name="container" namespace="">
+    <node_container pkg="rclcpp_components" exec="component_container_mt" name="container" namespace="" ros_args="--log-level autoware_api.external.rtc_controller.container:=warn">
       <composable_node pkg="autoware_iv_external_api_adaptor" plugin="external_api::RTCController" name="node"/>
     </node_container>
   </group>

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -16,6 +16,8 @@
   <arg name="nearest_search_param_path"/>
   <!-- rtc -->
   <arg name="rtc_auto_mode_manager_param_path"/>
+  <!-- mission planner -->
+  <arg name="mission_planner_param_path"/>
   <!-- behavior path planner -->
   <arg name="side_shift_param_path"/>
   <arg name="avoidance_param_path"/>
@@ -58,7 +60,9 @@
     <!-- mission planning module -->
     <group>
       <push-ros-namespace namespace="mission_planning"/>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml">
+        <arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
+      </include>
     </group>
 
     <!-- scenario planning module -->

perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp

@@ -58,8 +58,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter(
     if (index == -1) {                 // empty voxel
       std::vector<int> nn_indices(1);  // nn means nearest neighbor
       std::vector<float> nn_distances(1);
-      tree_->nearestKSearch(pcl_input->points.at(i), 1, nn_indices, nn_distances);
-      if (distance_threshold_ * distance_threshold_ < nn_distances.at(0)) {
+      if (
+        tree_->radiusSearch(
+          pcl_input->points.at(i), distance_threshold_, nn_indices, nn_distances, 1) == 0) {
         pcl_output->points.push_back(pcl_input->points.at(i));
       }
     }

perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml

@@ -6,6 +6,7 @@
     min_cluster_size: 10
     max_cluster_size: 3000
     use_height: false
+    input_frame: "base_link"
     max_x: 70.0
     min_x: -70.0
     max_y: 70.0

perception/probabilistic_occupancy_grid_map/README.md

@@ -5,6 +5,43 @@
 This package outputs the probability of having an obstacle as occupancy grid map.
 ![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif)
 
+## Settings
+
+Occupancy grid map is generated on `map_frame`, and grid orientation is fixed.
+
+You may need to choose `output_frame` which means grid map origin. Default is `base_link`, but your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) would be the better choice.
+
+### Each config paramters
+
+Config parameters are managed in `config/*.yaml` and here shows its outline.
+
+- Pointcloud based occupancy grid map
+
+| Ros param name                    | Default value |
+| --------------------------------- | ------------- |
+| map_frame                         | "map"         |
+| base_link_frame                   | "base_link"   |
+| output_frame                      | "base_link"   |
+| use_height_filter                 | true          |
+| enable_single_frame_mode          | false         |
+| map_length                        | 100.0 [m]     |
+| map_width                         | 100.0 [m]     |
+| map_resolution                    | 0.5 [m]       |
+| input_obstacle_pointcloud         | true          |
+| input_obstacle_and_raw_pointcloud | true          |
+
+- Laserscan based occupancy grid map
+
+| Ros param name           | Default value |
+| ------------------------ | ------------- |
+| map_length               | 100 [m]       |
+| map_resolution           | 0.5 [m]       |
+| use_height_filter        | true          |
+| enable_single_frame_mode | false         |
+| map_frame                | "map"         |
+| base_link_frame          | "base_link"   |
+| output_frame             | "base_link"   |
+
 ## References/External links
 
 - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md)

perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    map_frame: "map"
+    base_link_frame: "base_link"
+    output_frame: "base_link"
+    # using top of the main lidar frame is appropriate. ex) velodyne_top
+
+    use_height_filter: true
+    enable_single_frame_mode: false
+    map_length: 100.0
+    map_width: 100.0
+    map_resolution: 0.5
+    input_obstacle_pointcloud: true
+    input_obstacle_and_raw_pointcloud: true

perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    map_length: 100     # [m]
+    map_resolution: 0.5 # [m]
+
+    use_height_filter: true
+    enable_single_frame_mode: false
+
+    # grid map coordinate
+    map_frame: "map"
+    base_link_frame: "base_link"
+    output_frame: "base_link"
+    # center of grid_map. Main LiDAR frame is preferable like: "velodyne_top"

perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp

@@ -92,6 +92,7 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node
   // ROS Parameters
   std::string map_frame_;
   std::string base_link_frame_;
+  std::string output_frame_;
   bool use_height_filter_;
   bool enable_single_frame_mode_;
 };

perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp

@@ -72,7 +72,7 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D
 
   void updateWithPointCloud(
     const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
-    const Pose & robot_pose);
+    const Pose & robot_pose, const Pose & gridmap_origin);
 
   void updateOrigin(double new_origin_x, double new_origin_y) override;
 

perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp

@@ -82,6 +82,7 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node
   // ROS Parameters
   std::string map_frame_;
   std::string base_link_frame_;
+  std::string output_frame_;
   bool use_height_filter_;
   bool enable_single_frame_mode_;
 };