Skip to content

Commit d46c91c

Browse files
RollingOatRollingOat
committed
update map_plan_launch to ROS2
1 parent 9be2130 commit d46c91c

File tree

8 files changed

+197
-28
lines changed

8 files changed

+197
-28
lines changed

‎autonomy_core/map_plan/map_plan_launch/CMakeLists.txt‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,16 @@
11
cmake_minimum_required(VERSION 3.10)
22
project(map_plan_launch)
33

4-
find_package(catkin REQUIRED)
5-
catkin_package()
4+
# find_package(catkin REQUIRED)
5+
# catkin_package()
6+
find_package(ament_cmake REQUIRED)
7+
8+
install(
9+
DIRECTORY launch
10+
DESTINATION share/${PROJECT_NAME}
11+
)
12+
install(
13+
DIRECTORY config
14+
DESTINATION share/${PROJECT_NAME}
15+
)
16+
ament_package()

‎autonomy_core/map_plan/map_plan_launch/config/mapper.yaml‎

Lines changed: 27 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,29 +1,31 @@
1-
robot_r: 0.50 # robot radius, local and global maps will be dilated in x and y with a radius of [ceil(robot_r/res)] cells
2-
robot_h: 0.25 # robot height, local and global maps will be dilated in z direction for 2*[ceil(robot_h/res)] cells (if robot_r is set as 0, there will not be any dilation along z either).
1+
/mapper:
2+
ros__parameters:
3+
robot_r: 0.50 # robot radius, local and global maps will be dilated in x and y with a radius of [ceil(robot_r/res)] cells
4+
robot_h: 0.25 # robot height, local and global maps will be dilated in z direction for 2*[ceil(robot_h/res)] cells (if robot_r is set as 0, there will not be any dilation along z either).
35

4-
# global mapper config
5-
global:
6-
center_x: 0.0 # center of the map in x-axis
7-
center_y: 0.0 # center of the map in y-axis
8-
range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500)
9-
range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500
10-
center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height
11-
range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10)
12-
resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments)
13-
num_point_cloud_skip: 3 # the global map update frequency = point cloud frequency / num_point_cloud_skip
14-
max_raycast_range: 30.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
15-
dilate_xy: true # dilate global map along x and y axis (recommended value: true)
16-
dilate_z: false # dilate global map along z axis (recommended value: false)
17-
decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
6+
# global mapper config
7+
global:
8+
center_x: 0.0 # center of the map in x-axis
9+
center_y: 0.0 # center of the map in y-axis
10+
range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500)
11+
range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500
12+
center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height
13+
range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10)
14+
resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments)
15+
num_point_cloud_skip: 3 # the global map update frequency = point cloud frequency / num_point_cloud_skip
16+
max_raycast_range: 30.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
17+
dilate_xy: true # dilate global map along x and y axis (recommended value: true)
18+
dilate_z: false # dilate global map along z axis (recommended value: false)
19+
decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
1820

19-
local: # local map range z and center_z will be the same as storage map
20-
# local planning horizon will be from robot to the local voxel map boundary
21-
# center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction
22-
range_x: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
23-
range_y: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
24-
range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy)
25-
resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments)
26-
max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
27-
decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
21+
local: # local map range z and center_z will be the same as storage map
22+
# local planning horizon will be from robot to the local voxel map boundary
23+
# center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction
24+
range_x: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
25+
range_y: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
26+
range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy)
27+
resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments)
28+
max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
29+
decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
2830

2931

‎autonomy_core/map_plan/map_plan_launch/launch/depth_to_cloud.launch.py‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import ComposableNodeContainer
3+
from launch_ros.descriptions import ComposableNode
4+
from launch_ros.actions import LoadComposableNodes
5+
from launch.substitutions import LaunchConfiguration
6+
7+
def generate_launch_description():
8+
# Arguments
9+
image_raw = LaunchConfiguration('image_raw', default='image_raw')
10+
points = LaunchConfiguration('points', default='points')
11+
12+
# Create a component container
13+
container = ComposableNodeContainer(
14+
name='depth_image_container',
15+
namespace='',
16+
package='rclcpp_components',
17+
executable='component_container',
18+
output='screen',
19+
)
20+
21+
# Metric conversion component
22+
metric_rect = ComposableNode(
23+
package='depth_image_proc',
24+
plugin='depth_image_proc::ConvertMetricNode',
25+
name='metric_rect',
26+
remappings=[
27+
('image_raw', image_raw),
28+
('image', 'image_rect'),
29+
],
30+
)
31+
32+
# Point cloud conversion component
33+
cloudify = ComposableNode(
34+
package='depth_image_proc',
35+
plugin='depth_image_proc::PointCloudXyzNode',
36+
name='cloudify',
37+
remappings=[
38+
('points', points),
39+
],
40+
)
41+
42+
return LaunchDescription([
43+
container,
44+
LoadComposableNodes(
45+
target_container=container,
46+
composable_node_descriptions=[metric_rect, cloudify],
47+
)
48+
])

