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.

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

No costmaps being published #3034

Closed
tcdtech opened this issue Jun 21, 2022 · 9 comments
Closed

No costmaps being published #3034

tcdtech opened this issue Jun 21, 2022 · 9 comments
Labels
question Further information is requested

Comments

@tcdtech
Copy link

tcdtech commented Jun 21, 2022

Bug? report

Required Info:

  • Operating System:
    Ubuntu 22.04 Jammy
  • ROS2 Version:
    Humble
  • Version or commit hash:
    ros-humble-navigation2 1.1.0-2jammy.20220607.182532
  • DDS implementation:
    Fast-RTPS (I believe, as it's the default)

Steps to reproduce issue

I'm running the nav2 stack pretty much as specified in the nav2_bringup package but without AMCL or obstruction layers as I currently have no LIDAR.

Expected behavior

I should be able to see (and debug) my costmaps.

Actual behavior

I can't see any costmaps in rviz2 and don't see anything when doing "ros2 topic echo" on the costmap topics.

Rudimentary navigation seems to work (robot will drive from A to B and will plan paths around objects in my static map) so the nav stack seems to have a costmap available to it. I'm not sure if they're being inflated properly though.

Additional information

Not sure if this is a bug or if I'm doing something wrong.

  • They do not show up in RViz (No map received warning)
  • If I do 'ros2 topic echo /global_costmap/costmap' I get no output no matter what order I run things in.
  • The same is true for /global_costmap/costmap_raw, /global_costmap/costmap_updates, /local_costmap/costmap, /local_costmap/costmap_raw and /local_costmap/costmap_updates
  • The footprint is being published for both global and local nodes though so they're definitely running.

The launch file is almost identical to the nav2_bringup one (though I'm happy to provide my version if it would help).

These are the costmap parameters I'm using though I've tried changing almost all of them already.

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 0.5
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [0.3,-0.15], [0.3,0.15], [0.225,0.15], [0.225,0.5], [-0.225, 0.5], [-0.225, 0.15], [-0.3,0.15], [-0.3, -0.15] ]"
      resolution: 0.05
      track_unknown_space: False
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: False
        subscribe_to_updates: True
        map_topic: /map
        enabled: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
        enabled: True
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

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
      footprint: "[ [-0.15, 0.3], [0.15,0.3], [0.15,0.225], [0.5,0.225], [0.5,-0.225], [0.15,-0.225], [0.15,-0.3], [-0.15,-0.3] ]"
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: False
        map_topic: /map
        enabled: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
        enabled: True
      always_send_full_costmap: True
@SteveMacenski
Copy link
Member

SteveMacenski commented Jun 21, 2022

Note #3014, I think that's really the crux of this ticket where we are tracking this issue

will plan paths around objects in my static map

Your comment about echoing is probably due to the QoS and since you can see the static layer in rviz means its being published.

@tcdtech
Copy link
Author

tcdtech commented Jun 21, 2022

I read that bug report before posting mine. Wasn't sure if it was relevant to me or not.

Key differences:

  • I'm not using the obstacle layer at all.
  • I have a static layer applied to both local and global costmaps (which someone mentioned fixed that issue).

To clarify, I can view the /map topic in RViz but none of the costmaps ever show up, no matter what QoS settings I set or what order I start things in (RViz before or after the navstack makes no difference).

Does it still sound like the same issue? I'm not at all familiar with the source code..

Screenshot 2022-06-21 212456

@SteveMacenski
Copy link
Member

SteveMacenski commented Jun 21, 2022

Oh neat, can you post your logs? Maybe something just didn't finish transition right. Post your parameters and the terminal output from launch

Edit: I just tested the binaries and they seem to work for me, so I suspect it has to do with parameters or something failing to bringup properly.

I highly doubt this issue is related to Fast-DDS, but its worth trying Cyclone real quick just given we're finding a few quirks here and there from the transition.

@tcdtech
Copy link
Author

tcdtech commented Jun 21, 2022

Tyvm for your time/help btw.

I was just trying Cyclone-DDS. It doesn't seem to make a difference. Will restart everything and try again just incase parts of the networks are still using Fast-DDS.

Launch terminal output

[INFO] [launch]: All log files can be found below /home/tom/.ros/log/2022-06-21-22-02-39-389046-tom-VirtualBox-4176
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_server-1]: process started with pid [4178]
[INFO] [controller_server-2]: process started with pid [4180]
[INFO] [smoother_server-3]: process started with pid [4182]
[INFO] [planner_server-4]: process started with pid [4184]
[INFO] [behavior_server-5]: process started with pid [4211]
[INFO] [bt_navigator-6]: process started with pid [4213]
[INFO] [waypoint_follower-7]: process started with pid [4228]
[INFO] [lifecycle_manager-8]: process started with pid [4231]
[smoother_server-3] [INFO] [1655845360.968481024] [smoother_server]: 
[smoother_server-3] 	smoother_server lifecycle node launched. 
[smoother_server-3] 	Waiting on external lifecycle transitions to activate
[smoother_server-3] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[smoother_server-3] [INFO] [1655845360.971962158] [smoother_server]: Creating smoother server
[bt_navigator-6] [INFO] [1655845360.973841346] [bt_navigator]: 
[bt_navigator-6] 	bt_navigator lifecycle node launched. 
[bt_navigator-6] 	Waiting on external lifecycle transitions to activate
[bt_navigator-6] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-6] [INFO] [1655845360.973923499] [bt_navigator]: Creating
[planner_server-4] [INFO] [1655845360.997047382] [planner_server]: 
[planner_server-4] 	planner_server lifecycle node launched. 
[planner_server-4] 	Waiting on external lifecycle transitions to activate
[planner_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-4] [INFO] [1655845361.003563966] [planner_server]: Creating
[lifecycle_manager-8] [INFO] [1655845361.149762354] [lifecycle_manager_navigation]: Creating
[lifecycle_manager-8] [INFO] [1655845361.504895657] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[behavior_server-5] [INFO] [1655845361.775748970] [behavior_server]: 
[behavior_server-5] 	behavior_server lifecycle node launched. 
[behavior_server-5] 	Waiting on external lifecycle transitions to activate
[behavior_server-5] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-7] [INFO] [1655845362.470568889] [waypoint_follower]: 
[waypoint_follower-7] 	waypoint_follower lifecycle node launched. 
[waypoint_follower-7] 	Waiting on external lifecycle transitions to activate
[waypoint_follower-7] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-7] [INFO] [1655845362.471101123] [waypoint_follower]: Creating
[map_server-1] [INFO] [1655845362.649547247] [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] [1655845362.650125279] [map_server]: Creating
[controller_server-2] [INFO] [1655845363.303613155] [controller_server]: 
[controller_server-2] 	controller_server lifecycle node launched. 
[controller_server-2] 	Waiting on external lifecycle transitions to activate
[controller_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-2] [INFO] [1655845363.312442590] [controller_server]: Creating controller server
[planner_server-4] [INFO] [1655845364.672643226] [global_costmap.global_costmap]: 
[planner_server-4] 	global_costmap lifecycle node launched. 
[planner_server-4] 	Waiting on external lifecycle transitions to activate
[planner_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-4] [INFO] [1655845364.688895319] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-8] [INFO] [1655845365.350184071] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[lifecycle_manager-8] [INFO] [1655845365.350232968] [lifecycle_manager_navigation]: Configuring map_server
[map_server-1] [INFO] [1655845365.383835030] [map_server]: Configuring
[controller_server-2] [INFO] [1655845365.419280602] [local_costmap.local_costmap]: 
[controller_server-2] 	local_costmap lifecycle node launched. 
[controller_server-2] 	Waiting on external lifecycle transitions to activate
[controller_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-2] [INFO] [1655845365.419791302] [local_costmap.local_costmap]: Creating Costmap
[lifecycle_manager-8] [INFO] [1655845366.038818345] [lifecycle_manager_navigation]: Configuring controller_server
[controller_server-2] [INFO] [1655845366.039740749] [controller_server]: Configuring controller interface
[controller_server-2] [INFO] [1655845366.040636001] [controller_server]: getting goal checker plugins..
[controller_server-2] [INFO] [1655845366.054031800] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-2] [INFO] [1655845366.054191240] [local_costmap.local_costmap]: Configuring
[controller_server-2] [INFO] [1655845366.293403009] [local_costmap.local_costmap]: Using plugin "static_layer"
[controller_server-2] [INFO] [1655845366.315467416] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[controller_server-2] [INFO] [1655845366.376014137] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[controller_server-2] [INFO] [1655845366.376148626] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-2] [INFO] [1655845366.407795240] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-2] [INFO] [1655845366.914678565] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-2] [INFO] [1655845366.980932930] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-2] [INFO] [1655845367.006016992] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
[controller_server-2] [INFO] [1655845367.028814848] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-2] [INFO] [1655845367.084781437] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-2] [INFO] [1655845367.438525601] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-2] [INFO] [1655845367.455682636] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.456973356] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-2] [INFO] [1655845367.473379005] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.475117266] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-2] [INFO] [1655845367.480914810] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.484038990] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-2] [INFO] [1655845367.491683262] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.494683411] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-2] [INFO] [1655845367.507971096] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.510091628] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-2] [INFO] [1655845367.518607925] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.520495523] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-2] [INFO] [1655845367.525482177] [controller_server]: Critic plugin initialized
[controller_server-2] [INFO] [1655845367.525707399] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-8] [INFO] [1655845367.729665764] [lifecycle_manager_navigation]: Configuring smoother_server
[smoother_server-3] [INFO] [1655845367.730326219] [smoother_server]: Configuring smoother server
[smoother_server-3] [INFO] [1655845367.940009829] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
[smoother_server-3] [INFO] [1655845367.969404430] [smoother_server]: Smoother Server has simple_smoother  smoothers available.
[lifecycle_manager-8] [INFO] [1655845368.158963396] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-4] [INFO] [1655845368.159429842] [planner_server]: Configuring
[planner_server-4] [INFO] [1655845368.159454689] [global_costmap.global_costmap]: Configuring
[planner_server-4] [INFO] [1655845368.343484197] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-4] [INFO] [1655845368.402501025] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-4] [INFO] [1655845368.421189908] [global_costmap.global_costmap]: Subscribing to updates
[planner_server-4] [INFO] [1655845368.466728305] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-4] [INFO] [1655845368.466798039] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-4] [INFO] [1655845368.487979526] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-4] [INFO] [1655845368.667606915] [planner_server]: Created global planner plugin GridBased of type nav2_smac_planner/SmacPlannerHybrid
[planner_server-4] [INFO] [1655845368.667725049] [planner_server]: Configuring GridBased of type SmacPlannerHybrid
[planner_server-4] [INFO] [1655845368.720347638] [planner_server]: Even sized heuristic lookup table size set 400.000000, increasing size by 1 to make odd
[planner_server-4] [INFO] [1655845371.069272218] [planner_server]: Configured plugin GridBased of type SmacPlannerHybrid with maximum iterations 1000000, and allowing unknown traversal. Using motion model: Dubin.
[planner_server-4] [INFO] [1655845371.072060547] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-8] [INFO] [1655845371.505882782] [lifecycle_manager_navigation]: Configuring behavior_server
[behavior_server-5] [INFO] [1655845371.506710111] [behavior_server]: Configuring
[behavior_server-5] [INFO] [1655845371.755938998] [behavior_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
[behavior_server-5] [INFO] [1655845371.763135673] [behavior_server]: Configuring spin
[behavior_server-5] [INFO] [1655845372.006975985] [behavior_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
[behavior_server-5] [INFO] [1655845372.010582251] [behavior_server]: Configuring backup
[behavior_server-5] [INFO] [1655845372.229774204] [behavior_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
[behavior_server-5] [INFO] [1655845372.230390207] [behavior_server]: Configuring drive_on_heading
[behavior_server-5] [INFO] [1655845372.639301911] [behavior_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
[behavior_server-5] [INFO] [1655845372.640346746] [behavior_server]: Configuring wait
[lifecycle_manager-8] [INFO] [1655845372.977795823] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-6] [INFO] [1655845372.978579675] [bt_navigator]: Configuring
[lifecycle_manager-8] [INFO] [1655845374.617042144] [lifecycle_manager_navigation]: Configuring waypoint_follower
[waypoint_follower-7] [INFO] [1655845374.617466255] [waypoint_follower]: Configuring
[waypoint_follower-7] [INFO] [1655845375.014287746] [waypoint_follower]: Created waypoint_task_executor : wait_at_waypoint of type nav2_waypoint_follower::WaitAtWaypoint
[lifecycle_manager-8] [INFO] [1655845375.040588499] [lifecycle_manager_navigation]: Activating map_server
[map_server-1] [INFO] [1655845375.043836978] [map_server]: Activating
[map_server-1] [INFO] [1655845375.044470480] [map_server]: Creating bond (map_server) to lifecycle manager.
[planner_server-4] [INFO] [1655845375.067419659] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 600 X 600 at 0.050000 m/pix
[controller_server-2] [INFO] [1655845375.072654540] [local_costmap.local_costmap]: StaticLayer: Resizing static layer to 600 X 600 at 0.050000 m/pix
[lifecycle_manager-8] [INFO] [1655845375.224717971] [lifecycle_manager_navigation]: Server map_server connected with bond.
[lifecycle_manager-8] [INFO] [1655845375.225626638] [lifecycle_manager_navigation]: Activating controller_server
[controller_server-2] [INFO] [1655845375.227809372] [controller_server]: Activating
[controller_server-2] [INFO] [1655845375.227940092] [local_costmap.local_costmap]: Activating
[controller_server-2] [INFO] [1655845375.227972464] [local_costmap.local_costmap]: Checking transform
[controller_server-2] [INFO] [1655845375.228226532] [local_costmap.local_costmap]: start
[controller_server-2] [INFO] [1655845375.278415949] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845375.457881379] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-8] [INFO] [1655845375.457939618] [lifecycle_manager_navigation]: Activating smoother_server
[smoother_server-3] [INFO] [1655845375.466729975] [smoother_server]: Activating
[smoother_server-3] [INFO] [1655845375.466781746] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845375.656539273] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
[lifecycle_manager-8] [INFO] [1655845375.656661385] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-4] [INFO] [1655845375.657425558] [planner_server]: Activating
[planner_server-4] [INFO] [1655845375.657453735] [global_costmap.global_costmap]: Activating
[planner_server-4] [INFO] [1655845375.657464534] [global_costmap.global_costmap]: Checking transform
[planner_server-4] [INFO] [1655845375.657616110] [global_costmap.global_costmap]: start
[planner_server-4] [INFO] [1655845375.709703994] [planner_server]: Activating plugin GridBased of type SmacPlannerHybrid
[planner_server-4] [INFO] [1655845375.745217020] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845376.031783918] [lifecycle_manager_navigation]: Server planner_server connected with bond.
[lifecycle_manager-8] [INFO] [1655845376.031935955] [lifecycle_manager_navigation]: Activating behavior_server
[behavior_server-5] [INFO] [1655845376.034026609] [behavior_server]: Activating
[behavior_server-5] [INFO] [1655845376.034085600] [behavior_server]: Activating spin
[behavior_server-5] [INFO] [1655845376.034105677] [behavior_server]: Activating backup
[behavior_server-5] [INFO] [1655845376.034118074] [behavior_server]: Activating drive_on_heading
[behavior_server-5] [INFO] [1655845376.034134221] [behavior_server]: Activating wait
[behavior_server-5] [INFO] [1655845376.034156169] [behavior_server]: Creating bond (behavior_server) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845376.408853996] [lifecycle_manager_navigation]: Server behavior_server connected with bond.
[lifecycle_manager-8] [INFO] [1655845376.410449726] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-6] [INFO] [1655845376.414297922] [bt_navigator]: Activating
[bt_navigator-6] [INFO] [1655845381.536547309] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845381.940127541] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
[lifecycle_manager-8] [INFO] [1655845381.940228654] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-7] [INFO] [1655845381.943561719] [waypoint_follower]: Activating
[waypoint_follower-7] [INFO] [1655845381.943620157] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1655845382.314041617] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
[lifecycle_manager-8] [INFO] [1655845382.315013559] [lifecycle_manager_navigation]: Managed nodes are active
[lifecycle_manager-8] [INFO] [1655845382.323647812] [lifecycle_manager_navigation]: Creating bond timer...

