teb_local_planner icon indicating copy to clipboard operation
teb_local_planner copied to clipboard

`void tf2::fromMsg<geometry_msgs::msg::Quaternion_<std::allocator<void> >, tf2::Quaternion>(geometry_msgs::msg::Quaternion_<std::allocator<void> > const&, tf2::Quaternion&)` nav2_bringup/composed_bringup symbol lookup error

Open sci-42ver opened this issue 2 years ago • 3 comments

ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True
  • environment I used ros2 rolling and cloned nav2 and teb_local_planner into the same repository I also installed the dependencys

  • here is my directory tree

08:36:41 czg@czg-Lenovo-Legion-R7000-2020 nav2_ws ±|master ✗|→ tree -L 1 src/
src/
├── build
├── costmap_converter
├── hls_lfcd_lds_driver
├── install
├── log
├── navigation2
├── navigation2_tutorials
├── slam_toolbox
├── teb_local_planner
└── turtlebot3

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
  • I think main problem is

[composed_bringup-9] [INFO] [1642682600.234018235] [bt_navigator]: Begin navigating from current location to (0.38, -0.60) [composed_bringup-9] [INFO] [1642682600.255151534] [controller_server]: Received a goal, begin computing control effort. [composed_bringup-9] [WARN] [1642682600.255236810] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded goal_checker . This warning will appear once. [composed_bringup-9] /home/czg/nav2_ws/install/nav2_bringup/lib/nav2_bringup/composed_bringup: symbol lookup error: /home/czg/nav2_ws/install/teb_local_planner/lib/libteb_local_planner.so: undefined symbol: ZN3tf27fromMsgIN13geometry_msgs3msg11Quaternion_ISaIvEEENS_10QuaternionEEEvRKT_RT0 [ERROR] [composed_bringup-9]: process has died [pid 48842, exit code 127, cmd '/home/czg/nav2_ws/install/nav2_bringup/lib/nav2_bringup/composed_bringup --ros-args --params-file /tmp/tmp9b7q7zvt --params-file /tmp/launch_params_dyqx3188 -r /tf:=tf -r /tf_static:=tf_static'].

  • the function can‘t be found.
08:52:22 czg@czg-Lenovo-Legion-R7000-2020 ~ → c++filt _ZN3tf27fromMsgIN13geometry_msgs3msg11Quaternion_ISaIvEEENS_10QuaternionEEEvRKT_RT0_
void tf2::fromMsg<geometry_msgs::msg::Quaternion_<std::allocator<void> >, tf2::Quaternion>(geometry_msgs::msg::Quaternion_<std::allocator<void> > const&, tf2::Quaternion&)
  • my main package branches are

08:55:55 czg@czg-Lenovo-Legion-R7000-2020 navigation2 ±|main ✗|→ cd ../teb_local_planner/ 08:57:06 czg@czg-Lenovo-Legion-R7000-2020 teb_local_planner ±|ros2-master ✗|→

  • the tf error is also weird, however it doesn‘t influence the main process greatly.
 [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
- Translation: [-2.000, -0.500, 0.019]
- Rotation: in Quaternion [0.000, 0.001, 0.003, 1.000]
At time 111.209000000
- Translation: [-2.000, -0.500, 0.019]
- Rotation: in Quaternion [0.000, 0.001, 0.003, 1.000]
^C[INFO] [1642682708.120119739] [rclcpp]: signal_handler(signum=2)

sci-42ver avatar Jan 20 '22 12:01 sci-42ver

  • my launch
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

  • here is my params.yaml modefied on the 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

sci-42ver avatar Jan 20 '22 13:01 sci-42ver

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
 {

sci-42ver avatar Jan 21 '22 14:01 sci-42ver

https://github.com/ros2/geometry2/pull/485 is related and potentially fixes it without your changes, but it needs to be merged 😵‍💫

guilyx avatar Jan 31 '22 20:01 guilyx

This has been merged!

SteveMacenski avatar Aug 24 '22 22:08 SteveMacenski