diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index ceea797..4633d56 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -12,7 +12,7 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: base_footprint + base_frame: base_link scan_topic: scan_filtered mode: mapping #localization @@ -28,20 +28,20 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 1.0 #was 5.0 resolution: 0.05 - max_laser_range: 200.0 #for rastering images, standard was 20 + max_laser_range: 12.0 #for rastering images, standard was 20 minimum_time_interval: 0.5 transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true + enable_interactive_mode: false # General Parameters use_scan_matching: true use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.5 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 + minimum_travel_distance: 0.0 # was 0.5 + minimum_travel_heading: 0.0 # was 0.5 + scan_buffer_size: 20 # was 10 + scan_buffer_maximum_scan_distance: 12.0 # was 10.0 link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 diff --git a/config/nav2_params copy.yaml b/config/nav2_params copy.yaml deleted file mode 100644 index 14fe6e4..0000000 --- a/config/nav2_params copy.yaml +++ /dev/null @@ -1,348 +0,0 @@ -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: "base_footprint" - 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: "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: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - # '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 - -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: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.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 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - 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: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 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: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: 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 - -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: odom - robot_base_frame: base_link - 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: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 7eff0ca..d7d19e0 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -12,25 +12,23 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: base_footprint - #scan_topic: scan + base_frame: base_link scan_topic: scan_filtered - use_map_saver: true mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given # will use pose - #map_file_name: test_steve - #map_start_pose: [0.0, 0.0, 0.0] - #map_start_at_dock: true + #map_file_name: /home/bjorn/Documents/ros-projects/cps_bot_mini_ws/my_map_serial + # map_start_pose: [0.0, 0.0, 0.0] + map_start_at_dock: true debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 5.0 + map_update_interval: 1.0 #was 5.0 resolution: 0.05 - max_laser_range: 20.0 #for rastering images + max_laser_range: 12.0 #for rastering images, standard was 20 minimum_time_interval: 0.5 transform_timeout: 0.2 tf_buffer_duration: 30. @@ -40,10 +38,10 @@ slam_toolbox: # General Parameters use_scan_matching: true use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.5 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 + minimum_travel_distance: 0.0 # was 0.5 + minimum_travel_heading: 0.0 # was 0.5 + scan_buffer_size: 20 # was 10 + scan_buffer_maximum_scan_distance: 12.0 # was 10.0 link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 @@ -82,7 +80,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_footprint" + base_frame_id: "base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -90,7 +88,7 @@ amcl: global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 - laser_max_range: 7.0 #was 100.0 + laser_max_range: 12.0 #was 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 @@ -115,6 +113,21 @@ amcl: z_short: 0.05 scan_topic: scan_filtered +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 + bt_navigator: ros__parameters: use_sim_time: True @@ -174,7 +187,6 @@ bt_navigator: - 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: @@ -187,50 +199,93 @@ bt_navigator_navigate_to_pose_rclcpp_node: controller_server: ros__parameters: use_sim_time: True - controller_frequency: 100.0 # was 20.0 + controller_frequency: 20.0 # was 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 #new progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older - goal_checker_plugins: ["goal_checker"] + goal_checker_plugins: ["general_goal_checker"] # was "goal_checker" controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 - goal_checker: + general_goal_checker: # was goal_checker plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 # was 0.25 yaw_goal_tolerance: 0.25 # was 0.25 stateful: True FollowPath: - plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.5 - lookahead_dist: 0.6 - min_lookahead_dist: 0.3 - max_lookahead_dist: 0.9 - 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.007 # was 0.05 - approach_velocity_scaling_dist: 0.6 - use_collision_detection: true - max_allowed_time_to_collision_up_to_carrot: 1.0 - use_regulated_linear_velocity_scaling: true - use_fixed_curvature_lookahead: false - curvature_lookahead_dist: 0.25 - use_cost_regulated_linear_velocity_scaling: false - regulated_linear_scaling_min_radius: 0.9 - regulated_linear_scaling_min_speed: 0.25 - use_rotate_to_heading: false # was true, cannot be set together with allow_reversing - allow_reversing: true # was false - rotate_to_heading_min_angle: 0.785 - max_angular_accel: 3.2 - max_robot_pose_search_dist: 10.0 - use_interpolation: true # was false - + # plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + # desired_linear_vel: 0.5 + # lookahead_dist: 0.6 + # min_lookahead_dist: 0.3 + # max_lookahead_dist: 0.9 + # 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.007 # was 0.05 + # approach_velocity_scaling_dist: 0.6 + # use_collision_detection: true + # max_allowed_time_to_collision_up_to_carrot: 1.0 + # use_regulated_linear_velocity_scaling: true + # use_fixed_curvature_lookahead: false + # curvature_lookahead_dist: 0.25 + # use_cost_regulated_linear_velocity_scaling: false + # regulated_linear_scaling_min_radius: 0.9 + # regulated_linear_scaling_min_speed: 0.25 + # use_rotate_to_heading: false # was true, cannot be set together with allow_reversing + # allow_reversing: true # was false + # rotate_to_heading_min_angle: 0.785 + # max_angular_accel: 3.2 + # max_robot_pose_search_dist: 10.0 + # use_interpolation: true # was false + + ## Changed to DWB again for Loki + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: False # was True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.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 + local_costmap: local_costmap: ros__parameters: @@ -242,14 +297,17 @@ local_costmap: rolling_window: true width: 3 height: 3 - resolution: 0.05 + resolution: 0.06 # was 0.05 #robot_radius: 0.22 footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["static_layer", "voxel_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 10.0 # was 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 4.0 # was 10.0 + inflation_radius: 0.45 # was 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -270,9 +328,6 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 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: @@ -285,7 +340,7 @@ global_costmap: use_sim_time: True #robot_radius: 0.22 footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" - resolution: 0.05 + resolution: 0.06 # was 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: @@ -307,46 +362,38 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 10.0 # was 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 4.0 # was 10.0 + inflation_radius: 0.45 # was 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 # new planner_plugins: ["GridBased"] use_sim_time: True + # GridBased: + # plugin: "nav2_smac_planner/SmacPlanner2D" + # tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + # downsample_costmap: false # whether or not to downsample the map + # downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + # allow_unknown: true # allow traveling in unknown space + # max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + # max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + # max_planning_time: 2.0 # max time in s for planner to plan, smooth + # cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + # use_final_approach_orientation: true # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + # smoother: + # max_iterations: 1000 + # w_smooth: 0.3 + # w_data: 0.2 + # tolerance: 0.1 + ## Changed back to Navfn planner for Loki GridBased: - plugin: "nav2_smac_planner/SmacPlanner2D" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: true # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 0.1 + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true smoother_server: ros__parameters: @@ -411,4 +458,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 + velocity_timeout: 1.0 \ No newline at end of file diff --git a/launch/robot_lidar.launch.py b/launch/robot_lidar.launch.py index 92b3a08..8bdeef5 100644 --- a/launch/robot_lidar.launch.py +++ b/launch/robot_lidar.launch.py @@ -23,69 +23,6 @@ from launch_ros.descriptions import ParameterValue def generate_launch_description(): - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("cps_loki_description"), - "urdf", - "odrive_diffbot.urdf.xacro" - ] - ), - ] - ) - robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("cps_loki_bringup"), - "config", - "diffbot_controllers.yaml", - ] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - output="both", - ) - - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["diffbot_base_controller", "-c", "/controller_manager"], - ) - - joystick_spawner = Node( - package="joy", - executable="joy_node" - ) - - teleop_spawner = Node( - package="rmp220_teleop", - executable="rmp220_teleop" - ) - - cam_node = Node( - package="ros2_cam_openCV", - executable="cam_node" - ) lidar_dir = os.path.join(get_package_share_directory('cps_loki_bringup'), 'config', 'lsx10.yaml') lidar_node = LifecycleNode( @@ -99,12 +36,5 @@ def generate_launch_description(): ) return LaunchDescription([ - #control_node, - #robot_state_pub_node, - #joint_state_broadcaster_spawner, - #robot_controller_spawner, - #joystick_spawner, - #teleop_spawner, - #cam_node, lidar_node ]) diff --git a/launch/robot_mapper.launch.py b/launch/robot_mapper.launch.py index e969f6a..c5780dd 100644 --- a/launch/robot_mapper.launch.py +++ b/launch/robot_mapper.launch.py @@ -24,71 +24,7 @@ from launch.substitutions import LaunchConfiguration def generate_launch_description(): - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("cps_loki_description"), - "urdf", - "odrive_diffbot.urdf.xacro" - ] - ), - ] - ) - robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("cps_loki_bringup"), - "config", - "diffbot_controllers.yaml", - ] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - output="both", - ) - - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["diffbot_base_controller", "-c", "/controller_manager"], - ) - - joystick_spawner = Node( - package="joy", - executable="joy_node" - ) - - teleop_spawner = Node( - package="rmp220_teleop", - executable="rmp220_teleop" - ) - - cam_node = Node( - package="ros2_cam_openCV", - executable="cam_node" - ) - - use_sim_time = False slam_params_file = PathJoinSubstitution( [ FindPackageShare("cps_loki_bringup"), @@ -96,6 +32,7 @@ def generate_launch_description(): "mapper_params_online_async.yaml" ] ) + mapper_node = Node( package="slam_toolbox", executable="async_slam_toolbox_node", @@ -107,25 +44,6 @@ def generate_launch_description(): ], ) - lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') - lidar_node = LifecycleNode( - package='lslidar_driver', - executable='lslidar_driver_node', - name='lslidar_driver_node', - output='screen', - emulate_tty=True, - namespace='', - parameters=[lidar_dir], - ) - return LaunchDescription([ - #control_node, - #robot_state_pub_node, - #joint_state_broadcaster_spawner, - #robot_controller_spawner, - #joystick_spawner, - #teleop_spawner, - #cam_node, - #lidar_node, mapper_node ]) \ No newline at end of file