And i have another try.
I modify the nvblox_examples/nvblox_examples_bringup/launch/navigation/nvblox_carter_navigation.launch.py
from typing import List
import isaac_ros_launch_utils.all_types as lut
import isaac_ros_launch_utils as lu
from nvblox_ros_python_utils.nvblox_launch_utils import NvbloxMode
from nvblox_ros_python_utils.nvblox_constants import NVBLOX_CONTAINER_NAME
from launch import LaunchDescription
from launch_ros.actions import Node
def add_nvblox_carter_navigation(args: lu.ArgumentContainer) -> List[lut.Action]:
actions = []
# Enabling nav2
actions.append(
lu.set_parameter(
namespace='/local_costmap/local_costmap',
parameter='plugins',
value=['nvblox_layer', 'inflation_layer'],
))
actions.append(
lu.set_parameter(
namespace='/global_costmap/global_costmap',
parameter='plugins',
value=['nvblox_layer', 'inflation_layer'],
))
# -----------------------------
# Nav2 parameter file
# -----------------------------
nav_params_path = lu.get_path(
'nvblox_examples_bringup',
'config/navigation/carter_nav2_new.yaml'
)
actions.append(lut.SetParametersFromFile(str(nav_params_path)))
actions.append(lut.SetParameter('use_sim_time', False))
# -----------------------------
# Select nvblox map slice topic
# -----------------------------
mode = NvbloxMode[args.mode]
if mode is NvbloxMode.static:
costmap_topic_name = '/nvblox_node/static_map_slice'
elif mode in [NvbloxMode.dynamic, NvbloxMode.people_segmentation]:
costmap_topic_name = '/nvblox_node/combined_map_slice'
else:
raise RuntimeError(f'Navigation mode {mode} not supported')
actions.append(
lu.set_parameter(
namespace='/global_costmap/global_costmap',
parameter='nvblox_layer.nvblox_map_slice_topic',
value=costmap_topic_name,
))
actions.append(
lu.set_parameter(
namespace='/local_costmap/local_costmap',
parameter='nvblox_layer.nvblox_map_slice_topic',
value=costmap_topic_name,
))
# -----------------------------
# Nav2 nodes (NON-composable)
# -----------------------------
actions.extend([
Node(
package='nav2_controller',
executable='controller_server',
name='controller_server',
output='screen',
parameters=[nav_params_path],
),
Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[nav_params_path],
),
Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[nav_params_path],
),
Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
parameters=[nav_params_path],
),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'autostart': True,
'node_names': [
'controller_server',
'planner_server',
'bt_navigator',
'behavior_server',
]
}],
),
])
return actions
def generate_launch_description() -> LaunchDescription:
args = lu.ArgumentContainer()
args.add_arg('mode')
args.add_arg('container_name', NVBLOX_CONTAINER_NAME)
args.add_opaque_function(add_nvblox_carter_navigation)
return LaunchDescription(args.get_launch_actions())
And yaml
# =====================
# Behavior Tree Navigator
# =====================
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
navigators: ["navigate_to_pose", "navigate_through_poses"]
navigate_to_pose:
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
navigate_through_poses:
plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
# =====================
# Controller Server
# =====================
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
wz_max: 1.9
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
critics: ["ObstaclesCritic", "GoalCritic", "PathAlignCritic", "PathFollowCritic"]
ObstaclesCritic:
enabled: true
cost_power: 1
repulsion_weight: 4.0
critical_weight: 20.0
consider_footprint: True
collision_cost: 10000.0
collision_margin_distance: 0.1
near_goal_distance: 0.5
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 1.4
progress_checker_plugins: ["progress_checker"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
goal_checker_plugins: ["stopped"]
stopped:
plugin: "nav2_controller::StoppedGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
trans_stopped_velocity: 0.1
rot_stopped_velocity: 0.1
# =====================
# Local Costmap (nvblox + camera)
# =====================
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: False
global_frame: odom
robot_base_frame: base_link
rolling_window: true
width: 6
height: 6
resolution: 0.05
footprint: "[ [0.14, 0.25], [0.14, -0.25], [-0.607, -0.25], [-0.607, 0.25] ]"
plugins: ['nvblox_layer', 'inflation_layer']
nvblox_layer:
plugin: "nvblox::nav2::NvbloxCostmapLayer"
enabled: True
nav2_costmap_global_frame: odom
nvblox_map_slice_topic: "/nvblox_node/static_map_slice"
convert_to_binary_costmap: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: True
cost_scaling_factor: 10.0
inflation_radius: 0.8
always_send_full_costmap: True
# =====================
# Global Costmap (nvblox + static map)
# =====================
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
footprint: "[ [0.14, 0.25], [0.14, -0.25], [-0.607, -0.25], [-0.607, 0.25] ]"
resolution: 0.05
width: 60
height: 60
origin_x: -30.0
origin_y: -30.0
track_unknown_space: False
plugins: ['nvblox_layer', 'inflation_layer']
nvblox_layer:
plugin: "nvblox::nav2::NvbloxCostmapLayer"
enabled: True
nav2_costmap_global_frame: map
nvblox_map_slice_topic: "/nvblox_node/combined_map_slice"
convert_to_binary_costmap: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: True
cost_scaling_factor: 10.0
inflation_radius: 0.8
always_send_full_costmap: True
behavior_server:
ros__parameters:
use_sim_time: False
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
plugin: "nav2_behaviors::Spin"
backup:
plugin: "nav2_behaviors::BackUp"
wait:
plugin: "nav2_behaviors::Wait"
drive_on_heading:
plugin: "nav2_behaviors::DriveOnHeading"
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.1
# =====================
# Map Server
# =====================
map_server:
ros__parameters:
use_sim_time: False
yaml_filename: " "
# =====================
# Planner Server
# =====================
planner_server:
ros__parameters:
use_sim_time: False
expected_planner_frequency: 1.0
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner::SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
tolerance: 0.25
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "REEDS_SHEPP"
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 1.5
minimum_turning_radius: 0.20
reverse_penalty: 2.0
change_penalty: 0.0
non_straight_penalty: 1.2
cost_penalty: 3.0
retrospective_penalty: 0.015
lookup_table_size: 20.0
cache_obstacle_heuristic: false
debug_visualizations: false
use_quadratic_cost_penalty: False
downsample_obstacle_heuristic: True
allow_primitive_interpolation: False
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
# =====================
# Smoother Server
# =====================
smoother_server:
ros__parameters:
use_sim_time: False
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
And run
ros2 launch nvblox_examples_bringup realsense_example.launch.py
ros2 launch nvblox_examples_bringup nvblox_carter_navigation.launch.py mode:=static
I get the path, but the costmap does not publish , but the path does not consider obstacles