search

robotics-upo / nav2_social_costmap_plugin

Plugin-based layer for social navigation in the Nav2 system of ROS2
MIT License
15 stars 6 forks source link

ros2 social navigation layer foxy distribution #4

Open NamTruongTran0 opened 10 months ago

NamTruongTran0 commented 10 months ago

Hey,

first thank you so much for your contributions.

I tried your social navigation layer foxy distro some days ago.

neuronbot_params.yaml file:

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.9
    alpha2: 0.1
    alpha3: 0.05
    alpha4: 0.01
    alpha5: 0.04
    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.02
    pf_z: 0.85
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 2
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.02
    tf_broadcast: true
    transform_tolerance: 0.5
    update_min_a: 0.06
    update_min_d: 0.025
    z_hit: 0.7
    z_max: 0.001
    z_rand: 0.059
    z_short: 0.24

    # Initial Pose
    set_initial_pose: True
    initial_pose.x: -4.0
    initial_pose.y: -5.0
    initial_pose.z: 0.01
    initial_pose.yaw: 0.0      

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    #default_bt_xml_filename: "navigate_w_replanning_time.xml"
    default_bt_xml_filename: "bt_nav2.xml"
    #default_bt_xml_filename: "follow_point.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_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_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_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_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 30.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: -1.0
      min_vel_y: 0.0
      max_vel_x: 1.0
      max_vel_y: 0.0
      max_vel_theta: 1.5
      min_speed_xy: 0.0
      max_speed_xy: 1.0
      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: 0.5
      acc_lim_y: 0.0
      acc_lim_theta: 1.0
      decel_lim_x: -0.5
      decel_lim_y: 0.0
      decel_lim_theta: -1.0
      vx_samples: 20
      vy_samples: 0
      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.04
      PathAlign.scale: 50.0
      GoalAlign.scale: 10.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 50.0
      GoalDist.scale: 10.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.3
      #plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      #plugins: ["obstacle_layer","voxel_layer", "social_layer", "inflation_layer"]
      #plugins: ["obstacle_layer", "voxel_layer", "gradient_layer"]
      plugins: ["obstacle_layer", "voxel_layer", "social_layer"]
      gradient_layer:
        plugin: nav2_gradient_costmap_plugin/GradientLayer
        enabled: True
      social_layer:
        plugin: "nav2_social_costmap_plugin::SocialLayer"
        enabled: True
        cutoff: 10.0
        amplitude: 255.0
        publish_occgrid: False
        use_passing: True
        use_vel_factor: True
        speed_factor_multiplier: 5.0
        covariance_front_height: 0.4
        covariance_front_width: 0.25
        covariance_rear_height: 0.25
        covariance_rear_width: 0.25
        covariance_right_height: 0.3
        covariance_right_width: 0.2
        covariance_when_still: 0.25
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        #inflation_radius
        inflation_radius: 0.55
      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"
          obstacle_range: 2.5
          raytrace_range: 10.0
      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: pointcloud
        pointcloud:
          topic: /camera/pointcloud
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 2.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: False      
      robot_radius: 0.20
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      #plugins: ["static_layer", "voxel_layer", "social_layer","inflation_layer"] 
      #plugins: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"]
      #plugins: ["static_layer", "obstacle_layer", "voxel_layer", "social_layer"]
      gradient_layer:
        plugin: nav2_gradient_costmap_plugin/GradientLayer
        enabled: True  
      social_layer:
        plugin: "nav2_social_costmap_plugin::SocialLayer"
        enabled: True
        cutoff: 10.0
        amplitude: 255.0
        publish_occgrid: False
        use_passing: True
        use_vel_factor: True
        speed_factor_multiplier: 5.0
        covariance_front_height: 0.4
        covariance_front_width: 0.25
        covariance_rear_height: 0.25
        covariance_rear_width: 0.25
        covariance_right_height: 0.3
        covariance_right_width: 0.2
        covariance_when_still: 0.25
      resolution: 0.05
      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"
          obstacle_range: 2.5
          raytrace_range: 10.0
      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: pointcloud
        pointcloud:
          topic: /camera/pointcloud
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"      
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"        
        cost_scaling_factor: 1.0
        #inflation_radius 
        inflation_radius: 0.55             
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "turtlebot3_world.yaml"

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_footprint
    transform_timeout: 0.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.3
    min_rotational_vel: 0.05
    rotational_acc_lim: 0.3

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