Full Params file

# map_server_params.yaml
map_server:
    ros__parameters:
        yaml_filename: "/home/tom/dev_ws/install/kryton/share/kryton/maps/map.yaml"

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 0.5
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [0.3,-0.15], [0.3,0.15], [0.225,0.15], [0.225,0.5], [-0.225, 0.5], [-0.225, 0.15], [-0.3,0.15], [-0.3, -0.15] ]"
      resolution: 0.05
      track_unknown_space: False
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        subscribe_to_updates: True
        map_topic: /map
        enabled: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
        enabled: True
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

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
      footprint: "[ [-0.15, 0.3], [0.15,0.3], [0.15,0.225], [0.5,0.225], [0.5,-0.225], [0.15,-0.225], [0.15,-0.3], [-0.15,-0.3] ]"
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        map_topic: /map
        enabled: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
        enabled: True
      always_send_full_costmap: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/local_ekf
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # '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_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
    - nav2_single_trigger_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

bt_navigator_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.4
      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.6
      max_vel_y: 0.0
      max_vel_theta: 3.0
      min_speed_xy: 0.0
      max_speed_xy: 0.6
      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: 1.0
      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
      
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      allow_unknown: true
      minimum_turning_radius: 0.05

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    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

Launch file for good measure


import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='kryton').find('kryton')
    configured_params = map_server_config = os.path.join(pkg_share, 'config/map_server_params.yaml')

    use_respawn = False


    remappings = [('/tf', 'tf'),
                    ('/tf_static', 'tf_static')]

    map_server = launch_ros.actions.Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        parameters=[map_server_config]
    )


    controller = launch_ros.actions.Node(
                package='nav2_controller',
                executable='controller_server',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    smoother = launch_ros.actions.Node(
                package='nav2_smoother',
                executable='smoother_server',
                name='smoother_server',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    planner = launch_ros.actions.Node(
                package='nav2_planner',
                executable='planner_server',
                name='planner_server',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    behaviours = launch_ros.actions.Node(
                package='nav2_behaviors',
                executable='behavior_server',
                name='behavior_server',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    navigator = launch_ros.actions.Node(
                package='nav2_bt_navigator',
                executable='bt_navigator',
                name='bt_navigator',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    waypoint_follower = launch_ros.actions.Node(
                package='nav2_waypoint_follower',
                executable='waypoint_follower',
                name='waypoint_follower',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                remappings=remappings)
    #velocity_smoother = launch_ros.actions.Node(
    #            package='nav2_velocity_smoother',
    #            executable='velocity_smoother',
    #            name='velocity_smoother',
    #            output='screen',
    #            respawn=use_respawn,
    #            respawn_delay=2.0,
    #            parameters=[configured_params],
    #            remappings=remappings +
    #                       [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')])


    lifecycle_nodes = ['map_server',
                       'controller_server',
                       'smoother_server',
                       'planner_server',
                       'behavior_server',
                       'bt_navigator',
                       'waypoint_follower']

    lifecycle_manager = launch_ros.actions.Node(
                package='nav2_lifecycle_manager',
                executable='lifecycle_manager',
                name='lifecycle_manager_navigation',
                output='screen',
                parameters=[
                            #{'use_sim_time': use_sim_time},
                            {'autostart': True},
                            {'node_names': lifecycle_nodes}
                            ])


    return launch.LaunchDescription([

        map_server,
        controller,
        smoother,
        planner,
        behaviours,
        navigator,
        waypoint_follower,
        #velocity_smoother,
        
        lifecycle_manager
    ])

@SteveMacenski
Copy link
Member

      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        map_topic: /map

The only thing that comes to mind looking over that is maybe your static layer isn't getting the initial map? Topic is wrong or the QoS settings are wrong (e.g. map_subscribe_transient_local).

Your launch file does something weird and has a second configuration file its setting for map server

    configured_params = map_server_config = os.path.join(pkg_share, 'config/map_server_params.yaml')

I'd look into that. What if you start from just the example tb3_simulation_launch.py file? Does that work? If so, I think you can start working backward from there to find the issue.

@SteveMacenski SteveMacenski reopened this Jun 21, 2022
@SteveMacenski SteveMacenski added the question Further information is requested label Jun 21, 2022
@tcdtech
Copy link
Author

tcdtech commented Jun 21, 2022

I've tried with map_subscript_transient_local as both true and false.

I've tried map and /map as the map topic (it's definitely 'map' because subscribing to that topic for the map display in rviz works)

I believe these three lines in the launch log imply that the static layer is getting the map:

[map_server-1] [INFO] [1655845375.044470480] [map_server]: Creating bond (map_server) to lifecycle manager.
[planner_server-4] [INFO] [1655845375.067419659] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 600 X 600 at 0.050000 m/pix
[controller_server-2] [INFO] [1655845375.072654540] [local_costmap.local_costmap]: StaticLayer: Resizing static layer to 600 X 600 at 0.050000 m/pix

The map file is 600x600 pixels at 0.05m/pix and I don't define that anywhere other than the map config so (unless I'm missing something) I'm pretty sure both global and local costmaps 'see' the original map when it's published on the /map topic.

You're right about that double assignment being a little weird, it was just a cheap quick way (lazy) thing I did when I merged the map_server config with the nav config from an earlier testing configuration. I'll tidy that up.

I'll try the default launch file and report back.

@SteveMacenski
Copy link
Member

I believe these three lines in the launch log imply that the static layer is getting the map

Agreed.

I'm not actually referring to double assignment as I am that you have a separate config/map_server_params.yaml being used (unless that's what you provided me above and its just named map_server_params but contains everything?) but the main file you linked to above isn't set anywhere in your launch file.

@tcdtech
Copy link
Author

tcdtech commented Jun 23, 2022

unless that's what you provided me above and its just named map_server_params but contains everything?
This was correct. The parameter file was just named poorly..

So I've figured it out and it was, of course, the dumbest thing. Got the tb3_simulation_launch going (which produced costmaps fine) then copied the key files over to my package and started breaking it down.

use_sim_time: True in my parameters file was the only culprit, I'd removed the reference to RewrittenYaml() in the launch file so it wasn't being re-written to False. I'm not running a sim, so several of the nodes were waiting for the sim clock to tick which it never did. I didn't even know that's how use_sim_time worked, hadn't used gazebo before last night.

Thank you so so much for your help, time and everything else you do for ROS. As tivial as it ended up being I wouldn't have worked it out for a long time without your advice.

Bonus robot & cat tax:
https://photos.app.goo.gl/Zz4VmYNZS5LgBGSJ8

That's just my testbed, running on windscreen wiper motors and and old nissan leaf battery module. I'm welding up the chassis for my second bot as we speak!

@RobinBaruffa
Copy link

Had the exact same problem, thanks for the answer

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
question Further information is requested
Projects
None yet
Development

No branches or pull requests

3 participants