Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ros2 social navigation layer foxy distribution #4

Open
NamTruongTran0 opened this issue Aug 23, 2023 · 11 comments
Open

ros2 social navigation layer foxy distribution #4

NamTruongTran0 opened this issue Aug 23, 2023 · 11 comments

Comments

@NamTruongTran0
Copy link

NamTruongTran0 commented Aug 23, 2023 •
edited
Loading

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
Copy link
Member

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:

  • Check if the plugin is properly loaded when you launch the navigation (no errors or warnings printed in the terminal).
  • 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.
  • Then, you could enable the "publish_occgrid" parameter to publish and occgrid so you can visualize (by adding it on RViz) if something is being published.

@NamTruongTran0
Copy link
Author

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
Copy link
Member

noeperez commented Aug 23, 2023 via email •
edited
Loading

@NamTruongTran0
Copy link
Author

Ok i thought its just plug and play.

@noeperez
Copy link
Member

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
Copy link
Author

NamTruongTran0 commented Aug 23, 2023 •
edited
Loading

Hey noeperez,

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

@noeperez
Copy link
Member

noeperez commented Aug 24, 2023 •
edited
Loading

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.

@immanueln98
Copy link

Hi neoperez, what frame should the people messages be in when publishing to the social layer?

@noeperez
Copy link
Member

Hi!

You can publish the people in the frame that you want if the frame is part of a well-formed TF tree. The node will transform the coordinates to the global frame of the layer automatically.

@immanueln98
Copy link

immanueln98 commented Oct 30, 2024 •
edited
Loading

Hi!

You can publish the people in the frame that you want if the frame is part of a well-formed TF tree. The node will transform the coordinates to the global frame of the layer automatically.

Thanks Noe for your reply!
I'm currently publishing them in the map frame. I receive detected objects from stereo labs zed camera mounted on my clearpath husky a200 platform. I am able to view the cost maps of people in rviz2 but when navigating, the robot doesn't seem to pick them up and just plans a path right through. What could I be doing wrong if you do know?

@noeperez
Copy link
Member

Are you properly adding this layer to your local or global costmap of the navigation system? I mean, when you say "I am able to view the cost maps of people in rviz2", are you talking about the local or global costmap? You should check (visualizing in RViz) that the people of this layer is properly added to the desired costmap (along with the other layers). In any case, I think it's just a problem of configuration of the navigation system.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@noeperez @immanueln98 @NamTruongTran0