Thank you once again for your contribution. I Really appreciate your time and effort :)

noeperez commented 10 months ago

Hello,

today we updated the repository to the Humble distro. Anyway, I can tell that I'm working with the previous Foxy version in my laptop without any problem. (And probably I will create a branch for maintaining the Foxy version).
To help you I would need more info, I do not watch anything helpful in the video, and your neuronbot_params.yaml seems to be ok. Some things to check:

NamTruongTran0 commented 10 months ago

Hey noeperez,

thank you so much for your fast answer i really appreciate it.

Check if the plugin is properly loaded when you launch the navigation (no errors or warnings printed in the terminal):

[INFO] [launch]: All log files can be found below /home/nartmangnourt/.ros/log/2023-08-23-17-36-40-907166-nartmangnourt-1781269
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_server-1]: process started with pid [1781271]
[INFO] [amcl-2]: process started with pid [1781273]
[INFO] [lifecycle_manager-3]: process started with pid [1781275]
[INFO] [controller_server-4]: process started with pid [1781277]
[INFO] [planner_server-5]: process started with pid [1781279]
[INFO] [recoveries_server-6]: process started with pid [1781281]
[INFO] [bt_navigator-7]: process started with pid [1781283]
[INFO] [waypoint_follower-8]: process started with pid [1781285]
[INFO] [lifecycle_manager-9]: process started with pid [1781287]
[controller_server-4] 1692805001.421423 [0] controller: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[lifecycle_manager-3] 1692805001.421668 [0] lifecycle_: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[amcl-2] 1692805001.423996 [0]       amcl: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[waypoint_follower-8] 1692805001.424964 [0] waypoint_f: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[lifecycle_manager-9] 1692805001.426446 [0] lifecycle_: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[lifecycle_manager-3] [INFO] [1692805001.426708006] [lifecycle_manager_localization]: Creating
[planner_server-5] 1692805001.427194 [0] planner_se: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[map_server-1] 1692805001.430119 [0] map_server: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[lifecycle_manager-3] [INFO] [1692805001.430873180] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
[waypoint_follower-8] [INFO] [1692805001.431652602] [waypoint_follower]: 
[waypoint_follower-8]   waypoint_follower lifecycle node launched. 
[waypoint_follower-8]   Waiting on external lifecycle transitions to activate
[waypoint_follower-8]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-8] [INFO] [1692805001.431748412] [waypoint_follower]: Creating
[lifecycle_manager-3] [INFO] [1692805001.431828183] [lifecycle_manager_localization]: Starting managed nodes bringup...
[lifecycle_manager-3] [INFO] [1692805001.431855613] [lifecycle_manager_localization]: Configuring map_server
[lifecycle_manager-9] [INFO] [1692805001.432603165] [lifecycle_manager_navigation]: Creating
[bt_navigator-7] 1692805001.433163 [0] bt_navigat: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[recoveries_server-6] 1692805001.434024 [0] recoveries: using network interface enp34s0 (udp/192.168.178.46) selected arbitrarily from: enp34s0, docker0
[planner_server-5] [INFO] [1692805001.436893519] [planner_server]: 
[planner_server-5]  planner_server lifecycle node launched. 
[planner_server-5]  Waiting on external lifecycle transitions to activate
[planner_server-5]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-9] [INFO] [1692805001.439313027] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[planner_server-5] [INFO] [1692805001.441128003] [planner_server]: Creating
[map_server-1] [INFO] [1692805001.441944656] [map_server]: 
[map_server-1]  map_server lifecycle node launched. 
[map_server-1]  Waiting on external lifecycle transitions to activate
[map_server-1]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-1] [INFO] [1692805001.442042536] [map_server]: Creating
[amcl-2] [INFO] [1692805001.445981519] [amcl]: 
[amcl-2]    amcl lifecycle node launched. 
[amcl-2]    Waiting on external lifecycle transitions to activate
[lifecycle_manager-9] [INFO] [1692805001.446163860] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[lifecycle_manager-9] [INFO] [1692805001.446207670] [lifecycle_manager_navigation]: Configuring controller_server
[amcl-2]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[amcl-2] [INFO] [1692805001.448218906] [amcl]: Creating
[recoveries_server-6] [INFO] [1692805001.449705741] [recoveries_server]: 
[recoveries_server-6]   recoveries_server lifecycle node launched. 
[recoveries_server-6]   Waiting on external lifecycle transitions to activate
[recoveries_server-6]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-7] [INFO] [1692805001.453992686] [bt_navigator]: 
[bt_navigator-7]    bt_navigator lifecycle node launched. 
[bt_navigator-7]    Waiting on external lifecycle transitions to activate
[bt_navigator-7]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-7] [INFO] [1692805001.456455453] [bt_navigator]: Creating
[controller_server-4] [INFO] [1692805001.461028849] [controller_server]: 
[controller_server-4]   controller_server lifecycle node launched. 
[controller_server-4]   Waiting on external lifecycle transitions to activate
[controller_server-4]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-1] [INFO] [1692805001.465711494] [map_server]: Configuring
[map_server-1] [INFO] [map_io]: Loading yaml file: /home/nartmangnourt/follower_ws/install/arena_nav2_bringup/share/arena_nav2_bringup/maps/mememan.yaml
[map_server-1] [DEBUG] [map_io]: resolution: 0.05
[map_server-1] [DEBUG] [map_io]: origin[0]: -12.2
[map_server-1] [DEBUG] [map_io]: origin[1]: -13.8
[map_server-1] [DEBUG] [map_io]: origin[2]: 0
[map_server-1] [DEBUG] [map_io]: free_thresh: 0.196
[map_server-1] [DEBUG] [map_io]: occupied_thresh: 0.65
[map_server-1] [DEBUG] [map_io]: mode: trinary
[map_server-1] [DEBUG] [map_io]: negate: 0
[map_server-1] [INFO] [map_io]: Loading image_file: /home/nartmangnourt/follower_ws/install/arena_nav2_bringup/share/arena_nav2_bringup/maps/mememan.pgm
[controller_server-4] [INFO] [1692805001.471895714] [controller_server]: Creating controller server
[planner_server-5] [INFO] [1692805001.473736920] [global_costmap.global_costmap]: 
[planner_server-5]  global_costmap lifecycle node launched. 
[planner_server-5]  Waiting on external lifecycle transitions to activate
[planner_server-5]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-5] [INFO] [1692805001.476614070] [global_costmap.global_costmap]: Creating Costmap
[controller_server-4] [INFO] [1692805001.483099751] [local_costmap.local_costmap]: 
[controller_server-4]   local_costmap lifecycle node launched. 
[controller_server-4]   Waiting on external lifecycle transitions to activate
[controller_server-4]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-4] [INFO] [1692805001.485391569] [local_costmap.local_costmap]: Creating Costmap
[controller_server-4] [INFO] [1692805001.498228200] [controller_server]: Configuring controller interface
[controller_server-4] [INFO] [1692805001.498573622] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-4] [INFO] [1692805001.498614242] [local_costmap.local_costmap]: Configuring
[controller_server-4] [INFO] [1692805001.503335197] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[controller_server-4] [INFO] [1692805001.509017536] [local_costmap.local_costmap]: Subscribed to Topics: scan
[map_server-1] [DEBUG] [map_io]: Read map /home/nartmangnourt/follower_ws/install/arena_nav2_bringup/share/arena_nav2_bringup/maps/mememan.pgm: 544 X 512 map @ 0.05 m/cell
[controller_server-4] [INFO] [1692805001.513151540] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[controller_server-4] [INFO] [1692805001.513187090] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-4] [INFO] [1692805001.515052356] [local_costmap.local_costmap]: Subscribed to Topics: pointcloud
[lifecycle_manager-3] [INFO] [1692805001.518050696] [lifecycle_manager_localization]: Configuring amcl
[amcl-2] [INFO] [1692805001.518575648] [amcl]: Configuring
[amcl-2] [INFO] [1692805001.518706908] [amcl]: initTransforms
[controller_server-4] [INFO] [1692805001.522897732] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-4] [INFO] [1692805001.522934572] [local_costmap.local_costmap]: Using plugin "social_layer"
[controller_server-4] [INFO] [1692805001.525747061] [local_costmap.local_costmap]: SocialLayer: subscribed to topic /people
[amcl-2] [INFO] [1692805001.527559757] [amcl]: initPubSub
[controller_server-4] [INFO] [1692805001.528910712] [local_costmap.local_costmap]: Initialized plugin "social_layer"
[amcl-2] [INFO] [1692805001.532394053] [amcl]: Subscribed to map topic.
[controller_server-4] [INFO] [1692805001.534160459] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-4] [INFO] [1692805001.535712804] [controller_server]: Created goal_checker : goal_checker of type nav2_controller::SimpleGoalChecker
[lifecycle_manager-3] [INFO] [1692805001.536861438] [lifecycle_manager_localization]: Activating map_server
[map_server-1] [INFO] [1692805001.537096889] [map_server]: Activating
[lifecycle_manager-3] [INFO] [1692805001.538291803] [lifecycle_manager_localization]: Activating amcl
[amcl-2] [INFO] [1692805001.538305733] [amcl]: Received a 544 X 512 map @ 0.050 m/pix
[controller_server-4] [INFO] [1692805001.539806608] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[amcl-2] [INFO] [1692805001.541037652] [amcl]: Activating
[amcl-2] [WARN] [1692805001.541072242] [amcl]: Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, will be published as nav2_msgs/ParticleCloud in the future
[amcl-2] [INFO] [1692805001.541093872] [amcl]: initialPoseReceived
[amcl-2] [INFO] [1692805001.541314162] [amcl]: Setting pose (1692805001.541314): 0.000 0.000 0.000
[controller_server-4] [INFO] [1692805001.541907114] [controller_server]: Setting transform_tolerance to 0.200000
[lifecycle_manager-3] [INFO] [1692805001.543385719] [lifecycle_manager_localization]: Managed nodes are active
[controller_server-4] [INFO] [1692805001.555914211] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-4] [INFO] [1692805001.557034014] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.557277955] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-4] [INFO] [1692805001.558280178] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.558542159] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-4] [INFO] [1692805001.559142581] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.559384852] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-4] [INFO] [1692805001.560098574] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.560333345] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-4] [INFO] [1692805001.561015107] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.561253358] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-4] [INFO] [1692805001.561680550] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.561893550] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-4] [INFO] [1692805001.562291292] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1692805001.562315682] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-9] [INFO] [1692805001.565828873] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-5] [INFO] [1692805001.566035884] [planner_server]: Configuring
[planner_server-5] [INFO] [1692805001.566078464] [global_costmap.global_costmap]: Configuring
[planner_server-5] [INFO] [1692805001.569566555] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-5] [INFO] [1692805001.574320511] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-5] [INFO] [1692805001.575022563] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-5] [INFO] [1692805001.575047733] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-5] [INFO] [1692805001.575993036] [global_costmap.global_costmap]: Subscribed to Topics: scan
[planner_server-5] [INFO] [1692805001.579685779] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-5] [INFO] [1692805001.579720819] [global_costmap.global_costmap]: Using plugin "voxel_layer"
[planner_server-5] [INFO] [1692805001.580694122] [global_costmap.global_costmap]: Subscribed to Topics: pointcloud
[planner_server-5] [INFO] [1692805001.585528308] [global_costmap.global_costmap]: Initialized plugin "voxel_layer"
[planner_server-5] [INFO] [1692805001.585562318] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-5] [INFO] [1692805001.586749812] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-5] [INFO] [1692805001.587075423] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 544 X 512 at 0.050000 m/pix
[planner_server-5] [INFO] [1692805001.591520957] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-5] [INFO] [1692805001.591557657] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-5] [INFO] [1692805001.594335067] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-9] [INFO] [1692805001.597012055] [lifecycle_manager_navigation]: Configuring recoveries_server
[recoveries_server-6] [INFO] [1692805001.597249546] [recoveries_server]: Configuring
[recoveries_server-6] [INFO] [1692805001.603601607] [recoveries_server]: Creating recovery plugin spin of type nav2_recoveries/Spin
[recoveries_server-6] [INFO] [1692805001.605135812] [recoveries_server]: Configuring spin
[recoveries_server-6] [INFO] [1692805001.609405276] [recoveries_server]: Creating recovery plugin backup of type nav2_recoveries/BackUp
[recoveries_server-6] [INFO] [1692805001.610559100] [recoveries_server]: Configuring backup
[recoveries_server-6] [INFO] [1692805001.613119888] [recoveries_server]: Creating recovery plugin wait of type nav2_recoveries/Wait
[recoveries_server-6] [INFO] [1692805001.614112471] [recoveries_server]: Configuring wait
[lifecycle_manager-9] [INFO] [1692805001.616844141] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-7] [INFO] [1692805001.617027291] [bt_navigator]: Configuring
[bt_navigator-7] [INFO] [1692805001.641112280] [bt_navigator_rclcpp_node]: Waiting for "compute_path_to_pose" action server
[bt_navigator-7] [INFO] [1692805001.641313291] [bt_navigator_rclcpp_node]: "ComputePathToPose" BtActionNode initialized
[bt_navigator-7] [INFO] [1692805001.643113187] [bt_navigator_rclcpp_node]: Waiting for "follow_path" action server
[bt_navigator-7] [INFO] [1692805001.643248017] [bt_navigator_rclcpp_node]: "FollowPath" BtActionNode initialized
[lifecycle_manager-9] [INFO] [1692805001.644764882] [lifecycle_manager_navigation]: Configuring waypoint_follower
[waypoint_follower-8] [INFO] [1692805001.645039723] [waypoint_follower]: Configuring
[lifecycle_manager-9] [INFO] [1692805001.653683801] [lifecycle_manager_navigation]: Activating controller_server
[controller_server-4] [INFO] [1692805001.653885972] [controller_server]: Activating
[controller_server-4] [INFO] [1692805001.653933962] [local_costmap.local_costmap]: Activating
[controller_server-4] [INFO] [1692805001.653955832] [local_costmap.local_costmap]: Checking transform
[controller_server-4] [INFO] [1692805001.654077133] [local_costmap.local_costmap]: start
[lifecycle_manager-9] [INFO] [1692805001.655242587] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-5] [INFO] [1692805001.655459977] [planner_server]: Activating
[planner_server-5] [INFO] [1692805001.655511578] [global_costmap.global_costmap]: Activating
[planner_server-5] [INFO] [1692805001.655529118] [global_costmap.global_costmap]: Checking transform
[planner_server-5] [INFO] [1692805001.655576178] [global_costmap.global_costmap]: Timed out waiting for transform from base_footprint to map to become available, tf error: Could not find a connection between 'map' and 'base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[amcl-2] [INFO] [1692805001.691850637] [amcl]: createLaserObject
[planner_server-5] [INFO] [1692805002.155691821] [global_costmap.global_costmap]: Timed out waiting for transform from base_footprint to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 163.166000 but the earliest data is at time 163.211000, when looking up transform from frame [base_footprint] to frame [map]
[planner_server-5] [INFO] [1692805002.655728626] [global_costmap.global_costmap]: start
[planner_server-5] [INFO] [1692805002.707280645] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[lifecycle_manager-9] [INFO] [1692805002.707929967] [lifecycle_manager_navigation]: Activating recoveries_server
[recoveries_server-6] [INFO] [1692805002.708195098] [recoveries_server]: Activating
[recoveries_server-6] [INFO] [1692805002.708248648] [recoveries_server]: Activating spin
[recoveries_server-6] [INFO] [1692805002.708273698] [recoveries_server]: Activating backup
[recoveries_server-6] [INFO] [1692805002.708295028] [recoveries_server]: Activating wait
[lifecycle_manager-9] [INFO] [1692805002.708705590] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-7] [INFO] [1692805002.708929950] [bt_navigator]: Activating
[lifecycle_manager-9] [INFO] [1692805002.709400032] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-8] [INFO] [1692805002.709608193] [waypoint_follower]: Activating
[lifecycle_manager-9] [INFO] [1692805002.710058314] [lifecycle_manager_navigation]: Managed nodes are active

