Closed sci-42ver closed 2 years ago
ros2 launch teb_local_planner teb_tb3_simulation_launch.py
# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# This is all-in-one launch script intended for use by nav2 developers.
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from nav2_common.launch import HasNodeParams, RewrittenYaml
def generate_launch_description():
# Get the launch directory
teb_launch_dir = os.path.join(
get_package_share_directory('teb_local_planner'), 'launch')
teb_param_dir = os.path.join(
get_package_share_directory('teb_local_planner'), 'params')
nav2_bringup_launch_dir = os.path.join(
get_package_share_directory('nav2_bringup'), 'launch')
nav2_bringup_rviz_dir = os.path.join(
get_package_share_directory('nav2_bringup'), 'rviz')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
lifecycle_nodes = ['map_saver']
# params_file = os.path.join(teb_param_dir, 'teb_params.yaml')
# rviz_config_file = os.path.join(teb_rviz_dir, 'rviz_test_optim.rviz')
# params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml')
params_file = os.path.join(teb_launch_dir, 'teb_params_add.yaml')
rviz_config_file = os.path.join(nav2_bringup_rviz_dir, 'nav2_default_view.rviz')
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, 'tb3_simulation_launch.py')),
launch_arguments={
'params_file': params_file,
'rviz_config_file': rviz_config_file,
'slam': 'True'}.items())
ld = LaunchDescription()
ld.add_action(bringup_cmd)
print(ld)
return ld
params/nav2_params.yaml
of the package nav2_bringup
main change the controller_server
controller_server:
ros__parameters:
odom_topic: /odom
use_sim_time: True
controller_frequency: 5.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_plugin_types: ["teb_local_planner::TebLocalPlannerROS"]
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
FollowPath:
plugin: "teb_local_planner::TebLocalPlannerROS"
teb_autosize: 1.0
dt_ref: 0.3
dt_hysteresis: 0.1
max_samples: 500
global_plan_overwrite_orientation: False
allow_init_with_backwards_motion: False
max_global_plan_lookahead_dist: 3.0
global_plan_viapoint_sep: 0.3
global_plan_prune_distance: 1.0
exact_arc_length: False
feasibility_check_no_poses: 2
publish_feedback: False
# Robot
max_vel_x: 0.26
max_vel_theta: 1.0
acc_lim_x: 2.5
acc_lim_theta: 3.2
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "circular"
radius: 0.17 # for type "circular"
# GoalTolerance
free_goal_vel: False
# Obstacles
min_obstacle_dist: 0.27
inflation_dist: 0.6
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 15
dynamic_obstacle_inflation_dist: 0.6
include_dynamic_obstacles: True
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
costmap_converter_spin_thread: True
costmap_converter_rate: 5
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
obstacle_cost_exponent: 4.0
weight_max_vel_x: 0.5
weight_max_vel_theta: 0.5
weight_acc_lim_x: 0.5
weight_acc_lim_theta: 10.5
weight_kinematics_nh: 1000.0
weight_kinematics_forward_drive: 3.0
weight_kinematics_turning_radius: 1.0
weight_optimaltime: 1.0 # must be > 0
weight_shortest_path: 0.0
weight_obstacle: 100.0
weight_inflation: 0.2
weight_dynamic_obstacle: 10.0 # not in use yet
weight_dynamic_obstacle_inflation: 0.2
weight_viapoint: 50.0
weight_adapt_factor: 2.0
# Homotopy Class Planner
enable_homotopy_class_planning: True
enable_multithreading: True
max_number_classes: 4
selection_cost_hysteresis: 5.0
selection_prefer_initial_plan: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: True
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5.0
roadmap_graph_area_length_scale: 1.0
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_heading_threshold: 0.45
switching_blocking_period: 0.0
viapoints_all_candidates: True
delete_detours_backwards: True
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: False
visualize_with_time_as_z_axis_scale: 0.0
# Recovery
shrink_horizon_backup: True
shrink_horizon_min_duration: 10.0
oscillation_recovery: True
oscillation_v_eps: 0.1
oscillation_omega_eps: 0.1
oscillation_recovery_min_duration: 10.0
oscillation_filter_duration: 10.0
I solved my problem after making the following changes:
diff --git a/include/teb_local_planner/pose_se2.h b/include/teb_local_planner/pose_se2.h
index addd0dd..4ec990e 100755
--- a/include/teb_local_planner/pose_se2.h
+++ b/include/teb_local_planner/pose_se2.h
@@ -46,9 +46,10 @@
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose2_d.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
#include <tf2/convert.h>
#include <tf2/utils.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
namespace teb_local_planner
{
https://github.com/ros2/geometry2/pull/485 is related and potentially fixes it without your changes, but it needs to be merged 😵💫
This has been merged!
I followed the authentic tutorial mainly and I tried the slam and navigation simulation tutorial this can run properly.
environment I used
ros2 rolling
and clonednav2
andteb_local_planner
into the same repository I also installed the dependencyshere is my directory tree
08:48:50 czg@czg-Lenovo-Legion-R7000-2020 nav2_ws ±|master ✗|→ tree -L 2 src/teb_local_planner/ src/teb_local_planner/ ├── README.md ├── teb_local_planner │ ├── cfg │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── cmake_modules │ ├── include │ ├── launch │ ├── LICENSE │ ├── package.xml │ ├── params │ ├── scripts │ ├── src │ ├── teb_local_planner_plugin.xml │ └── test └── teb_msgs ├── CMakeLists.txt ├── msg └── package.xml
08:52:22 czg@czg-Lenovo-Legion-R7000-2020 ~ → c++filt _ZN3tf27fromMsgIN13geometry_msgs3msg11Quaternion_ISaIvEEENS_10QuaternionEEEvRKTRT0 void tf2::fromMsg<geometrymsgs::msg::Quaternion<std::allocator >, tf2::Quaternion>(geometrymsgs::msg::Quaternion<std::allocator > const&, tf2::Quaternion&)
[composed_bringup-9] [INFO] [1642682593.611071527] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.
08:44:58 czg@czg-Lenovo-Legion-R7000-2020 ~ → ros2 run tf2_ros tf2_echo odom base_link [INFO] [1642682705.299311668] [tf2_echo]: Waiting for transform odom -> base_link: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist At time 110.223000000