search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.49k stars 1.26k forks source link

New Behavior Tree Action Node not Initializing #2423

Closed kevinlyu1 closed 3 years ago

kevinlyu1 commented 3 years ago

Bug report

Required Info:

Steps to reproduce issue

  1. Create a custom node. What I did was just copy and paste the "wait" action and then change it to "wait_to" action.
  2. Create a behavior tree using the custom action.
  3. Run the bt_navigator so that it tries to initialize the node.

Expected behavior

Starts up action server for the new action node "wait_to" that is created by me.

Actual behavior

Does not start up action server for new action node created. Instead the action server says that it's "Waiting for "wait_to" action server, but never says afterwards ""WaitTo" BtActionNode initialized". Below is an example of what happens on the terminal. ` [INFO] [launch]: All log files can be found below /home/nvidia/.ros/log/2021-06-23-11-29-42-376251-viprd-1-81455 [INFO] [launch]: Default logging verbosity is set to INFO urdf_file_name : robot_description.urdf [INFO] [robot_state_publisher-1]: process started with pid [81457] [INFO] [joy_teleop-2]: process started with pid [81459] [INFO] [roboteq_driver-3]: process started with pid [81461] [INFO] [vel_switch-4]: process started with pid [81463] [INFO] [urg_node_driver-5]: process started with pid [81465] [INFO] [um7_node-6]: process started with pid [81467] [INFO] [ekf_node-7]: process started with pid [81469] [INFO] [cartographer_node-8]: process started with pid [81472] [INFO] [occupancy_grid_node-9]: process started with pid [81503] [INFO] [controller_server-10]: process started with pid [81508] [INFO] [planner_server-11]: process started with pid [81531] [INFO] [recoveries_server-12]: process started with pid [81537] [INFO] [bt_navigator-13]: process started with pid [81540] [INFO] [lifecycle_manager-14]: process started with pid [81543] [roboteq_driver-3] Opened port /dev/roboteq_sdc [robot_state_publisher-1] [WARN] [1624462183.699825577] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed in the future. [robot_state_publisher-1] Parsing robot urdf xml string. [robot_state_publisher-1] Link imu_link had 0 children [robot_state_publisher-1] Link laser_link had 0 children [robot_state_publisher-1] [INFO] [1624462183.729233032] [robot_state_publisher]: got segment base_link [robot_state_publisher-1] [INFO] [1624462183.729343017] [robot_state_publisher]: got segment imu_link [robot_state_publisher-1] [INFO] [1624462183.729386314] [robot_state_publisher]: got segment laser_link [roboteq_driver-3] [INFO] [1624462183.779836580] [roboteq_driver]: Roboteq driver started [urg_node_driver-5] [INFO] [1624462183.810089741] [urg_node]: Connected to network device with intensity and ID: B2009259 [urg_node_driver-5] [INFO] [1624462183.851348249] [urg_node]: Streaming data. [um7_node-6] [INFO] [1624462183.885968951] [um7_node]: um7_driver successfully connected to serial port /dev/um7. [um7_node-6] [INFO] [1624462183.962175716] [um7_node]: Setting update rate to 20Hz [um7_node-6] [INFO] [1624462184.041615096] [um7_node]: Sending command: zero gyroscopes

[bt_navigator-13] [INFO] [1624462184.308300519] [bt_navigator]: [bt_navigator-13] bt_navigator lifecycle node launched. [bt_navigator-13] Waiting on external lifecycle transitions to activate [bt_navigator-13] See https://design.ros2.org/articles/node_lifecycle.html for more information.

[controller_server-10] [INFO] [1624462184.322431663] [controller_server]: [controller_server-10] controller_server lifecycle node launched. [controller_server-10] Waiting on external lifecycle transitions to activate [controller_server-10] See https://design.ros2.org/articles/node_lifecycle.html for more information. [planner_server-11] [INFO] [1624462184.391980909] [planner_server]: [planner_server-11] planner_server lifecycle node launched. [planner_server-11] Waiting on external lifecycle transitions to activate [planner_server-11] See https://design.ros2.org/articles/node_lifecycle.html for more information. [recoveries_server-12] [INFO] [1624462184.442386535] [recoveries_server]: [recoveries_server-12] recoveries_server lifecycle node launched. [recoveries_server-12] Waiting on external lifecycle transitions to activate [recoveries_server-12] See https://design.ros2.org/articles/node_lifecycle.html for more information. [lifecycle_manager-14] [INFO] [1624462184.539913299] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients [lifecycle_manager-14] [INFO] [1624462184.592549415] [lifecycle_manager_navigation]: Starting managed nodes bringup... [lifecycle_manager-14] [INFO] [1624462184.592733353] [lifecycle_manager_navigation]: Configuring controller_server

[controller_server-10] [INFO] [1624462184.697051206] [controller_server]: Creating controller server [planner_server-11] [INFO] [1624462184.835566043] [global_costmap.global_costmap]: [planner_server-11] global_costmap lifecycle node launched. [planner_server-11] Waiting on external lifecycle transitions to activate [planner_server-11] See https://design.ros2.org/articles/node_lifecycle.html for more information. [planner_server-11] [INFO] [1624462184.840088113] [global_costmap.global_costmap]: Creating Costmap [controller_server-10] [INFO] [1624462184.958350613] [local_costmap.local_costmap]: [controller_server-10] local_costmap lifecycle node launched. [controller_server-10] Waiting on external lifecycle transitions to activate [controller_server-10] See https://design.ros2.org/articles/node_lifecycle.html for more information. [controller_server-10] [INFO] [1624462184.961582811] [local_costmap.local_costmap]: Creating Costmap [controller_server-10] [INFO] [1624462185.020491003] [controller_server]: Configuring controller interface [controller_server-10] [INFO] [1624462185.020790430] [controller_server]: Controller frequency set to 10.0000Hz

