diff --git a/nav2_dynamic_bringup/CMakeLists.txt b/nav2_dynamic_bringup/CMakeLists.txt new file mode 100644 index 0000000..e5573d0 --- /dev/null +++ b/nav2_dynamic_bringup/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_dynamic_bringup) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(navigation2 REQUIRED) + +nav2_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/nav2_dynamic_bringup/README.md b/nav2_dynamic_bringup/README.md new file mode 100644 index 0000000..131ba4d --- /dev/null +++ b/nav2_dynamic_bringup/README.md @@ -0,0 +1,130 @@ +# nav2_dynamic_bringup + +The `nav2_dynamic_bringup` package is an example bringup system for launching CARLA to test navigation in a dynamic environment. + +To setup a dynamic environment, you need to run the following scripts. + +-------------- + +### Installing CARLA 0.9.13 on Ubuntu 22.04 + +#### Installation Procedure (Docker) + +Follow the link below and execute the provided script: https://carla.readthedocs.io/en/docs-preview/build_docker/ +```shell +docker pull carlasim/carla:0.9.13 +``` + +#### Installation Procedure (Native) +Alternatively, you can download CARLA manually from the following link: +https://github.com/carla-simulator/carla/releases/tag/0.9.13 and download `CARLA_0.9.13.tar.gz`. + + +------------------- + + +### Installing CARLA ROS Bridge (Docker) +```bash +mkdir -p ~/carla-ros-bridge && cd ~/carla-ros-bridge +git clone --recurse-submodules https://github.com/chiragmakwana0296/ros-bridge.git -b fix/docker src/ros-bridge +cd src/ros-bridge/docker +docker compose build build_image +docker compose up colcon_build +``` +#### Host setup +```bash +sudo apt install ros-$ROS_DISTRO-rmw-cyclonedds-cpp +# add below lines to ~/.bashrc +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +export FASTRTPS_DEFAULT_PROFILES_FILE=~/carla-ros-bridge/docker/content/dds.xml +``` + + +### Installing CARLA ROS Bridge (Native) + +Clone the master branch of ```carla_ros_bridge``` and follow the official installation guide: +https://carla.readthedocs.io/projects/ros-bridge/en/latest/ros_installation_ros2/ + +```shell +mkdir -p ~/carla-ros-bridge && cd ~/carla-ros-bridge + +git clone --recurse-submodules https://github.com/carla-simulator/ros-bridge.git src/ros-bridge + +rosdep update +rosdep install --from-paths src --ignore-src -r + +colcon build +``` +#### Installing `.egg` and `.whl` Files +For running CARLA ROS Bridge on Ubuntu 22.04, you need compatible .egg and .whl files. +Download them from the following link: https://github.com/gezp/carla_ros/releases/tag/carla-0.9.13-ubuntu-22.04 + +Copy the downloaded files to the CARLA Python API directory: +* carla-0.9.13-cp310-cp310-linux_x86_64.whl +* carla-0.9.13-py3.10-linux-x86_64.egg + +```shell +cp carla-0.9.13-cp310-cp310-linux_x86_64.whl /path_to_carla/CARLA_0.9.13/PythonAPI/carla/dist/ +cp carla-0.9.13-py3.10-linux_x86_64.egg /path_to_carla/CARLA_0.9.13/PythonAPI/carla/dist/ +``` + +#### Installing the `.whl` File +```shell +cd /path_to_carla/CARLA_0.9.13/PythonAPI/carla/dist/ +pip3 install carla-0.9.13-cp310-cp310-linux_x86_64.whl +``` + +#### Setting Up Environment Variables +Add the CARLA paths to your `.bashrc` file: +```shell +export CARLA_ROOT=/path-to-carla/CARLA_0.9.13/ +export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.13-py3.10-linux-x86_64.egg:$CARLA_ROOT/PythonAPI/carla +``` + +Then, source the file: +```shell +source ~/.bashrc +``` + + +### Running CARLA and CARLA-ROS Bridge + +------------------------------- +#### Run CARLA Simulation (docker) +1. To run the CARLA simulation, execute the following command in Terminal 1: +```shell +sudo docker run --privileged --gpus all --net=host -e DISPLAY=:1 carlasim/carla:0.9.13 /bin/bash ./CarlaUE4.sh +``` + +#### CARLA ROS Bridge (Docker) +Run bridge +```bash +docker compose up run_bridge +``` +To generate thetraffic +```bash +docker compose up generate_traffic +``` + +#### CARLA ROS Bridge (Native Ubuntu 22.04) +2. To run the CARLA ROS bridge, execute the following command in Terminal 2: +```shell +ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py +``` + +3. To send command signals to CARLA, run the following command in Terminal 3: +```shell +ros2 run carla_twist_to_control carla_twist_to_control +``` + +4. To generate traffic, execute the following commands in Terminal 4: +```shell +cd /path_to_carla/CARLA_0.9.13/PythonAPI/examples/ +python3 generate_traffic.py +``` + +#### Nav2 Stack for Navigation +5. To run the navigation stack, execute the following command in Terminal 5: +```shell +ros2 launch nav2_dynamic_bringup navigation_launch +``` \ No newline at end of file diff --git a/nav2_dynamic_bringup/launch/navigation_launch.py b/nav2_dynamic_bringup/launch/navigation_launch.py new file mode 100644 index 0000000..bbb4261 --- /dev/null +++ b/nav2_dynamic_bringup/launch/navigation_launch.py @@ -0,0 +1,259 @@ + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_dynamic_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('/carla/ego_vehicle/twist', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('/carla/ego_vehicle/twist', 'cmd_vel_nav'), ('cmd_vel_smoothed', '/carla/ego_vehicle/twist')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('/carla/ego_vehicle/twist', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('/carla/ego_vehicle/twist', 'cmd_vel_nav'), ('cmd_vel_smoothed', '/carla/ego_vehicle/twist')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_dynamic_bringup/package.xml b/nav2_dynamic_bringup/package.xml new file mode 100644 index 0000000..1597c64 --- /dev/null +++ b/nav2_dynamic_bringup/package.xml @@ -0,0 +1,32 @@ + + + + nav2_dynamic_bringup + 0.0.0 + Bringup scripts and configurations for the Nav2 nav2_dynamic_bringup stack + Pravin Behera + Apache-2.0 + + ament_cmake + nav2_common + + navigation2 + launch_ros + + launch_ros + navigation2 + nav2_common + slam_toolbox + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + launch + launch_testing + + + ament_cmake + + + diff --git a/nav2_dynamic_bringup/params/nav2_params.yaml b/nav2_dynamic_bringup/params/nav2_params.yaml new file mode 100644 index 0000000..ab9c458 --- /dev/null +++ b/nav2_dynamic_bringup/params/nav2_params.yaml @@ -0,0 +1,398 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: ego_vehicle + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: map + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: map #"odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: ego_vehicle + odom_topic: /carla/ego_vehicle/odometry + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 5.0 #0.26 + max_vel_y: 0.0 + max_vel_theta: 5.0 + min_speed_xy: 0.0 + max_speed_xy: 5.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + # FollowPath: + # plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + # desired_linear_vel: 20.0 + # allow_reversing: False + # lookahead_dist: 4.0 + # min_lookahead_dist: 1.0 + # max_lookahead_dist: 4.0 + # lookahead_time: 1.5 + # rotate_to_heading_angular_vel: 1.8 + # transform_tolerance: 0.1 + # use_velocity_scaled_lookahead_dist: false + # min_approach_linear_velocity: 0.05 + # approach_velocity_scaling_dist: 1.0 + # use_collision_detection: true + # max_allowed_time_to_collision_up_to_carrot: 1.0 + # use_regulated_linear_velocity_scaling: false + # use_cost_regulated_linear_velocity_scaling: true + # regulated_linear_scaling_min_radius: 1.0 + # regulated_linear_scaling_min_speed: 0.25 + # use_rotate_to_heading: False + # rotate_to_heading_min_angle: 0.785 + # max_angular_accel: 0.1 + # max_robot_pose_search_dist: 10.0 + # use_interpolation: false + # cost_scaling_dist: 0.3 + # cost_scaling_gain: 1.0 + # inflation_cost_scaling_factor: 3.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: map + robot_base_frame: ego_vehicle + use_sim_time: True + rolling_window: true + width: 50 #3 + height: 50 #3 + resolution: 0.5 + robot_radius: 0.22 + # footprint: "[[-1.5, 0.75], [-1.5, -0.75], [1.5, -0.75], [1.5, 0.75]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 50 #16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /carla/ego_vehicle/lidar + max_obstacle_height: 2.0 + min_obstacle_height: 0.05 + clearing: True + marking: True + data_type: "PointCloud2" #"LaserScan" + raytrace_max_range: 50.0 #3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 40.0 #2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: ego_vehicle + width: 100 #3 + height: 100 #3 + use_sim_time: True + robot_radius: 0.22 + resolution: 0.5 + track_unknown_space: true + rolling_window: true + # plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /carla/ego_vehicle/lidar #/scan + max_obstacle_height: 5.0 + min_obstacle_height: 0.05 + clearing: True + marking: True + data_type: "PointCloud2" #"LaserScan" + raytrace_max_range: 50.0 #3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 40.0 #2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + map_topic: /map + enabled: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + # GridBased: + # plugin: "nav2_navfn_planner/NavfnPlanner" + # tolerance: 0.5 + # use_astar: false + # allow_unknown: true + GridBased: + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.5 + downsample_costmap: False + downsampling_factor: 1 + allow_unknown: True + max_iterations: -1 + max_on_approach_iterations: 1000 + max_planning_time: 2.0 + motion_model_for_search: "MOORE" + cost_travel_multiplier: 2.0 + use_final_approach_orientation: False + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: map + robot_base_frame: ego_vehicle + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [5.0, 0.0, 1.0] + min_velocity: [-5.0, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /carla/ego_vehicle/odometry + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_dynamic_bringup/rviz/carla_nav2_dynamic.rviz b/nav2_dynamic_bringup/rviz/carla_nav2_dynamic.rviz new file mode 100644 index 0000000..7ea0307 --- /dev/null +++ b/nav2_dynamic_bringup/rviz/carla_nav2_dynamic.rviz @@ -0,0 +1,298 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /Global costmap1 + Splitter Ratio: 0.5 + Tree Height: 719 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: ego_vehicle + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.9887002110481262 + Min Color: 0; 0; 0 + Min Intensity: 0.8190271854400635 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /carla/ego_vehicle/lidar + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + ego_vehicle: + Value: true + ego_vehicle/depth_front: + Value: true + ego_vehicle/dvs_front: + Value: true + ego_vehicle/gnss: + Value: true + ego_vehicle/imu: + Value: true + ego_vehicle/lidar: + Value: true + ego_vehicle/radar_front: + Value: true + ego_vehicle/rgb_front: + Value: true + ego_vehicle/rgb_view: + Value: true + ego_vehicle/semantic_lidar: + Value: true + ego_vehicle/semantic_segmentation_front: + Value: true + map: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + ego_vehicle: + ego_vehicle/depth_front: + {} + ego_vehicle/dvs_front: + {} + ego_vehicle/gnss: + {} + ego_vehicle/imu: + {} + ego_vehicle/lidar: + {} + ego_vehicle/radar_front: + {} + ego_vehicle/rgb_front: + {} + ego_vehicle/rgb_view: + {} + ego_vehicle/semantic_lidar: + {} + ego_vehicle/semantic_segmentation_front: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Global costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Local costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 198; 70; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Global Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /received_global_plan + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 34.810794830322266 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.4009263813495636 + Y: -0.9728416204452515 + Z: -0.48558980226516724 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9953980445861816 + Target Frame: ego_vehicle + Value: Orbit (rviz) + Yaw: 1.4354065656661987 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 1990 + Y: 27