Errors or warnings in the terminal: -->As you can see only this occur to be a problem but its not effect the anvigation. So i guess we can ignore it?: [planner_server-5] [INFO] [1692805002.155691821] [global_costmap.global_costmap]: Timed out waiting for transform from base_footprint to map to become available, tf error: Lookup would require extrapolation into the past. Requested time 163.166000 but the earliest data is at time 163.211000, when looking up transform from frame [base_footprint] to frame [map]

Social navigation plugin is loaded properly?: --> I guess the social layer is loaded properly because there are no errors:

[controller_server-4] [INFO] [1692805001.522934572] [local_costmap.local_costmap]: Using plugin "social_layer"
[controller_server-4] [INFO] [1692805001.525747061] [local_costmap.local_costmap]: SocialLayer: subscribed to topic /people
[controller_server-4] [INFO] [1692805001.528910712] [local_costmap.local_costmap]: Initialized plugin "social_layer"

Check that you are properly publishing the positions and velocities of your moving agents in the topic "/people" using the message people_msgs/msg/People.: --> The command ros2 topic echo /people showing nothing and there are no publisher:

ros2 topic info /people
Type: people_msgs/msg/People
Publisher count: 0
Subscription count: 2

Thank you very much neoperez

noeperez commented 10 months ago

Hello,

As you have seen, nobody is publishing in the topic /people, that's the problem. You need to provide the info of the moving agents to the layer. To do it you need to program a node to take your agents data, to fill the people_msgs/People messages and to publish them in the topic /people.

NamTruongTran0 commented 10 months ago

Ok i thought its just plug and play.

noeperez commented 10 months ago

Hi again, Sorry, I can't do anything else. You need to understand how you are including and publishing your people in your simulation. Then, reuse than info to publish it in the required format of the layer. That layer can not be plug and play, because the data about the people is not something standard included in the regular nav2 system.

NamTruongTran0 commented 10 months ago

Hey noeperez,

thank you so much. I'm really grateful for your help and your contributions ;)

noeperez commented 10 months ago

Yes, both topics contain the relevant information that you need, but first you need to know what type of messages are being published through each topic. Then, you need to check the message type required by the social layer (which is people_msgs/People) and check the data that you need to fill. You can not send the messages of the pedsim_simulator topics directly to the topic /people. You need to send the proper message to the proper topic names. That is ROS basis.