[controller_server-10] [INFO] [1624462185.058669634] [local_costmap.local_costmap]: Using plugin "voxel_layer" [controller_server-10] [INFO] [1624462185.070305933] [local_costmap.local_costmap]: Subscribed to Topics: scan [controller_server-10] [INFO] [1624462185.103445945] [local_costmap.local_costmap]: Initialized plugin "voxel_layer" [controller_server-10] [INFO] [1624462185.103675835] [local_costmap.local_costmap]: Using plugin "inflation_layer" [controller_server-10] [INFO] [1624462185.104804201] [local_costmap.local_costmap]: Initialized plugin "inflation_layer" [controller_server-10] [INFO] [1624462185.146747198] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker [controller_server-10] [INFO] [1624462185.152822470] [controller_server]: Created goal_checker : goal_checker of type nav2_controller::SimpleGoalChecker [controller_server-10] [INFO] [1624462185.168818405] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner [controller_server-10] [INFO] [1624462185.176079708] [controller_server]: Setting transform_tolerance to 0.200000 [controller_server-10] [INFO] [1624462185.251701026] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic) [controller_server-10] [INFO] [1624462185.253744795] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.256655357] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic) [controller_server-10] [INFO] [1624462185.260159463] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.260481003] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic) [controller_server-10] [INFO] [1624462185.260645773] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.260804879] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic) [controller_server-10] [INFO] [1624462185.269315637] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.271177323] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic) [controller_server-10] [INFO] [1624462185.272630972] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.272866079] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic) [controller_server-10] [INFO] [1624462185.273168483] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.273322692] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic) [controller_server-10] [INFO] [1624462185.277490166] [controller_server]: Critic plugin initialized [controller_server-10] [INFO] [1624462185.277628696] [controller_server]: Controller Server has FollowPath controllers available. [lifecycle_manager-14] [INFO] [1624462185.308623050] [lifecycle_manager_navigation]: Configuring planner_server

[planner_server-11] [INFO] [1624462185.336054737] [global_costmap.global_costmap]: Using plugin "static_layer" [planner_server-11] [INFO] [1624462185.357344719] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with volatile durability [planner_server-11] [INFO] [1624462185.360096592] [global_costmap.global_costmap]: Initialized plugin "static_layer" [planner_server-11] [INFO] [1624462185.360214418] [global_costmap.global_costmap]: Using plugin "obstacle_layer" [planner_server-11] [INFO] [1624462185.360623030] [global_costmap.global_costmap]: Subscribed to Topics: scan [planner_server-11] [INFO] [1624462185.366378619] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer" [planner_server-11] [INFO] [1624462185.366501661] [global_costmap.global_costmap]: Using plugin "inflation_layer" [planner_server-11] [INFO] [1624462185.375521096] [global_costmap.global_costmap]: Initialized plugin "inflation_layer" [planner_server-11] [INFO] [1624462185.421404332] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner [planner_server-11] [INFO] [1624462185.421561806] [planner_server]: Configuring plugin GridBased of type NavfnPlanner [planner_server-11] [INFO] [1624462185.456210763] [planner_server]: Planner Server has GridBased planners available. [lifecycle_manager-14] [INFO] [1624462185.485492361] [lifecycle_manager_navigation]: Configuring recoveries_server

[recoveries_server-12] [INFO] [1624462185.522941672] [recoveries_server]: Creating recovery plugin spin of type nav2_recoveries/Spin [recoveries_server-12] [INFO] [1624462185.526301616] [recoveries_server]: Configuring spin [recoveries_server-12] [INFO] [1624462185.569332146] [recoveries_server]: Creating recovery plugin back_up of type nav2_recoveries/BackUp [recoveries_server-12] [INFO] [1624462185.573785127] [recoveries_server]: Configuring back_up [recoveries_server-12] [INFO] [1624462185.607266550] [recoveries_server]: Creating recovery plugin wait of type nav2_recoveries/Wait [recoveries_server-12] [INFO] [1624462185.611168005] [recoveries_server]: Configuring wait [recoveries_server-12] [INFO] [1624462185.646280424] [recoveries_server]: Creating recovery plugin sms_recovery of type sdr_recoveries/SmsRecovery [recoveries_server-12] [INFO] [1624462185.658123413] [recoveries_server]: Configuring sms_recovery [lifecycle_manager-14] [INFO] [1624462185.694850636] [lifecycle_manager_navigation]: Configuring bt_navigator

[bt_navigator-13] [INFO] [1624462185.925250666] [bt_navigator_rclcpp_node]: Waiting for "wait_to" action server [planner_server-11] [INFO] [1624462186.156290416] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 128 X 240 at 0.050000 m/pix [planner_server-11] [INFO] [1624462215.157847318] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 128 X 242 at 0.050000 m/pix [cartographer_node-8] [WARN] [1624462223.207995676] [cartographer_ros]: W0623 11:30:23.000000 81472 range_data_collator.cc:76] Dropped 17 earlier points. `

Additional information

I have let the code sit for 10+ minutes and the action node still does not initialize. Please let me know if there is anything more information I can provide to solve this problem.

SteveMacenski commented 3 years ago

We do not offer private support for custom things in this medium (I'm sure if you're interested, I can find someone that would be willing to take a small contracting role to help). Your best (free) bet would be ROS Answers. I can say very confidently that the action node base class works fine, since its being used by every single server in Nav2 and none of them have any special differentiators, and it also has 90%+ unit test coverage with a dummy demo.