From 7a0f84ce6a03b7f7b0449dbd2dbcf2091f26aa68 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 14 Mar 2023 18:02:13 +0900 Subject: [PATCH] feat(probablisitic_occupancy_grid_map): add scan_frame option for gridmap generation (#3032) * add scan_frame and raytrace center Signed-off-by: yoshiri * add scan frame to laserscan based method Signed-off-by: yoshiri * update readme Signed-off-by: yoshiri * fix typo Signed-off-by: yoshiri * update laucher in perception_launch Signed-off-by: yoshiri * fix config and launch file Signed-off-by: yoshiri * fixed laserscan based launcher Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- ...serscan_based_occupancy_grid_map.launch.py | 64 ++++++++------- ...ntcloud_based_occupancy_grid_map.launch.py | 60 ++++++++------ .../launch/perception.launch.xml | 1 + .../CMakeLists.txt | 1 + .../README.md | 10 ++- ...erscan_based_occupancy_grid_map.param.yaml | 6 +- ...tcloud_based_occupancy_grid_map.param.yaml | 8 +- .../image/gridmap_frame_settings.drawio.svg | 80 +++++++++++++++++++ ...aserscan_based_occupancy_grid_map_node.hpp | 3 +- .../occupancy_grid_map.hpp | 2 +- ...intcloud_based_occupancy_grid_map_node.hpp | 3 +- ...serscan_based_occupancy_grid_map.launch.py | 79 +++++++++--------- ...ntcloud_based_occupancy_grid_map.launch.py | 64 +++++++-------- ...aserscan_based_occupancy_grid_map_node.cpp | 37 ++++++--- .../occupancy_grid_map.cpp | 57 ++++++++++--- ...intcloud_based_occupancy_grid_map_node.cpp | 24 ++++-- 16 files changed, 329 insertions(+), 170 deletions(-) create mode 100644 perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py index d371fa43986ab..7ed97fe6c0ebd 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,22 +22,19 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] + laserscan_based_occupancy_grid_map_node_params["input_obstacle_pointcloud"] = bool( + LaunchConfiguration("input_obstacle_pointcloud").perform(context) ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), + laserscan_based_occupancy_grid_map_node_params["input_obstacle_and_raw_pointcloud"] = bool( + LaunchConfiguration("input_obstacle_and_raw_pointcloud").perform(context) ) composable_nodes = [ @@ -53,7 +51,9 @@ def add_launch_arg(name: str, default_value=None): ], parameters=[ { - "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "target_frame": laserscan_based_occupancy_grid_map_node_params[ + "scan_origin_frame" + ], # Leave disabled to output scan in pointcloud frame "transform_tolerance": 0.01, "min_height": 0.0, "max_height": 2.0, @@ -85,16 +85,7 @@ def add_launch_arg(name: str, default_value=None): ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), - "input_obstacle_and_raw_pointcloud": LaunchConfiguration( - "input_obstacle_and_raw_pointcloud" - ), - } - ], + parameters=[laserscan_based_occupancy_grid_map_node_params], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] @@ -115,6 +106,25 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ add_launch_arg("use_multithread", "false"), @@ -128,11 +138,11 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg("param_file", "config/laserscan_based_occupancy_grid_map.param.yaml"), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py index 2feefdfb1053d..e8a9e173cfe0a 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,23 +22,16 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ + "ros__parameters" + ] composable_nodes = [ ComposableNode( @@ -49,12 +43,7 @@ def add_launch_arg(name: str, default_value=None): ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - } - ], + parameters=[pointcloud_based_occupancy_grid_map_node_params], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] @@ -75,18 +64,37 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ - add_launch_arg("use_multithread", "False"), - add_launch_arg("use_intra_process", "True"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), + add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ffc6f908983bb..b2a800c5b278d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -76,6 +76,7 @@ + diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 89906f86c5ce5..df0090d1b6cee 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -48,4 +48,5 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 655846f5cade1..d0fa909d58b40 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,7 +9,9 @@ This package outputs the probability of having an obstacle as occupancy grid map Occupancy grid map is generated on `map_frame`, and grid orientation is fixed. -You may need to choose `output_frame` which means grid map origin. Default is `base_link`, but your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) would be the better choice. +You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which means sensor origin and gridmap origin respectively. Especially, set your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) as a `scan_origin_frame` would result in better performance. + +![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) ### Each config paramters @@ -21,7 +23,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline. | --------------------------------- | ------------- | | map_frame | "map" | | base_link_frame | "base_link" | -| output_frame | "base_link" | +| scan_origin_frame | "base_link" | +| gridmap_origin_frame | "base_link" | | use_height_filter | true | | enable_single_frame_mode | false | | map_length | 100.0 [m] | @@ -40,7 +43,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline. | enable_single_frame_mode | false | | map_frame | "map" | | base_link_frame | "base_link" | -| output_frame | "base_link" | +| scan_origin_frame | "base_link" | +| gridmap_origin_frame | "base_link" | ## References/External links diff --git a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml index c3e498b3e3320..8c8f1857ce279 100644 --- a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml @@ -2,8 +2,10 @@ ros__parameters: map_frame: "map" base_link_frame: "base_link" - output_frame: "base_link" - # using top of the main lidar frame is appropriate. ex) velodyne_top + # center of the grid map + gridmap_origin_frame: "base_link" + # ray-tracing center: main sensor frame is preferable like: "velodyne_top" + scan_origin_frame: "base_link" use_height_filter: true enable_single_frame_mode: false diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml index 3b84200d118af..49a1a7097d308 100644 --- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - map_length: 100 # [m] + map_length: 100.0 # [m] map_resolution: 0.5 # [m] use_height_filter: true @@ -9,5 +9,7 @@ # grid map coordinate map_frame: "map" base_link_frame: "base_link" - output_frame: "base_link" - # center of grid_map. Main LiDAR frame is preferable like: "velodyne_top" + # center of the grid map + gridmap_origin_frame: "base_link" + # ray-tracing center: main sensor frame is preferable like: "velodyne_top" + scan_origin_frame: "base_link" diff --git a/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg new file mode 100644 index 0000000000000..d1aaaa2c51176 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ scan_origin_frame +
+
+
+ + scan_origin_frame + + + + + +
+
+
+ gridmap_origin_frame +
+
+
+ + gridmap_origin_frame + + + + + + +
+
+
+ gridmap_origin = center of the gridmap +
+
+
+ + gridmap_origin = center of the gridmap + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 857d657618d20..fbef72e29e0f2 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -92,7 +92,8 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node // ROS Parameters std::string map_frame_; std::string base_link_frame_; - std::string output_frame_; + std::string gridmap_origin_frame_; + std::string scan_origin_frame_; bool use_height_filter_; bool enable_single_frame_mode_; }; diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp index a8def3560f86f..88f2cfbc146f3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -72,7 +72,7 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D void updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, - const Pose & robot_pose, const Pose & gridmap_origin); + const Pose & robot_pose, const Pose & scan_origin); void updateOrigin(double new_origin_x, double new_origin_y) override; diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 68aed57677909..5b7571fa8abcc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -82,7 +82,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node // ROS Parameters std::string map_frame_; std::string base_link_frame_; - std::string output_frame_; + std::string gridmap_origin_frame_; + std::string scan_origin_frame_; bool use_height_filter_; bool enable_single_frame_mode_; }; diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 1e7bbef48338c..7ed97fe6c0ebd 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -12,15 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -29,29 +25,17 @@ import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - +def launch_setup(context, *args, **kwargs): # load parameter files - param_file = os.path.join( - get_package_share_directory("probablistic_occupancy_grid_map"), - "config/laserscan_based_occupancy_grid_map.param.yaml", - ) + param_file = LaunchConfiguration("param_file").perform(context) with open(param_file, "r") as f: laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] + laserscan_based_occupancy_grid_map_node_params["input_obstacle_pointcloud"] = bool( + LaunchConfiguration("input_obstacle_pointcloud").perform(context) + ) + laserscan_based_occupancy_grid_map_node_params["input_obstacle_and_raw_pointcloud"] = bool( + LaunchConfiguration("input_obstacle_and_raw_pointcloud").perform(context) + ) composable_nodes = [ ComposableNode( @@ -68,7 +52,7 @@ def add_launch_arg(name: str, default_value=None): parameters=[ { "target_frame": laserscan_based_occupancy_grid_map_node_params[ - "output_frame" + "scan_origin_frame" ], # Leave disabled to output scan in pointcloud frame "transform_tolerance": 0.01, "min_height": 0.0, @@ -101,37 +85,48 @@ def add_launch_arg(name: str, default_value=None): ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[ - { - "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), - "input_obstacle_and_raw_pointcloud": LaunchConfiguration( - "input_obstacle_and_raw_pointcloud" - ), - }.update(laserscan_based_occupancy_grid_map_node_params) - ], + parameters=[laserscan_based_occupancy_grid_map_node_params], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] occupancy_grid_map_container = ComposableNodeContainer( - condition=LaunchConfigurationEquals("container", ""), - name="occupancy_grid_map_container", + name=LaunchConfiguration("container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals("container", ""), composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container"), + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), ) return LaunchDescription( [ - add_launch_arg("container", ""), add_launch_arg("use_multithread", "false"), add_launch_arg("use_intra_process", "false"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), @@ -143,9 +138,11 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), + add_launch_arg("param_file", "config/laserscan_based_occupancy_grid_map.param.yaml"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index af5ecdfa02873..e8a9e173cfe0a 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -12,15 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -29,27 +25,9 @@ import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - +def launch_setup(context, *args, **kwargs): # load parameter files - param_file = os.path.join( - get_package_share_directory("probablistic_occupancy_grid_map"), - "config/pointcloud_based_occupancy_grid_map.param.yaml", - ) + param_file = LaunchConfiguration("param_file").perform(context) with open(param_file, "r") as f: pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ "ros__parameters" @@ -71,32 +49,52 @@ def add_launch_arg(name: str, default_value=None): ] occupancy_grid_map_container = ComposableNodeContainer( - condition=LaunchConfigurationEquals("container", ""), - name="occupancy_grid_map_container", + name=LaunchConfiguration("container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals("container", ""), composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container"), + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), ) return LaunchDescription( [ - add_launch_arg("container", ""), add_launch_arg("use_multithread", "false"), - add_launch_arg("use_intra_process", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), + add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b65dfb97e2f84..2ded71a0ec0b4 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -93,6 +93,18 @@ bool cropPointcloudByHeight( pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); return pose; } + +geometry_msgs::msg::Pose getPose( + const builtin_interfaces::msg::Time & stamp, const tf2_ros::Buffer & tf2, + const std::string & source_frame, const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = + tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} } // namespace namespace occupancy_grid_map @@ -112,7 +124,8 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( /* params */ map_frame_ = declare_parameter("map_frame", "map"); base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - output_frame_ = declare_parameter("output_frame", "base_link"); + gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); + scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); use_height_filter_ = declare_parameter("use_height_filter", true); enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); const double map_length{declare_parameter("map_length", 100.0)}; @@ -209,17 +222,15 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa PointCloud2 trans_laserscan_pc{}; PointCloud2 trans_obstacle_pc{}; PointCloud2 trans_raw_pc{}; - Pose pose{}; + Pose gridmap_origin{}; + Pose scan_origin{}; try { transformPointcloud(*laserscan_pc_ptr, *tf2_, map_frame_, trans_laserscan_pc); transformPointcloud(filtered_obstacle_pc, *tf2_, map_frame_, trans_obstacle_pc); transformPointcloud(filtered_raw_pc, *tf2_, map_frame_, trans_raw_pc); - // pose = getPose(laserscan_pc_ptr->header, *tf2_, map_frame_); - geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2_->lookupTransform( - map_frame_, output_frame_, laserscan_pc_ptr->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + gridmap_origin = + getPose(laserscan_pc_ptr->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); + scan_origin = getPose(laserscan_pc_ptr->header.stamp, *tf2_, scan_origin_frame_, map_frame_); } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(get_logger(), ex.what()); return; @@ -231,16 +242,16 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa occupancy_grid_map_updater_ptr_->getSizeInCellsY(), occupancy_grid_map_updater_ptr_->getResolution()); single_frame_occupancy_grid_map.updateOrigin( - pose.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, - pose.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); + gridmap_origin.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); single_frame_occupancy_grid_map.updateFreespaceCells(trans_raw_pc); - single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose); + single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, scan_origin); single_frame_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc); if (enable_single_frame_mode_) { // publish occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( - map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, + map_frame_, laserscan_pc_ptr->header.stamp, gridmap_origin.position.z, single_frame_occupancy_grid_map)); } else { // Update with bayes filter @@ -248,7 +259,7 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa // publish occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( - map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, + map_frame_, laserscan_pc_ptr->header.stamp, gridmap_origin.position.z, *occupancy_grid_map_updater_ptr_)); } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp index 213db4330f850..b6b69ebf44391 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -81,6 +81,30 @@ void transformPointcloud( output.header.stamp = input.header.stamp; output.header.frame_id = ""; } + +/** + * @brief Get the Inverse Pose object + * + * @param input + * @return geometry_msgs::msg::Pose inverted pose + */ +geometry_msgs::msg::Pose getInversePose(const geometry_msgs::msg::Pose & pose) +{ + tf2::Vector3 position(pose.position.x, pose.position.y, pose.position.z); + tf2::Quaternion orientation( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Transform tf(orientation, position); + const auto inv_tf = tf.inverse(); + geometry_msgs::msg::Pose inv_pose; + inv_pose.position.x = inv_tf.getOrigin().x(); + inv_pose.position.y = inv_tf.getOrigin().y(); + inv_pose.position.z = inv_tf.getOrigin().z(); + inv_pose.orientation.x = inv_tf.getRotation().x(); + inv_pose.orientation.y = inv_tf.getRotation().y(); + inv_pose.orientation.z = inv_tf.getRotation().z(); + inv_pose.orientation.w = inv_tf.getRotation().w(); + return inv_pose; +} } // namespace namespace costmap_2d @@ -165,22 +189,28 @@ void OccupancyGridMap::updateOrigin(double new_origin_x, double new_origin_y) * * @param raw_pointcloud raw point cloud on a certain frame (usually base_link) * @param obstacle_pointcloud raw point cloud on a certain frame (usually base_link) - * @param robot_pose frame of point cloud (usually base_link) - * @param gridmap_origin manually chosen grid map origin frame + * @param robot_pose frame of the input point cloud (usually base_link) + * @param scan_origin manually chosen grid map origin frame */ void OccupancyGridMap::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, - const Pose & robot_pose, const Pose & gridmap_origin) + const Pose & robot_pose, const Pose & scan_origin) { constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); - // Transform to map frame - PointCloud2 trans_raw_pointcloud, trans_obstacle_pointcloud; - transformPointcloud(raw_pointcloud, robot_pose, trans_raw_pointcloud); - transformPointcloud(obstacle_pointcloud, robot_pose, trans_obstacle_pointcloud); + // Transform from base_link to map frame + PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame + transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud); + transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud); + + // Transform from map frame to scan frame + PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud; // point cloud in scan frame + const auto scan2map_pose = getInversePose(scan_origin); // scan -> map transform pose + transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud); + transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud); // Create angle bins struct BinInfo @@ -198,17 +228,18 @@ void OccupancyGridMap::updateWithPointCloud( std::vector> raw_pointcloud_angle_bins; obstacle_pointcloud_angle_bins.resize(angle_bin_size); raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator iter_x(raw_pointcloud, "x"), iter_y(raw_pointcloud, "y"), - iter_wx(trans_raw_pointcloud, "x"), iter_wy(trans_raw_pointcloud, "y"); + for (PointCloud2ConstIterator iter_x(scan_raw_pointcloud, "x"), + iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"), + iter_wy(map_raw_pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { const double angle = atan2(*iter_y, *iter_x); const int angle_bin_index = (angle - min_angle) / angle_increment; raw_pointcloud_angle_bins.at(angle_bin_index) .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); } - for (PointCloud2ConstIterator iter_x(obstacle_pointcloud, "x"), - iter_y(obstacle_pointcloud, "y"), iter_wx(trans_obstacle_pointcloud, "x"), - iter_wy(trans_obstacle_pointcloud, "y"); + for (PointCloud2ConstIterator iter_x(scan_obstacle_pointcloud, "x"), + iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"), + iter_wy(map_obstacle_pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { const double angle = atan2(*iter_y, *iter_x); int angle_bin_index = (angle - min_angle) / angle_increment; @@ -248,7 +279,7 @@ void OccupancyGridMap::updateWithPointCloud( : obstacle_pointcloud_angle_bin.back(); } raytrace( - gridmap_origin.position.x, gridmap_origin.position.y, end_distance.wx, end_distance.wy, + scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, occupancy_cost_value::FREE_SPACE); } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index cbfcce22d1b61..c594a0be0703f 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -93,6 +93,18 @@ geometry_msgs::msg::Pose getPose( pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); return pose; } + +geometry_msgs::msg::Pose getPose( + const builtin_interfaces::msg::Time & stamp, const tf2_ros::Buffer & tf2, + const std::string & source_frame, const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = + tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} } // namespace namespace occupancy_grid_map @@ -111,7 +123,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( /* params */ map_frame_ = declare_parameter("map_frame", "map"); base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - output_frame_ = declare_parameter("output_frame", "base_link"); + gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); + scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); use_height_filter_ = declare_parameter("use_height_filter", true); enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); const double map_length{declare_parameter("map_length", 100.0)}; @@ -174,12 +187,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( // Get from map to sensor frame pose Pose robot_pose{}; Pose gridmap_origin{}; + Pose scan_origin{}; try { robot_pose = getPose(input_raw_msg->header, *tf2_, map_frame_); - geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2_->lookupTransform( - map_frame_, output_frame_, input_raw_msg->header.stamp, rclcpp::Duration::from_seconds(0.5)); - gridmap_origin = tier4_autoware_utils::transform2pose(tf_stamped.transform); + gridmap_origin = getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); + scan_origin = getPose(input_raw_msg->header.stamp, *tf2_, scan_origin_frame_, map_frame_); } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(get_logger(), ex.what()); return; @@ -194,7 +206,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( gridmap_origin.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, gridmap_origin.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); single_frame_occupancy_grid_map.updateWithPointCloud( - filtered_raw_pc, filtered_obstacle_pc, robot_pose, gridmap_origin); + filtered_raw_pc, filtered_obstacle_pc, robot_pose, scan_origin); if (enable_single_frame_mode_) { // publish