diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index b20a835f80e39..10bf11847c2f5 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } + void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id) + { + m_marker_common.deleteMarker(marker_id); + } + protected: /// \brief Convert given shape msg into a Marker /// \tparam ClassificationContainerT List type with ObjectClassificationMsg diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index ee2c7e0d5c3b9..5493f1dd594ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; + std::set existing_marker_ids; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 870b8f2c77543..24e67a6f44e95 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -15,6 +15,7 @@ #include #include +#include namespace autoware { @@ -130,7 +131,7 @@ std::vector PredictedObjectsDisplay: auto acceleration_text_marker_ptr = acceleration_text_marker.value(); acceleration_text_marker_ptr->header = msg->header; acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(acceleration_text_marker_ptr); + markers.push_back(acceleration_text_marker_ptr); } // Get marker for twist @@ -195,11 +196,19 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) std::unique_lock lock(mutex); if (!markers.empty()) { - clear_markers(); - + std::set new_marker_ids = std::set(); for (const auto & marker : markers) { + rviz_default_plugins::displays::MarkerID marker_id = + rviz_default_plugins::displays::MarkerID(marker->ns, marker->id); add_marker(marker); + new_marker_ids.insert(marker_id); + } + for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) { + if (new_marker_ids.find(*itr) == new_marker_ids.end()) { + deleteMarker(*itr); + } } + existing_marker_ids = new_marker_ids; markers.clear(); } diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp new file mode 100644 index 0000000000000..ac9b124a02cdb --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp @@ -0,0 +1,35 @@ +// 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__ROS__PARAMETER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ +template +T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) +{ + if (node.has_parameter(name)) { + return node.get_parameter(name).get_value(); + } + + return node.declare_parameter(name); +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 450549f75fdec..2aa5ae0c7e6ef 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -31,6 +31,7 @@ #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/msg_operation.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/processing_time_publisher.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index c443318a146ac..ed527c38824fc 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -69,8 +69,8 @@ void Lanelet2MapLoaderNode::on_map_projector_info( const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file - const auto map = - load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude); + const auto map = load_map( + lanelet2_filename, msg->projector_type, msg->map_origin.latitude, msg->map_origin.longitude); if (!map) { return; } diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index e31db7890cf47..13808e6bf0228 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -34,8 +34,9 @@ If you want to use MGRS, please specify the MGRS grid as well. ```yaml # map_projector_info.yaml -type: "MGRS" -mgrs_grid: "54SUE" +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE ``` ### Using LocalCartesianUTM @@ -44,10 +45,12 @@ If you want to use local cartesian UTM, please specify the map origin as well. ```yaml # map_projector_info.yaml -type: "LocalCartesianUTM" +projector_type: LocalCartesianUTM +vertical_datum: WGS84 map_origin: - latitude: 35.6092 - longitude: 139.7303 + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] ``` ## Published Topics diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp index 77958c20a9e75..f213432b66b50 100644 --- a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -46,10 +46,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str tier4_map_msgs::msg::MapProjectorInfo msg; if (is_local) { - msg.type = "local"; + msg.projector_type = "local"; } else { - msg.type = "MGRS"; + msg.projector_type = "MGRS"; msg.mgrs_grid = projector.getProjectedMGRSGrid(); } + + // We assume that the vertical datum of the map is WGS84 when using lanelet2 map. + // However, do note that this is not always true, and may cause problems in the future. + // Thus, please consider using the map_projector_info.yaml instead of this deprecated function. + msg.vertical_datum = "WGS84"; return msg; } diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 2bd8a9cfa243a..ec38f0ff3642e 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -27,13 +27,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi YAML::Node data = YAML::LoadFile(filename); tier4_map_msgs::msg::MapProjectorInfo msg; - msg.type = data["type"].as(); - if (msg.type == "MGRS") { + msg.projector_type = data["projector_type"].as(); + if (msg.projector_type == "MGRS") { + msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.type == "LocalCartesianUTM") { + } else if (msg.projector_type == "LocalCartesianUTM") { + msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); - } else if (msg.type == "local") { + } else if (msg.projector_type == "local") { ; // do nothing } else { throw std::runtime_error( diff --git a/map/map_projection_loader/test/data/map_projector_info_local.yaml b/map/map_projection_loader/test/data/map_projector_info_local.yaml new file mode 100644 index 0000000000000..2e5d27c3e8143 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_local.yaml @@ -0,0 +1 @@ +projector_type: local diff --git a/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml b/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml new file mode 100644 index 0000000000000..854a6ad7b5a21 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml @@ -0,0 +1,6 @@ +projector_type: LocalCartesianUTM +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml b/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml new file mode 100644 index 0000000000000..5446de4898900 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml @@ -0,0 +1,3 @@ +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/data/projection_info_local.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml deleted file mode 100644 index cea4aaf31d439..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_local.yaml +++ /dev/null @@ -1 +0,0 @@ -type: local diff --git a/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml deleted file mode 100644 index c7500d37bb35a..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml +++ /dev/null @@ -1,4 +0,0 @@ -type: LocalCartesianUTM -map_origin: - latitude: 35.6762 - longitude: 139.6503 diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml deleted file mode 100644 index d7687521be5fa..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_mgrs.yaml +++ /dev/null @@ -1,2 +0,0 @@ -type: MGRS -mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index 206053fd2e8d2..fdfe96434fe0d 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -95,7 +95,7 @@ TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.projector_type, "MGRS"); EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); } @@ -109,7 +109,7 @@ TEST(TestLoadFromLanelet2Map, LoadLocalGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "local"); + EXPECT_EQ(projector_info.projector_type, "local"); } TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) @@ -122,7 +122,7 @@ TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.projector_type, "MGRS"); } int main(int argc, char ** argv) diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index 920f5e3be11a1..ec13df010c8c3 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_local_cartesian_utm.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_local_cartesian_utm.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,18 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) self.assertEqual( self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] ) diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index 8ab0db43e32a7..ccec68486b79c 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_local.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_local.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,17 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) self.test_node.destroy_subscription(subscription) diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f18f9b7c17e75..200504d0ea18f 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_mgrs.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,18 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) self.test_node.destroy_subscription(subscription) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 744eb7af36a5b..a890c19e25d06 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -38,11 +38,18 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "euclidean_cluster"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void EuclideanClusterNode::onPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); + // convert ros to pcl pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); @@ -65,6 +72,14 @@ void EuclideanClusterNode::onPointCloud( convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); } + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index e68a346a9a7d3..16e179b2512d3 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -17,13 +17,15 @@ #include "euclidean_cluster/euclidean_cluster.hpp" #include +#include +#include #include #include #include +#include #include - namespace euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node @@ -39,6 +41,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 01856f3e6c836..53fb4df8977d8 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -42,11 +42,18 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique( + this, "voxel_grid_based_euclidean_cluster"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void VoxelGridBasedEuclideanClusterNode::onPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); + // convert ros to pcl pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); @@ -69,6 +76,14 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); } + if (debug_publisher_) { + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index d05e5ca1356fe..18330b5f12074 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -17,6 +17,8 @@ #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include +#include +#include #include #include @@ -39,6 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 0765490502ec4..a76310972c738 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Shunsuke Miura Yoshi Ri + Takayuki Murooka Apache License 2.0 ament_cmake diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 8d633dbd5af64..5360562e231ba 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -313,8 +313,11 @@ bool validateIsolatedLaneletLength( const auto & end_point = center_line.back(); // calc approx distance between closest point and end point const double approx_distance = lanelet::geometry::distance2d(obj_point, end_point); - const double min_length = - object.kinematics.twist_with_covariance.twist.linear.x * prediction_time; + // calc min length for prediction + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double min_length = abs_speed * prediction_time; return approx_distance > min_length; } @@ -553,8 +556,11 @@ ObjectClassification::_label_type changeLabelForPrediction( // if object is within road lanelet and satisfies yaw constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bicycle 25 km/h - const bool high_speed_object = - object.kinematics.twist_with_covariance.twist.linear.x > high_speed_threshold; + // calc abs speed from x and y velocity + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > high_speed_threshold; // if the object is within lanelet, do the same estimation with vehicle if (within_road_lanelet) { @@ -570,8 +576,10 @@ ObjectClassification::_label_type changeLabelForPrediction( const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = 25.0 / 18.0 * 5.0; // Max human being motion speed is 25km/h - const bool high_speed_object = - object.kinematics.twist_with_covariance.twist.linear.x > max_velocity_for_human_mps; + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > max_velocity_for_human_mps; // fast, human-like object: like segway if (within_road_lanelet && high_speed_object) { return label; // currently do nothing @@ -826,9 +834,10 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // For too-slow vehicle - if ( - std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) < - min_velocity_for_map_based_prediction_) { + const double abs_obj_speed = std::hypot( + transformed_object.kinematics.twist_with_covariance.twist.linear.x, + transformed_object.kinematics.twist_with_covariance.twist.linear.y); + if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(transformed_object); predicted_path.confidence = 1.0; @@ -1046,6 +1055,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); + // assumption: the object vx is much larger than vy if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { if ( object.kinematics.orientation_availability == @@ -1064,6 +1074,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) } object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; } return; @@ -1326,7 +1337,9 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time) { - const double obj_vel = std::fabs(object.kinematics.twist_with_covariance.twist.linear.x); + const double obj_vel = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); std::vector all_ref_paths; for (const auto & current_lanelet_data : current_lanelets_data) { @@ -1667,21 +1680,22 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( /* == Nonlinear model == * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - vy_k * sin(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + vy_k * cos(yaw_k) * dt * yaw_{k+1} = yaw_k + (wz_k) * dt * */ const double vx = twist.linear.x; + const double vy = twist.linear.y; const double wz = twist.angular.z; const double prev_yaw = tf2::getYaw(delayed_pose.orientation); const double prev_x = delayed_pose.position.x; const double prev_y = delayed_pose.position.y; const double prev_z = delayed_pose.position.z; - const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt; - const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt; + const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt - vy * std::sin(prev_yaw) * dt; + const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt + vy * std::cos(prev_yaw) * dt; const double curr_z = prev_z; const double curr_yaw = prev_yaw + wz * dt; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index 0f7fc50ed535f..8ca12e3191dc5 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -65,6 +65,18 @@ $$ \end{array}\right] $$ +#### remarks on the output twist + +Remarks that the velocity $v_{k}$ is the norm of velocity of vehicle, not the longitudinal velocity. +So the output twist in the object coordinate $(x,y)$ is calculated as follows. + +$$ +\begin{aligned} +v_{x} &= v_{k} \cos \left(\beta_{k}\right) \\ +v_{y} &= v_{k} \sin \left(\beta_{k}\right) +\end{aligned} +$$ + ## Anchor point based estimation To separate the estimation of the position and the shape, we use anchor point based position estimation. diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 2b0fff35729d0..006a87430965f 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -427,7 +427,8 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 73916742f17bb..50f582d6a2501 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -478,7 +478,8 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 27c2a7b821eb6..978b5e346efb2 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -494,7 +494,8 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 26f53cc91c4cf..589233cf5a441 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -140,7 +140,8 @@ # collision check parameters check_all_predicted_path: false # [-] time_resolution: 0.5 # [s] - time_horizon: 10.0 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 2.0 # [-] hysteresis_factor_safe_count: 10 # [-] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 0fc10109240f9..c9f4d6c909900 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -51,24 +51,22 @@ using visualization_msgs::msg::MarkerArray; struct CollisionCheckDebug { - std::string failed_reason; - std::size_t lane_id{0}; - Pose current_pose{}; - Twist current_twist{}; - Twist object_twist{}; - Pose expected_ego_pose{}; - Pose expected_obj_pose{}; - Pose relative_to_ego{}; - double rss_longitudinal{0.0}; - double ego_to_obj_margin{0.0}; - double longitudinal_offset{0.0}; - double lateral_offset{0.0}; - bool is_front{false}; - bool allow_lane_change{false}; - std::vector lerped_path; - std::vector ego_predicted_path{}; - Polygon2d ego_polygon{}; - Polygon2d obj_polygon{}; + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Pose current_pose{}; ///< Ego vehicle's current pose. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. + double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. + std::vector lerped_path; ///< Interpolated ego vehicle path. + std::vector ego_predicted_path{}; ///< Predicted future path of ego vehicle. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugMap = std::unordered_map; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 5484ef834a177..c0ed2c975a54d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -187,7 +187,8 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double safety_check_time_horizon{0.0}; + double time_horizon_for_front_object{0.0}; + double time_horizon_for_rear_object{0.0}; double safety_check_time_resolution{0.0}; // find adjacent lane vehicles diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 6b7830147ebf9..e2934a02b63d8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -89,7 +89,7 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const bool is_object_front, const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 79947d826ed10..7b5b1f562f1b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index fca53930a0ec5..cd0236f909b14 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -56,12 +56,13 @@ MarkerArray showObjectInfo( std::ostringstream ss; - ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason + ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset - << "\nLongitudinal offset: " << info.longitudinal_offset + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No"); + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); obj_marker.text = ss.str(); @@ -195,8 +196,8 @@ MarkerArray showPolygon( int32_t idx = {0}; for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = info.allow_lane_change ? green_color : red_color; - const auto & ego_polygon = info.ego_polygon.outer(); + const auto & color = info.is_safe ? green_color : red_color; + const auto & ego_polygon = info.extended_ego_polygon.outer(); const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); @@ -214,7 +215,7 @@ MarkerArray showPolygon( marker_array.markers.push_back(text_marker); - const auto & obj_polygon = info.obj_polygon.outer(); + const auto & obj_polygon = info.extended_obj_polygon.outer(); obj_marker.id = ++id; obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); obj_marker.points.reserve(obj_polygon.size()); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index e0c79a46d731e..f68e54266fe85 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1845,8 +1845,10 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); + const auto ego_predicted_path_for_front_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); + const auto ego_predicted_path_for_rear_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1875,8 +1877,18 @@ bool AvoidanceModule::isSafePath( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + const auto obj_polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); + + const auto & ego_predicted_path = + is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 2d4a6b9c42b98..8900e2b3e827c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -23,68 +23,62 @@ namespace behavior_path_planner { -namespace -{ -template -T get_parameter(rclcpp::Node * node, const std::string & ns) -{ - if (node->has_parameter(ns)) { - return node->get_parameter(ns).get_value(); - } - - return node->declare_parameter(ns); -} -} // namespace - AvoidanceModuleManager::AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) : SceneModuleManagerInterface(node, name, config, {"left", "right"}) { using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters p{}; // general params { std::string ns = "avoidance."; p.resample_interval_for_planning = - get_parameter(node, ns + "resample_interval_for_planning"); + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - get_parameter(node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); + getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = - get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = get_parameter(node, ns + "disable_path_update"); - p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); - p.print_debug_info = get_parameter(node, ns + "print_debug_info"); + getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); } // drivable area { std::string ns = "avoidance."; - p.use_adjacent_lane = get_parameter(node, ns + "use_adjacent_lane"); - p.use_opposite_lane = get_parameter(node, ns + "use_opposite_lane"); - p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); - p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); + p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = + getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); - param.execute_num = get_parameter(node, ns + "execute_num"); - param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); - param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); - param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.is_target = getOrDeclareParameter(*node, ns + "is_target"); + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = - get_parameter(node, ns + "safety_buffer_longitudinal"); + getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); return param; }; @@ -101,119 +95,131 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = - get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( + *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); + getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_forward_distance = - get_parameter(node, ns + "object_check_forward_distance"); + getOrDeclareParameter(*node, ns + "object_check_forward_distance"); p.object_check_backward_distance = - get_parameter(node, ns + "object_check_backward_distance"); - p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + getOrDeclareParameter(*node, ns + "object_check_backward_distance"); + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = - get_parameter(node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = - get_parameter(node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = - get_parameter(node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } // safety check general params { std::string ns = "avoidance.safety_check."; - p.enable_safety_check = get_parameter(node, ns + "enable"); - p.check_current_lane = get_parameter(node, ns + "check_current_lane"); - p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); - p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); - p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); - p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); - p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); + p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.check_unavoidable_object = + getOrDeclareParameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + p.check_all_predicted_path = + getOrDeclareParameter(*node, ns + "check_all_predicted_path"); + p.time_horizon_for_front_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); + p.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.safety_check_time_resolution = getOrDeclareParameter(*node, ns + "time_resolution"); p.safety_check_backward_distance = - get_parameter(node, ns + "safety_check_backward_distance"); + getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); p.hysteresis_factor_expand_rate = - get_parameter(node, ns + "hysteresis_factor_expand_rate"); - p.hysteresis_factor_safe_count = get_parameter(node, ns + "hysteresis_factor_safe_count"); + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = + getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); } // safety check rss params { std::string ns = "avoidance.safety_check."; p.rss_params.longitudinal_distance_min_threshold = - get_parameter(node, ns + "longitudinal_distance_min_threshold"); + getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = - get_parameter(node, ns + "longitudinal_velocity_delta_time"); + getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); p.rss_params.front_vehicle_deceleration = - get_parameter(node, ns + "expected_front_deceleration"); + getOrDeclareParameter(*node, ns + "expected_front_deceleration"); p.rss_params.rear_vehicle_deceleration = - get_parameter(node, ns + "expected_rear_deceleration"); + getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); p.rss_params.rear_vehicle_reaction_time = - get_parameter(node, ns + "rear_vehicle_reaction_time"); + getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); p.rss_params.rear_vehicle_safety_time_margin = - get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); p.rss_params.lateral_distance_max_threshold = - get_parameter(node, ns + "lateral_distance_max_threshold"); + getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); } // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = get_parameter(node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = get_parameter(node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = get_parameter(node, ns + "lateral_execution_threshold"); + p.soft_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); + p.lateral_execution_threshold = + getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = - get_parameter(node, ns + "lateral_small_shift_threshold"); + getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); p.lateral_avoid_check_threshold = - get_parameter(node, ns + "lateral_avoid_check_threshold"); - p.max_right_shift_length = get_parameter(node, ns + "max_right_shift_length"); - p.max_left_shift_length = get_parameter(node, ns + "max_left_shift_length"); + getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); } // avoidance maneuver (longitudinal) { std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = get_parameter(node, ns + "prepare_time"); - p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); - p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); - p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); - p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); + p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } // yield { std::string ns = "avoidance.yield."; - p.yield_velocity = get_parameter(node, ns + "yield_velocity"); + p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); } // stop { std::string ns = "avoidance.stop."; - p.stop_max_distance = get_parameter(node, ns + "max_distance"); - p.stop_buffer = get_parameter(node, ns + "stop_buffer"); + p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); + p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); } // policy { std::string ns = "avoidance.policy."; - p.policy_deceleration = get_parameter(node, ns + "deceleration"); - p.policy_lateral_margin = get_parameter(node, ns + "lateral_margin"); + p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); + p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately = - get_parameter(node, ns + "use_shorten_margin_immediately"); + getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); @@ -227,20 +233,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( // constraints (longitudinal) { std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = get_parameter(node, ns + "nominal_deceleration"); - p.nominal_jerk = get_parameter(node, ns + "nominal_jerk"); - p.max_deceleration = get_parameter(node, ns + "max_deceleration"); - p.max_jerk = get_parameter(node, ns + "max_jerk"); - p.max_acceleration = get_parameter(node, ns + "max_acceleration"); + p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); + p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); + p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); + p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); } // constraints (lateral) { std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = get_parameter>(node, ns + "velocity"); - p.lateral_max_accel_map = get_parameter>(node, ns + "max_accel_values"); - p.lateral_min_jerk_map = get_parameter>(node, ns + "min_jerk_values"); - p.lateral_max_jerk_map = get_parameter>(node, ns + "max_jerk_values"); + p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.lateral_max_accel_map = + getOrDeclareParameter>(*node, ns + "max_accel_values"); + p.lateral_min_jerk_map = + getOrDeclareParameter>(*node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = + getOrDeclareParameter>(*node, ns + "max_jerk_values"); if (p.velocity_map.empty()) { throw std::domain_error("invalid velocity map."); @@ -263,15 +272,15 @@ AvoidanceModuleManager::AvoidanceModuleManager( { std::string ns = "avoidance.shift_line_pipeline."; p.quantize_filter_threshold = - get_parameter(node, ns + "trim.quantize_filter_threshold"); + getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); p.same_grad_filter_1_threshold = - get_parameter(node, ns + "trim.same_grad_filter_1_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); p.same_grad_filter_2_threshold = - get_parameter(node, ns + "trim.same_grad_filter_2_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); p.same_grad_filter_3_threshold = - get_parameter(node, ns + "trim.same_grad_filter_3_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); p.sharp_shift_filter_threshold = - get_parameter(node, ns + "trim.sharp_shift_filter_threshold"); + getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); } parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 8e6ab24929a00..1f5cf8c2a9500 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -107,14 +107,15 @@ std::pair projectObstacleVelocityToTrajectory( const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double obj_vel_yaw = + obj_yaw + std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - return std::make_pair( - obj_vel_norm * std::cos(obj_vel_yaw - path_yaw), - obj_vel_norm * std::sin(obj_vel_yaw - path_yaw)); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_vel_yaw - path_yaw); + return std::make_pair(obj_vel_norm * std::cos(diff_yaw), obj_vel_norm * std::sin(diff_yaw)); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) @@ -517,7 +518,31 @@ void DynamicAvoidanceModule::updateTargetObjects() ? isLeft(prev_module_path->points, future_obj_pose->position) : is_object_left; - // 2.g. calculate longitudinal and lateral offset to avoid + // 2.g. check if the ego is not ahead of the object. + const double signed_dist_ego_to_obj = [&]() { + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); + const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + prev_module_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + if (0 < lon_offset_ego_to_obj) { + return std::max( + 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + + lat_lon_offset.min_lon_offset); + } + return std::min( + 0.0, lon_offset_ego_to_obj + planner_data_->parameters.rear_overhang + + lat_lon_offset.max_lon_offset); + }(); + if (signed_dist_ego_to_obj < 0) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since distance from ego to object (%f) is less " + "than 0.", + obj_uuid.c_str(), signed_dist_ego_to_obj); + continue; + } + + // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by + // "object_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 270e2064228af..072c1c62ae3d2 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -332,10 +332,9 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra for (const auto & [uuid, debug_data] : debug_data) { LaneChangeDebugMsg debug_msg; debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; + debug_msg.allow_lane_change = debug_data.is_safe; debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; + debug_msg.failed_reason = debug_data.unsafe_reason; debug_msg.velocity = std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 31aab9da87e2a..27f44433c6d4e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -27,122 +27,114 @@ namespace behavior_path_planner using route_handler::Direction; using utils::convertToSnakeCase; -namespace -{ -template -T get_parameter(rclcpp::Node * node, const std::string & ns) -{ - if (node->has_parameter(ns)) { - return node->get_parameter(ns).get_value(); - } - - return node->declare_parameter(ns); -} -} // namespace - LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const Direction direction, const LaneChangeModuleType type) : SceneModuleManagerInterface(node, name, config, {""}), direction_{direction}, type_{type} { + using tier4_autoware_utils::getOrDeclareParameter; + LaneChangeParameters p{}; const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.backward_lane_length = get_parameter(node, parameter("backward_lane_length")); + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.prediction_time_resolution = - get_parameter(node, parameter("prediction_time_resolution")); + getOrDeclareParameter(*node, parameter("prediction_time_resolution")); p.longitudinal_acc_sampling_num = - get_parameter(node, parameter("longitudinal_acceleration_sampling_num")); + getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); p.lateral_acc_sampling_num = - get_parameter(node, parameter("lateral_acceleration_sampling_num")); + getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); // parked vehicle detection p.object_check_min_road_shoulder_width = - get_parameter(node, parameter("object_check_min_road_shoulder_width")); + getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); p.object_shiftable_ratio_threshold = - get_parameter(node, parameter("object_shiftable_ratio_threshold")); + getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); // turn signal p.min_length_for_turn_signal_activation = - get_parameter(node, parameter("min_length_for_turn_signal_activation")); + getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); p.length_ratio_for_turn_signal_deactivation = - get_parameter(node, parameter("length_ratio_for_turn_signal_deactivation")); + getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); // acceleration - p.min_longitudinal_acc = get_parameter(node, parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = get_parameter(node, parameter("max_longitudinal_acc")); + p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check p.enable_prepare_segment_collision_check = - get_parameter(node, parameter("enable_prepare_segment_collision_check")); - p.prepare_segment_ignore_object_velocity_thresh = - get_parameter(node, parameter("prepare_segment_ignore_object_velocity_thresh")); + getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( + *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = - get_parameter(node, parameter("check_objects_on_current_lanes")); + getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); p.check_objects_on_other_lanes = - get_parameter(node, parameter("check_objects_on_other_lanes")); - p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); - - p.rss_params.longitudinal_distance_min_threshold = - get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_velocity_delta_time = - get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); + p.use_all_predicted_path = + getOrDeclareParameter(*node, parameter("use_all_predicted_path")); + + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.longitudinal_velocity_delta_time")); p.rss_params.front_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_front_deceleration")); + getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); p.rss_params.rear_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); p.rss_params.rear_vehicle_reaction_time = - get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); p.rss_params.rear_vehicle_safety_time_margin = - get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); p.rss_params.lateral_distance_max_threshold = - get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); - - p.rss_params_for_abort.longitudinal_distance_min_threshold = - get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); - p.rss_params_for_abort.longitudinal_velocity_delta_time = - get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params_for_abort.front_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.expected_rear_deceleration_for_abort")); p.rss_params_for_abort.rear_vehicle_reaction_time = - get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); p.rss_params_for_abort.rear_vehicle_safety_time_margin = - get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); p.rss_params_for_abort.lateral_distance_max_threshold = - get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); // target object { std::string ns = "lane_change.target_object."; - p.check_car = get_parameter(node, ns + "car"); - p.check_truck = get_parameter(node, ns + "truck"); - p.check_bus = get_parameter(node, ns + "bus"); - p.check_trailer = get_parameter(node, ns + "trailer"); - p.check_unknown = get_parameter(node, ns + "unknown"); - p.check_bicycle = get_parameter(node, ns + "bicycle"); - p.check_motorcycle = get_parameter(node, ns + "motorcycle"); - p.check_pedestrian = get_parameter(node, ns + "pedestrian"); + p.check_car = getOrDeclareParameter(*node, ns + "car"); + p.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.check_motorcycle = getOrDeclareParameter(*node, ns + "motorcycle"); + p.check_pedestrian = getOrDeclareParameter(*node, ns + "pedestrian"); } // lane change cancel p.cancel.enable_on_prepare_phase = - get_parameter(node, parameter("cancel.enable_on_prepare_phase")); + getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); p.cancel.enable_on_lane_changing_phase = - get_parameter(node, parameter("cancel.enable_on_lane_changing_phase")); - p.cancel.delta_time = get_parameter(node, parameter("cancel.delta_time")); - p.cancel.duration = get_parameter(node, parameter("cancel.duration")); - p.cancel.max_lateral_jerk = get_parameter(node, parameter("cancel.max_lateral_jerk")); - p.cancel.overhang_tolerance = get_parameter(node, parameter("cancel.overhang_tolerance")); + getOrDeclareParameter(*node, parameter("cancel.enable_on_lane_changing_phase")); + p.cancel.delta_time = getOrDeclareParameter(*node, parameter("cancel.delta_time")); + p.cancel.duration = getOrDeclareParameter(*node, parameter("cancel.duration")); + p.cancel.max_lateral_jerk = + getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); + p.cancel.overhang_tolerance = + getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); p.finish_judge_lateral_threshold = - get_parameter(node, parameter("finish_judge_lateral_threshold")); + getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); // debug marker - p.publish_debug_marker = get_parameter(node, parameter("publish_debug_marker")); + p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); // validation of parameters if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { @@ -196,6 +188,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( node, name, config, Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; rtc_interface_ptr_map_.clear(); const std::vector rtc_types = {"left", "right"}; @@ -211,34 +204,39 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( { std::string ns = "avoidance_by_lane_change."; p.execute_object_longitudinal_margin = - get_parameter(node, ns + "execute_object_longitudinal_margin"); + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); p.execute_only_when_lane_change_finish_before_object = - get_parameter(node, ns + "execute_only_when_lane_change_finish_before_object"); + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); } // general params { std::string ns = "avoidance."; p.resample_interval_for_planning = - get_parameter(node, ns + "resample_interval_for_planning"); + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - get_parameter(node, ns + "resample_interval_for_output"); + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); + getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); - param.execute_num = get_parameter(node, ns + "execute_num"); - param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); - param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); - param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.is_target = getOrDeclareParameter(*node, ns + "is_target"); + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); return param; }; @@ -255,41 +253,43 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = - get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( + *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); + getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_forward_distance = - get_parameter(node, ns + "object_check_forward_distance"); + getOrDeclareParameter(*node, ns + "object_check_forward_distance"); p.object_check_backward_distance = - get_parameter(node, ns + "object_check_backward_distance"); - p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + getOrDeclareParameter(*node, ns + "object_check_backward_distance"); + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = - get_parameter(node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = - get_parameter(node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = - get_parameter(node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } // safety check { std::string ns = "avoidance.safety_check."; p.hysteresis_factor_expand_rate = - get_parameter(node, ns + "hysteresis_factor_expand_rate"); + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 251720f4202d5..cf3312901b1d9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1361,17 +1361,17 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug); }; - const auto updateDebugInfo = - [&debug_data](std::pair & obj, bool is_allowed) { - const auto & key = obj.first; - auto & element = obj.second; - element.allow_lane_change = is_allowed; - if (debug_data.find(key) != debug_data.end()) { - debug_data[key] = element; - } else { - debug_data.insert(obj); - } - }; + const auto updateDebugInfo = [&debug_data]( + std::pair & obj, bool is_safe) { + const auto & key = obj.first; + auto & element = obj.second; + element.is_safe = is_safe; + if (debug_data.find(key) != debug_data.end()) { + debug_data[key] = element; + } else { + debug_data.insert(obj); + } + }; for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index c1a883d7a9b82..856c3220165e7 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1436,7 +1436,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const bool is_object_front, const std::shared_ptr & parameters) { if (path.points.empty()) { return {}; @@ -1445,7 +1445,8 @@ std::vector convertToPredictedPath( const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object + : parameters->time_horizon_for_rear_object; const auto & time_resolution = parameters->safety_check_time_resolution; const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); @@ -1477,7 +1478,8 @@ ExtendedPredictedObject transform( const auto & obj_velocity_norm = std::hypot( extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = + std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); const auto & time_resolution = parameters->safety_check_time_resolution; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 579862de93865..2939ca29c0a40 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -62,8 +81,8 @@ Polygon2d createExtendedPolygon( const double lat_offset = width / 2.0 + lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = lat_offset; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = lat_offset; } const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); @@ -113,8 +132,8 @@ Polygon2d createExtendedPolygon( const double right_lat_offset = min_y - lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; } const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); @@ -227,7 +246,7 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( - const PathWithLaneId & planned_path, + [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, @@ -261,19 +280,18 @@ bool checkCollision( debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; } // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { - debug.failed_reason = "overlap_polygon"; + debug.unsafe_reason = "overlap_polygon"; return false; } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); @@ -299,15 +317,15 @@ bool checkCollision( { debug.rss_longitudinal = rss_dist; - debug.ego_to_obj_margin = min_lon_length; - debug.ego_polygon = extended_ego_polygon; - debug.obj_polygon = extended_obj_polygon; + debug.inter_vehicle_distance = min_lon_length; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_obj_polygon = extended_obj_polygon; debug.is_front = is_object_front; } // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.failed_reason = "overlap_extended_polygon"; + debug.unsafe_reason = "overlap_extended_polygon"; return false; } } diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 09fef7a496b09..d64f1ebc39a51 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,6 +16,7 @@ #include #include +#include #include @@ -27,22 +28,26 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); - planner_param_.backward_length = node.declare_parameter(ns + ".backward_length"); + planner_param_.use_pass_judge_line = + getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + planner_param_.backward_length = getOrDeclareParameter(node, ns + ".backward_length"); planner_param_.ignore_width_from_center_line = - node.declare_parameter(ns + ".ignore_width_from_center_line"); + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); planner_param_.max_future_movement_time = - node.declare_parameter(ns + ".max_future_movement_time"); - planner_param_.threshold_yaw_diff = node.declare_parameter(ns + ".threshold_yaw_diff"); + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + planner_param_.threshold_yaw_diff = + getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = - node.declare_parameter(ns + ".adjacent_extend_width"); + getOrDeclareParameter(node, ns + ".adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 96e4e873ce111..30a1a81a1b50a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -26,86 +27,93 @@ namespace behavior_velocity_planner { using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".common.enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); // for crosswalk parameters auto & cp = crosswalk_planner_param_; - cp.show_processing_time = node.declare_parameter(ns + ".common.show_processing_time"); + cp.show_processing_time = getOrDeclareParameter(node, ns + ".common.show_processing_time"); // param for input data cp.traffic_light_state_timeout = - node.declare_parameter(ns + ".common.traffic_light_state_timeout"); + getOrDeclareParameter(node, ns + ".common.traffic_light_state_timeout"); // param for stop position cp.stop_distance_from_crosswalk = - node.declare_parameter(ns + ".stop_position.stop_distance_from_crosswalk"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); cp.stop_distance_from_object = - node.declare_parameter(ns + ".stop_position.stop_distance_from_object"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); cp.far_object_threshold = - node.declare_parameter(ns + ".stop_position.far_object_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = - node.declare_parameter(ns + ".stop_position.stop_position_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); // param for ego velocity cp.min_slow_down_velocity = - node.declare_parameter(ns + ".slow_down.min_slow_down_velocity"); - cp.max_slow_down_jerk = node.declare_parameter(ns + ".slow_down.max_slow_down_jerk"); - cp.max_slow_down_accel = node.declare_parameter(ns + ".slow_down.max_slow_down_accel"); - cp.no_relax_velocity = node.declare_parameter(ns + ".slow_down.no_relax_velocity"); + getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); + cp.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_jerk"); + cp.max_slow_down_accel = + getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_accel"); + cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle cp.stuck_vehicle_velocity = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_velocity"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = - node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); - cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); - cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); - cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic cp.ego_pass_first_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); cp.ego_pass_first_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); cp.ego_pass_first_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); cp.ego_pass_later_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); cp.max_offset_to_crosswalk_for_yield = - node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); cp.stop_object_velocity = - node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); - cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); + getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); + cp.min_object_velocity = + getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = - node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = - node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = - node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); + getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = - node.declare_parameter(ns + ".pass_judge.timeout_ego_stop_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); // param for target area & object cp.crosswalk_attention_range = - node.declare_parameter(ns + ".object_filtering.crosswalk_attention_range"); - cp.look_unknown = node.declare_parameter(ns + ".object_filtering.target_object.unknown"); - cp.look_bicycle = node.declare_parameter(ns + ".object_filtering.target_object.bicycle"); + getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.look_unknown = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); + cp.look_bicycle = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.bicycle"); cp.look_motorcycle = - node.declare_parameter(ns + ".object_filtering.target_object.motorcycle"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = - node.declare_parameter(ns + ".object_filtering.target_object.pedestrian"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 99888d0789559..16d32b8a49964 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include @@ -29,22 +30,24 @@ namespace behavior_velocity_planner { using lanelet::autoware::DetectionArea; +using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); - planner_param_.use_dead_line = node.declare_parameter(ns + ".use_dead_line"); - planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line"); - planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time"); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.use_dead_line = getOrDeclareParameter(node, ns + ".use_dead_line"); + planner_param_.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + planner_param_.use_pass_judge_line = + getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + planner_param_.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); planner_param_.hold_stop_margin_distance = - node.declare_parameter(ns + ".hold_stop_margin_distance"); + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); planner_param_.distance_to_judge_over_stop_line = - node.declare_parameter(ns + ".distance_to_judge_over_stop_line"); + getOrDeclareParameter(node, ns + ".distance_to_judge_over_stop_line"); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6a2b7200abb2e..9d6359ebe9b9b 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -29,94 +30,99 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc.intersection")), + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - node.declare_parameter( - std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getOrDeclareParameter( + node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = - node.declare_parameter(ns + ".common.attention_area_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = - node.declare_parameter(ns + ".common.attention_area_length"); + getOrDeclareParameter(node, ns + ".common.attention_area_length"); ip.common.attention_area_angle_thr = - node.declare_parameter(ns + ".common.attention_area_angle_threshold"); - ip.common.stop_line_margin = node.declare_parameter(ns + ".common.stop_line_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.stop_line_margin = getOrDeclareParameter(node, ns + ".common.stop_line_margin"); ip.common.intersection_velocity = - node.declare_parameter(ns + ".common.intersection_velocity"); + getOrDeclareParameter(node, ns + ".common.intersection_velocity"); ip.common.intersection_max_acc = - node.declare_parameter(ns + ".common.intersection_max_accel"); + getOrDeclareParameter(node, ns + ".common.intersection_max_accel"); ip.common.stop_overshoot_margin = - node.declare_parameter(ns + ".common.stop_overshoot_margin"); + getOrDeclareParameter(node, ns + ".common.stop_overshoot_margin"); ip.common.use_intersection_area = - node.declare_parameter(ns + ".common.use_intersection_area"); + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); ip.common.path_interpolation_ds = - node.declare_parameter(ns + ".common.path_interpolation_ds"); + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = - node.declare_parameter(ns + ".common.consider_wrong_direction_vehicle"); + getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); ip.stuck_vehicle.use_stuck_stopline = - node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_ignore_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* ip.stuck_vehicle.assumed_front_car_decel = - node.declare_parameter(ns + ".stuck_vehicle.assumed_front_car_decel"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.assumed_front_car_decel"); ip.stuck_vehicle.enable_front_car_decel_prediction = - node.declare_parameter(ns + ".stuck_vehicle.enable_front_car_decel_prediction"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_front_car_decel_prediction"); */ ip.stuck_vehicle.timeout_private_area = - node.declare_parameter(ns + ".stuck_vehicle.timeout_private_area"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); ip.collision_detection.min_predicted_path_confidence = - node.declare_parameter(ns + ".collision_detection.min_predicted_path_confidence"); + getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); ip.collision_detection.minimum_ego_predicted_velocity = - node.declare_parameter(ns + ".collision_detection.minimum_ego_predicted_velocity"); + getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = - node.declare_parameter(ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_end_margin_time"); + getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); + ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.normal.collision_start_margin_time"); + ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.normal.collision_end_margin_time"); + ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.relaxed.collision_start_margin_time"); + ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.relaxed.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = - node.declare_parameter(ns + ".collision_detection.keep_detection_vel_thr"); + getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); - ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = - node.declare_parameter(ns + ".occlusion.occlusion_attention_area_length"); - ip.occlusion.enable_creeping = node.declare_parameter(ns + ".occlusion.enable_creeping"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.enable_creeping = + getOrDeclareParameter(node, ns + ".occlusion.enable_creeping"); ip.occlusion.occlusion_creep_velocity = - node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); - ip.occlusion.peeking_offset = node.declare_parameter(ns + ".occlusion.peeking_offset"); - ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); - ip.occlusion.occupied_min = node.declare_parameter(ns + ".occlusion.occupied_min"); - ip.occlusion.do_dp = node.declare_parameter(ns + ".occlusion.do_dp"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_creep_velocity"); + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.do_dp = getOrDeclareParameter(node, ns + ".occlusion.do_dp"); ip.occlusion.before_creep_stop_time = - node.declare_parameter(ns + ".occlusion.before_creep_stop_time"); + getOrDeclareParameter(node, ns + ".occlusion.before_creep_stop_time"); ip.occlusion.min_vehicle_brake_for_rss = - node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); + getOrDeclareParameter(node, ns + ".occlusion.min_vehicle_brake_for_rss"); ip.occlusion.max_vehicle_velocity_for_rss = - node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); - ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); + getOrDeclareParameter(node, ns + ".occlusion.max_vehicle_velocity_for_rss"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); ip.occlusion.possible_object_bbox = - node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = - node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); } void IntersectionModuleManager::launchNewModules( @@ -263,10 +269,10 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node { const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); + mp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); } diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index c7ff0f496f551..60d659497e55a 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include @@ -29,23 +30,24 @@ namespace behavior_velocity_planner { using lanelet::autoware::NoStoppingArea; +using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; const auto & vi = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - pp.state_clear_time = node.declare_parameter(ns + ".state_clear_time"); - pp.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle_vel_thr"); - pp.stop_margin = node.declare_parameter(ns + ".stop_margin"); - pp.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - pp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); - pp.detection_area_length = node.declare_parameter(ns + ".detection_area_length"); + pp.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); + pp.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle_vel_thr"); + pp.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + pp.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + pp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + pp.detection_area_length = getOrDeclareParameter(node, ns + ".detection_area_length"); pp.stuck_vehicle_front_margin = - node.declare_parameter(ns + ".stuck_vehicle_front_margin"); + getOrDeclareParameter(node, ns + ".stuck_vehicle_front_margin"); pp.path_expand_width = vi.vehicle_width_m * 0.5; } diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index 5b1b3d489041f..f257012c05519 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -17,6 +17,7 @@ #include "scene_occlusion_spot.hpp" #include +#include #include @@ -28,6 +29,7 @@ namespace behavior_velocity_planner { using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; +using tier4_autoware_utils::getOrDeclareParameter; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -36,7 +38,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) auto & pp = planner_param_; // for detection type { - const std::string method = node.declare_parameter(ns + ".detection_method"); + const std::string method = getOrDeclareParameter(node, ns + ".detection_method"); if (method == "occupancy_grid") { // module id 0 pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID; module_id_ = DETECTION_METHOD::OCCUPANCY_GRID; @@ -50,7 +52,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } // for passable judgement { - const std::string pass_judge = node.declare_parameter(ns + ".pass_judge"); + const std::string pass_judge = getOrDeclareParameter(node, ns + ".pass_judge"); if (pass_judge == "current_velocity") { pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY; } else if (pass_judge == "smooth_velocity") { @@ -60,48 +62,52 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; } } - pp.use_object_info = node.declare_parameter(ns + ".use_object_info"); - pp.use_moving_object_ray_cast = node.declare_parameter(ns + ".use_moving_object_ray_cast"); - pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet"); - pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel"); - pp.pedestrian_radius = node.declare_parameter(ns + ".pedestrian_radius"); + pp.use_object_info = getOrDeclareParameter(node, ns + ".use_object_info"); + pp.use_moving_object_ray_cast = + getOrDeclareParameter(node, ns + ".use_moving_object_ray_cast"); + pp.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + pp.pedestrian_vel = getOrDeclareParameter(node, ns + ".pedestrian_vel"); + pp.pedestrian_radius = getOrDeclareParameter(node, ns + ".pedestrian_radius"); // debug - pp.is_show_occlusion = node.declare_parameter(ns + ".debug.is_show_occlusion"); - pp.is_show_cv_window = node.declare_parameter(ns + ".debug.is_show_cv_window"); - pp.is_show_processing_time = node.declare_parameter(ns + ".debug.is_show_processing_time"); + pp.is_show_occlusion = getOrDeclareParameter(node, ns + ".debug.is_show_occlusion"); + pp.is_show_cv_window = getOrDeclareParameter(node, ns + ".debug.is_show_cv_window"); + pp.is_show_processing_time = + getOrDeclareParameter(node, ns + ".debug.is_show_processing_time"); // threshold pp.detection_area_offset = - node.declare_parameter(ns + ".threshold.detection_area_offset"); + getOrDeclareParameter(node, ns + ".threshold.detection_area_offset"); pp.detection_area_length = - node.declare_parameter(ns + ".threshold.detection_area_length"); - pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel"); - pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance"); - pp.dist_thr = node.declare_parameter(ns + ".threshold.search_dist"); - pp.angle_thr = node.declare_parameter(ns + ".threshold.search_angle"); + getOrDeclareParameter(node, ns + ".threshold.detection_area_length"); + pp.stuck_vehicle_vel = getOrDeclareParameter(node, ns + ".threshold.stuck_vehicle_vel"); + pp.lateral_distance_thr = getOrDeclareParameter(node, ns + ".threshold.lateral_distance"); + pp.dist_thr = getOrDeclareParameter(node, ns + ".threshold.search_dist"); + pp.angle_thr = getOrDeclareParameter(node, ns + ".threshold.search_angle"); // ego additional velocity config - pp.v.safety_ratio = node.declare_parameter(ns + ".motion.safety_ratio"); - pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin"); - pp.v.max_slow_down_jerk = node.declare_parameter(ns + ".motion.max_slow_down_jerk"); - pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel"); - pp.v.non_effective_jerk = node.declare_parameter(ns + ".motion.non_effective_jerk"); + pp.v.safety_ratio = getOrDeclareParameter(node, ns + ".motion.safety_ratio"); + pp.v.safe_margin = getOrDeclareParameter(node, ns + ".motion.safe_margin"); + pp.v.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".motion.max_slow_down_jerk"); + pp.v.max_slow_down_accel = + getOrDeclareParameter(node, ns + ".motion.max_slow_down_accel"); + pp.v.non_effective_jerk = getOrDeclareParameter(node, ns + ".motion.non_effective_jerk"); pp.v.non_effective_accel = - node.declare_parameter(ns + ".motion.non_effective_acceleration"); - pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity"); + getOrDeclareParameter(node, ns + ".motion.non_effective_acceleration"); + pp.v.min_allowed_velocity = + getOrDeclareParameter(node, ns + ".motion.min_allowed_velocity"); // detection_area param pp.detection_area.min_occlusion_spot_size = - node.declare_parameter(ns + ".detection_area.min_occlusion_spot_size"); + getOrDeclareParameter(node, ns + ".detection_area.min_occlusion_spot_size"); pp.detection_area.min_longitudinal_offset = - node.declare_parameter(ns + ".detection_area.min_longitudinal_offset"); + getOrDeclareParameter(node, ns + ".detection_area.min_longitudinal_offset"); pp.detection_area.max_lateral_distance = - node.declare_parameter(ns + ".detection_area.max_lateral_distance"); + getOrDeclareParameter(node, ns + ".detection_area.max_lateral_distance"); pp.detection_area.slice_length = - node.declare_parameter(ns + ".detection_area.slice_length"); + getOrDeclareParameter(node, ns + ".detection_area.slice_length"); // occupancy grid param - pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max"); - pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min"); + pp.grid.free_space_max = getOrDeclareParameter(node, ns + ".grid.free_space_max"); + pp.grid.occupied_min = getOrDeclareParameter(node, ns + ".grid.occupied_min"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 22fb29110466a..075738f07d62e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -17,6 +17,7 @@ #include "scene_out_of_lane.hpp" #include +#include #include @@ -26,42 +27,47 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) { const std::string ns(getModuleName()); auto & pp = planner_param_; - pp.mode = node.declare_parameter(ns + ".mode"); + pp.mode = getOrDeclareParameter(node, ns + ".mode"); pp.skip_if_already_overlapping = - node.declare_parameter(ns + ".skip_if_already_overlapping"); + getOrDeclareParameter(node, ns + ".skip_if_already_overlapping"); - pp.time_threshold = node.declare_parameter(ns + ".threshold.time_threshold"); - pp.intervals_ego_buffer = node.declare_parameter(ns + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = node.declare_parameter(ns + ".intervals.objects_time_buffer"); - pp.ttc_threshold = node.declare_parameter(ns + ".ttc.threshold"); + pp.time_threshold = getOrDeclareParameter(node, ns + ".threshold.time_threshold"); + pp.intervals_ego_buffer = getOrDeclareParameter(node, ns + ".intervals.ego_time_buffer"); + pp.intervals_obj_buffer = + getOrDeclareParameter(node, ns + ".intervals.objects_time_buffer"); + pp.ttc_threshold = getOrDeclareParameter(node, ns + ".ttc.threshold"); - pp.objects_min_vel = node.declare_parameter(ns + ".objects.minimum_velocity"); + pp.objects_min_vel = getOrDeclareParameter(node, ns + ".objects.minimum_velocity"); pp.objects_use_predicted_paths = - node.declare_parameter(ns + ".objects.use_predicted_paths"); + getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); pp.objects_min_confidence = - node.declare_parameter(ns + ".objects.predicted_path_min_confidence"); + getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); - pp.overlap_min_dist = node.declare_parameter(ns + ".overlap.minimum_distance"); - pp.overlap_extra_length = node.declare_parameter(ns + ".overlap.extra_length"); + pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); + pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); - pp.skip_if_over_max_decel = node.declare_parameter(ns + ".action.skip_if_over_max_decel"); - pp.strict = node.declare_parameter(ns + ".action.strict"); - pp.dist_buffer = node.declare_parameter(ns + ".action.distance_buffer"); - pp.slow_velocity = node.declare_parameter(ns + ".action.slowdown.velocity"); + pp.skip_if_over_max_decel = + getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); + pp.strict = getOrDeclareParameter(node, ns + ".action.strict"); + pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); + pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = - node.declare_parameter(ns + ".action.slowdown.distance_threshold"); - pp.stop_dist_threshold = node.declare_parameter(ns + ".action.stop.distance_threshold"); + getOrDeclareParameter(node, ns + ".action.slowdown.distance_threshold"); + pp.stop_dist_threshold = + getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); - pp.extra_front_offset = node.declare_parameter(ns + ".ego.extra_front_offset"); - pp.extra_rear_offset = node.declare_parameter(ns + ".ego.extra_rear_offset"); - pp.extra_left_offset = node.declare_parameter(ns + ".ego.extra_left_offset"); - pp.extra_right_offset = node.declare_parameter(ns + ".ego.extra_right_offset"); + pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); + pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); + pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); + pp.extra_right_offset = getOrDeclareParameter(node, ns + ".ego.extra_right_offset"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 8887f0749a2d5..b262bf00cb57a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -14,14 +14,14 @@ #include "manager.hpp" +#include + #include #include namespace behavior_velocity_planner { -namespace -{ -} // namespace +using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -48,82 +48,75 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) { auto & p = planner_param_.common; - p.normal_min_jerk = node.has_parameter("normal.min_jerk") - ? node.get_parameter("normal.min_jerk").get_value() - : node.declare_parameter("normal.min_jerk"); - p.normal_min_acc = node.has_parameter("normal.min_acc") - ? node.get_parameter("normal.min_acc").get_value() - : node.declare_parameter("normal.min_acc"); - p.limit_min_jerk = node.has_parameter("limit.min_jerk") - ? node.get_parameter("limit.min_jerk").get_value() - : node.declare_parameter("limit.min_jerk"); - p.limit_min_acc = node.has_parameter("limit.min_acc") - ? node.get_parameter("limit.min_acc").get_value() - : node.declare_parameter("limit.min_acc"); + p.normal_min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + p.normal_min_acc = getOrDeclareParameter(node, "normal.min_acc"); + p.limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + p.limit_min_acc = getOrDeclareParameter(node, "limit.min_acc"); } { auto & p = planner_param_.run_out; - p.detection_method = node.declare_parameter(ns + ".detection_method"); - p.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet"); - p.specify_decel_jerk = node.declare_parameter(ns + ".specify_decel_jerk"); - p.stop_margin = node.declare_parameter(ns + ".stop_margin"); - p.passing_margin = node.declare_parameter(ns + ".passing_margin"); - p.deceleration_jerk = node.declare_parameter(ns + ".deceleration_jerk"); - p.detection_distance = node.declare_parameter(ns + ".detection_distance"); - p.detection_span = node.declare_parameter(ns + ".detection_span"); - p.min_vel_ego_kmph = node.declare_parameter(ns + ".min_vel_ego_kmph"); + p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); + p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); + p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); + p.deceleration_jerk = getOrDeclareParameter(node, ns + ".deceleration_jerk"); + p.detection_distance = getOrDeclareParameter(node, ns + ".detection_distance"); + p.detection_span = getOrDeclareParameter(node, ns + ".detection_span"); + p.min_vel_ego_kmph = getOrDeclareParameter(node, ns + ".min_vel_ego_kmph"); } { auto & p = planner_param_.detection_area; const std::string ns_da = ns + ".detection_area"; - p.margin_ahead = node.declare_parameter(ns_da + ".margin_ahead"); - p.margin_behind = node.declare_parameter(ns_da + ".margin_behind"); + p.margin_ahead = getOrDeclareParameter(node, ns_da + ".margin_ahead"); + p.margin_behind = getOrDeclareParameter(node, ns_da + ".margin_behind"); } { auto & p = planner_param_.mandatory_area; const std::string ns_da = ns + ".mandatory_area"; - p.decel_jerk = node.declare_parameter(ns_da + ".decel_jerk"); + p.decel_jerk = getOrDeclareParameter(node, ns_da + ".decel_jerk"); } { auto & p = planner_param_.dynamic_obstacle; const std::string ns_do = ns + ".dynamic_obstacle"; - p.use_mandatory_area = node.declare_parameter(ns_do + ".use_mandatory_area"); - p.min_vel_kmph = node.declare_parameter(ns_do + ".min_vel_kmph"); - p.max_vel_kmph = node.declare_parameter(ns_do + ".max_vel_kmph"); - p.diameter = node.declare_parameter(ns_do + ".diameter"); - p.height = node.declare_parameter(ns_do + ".height"); - p.max_prediction_time = node.declare_parameter(ns_do + ".max_prediction_time"); - p.time_step = node.declare_parameter(ns_do + ".time_step"); - p.points_interval = node.declare_parameter(ns_do + ".points_interval"); + p.use_mandatory_area = getOrDeclareParameter(node, ns_do + ".use_mandatory_area"); + p.min_vel_kmph = getOrDeclareParameter(node, ns_do + ".min_vel_kmph"); + p.max_vel_kmph = getOrDeclareParameter(node, ns_do + ".max_vel_kmph"); + p.diameter = getOrDeclareParameter(node, ns_do + ".diameter"); + p.height = getOrDeclareParameter(node, ns_do + ".height"); + p.max_prediction_time = getOrDeclareParameter(node, ns_do + ".max_prediction_time"); + p.time_step = getOrDeclareParameter(node, ns_do + ".time_step"); + p.points_interval = getOrDeclareParameter(node, ns_do + ".points_interval"); } { auto & p = planner_param_.approaching; const std::string ns_a = ns + ".approaching"; - p.enable = node.declare_parameter(ns_a + ".enable"); - p.margin = node.declare_parameter(ns_a + ".margin"); - p.limit_vel_kmph = node.declare_parameter(ns_a + ".limit_vel_kmph"); + p.enable = getOrDeclareParameter(node, ns_a + ".enable"); + p.margin = getOrDeclareParameter(node, ns_a + ".margin"); + p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); } { auto & p = planner_param_.state_param; const std::string ns_s = ns + ".state"; - p.stop_thresh = node.declare_parameter(ns_s + ".stop_thresh"); - p.stop_time_thresh = node.declare_parameter(ns_s + ".stop_time_thresh"); - p.disable_approach_dist = node.declare_parameter(ns_s + ".disable_approach_dist"); - p.keep_approach_duration = node.declare_parameter(ns_s + ".keep_approach_duration"); + p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); + p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); + p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); + p.keep_approach_duration = + getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); } { auto & p = planner_param_.slow_down_limit; const std::string ns_m = ns + ".slow_down_limit"; - p.enable = node.declare_parameter(ns_m + ".enable"); - p.max_jerk = node.declare_parameter(ns_m + ".max_jerk"); - p.max_acc = node.declare_parameter(ns_m + ".max_acc"); + p.enable = getOrDeclareParameter(node, ns_m + ".enable"); + p.max_jerk = getOrDeclareParameter(node, ns_m + ".max_jerk"); + p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); } debug_ptr_ = std::make_shared(node); diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index e502963e60b47..dacc99d55b68e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include @@ -28,25 +29,26 @@ namespace behavior_velocity_planner { using lanelet::autoware::SpeedBump; +using tier4_autoware_utils::getOrDeclareParameter; SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { std::string ns(getModuleName()); - planner_param_.slow_start_margin = node.declare_parameter(ns + ".slow_start_margin"); - planner_param_.slow_end_margin = node.declare_parameter(ns + ".slow_end_margin"); - planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info"); + planner_param_.slow_start_margin = getOrDeclareParameter(node, ns + ".slow_start_margin"); + planner_param_.slow_end_margin = getOrDeclareParameter(node, ns + ".slow_end_margin"); + planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); // limits for speed bump height and slow down speed ns += ".speed_calculation"; planner_param_.speed_calculation_min_height = - static_cast(node.declare_parameter(ns + ".min_height")); + static_cast(getOrDeclareParameter(node, ns + ".min_height")); planner_param_.speed_calculation_max_height = - static_cast(node.declare_parameter(ns + ".max_height")); + static_cast(getOrDeclareParameter(node, ns + ".max_height")); planner_param_.speed_calculation_min_speed = - static_cast(node.declare_parameter(ns + ".min_speed")); + static_cast(getOrDeclareParameter(node, ns + ".min_speed")); planner_param_.speed_calculation_max_speed = - static_cast(node.declare_parameter(ns + ".max_speed")); + static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } void SpeedBumpModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 390550fc9e563..5c9f0fd39c644 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" + #include #include #include @@ -22,20 +24,22 @@ namespace behavior_velocity_planner { using lanelet::TrafficSign; +using tier4_autoware_utils::getOrDeclareParameter; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); auto & p = planner_param_; - p.stop_margin = node.declare_parameter(ns + ".stop_margin"); - p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance"); - p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + p.hold_stop_margin_distance = + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); + p.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); p.use_initialization_stop_line_state = - node.declare_parameter(ns + ".use_initialization_stop_line_state"); + getOrDeclareParameter(node, ns + ".use_initialization_stop_line_state"); // debug p.show_stop_line_collision_check = - node.declare_parameter(ns + ".debug.show_stop_line_collision_check"); + getOrDeclareParameter(node, ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 5516710ff3d5d..1b0646f4bee00 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "manager.hpp" +#include + #include #include @@ -26,17 +28,19 @@ namespace behavior_velocity_planner { using lanelet::TrafficLight; +using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); - planner_param_.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout"); - planner_param_.enable_pass_judge = node.declare_parameter(ns + ".enable_pass_judge"); - planner_param_.yellow_lamp_period = node.declare_parameter(ns + ".yellow_lamp_period"); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); + planner_param_.yellow_lamp_period = + getOrDeclareParameter(node, ns + ".yellow_lamp_period"); pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index e5a2b0e43def2..3fe6c19a5f485 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -25,6 +26,7 @@ namespace behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; +using tier4_autoware_utils::getOrDeclareParameter; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -33,14 +35,15 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node { auto & p = planner_param_; - p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec"); - p.near_line_distance = node.declare_parameter(ns + ".near_line_distance"); - p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance"); - p.max_yaw_deviation_rad = - tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg")); + p.max_delay_sec = getOrDeclareParameter(node, ns + ".max_delay_sec"); + p.near_line_distance = getOrDeclareParameter(node, ns + ".near_line_distance"); + p.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + p.hold_stop_margin_distance = + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); + p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad( + getOrDeclareParameter(node, ns + ".max_yaw_deviation_deg")); p.check_timeout_after_stop_line = - node.declare_parameter(ns + ".check_timeout_after_stop_line"); + getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } } diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d0c1a47408929..d10ff52850752 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -26,6 +27,7 @@ namespace behavior_velocity_planner { using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -35,8 +37,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) // for walkway parameters auto & wp = walkway_planner_param_; wp.stop_distance_from_crosswalk = - node.declare_parameter(ns + ".stop_distance_from_crosswalk"); - wp.stop_duration = node.declare_parameter(ns + ".stop_duration"); + getOrDeclareParameter(node, ns + ".stop_distance_from_crosswalk"); + wp.stop_duration = getOrDeclareParameter(node, ns + ".stop_duration"); } void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 6eef2cc243ef5..3487e9515c48a 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,14 +27,14 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------ | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System | +| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 5c1f621626aa2..74c9734833232 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -17,7 +17,6 @@ #include "gnss_poser/gnss_stat.hpp" #include -#include #include #include #include @@ -56,27 +55,6 @@ double EllipsoidHeight2OrthometricHeight( } return OrthometricHeight; } -GNSSStat NavSatFix2LocalCartesianWGS84( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) -{ - GNSSStat local_cartesian; - - try { - GeographicLib::LocalCartesian localCartesian_origin( - nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, nav_sat_fix_origin_.altitude); - localCartesian_origin.Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - local_cartesian.x, local_cartesian.y, local_cartesian.z); - - local_cartesian.latitude = nav_sat_fix_msg.latitude; - local_cartesian.longitude = nav_sat_fix_msg.longitude; - local_cartesian.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert NavSatFix to LocalCartesian" << err.what()); - } - return local_cartesian; -} GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, int height_system) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index fa17bf0e6e232..d33bd022c3714 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,7 +18,7 @@ namespace gnss_poser { -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_WGS84 = 3, LOCAL_CARTESIAN_UTM = 4 }; +enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 }; struct GNSSStat { diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 3883a492358d6..8a0ffb8cab8c3 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,7 +13,7 @@ - + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7d244ce188eee..4f0723944913b 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -196,9 +196,6 @@ GNSSStat GNSSPoser::convert( } else if (coordinate_system == CoordinateSystem::MGRS) { gnss_stat = NavSatFix2MGRS( nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { - gnss_stat = - NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); } else { RCLCPP_ERROR_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 31e1727b1e034..8fda61f6f32ad 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -4,16 +4,44 @@ project(imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(gyro_bias_estimation_module SHARED + src/gyro_bias_estimation_module.cpp +) + ament_auto_add_library(imu_corrector_node SHARED src/imu_corrector_core.cpp - include/imu_corrector/imu_corrector_core.hpp ) +ament_auto_add_library(gyro_bias_estimator_node SHARED + src/gyro_bias_estimator.cpp +) + +target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module) + rclcpp_components_register_node(imu_corrector_node PLUGIN "imu_corrector::ImuCorrector" EXECUTABLE imu_corrector ) +rclcpp_components_register_node(gyro_bias_estimator_node + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator +) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_gyro_bias_estimation_module.cpp) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 8a00c4005ece2..b47ae1500f43b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -1,6 +1,6 @@ # imu_corrector -## Purpose +## imu_corrector `imu_corrector_node` is a node that correct imu data. @@ -19,8 +19,6 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. -## Inputs / Outputs - ### Input | Name | Type | Description | @@ -33,9 +31,7 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. | --------- | ----------------------- | ------------------ | | `~output` | `sensor_msgs::msg::Imu` | corrected imu data | -## Parameters - -### Core Parameters +### Parameters | Name | Type | Description | | ---------------------------- | ------ | ------------------------------------------------ | @@ -47,12 +43,33 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. | `angular_velocity_stddev_zz` | double | yaw rate standard deviation imu_link [rad/s] | | `acceleration_stddev` | double | acceleration standard deviation imu_link [m/s^2] | -## Assumptions / Known limits +## gyro_bias_estimator + +`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range. -## (Optional) Error detection and handling +Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped. -## (Optional) Performance characterization +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | -## (Optional) References/External links +### Output -## (Optional) Future extensions / Unimplemented parts +| Name | Type | Description | +| -------------------- | ------------------------------------ | ----------------------------- | +| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | + +### Parameters + +| Name | Type | Description | +| --------------------------- | ------ | ----------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | +| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | +| `data_num_threshold` | int | number of data used to calculate bias | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml new file mode 100644 index 0000000000000..d5868e1df416a --- /dev/null +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.0015 # [rad/s] + velocity_threshold: 0.0 # [m/s] + timestamp_threshold: 0.1 # [s] + data_num_threshold: 200 # [num] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml new file mode 100644 index 0000000000000..a25ce5f90ed27 --- /dev/null +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 07c6cfd9a570d..67cb5c1596040 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -20,6 +20,8 @@ tf2_ros tier4_autoware_utils + ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..2deb6088f6542 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -0,0 +1,71 @@ +// 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 "gyro_bias_estimation_module.hpp" + +namespace imu_corrector +{ +GyroBiasEstimationModule::GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold) +: velocity_threshold_(velocity_threshold), + timestamp_threshold_(timestamp_threshold), + data_num_threshold_(data_num_threshold), + is_stopped_(false) +{ +} + +void GyroBiasEstimationModule::update_gyro( + const double time, const geometry_msgs::msg::Vector3 & gyro) +{ + if (time - last_velocity_time_ > timestamp_threshold_) { + return; + } + if (!is_stopped_) { + return; + } + gyro_buffer_.push_back(gyro); + if (gyro_buffer_.size() > data_num_threshold_) { + gyro_buffer_.pop_front(); + } +} + +void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +{ + is_stopped_ = velocity <= velocity_threshold_; + last_velocity_time_ = time; +} + +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +{ + if (gyro_buffer_.size() < data_num_threshold_) { + throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + } + + geometry_msgs::msg::Vector3 bias; + bias.x = 0.0; + bias.y = 0.0; + bias.z = 0.0; + for (const auto & gyro : gyro_buffer_) { + bias.x += gyro.x; + bias.y += gyro.y; + bias.z += gyro.z; + } + bias.x /= gyro_buffer_.size(); + bias.y /= gyro_buffer_.size(); + bias.z /= gyro_buffer_.size(); + return bias; +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp new file mode 100644 index 0000000000000..6ebae942fff5d --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -0,0 +1,44 @@ +// 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 GYRO_BIAS_ESTIMATION_MODULE_HPP_ +#define GYRO_BIAS_ESTIMATION_MODULE_HPP_ + +#include + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModule +{ +public: + GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold); + geometry_msgs::msg::Vector3 get_bias() const; + void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); + void update_velocity(const double time, const double velocity); + +private: + const double velocity_threshold_; + const double timestamp_threshold_; + const size_t data_num_threshold_; + bool is_stopped_; + std::deque gyro_buffer_; + + double last_velocity_time_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp new file mode 100644 index 0000000000000..42795805f803e --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -0,0 +1,109 @@ +// 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 "gyro_bias_estimator.hpp" + +namespace imu_corrector +{ +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & node_options) +: Node("gyro_bias_validator", node_options), + gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), + angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), + angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), + angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + updater_(this), + gyro_bias_(std::nullopt) +{ + updater_.setHardwareID(get_name()); + updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + + const double velocity_threshold = declare_parameter("velocity_threshold"); + const double timestamp_threshold = declare_parameter("timestamp_threshold"); + const size_t data_num_threshold = + static_cast(declare_parameter("data_num_threshold")); + gyro_bias_estimation_module_ = std::make_unique( + velocity_threshold, timestamp_threshold, data_num_threshold); + + imu_sub_ = create_subscription( + "~/input/imu_raw", rclcpp::SensorDataQoS(), + [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); + twist_sub_ = create_subscription( + "~/input/twist", rclcpp::SensorDataQoS(), + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); + + gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); +} + +void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) +{ + // Update gyro data + gyro_bias_estimation_module_->update_gyro( + rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); + + // Estimate gyro bias + try { + gyro_bias_ = gyro_bias_estimation_module_->get_bias(); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + } + + // Publish results for debugging + if (gyro_bias_ != std::nullopt) { + Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); + gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); + } + + // Update diagnostics + updater_.force_update(); +} + +void GyroBiasEstimator::callback_twist( + const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + gyro_bias_estimation_module_->update_velocity( + rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); +} + +void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (gyro_bias_ == std::nullopt) { + stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); + } else { + // Validation + const bool is_bias_small_enough = + std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + stat.add("gyro_bias", "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.add( + "gyro_bias", + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " + "imu_corrector. You may also use the output of gyro_bias_estimator."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); + } + } +} + +} // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp new file mode 100644 index 0000000000000..37ca1d81dc8fa --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -0,0 +1,66 @@ +// 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 GYRO_BIAS_ESTIMATOR_HPP_ +#define GYRO_BIAS_ESTIMATOR_HPP_ + +#include "gyro_bias_estimation_module.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimator : public rclcpp::Node +{ +private: + using Imu = sensor_msgs::msg::Imu; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + using Vector3 = geometry_msgs::msg::Vector3; + +public: + explicit GyroBiasEstimator(const rclcpp::NodeOptions & node_options); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); + void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + + rclcpp::Publisher::SharedPtr gyro_bias_pub_; + + std::unique_ptr gyro_bias_estimation_module_; + + const double gyro_bias_threshold_; + const double angular_velocity_offset_x_; + const double angular_velocity_offset_y_; + const double angular_velocity_offset_z_; + + diagnostic_updater::Updater updater_; + + std::optional gyro_bias_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index edc82f6d56549..c31f4968f557b 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "imu_corrector/imu_corrector_core.hpp" +#include "imu_corrector_core.hpp" + +#include std::array transformCovariance(const std::array & cov) { diff --git a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp similarity index 92% rename from sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp rename to sensing/imu_corrector/src/imu_corrector_core.hpp index 1c20dc6977f61..c155288f3cb17 100644 --- a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -11,8 +11,8 @@ // 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 IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ -#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#ifndef IMU_CORRECTOR_CORE_HPP_ +#define IMU_CORRECTOR_CORE_HPP_ #include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -59,4 +59,4 @@ class ImuCorrector : public rclcpp::Node }; } // namespace imu_corrector -#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..a0da4d0e24e17 --- /dev/null +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -0,0 +1,65 @@ +// 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 "../src/gyro_bias_estimation_module.hpp" + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModuleTest : public ::testing::Test +{ +protected: + double velocity_threshold = 1.0; + double timestamp_threshold = 5.0; + size_t data_num_threshold = 10; + GyroBiasEstimationModule module = + GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); +}; + +TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); + ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); + ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) +{ + ASSERT_THROW(module.get_bias(), std::runtime_error); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_THROW(module.get_bias(), std::runtime_error); +} +} // namespace imu_corrector diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 61d5b2dde6103..43efdb2adfaf8 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -146,7 +146,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.twist.header = kinematic_state_msgs_->header; vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id; vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist; - if (map_projector_info_->type == "MGRS") { + if (map_projector_info_->projector_type == "MGRS") { lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position), map_projector_info_->mgrs_grid); vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; @@ -154,7 +154,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; - } else if (map_projector_info_->type == "LocalCartesianUTM") { + } else if (map_projector_info_->projector_type == "LocalCartesianUTM") { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; lanelet::Origin origin{position};