‎autonomy_core/map_plan/map_plan_launch/launch/map_plan.launch.xml‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<arg name="robot" default="$(env ROBOT_NAME 'quadrotor')"/>
4+
<arg name="real_robot" default="false"/>
5+
<arg name="use_mp" default="false"/>
6+
<arg name="min_dispersion_planner" default="false"/>
7+
<arg name="odom" default="odom"/>
8+
9+
<!-- lidar_frame arg is only used when real_robot is true, otherwise the mapper will just use the same frame as the point cloud in simulator -->
10+
<arg name="lidar_frame" default=""/>
11+
<arg name="mapper_config" default="$(find-pkg-share map_plan_launch)/config/mapper.yaml"/>
12+
<arg name="planner_config" default="$(find-pkg-share control_launch)/config/tracker_params_mp.yaml"/>
13+
<arg name="cloud" default="lidar"/>
14+
15+
<group>
16+
<push-ros-namespace namespace="$(var robot)"/>
17+
18+
<include file="$(find-pkg-share map_plan_launch)/launch/mapper.launch.xml">
19+
<arg name="odom_frame" value="$(var robot)/odom"/>
20+
<arg name="map_frame" value="$(var robot)/map"/>
21+
<arg name="lidar_frame" value="$(var lidar_frame)"/>
22+
<arg name="real_robot" value="$(var real_robot)"/>
23+
<arg name="mapper_config" value="$(var mapper_config)"/>
24+
<arg name="cloud" value="$(var cloud)"/>
25+
</include>
26+
27+
<group unless="$(var use_mp)">
28+
<include file="$(find-pkg-share map_plan_launch)/launch/planner.launch.xml"/>
29+
</group>
30+
31+
<group if="$(var use_mp)">
32+
<include file="$(find-pkg-share map_plan_launch)/launch/mp_planner.launch.xml">
33+
<arg name="planner_config" value="$(var planner_config)"/>
34+
<arg name="odom" value="$(var odom)"/>
35+
<arg name="min_dispersion_planner" value="$(var min_dispersion_planner)"/>
36+
</include>
37+
</group>
38+
</group>
39+
</launch>

‎autonomy_core/map_plan/map_plan_launch/launch/mapper.launch.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<arg name="cloud" default="lidar"/>
4+
<arg name="odom_frame" default="odom"/>
5+
<arg name="map_frame" default="map"/>
6+
<arg name="lidar_frame" default=""/>
7+
<arg name="real_robot" default="false"/>
8+
<arg name="mapper_config" default="$(find-pkg-share map_plan_launch)/config/mapper.yaml"/>
9+
<arg name="prefix" default="nice -n 1"/>
10+
11+
<node
12+
pkg="mapper"
13+
name="mapper"
14+
exec="local_global_mapper"
15+
output="screen"
16+
launch-prefix="$(var prefix)">
17+
<param name="real_robot" value="$(var real_robot)"/>
18+
<param name="map_frame" value="$(var map_frame)"/>
19+
<param name="odom_frame" value="$(var odom_frame)"/>
20+
<param name="lidar_frame" value="$(var lidar_frame)"/>
21+
<param from="$(var mapper_config)"/>
22+
<remap from="~/cloud" to="$(var cloud)"/>
23+
<remap from="~/voxel_map" to="voxel_map"/>
24+
<remap from="~/local_cloud" to="local_cloud"/>
25+
26+
</node>
27+
</launch>

‎autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch.xml‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<arg name="prefix" default="nice -n 2"/>
4+
<arg name="odom" default="odom"/>
5+
<arg name="min_dispersion_planner" default="false"/>
6+
<arg name="planner_config" default="$(find-pkg-share control_launch)/config/tracker_params_mp.yaml"/>
7+
8+
<node
9+
pkg="action_planner"
10+
exec="global_plan_server"
11+
name="global_plan_server"
12+
output="screen"
13+
launch-prefix="$(var prefix)">
14+
<param from="$(var planner_config)"/>
15+
<remap from="~/odom" to="$(var odom)"/>
16+
<remap from="~/global_voxel_map" to="mapper/global_voxel_map"/>
17+
</node>
18+
19+
<!-- If NOT use minimum dispersion motion primitive planner -->
20+
<group unless="$(var min_dispersion_planner)">
21+
<node
22+
pkg="action_planner"
23+
exec="local_plan_server"
24+
name="local_plan_server"
25+
output="screen"
26+
launch-prefix="$(var prefix)">
27+
28+
<param from="$(var planner_config)"/>
29+
<remap from="~/local_voxel_map" to="mapper/local_voxel_map"/>
30+
</node>
31+
</group>
32+
</launch>

‎autonomy_core/map_plan/map_plan_launch/launch/planner.launch.xml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<!-- use nice to set low priority for planner -->
4+
<node pkg="action_planner" exec="path_planner" name="tpplanner" clear_params="true" output="screen" launch-prefix="nice -n 1">
5+
<remap from="~voxel_map" to="voxel_map"/>
6+
<param name="verbose" value="true"/>
7+
</node>
8+
</launch>

‎autonomy_core/map_plan/map_plan_launch/package.xml‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,12 @@
77
<maintainer email="[email protected]">Xu Liu</maintainer>
88
<maintainer email="[email protected]">Chao Qu</maintainer>
99
<maintainer email="[email protected]">Guilherme Nardari</maintainer>
10+
<maintainer email="[email protected]">Jiuzhou Lei</maintainer>
1011

1112
<license>Penn Software License</license>
12-
<buildtool_depend>catkin</buildtool_depend>
13+
<buildtool_depend>ament_cmake</buildtool_depend>
1314

1415
<export>
16+
<build_type>ament_cmake</build_type>
1517
</export>
1618
</package>

0 commit comments

Comments
 (0)