ros-navigation / navigation2_tutorials Goto Github PK

Task exception was never retrieved
future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py:274> exception=SubstitutionFailure("executable 'costmap_filter_info_server' not found on the libexec directory '/opt/ros/foxy/lib/nav2_map_server' ")>
Traceback (most recent call last):
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 276, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 296, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 1 more time]
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/actions/node.py", line 426, in execute
    ret = super().execute(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 823, in execute
    self.__expand_substitutions(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 668, in __expand_substitutions
    cmd = [perform_substitutions(context, x) for x in self.__cmd]
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 668, in <listcomp>
    cmd = [perform_substitutions(context, x) for x in self.__cmd]
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in perform_substitutions
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in <listcomp>
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_context.py", line 197, in perform_substitution
    return substitution.perform(self)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/substitutions/executable_in_package.py", line 84, in perform
    raise SubstitutionFailure(
launch.substitutions.substitution_failure.SubstitutionFailure: executable 'costmap_filter_info_server' not found on the libexec directory '/opt/ros/foxy/lib/nav2_map_server' 

Finished <<< turtlebot3 [0.39s]                                                        
--- stderr: nav2_gradient_costmap_plugin                                               
/home/ieu/ros2_ws/src/navigation2_tutorials/nav2_gradient_costmap_plugin/src/gradient_layer.cpp: In member function ‘virtual void nav2_gradient_costmap_plugin::GradientLayer::onInitialize()’:
/home/ieu/ros2_ws/src/navigation2_tutorials/nav2_gradient_costmap_plugin/src/gradient_layer.cpp:70:21: error: ‘using SharedPtr = class std::shared_ptr<rclcpp_lifecycle::LifecycleNode>’ {aka ‘class std::shared_ptr<rclcpp_lifecycle::LifecycleNode>’} has no member named ‘lock’
   70 |   auto node = node_.lock();
      |                     ^~~~
make[2]: *** [CMakeFiles/nav2_gradient_costmap_plugin_core.dir/build.make:63: CMakeFiles/nav2_gradient_costmap_plugin_core.dir/src/gradient_layer.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:105: CMakeFiles/nav2_gradient_costmap_plugin_core.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< nav2_gradient_costmap_plugin [4.81s, exited with code 2]
Aborted  <<< nav2_straightline_planner [5.00s]                                     
Aborted  <<< nav2_sms_recovery [5.30s]                             

/accel/filtered
/clock
/demo/cmd_vel
/demo/imu
/demo/odom
/diagnostics
/joint_states
/odometry/filtered
/parameter_events
/robot_description
/rosout
/set_pose
/tf
/tf_static

/ekf_filter_node
  Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /demo/imu: sensor_msgs/msg/Imu
    /demo/odom: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped
  Publishers:
    /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /odometry/filtered: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /ekf_filter_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ekf_filter_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ekf_filter_node/get_parameters: rcl_interfaces/srv/GetParameters
    /ekf_filter_node/list_parameters: rcl_interfaces/srv/ListParameters
    /ekf_filter_node/set_parameters: rcl_interfaces/srv/SetParameters
    /ekf_filter_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /enable: std_srvs/srv/Empty
    /set_pose: robot_localization/srv/SetPose
    /toggle: robot_localization/srv/ToggleFilterProcessing
  Service Clients:

  Action Servers:

  Action Clients:

1620618863.7946322 [INFO] [launch]: All log files can be found below /home/maaruf/.ros/log/2021-05-10-09-24-23-791827-maaruf-Strix-15-GL503GE-9948
1620618863.7947869 [INFO] [launch]: Default logging verbosity is set to INFO
1620618863.9245000 [INFO] [gazebo-1]: process started with pid [9951]
1620618863.9249437 [INFO] [robot_state_publisher-2]: process started with pid [9953]
1620618863.9251087 [INFO] [spawn_entity.py-3]: process started with pid [9955]
1620618863.9252398 [INFO] [ekf_node-4]: process started with pid [9957]
1620618863.9253635 [INFO] [rviz2-5]: process started with pid [9959]
1620618863.9446864 [robot_state_publisher-2] Parsing robot urdf xml string.
1620618863.9452262 [robot_state_publisher-2] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
1620618863.9453862 [robot_state_publisher-2] Link base_footprint had 0 children
1620618863.9454966 [robot_state_publisher-2] Link front_caster had 0 children
1620618863.9455602 [robot_state_publisher-2] Link drivewhl_l_link had 0 children
1620618863.9456286 [robot_state_publisher-2] Link drivewhl_r_link had 0 children
1620618863.9457018 [robot_state_publisher-2] Link imu_link had 0 children
1620618863.9457703 [robot_state_publisher-2] [INFO] [1620618863.944830383] [robot_state_publisher]: got segment base_footprint
1620618863.9458444 [robot_state_publisher-2] [INFO] [1620618863.944961115] [robot_state_publisher]: got segment base_link
1620618863.9459641 [robot_state_publisher-2] [INFO] [1620618863.944977239] [robot_state_publisher]: got segment drivewhl_l_link
1620618863.9460473 [robot_state_publisher-2] [INFO] [1620618863.944983669] [robot_state_publisher]: got segment drivewhl_r_link
1620618863.9465437 [robot_state_publisher-2] [INFO] [1620618863.944988418] [robot_state_publisher]: got segment front_caster
1620618863.9466844 [robot_state_publisher-2] [INFO] [1620618863.944994314] [robot_state_publisher]: got segment imu_link
1620618865.6764433 [INFO] [spawn_entity.py-3]: process has finished cleanly [pid 9955]

ros__parameters:
  planner_plugin_types: ["nav2_straightline_planner/StraightLine"] # For Foxy and earlier
  planner_plugin_ids: ["GridBased"] # For Foxy and earlier
  plugins: ["GridBased"] # For Galactic and later
  use_sim_time: True
  GridBased:
    plugin: nav2_straightline_planner/StraightLine # For Galactic and later
    interpolation_resolution: 0.1

...
[lifecycle_manager-13] [INFO] [1612121985.772204302] [lifecycle_manager_navigation]: Managed nodes are active
[rviz2-4] [INFO] [1612121985.773500244] [rviz2]: Waiting for the lifecycle_manager_localization/is_active service...
[rviz2-4] [INFO] [1612121985.773628489] [rviz2]: Sending lifecycle_manager_localization/is_active request
[rviz2-4] [INFO] [1612121985.796956654] [rviz2]: Trying to create a map of size 384 x 384 using 1 swatches
[rviz2-4] Start navigation
[bt_navigator-11] [INFO] [1612121987.372087209] [bt_navigator]: Begin navigating from current location to (-1.61, 1.47)
[planner_server-9] [ERROR] [1612121987.374854305] [planner_server]: planner GridBased is not a valid planner. Planner names are: GridBased 
[planner_server-9] [WARN] [1612121987.374901048] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.61, 1.47)
[planner_server-9] [WARN] [1612121987.374919063] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-9] [INFO] [1612121987.394338651] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[planner_server-9] [ERROR] [1612121987.395192696] [planner_server]: planner GridBased is not a valid planner. Planner names are: GridBased 
[planner_server-9] [WARN] [1612121987.395233277] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.61, 1.47)
[planner_server-9] [WARN] [1612121987.395249987] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[controller_server-8] [INFO] [1612121987.404362275] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-9] [INFO] [1612121987.405222963] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[recoveries_server-10] [INFO] [1612121987.405698346] [recoveries_server]: Attempting spin
[recoveries_server-10] [INFO] [1612121987.406445149] [recoveries_server]: Turning 1.57 for spin recovery.
[recoveries_server-10] [INFO] [1612121988.406905943] [recoveries_server]: spin running...
[recoveries_server-10] [INFO] [1612121989.206973602] [recoveries_server]: spin completed successfully
[recoveries_server-10] [INFO] [1612121989.234869369] [recoveries_server]: Attempting wait
[recoveries_server-10] [INFO] [1612121990.235019975] [recoveries_server]: wait running...
[recoveries_server-10] [INFO] [1612121991.234981786] [recoveries_server]: wait running...
[recoveries_server-10] [INFO] [1612121992.234988247] [recoveries_server]: wait running...
[rviz2-4] [INFO] [1612121992.764902519] [rviz2]: Message Filter dropping message: frame 'odom' at time 3,808 for reason 'Unknown'
[rviz2-4] [INFO] [1612121992.969423367] [rviz2]: Message Filter dropping message: frame 'odom' at time 4,009 for reason 'Unknown'
[rviz2-4] [INFO] [1612121993.173612944] [rviz2]: Message Filter dropping message: frame 'odom' at time 4,210 for reason 'Unknown'
[recoveries_server-10] [INFO] [1612121993.234997738] [recoveries_server]: wait running...
[rviz2-4] [INFO] [1612121993.378075367] [rviz2]: Message Filter dropping message: frame 'odom' at time 4,410 for reason 'Unknown'
[rviz2-4] [INFO] [1612121993.548095092] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 3,801 for reason 'Unknown'
[rviz2-4] [INFO] [1612121993.549621195] [rviz2]: Message Filter dropping message: frame 'odom' at time 4,611 for reason 'Unknown'
[rviz2-4] [INFO] [1612121993.752391465] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 4,001 for reason 'Unknown'
...

colcon build --packages-select nav2_straightline_planner

Starting >>> nav2_straightline_planner
--- stderr: nav2_straightline_planner                             
In file included from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/src/straight_line_planner.cpp:46:
/home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/include/nav2_straightline_planner/straight_line_planner.hpp:67:8: error: ‘void nav2_straightline_planner::StraightLine::configure(const WeakPtr&, std::string, std::shared_ptr<tf2_ros::Buffer>, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>)’ marked ‘override’, but does not override
   67 |   void configure(
      |        ^~~~~~~~~
In file included from /opt/ros/foxy/include/class_loader/class_loader_core.hpp:57,
                 from /opt/ros/foxy/include/class_loader/class_loader.hpp:55,
                 from /opt/ros/foxy/include/class_loader/multi_library_class_loader.hpp:52,
                 from /opt/ros/foxy/include/pluginlib/class_loader.hpp:58,
                 from /home/srl/Documents/Github/nav_ws/install/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp:53,
                 from /home/srl/Documents/Github/nav_ws/install/nav2_core/include/nav2_core/global_planner.hpp:21,
                 from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/include/nav2_straightline_planner/straight_line_planner.hpp:51,
                 from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/src/straight_line_planner.cpp:46:
/opt/ros/foxy/include/class_loader/meta_object.hpp: In instantiation of ‘B* class_loader::impl::MetaObject<C, B>::create() const [with C = nav2_straightline_planner::StraightLine; B = nav2_core::GlobalPlanner]’:
/opt/ros/foxy/include/class_loader/meta_object.hpp:216:7:   required from here
/opt/ros/foxy/include/class_loader/meta_object.hpp:218:12: error: invalid new-expression of abstract class type ‘nav2_straightline_planner::StraightLine’
  218 |     return new C;
      |            ^~~~~
In file included from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/src/straight_line_planner.cpp:46:
/home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/include/nav2_straightline_planner/straight_line_planner.hpp:60:7: note:   because the following virtual functions are pure within ‘nav2_straightline_planner::StraightLine’:
   60 | class StraightLine : public nav2_core::GlobalPlanner
      |       ^~~~~~~~~~~~
In file included from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/include/nav2_straightline_planner/straight_line_planner.hpp:51,
                 from /home/srl/Documents/Github/nav_ws/src/navigation2/nav2_straightline_planner/src/straight_line_planner.cpp:46:
/home/srl/Documents/Github/nav_ws/install/nav2_core/include/nav2_core/global_planner.hpp:50:16: note: 	‘virtual void nav2_core::GlobalPlanner::configure(rclcpp_lifecycle::LifecycleNode::SharedPtr, std::string, std::shared_ptr<tf2_ros::Buffer>, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>)’
   50 |   virtual void configure(
      |                ^~~~~~~~~
make[2]: *** [CMakeFiles/nav2_straightline_planner_plugin.dir/build.make:63: CMakeFiles/nav2_straightline_planner_plugin.dir/src/straight_line_planner.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/nav2_straightline_planner_plugin.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< nav2_straightline_planner [3.64s, exited with code 2]

Summary: 0 packages finished [3.94s]
  1 package failed: nav2_straightline_planner
  1 package had stderr output: nav2_straightline_planner

[mapviz-1] [WARN] [1699186927.537189075] [mapviz]: [transform_manager]: No transformer from 'wgs84' to 'map'. If 'map' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [ERROR] [1699186927.537354140] [mapviz]: Error: No transform between wgs84 and map
.
.
[mapviz-1] [WARN] [1699186929.295457865] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.314493185] [mapviz]: [transform_manager]: No transformer from 'base_link' to 'tf'. If 'base_link' is a /tf frame, it may not have been broadcast recently.

ros2 launch nav2_gps_waypoint_follower_demo mapviz.launch.py
[INFO] [launch]: All log files can be found below /home/kevin/.ros/log/2023-11-05-13-22-00-531001-kevin-ThinkPad-T480-7436
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [mapviz-1]: process started with pid [7446]
[INFO] [initialize_origin.py-2]: process started with pid [7448]
[INFO] [static_transform_publisher-3]: process started with pid [7450]
[static_transform_publisher-3] [WARN] [1699186922.433718400] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-3] [INFO] [1699186922.684386487] [swri_transform]: Spinning until stopped - publishing transform
[static_transform_publisher-3] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-3] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-3] from 'map' to 'origin'
[mapviz-1] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[mapviz-1] [INFO] [1699186926.091411025] [mapviz]: Target frame selected: <none>
[initialize_origin.py-2] [INFO] [1699186926.684224944] [initialize_origin]: Origin: auto
[initialize_origin.py-2] [INFO] [1699186926.691551299] [initialize_origin]: Frame: map
[initialize_origin.py-2] /home/kevin/ros2_humble/install/rclpy/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[initialize_origin.py-2]   warnings.warn(
[mapviz-1] [INFO] [1699186927.135734224] [mapviz]: View scale: 1.000000 meters/pixel
[initialize_origin.py-2] [INFO] [1699186927.270977861] [initialize_origin]: Got NavSat message.
[initialize_origin.py-2] [INFO] [1699186927.291337853] [initialize_origin]: Origin from 'navsat' source set to 38.1614563499149, -122.45460631151656, 488.32879648171365
[initialize_origin.py-2] [INFO] [1699186927.305467454] [initialize_origin]: Successfully set origin; unsubscribing.
[mapviz-1] [INFO] [1699186927.306416807] [mapviz]: Subscribing to /local_xy_origin
[mapviz-1] [INFO] [1699186927.359070807] [mapviz]: Found mapviz plugin: mapviz_plugins/attitude_indicator
[mapviz-1] [INFO] [1699186927.359166139] [mapviz]: Found mapviz plugin: mapviz_plugins/coordinate_picker
[mapviz-1] [INFO] [1699186927.359196662] [mapviz]: Found mapviz plugin: mapviz_plugins/disparity
[mapviz-1] [INFO] [1699186927.359220443] [mapviz]: Found mapviz plugin: mapviz_plugins/draw_polygon
[mapviz-1] [INFO] [1699186927.359245504] [mapviz]: Found mapviz plugin: mapviz_plugins/float
[mapviz-1] [INFO] [1699186927.359280008] [mapviz]: Found mapviz plugin: mapviz_plugins/gps
[mapviz-1] [INFO] [1699186927.359312167] [mapviz]: Found mapviz plugin: mapviz_plugins/grid
[mapviz-1] [INFO] [1699186927.359343791] [mapviz]: Found mapviz plugin: mapviz_plugins/image
[mapviz-1] [INFO] [1699186927.359375453] [mapviz]: Found mapviz plugin: mapviz_plugins/laserscan
[mapviz-1] [INFO] [1699186927.359417993] [mapviz]: Found mapviz plugin: mapviz_plugins/marker
[mapviz-1] [INFO] [1699186927.359438878] [mapviz]: Found mapviz plugin: mapviz_plugins/measuring
[mapviz-1] [INFO] [1699186927.359460504] [mapviz]: Found mapviz plugin: mapviz_plugins/multires_image
[mapviz-1] [INFO] [1699186927.359482773] [mapviz]: Found mapviz plugin: mapviz_plugins/navsat
[mapviz-1] [INFO] [1699186927.359519369] [mapviz]: Found mapviz plugin: mapviz_plugins/occupancy_grid
[mapviz-1] [INFO] [1699186927.359553258] [mapviz]: Found mapviz plugin: mapviz_plugins/odometry
[mapviz-1] [INFO] [1699186927.359578606] [mapviz]: Found mapviz plugin: mapviz_plugins/path
[mapviz-1] [INFO] [1699186927.359612085] [mapviz]: Found mapviz plugin: mapviz_plugins/plan_route
[mapviz-1] [INFO] [1699186927.359665948] [mapviz]: Found mapviz plugin: mapviz_plugins/point_click_publisher
[mapviz-1] [INFO] [1699186927.359727957] [mapviz]: Found mapviz plugin: mapviz_plugins/pointcloud2
[mapviz-1] [INFO] [1699186927.359778964] [mapviz]: Found mapviz plugin: mapviz_plugins/pose
[mapviz-1] [INFO] [1699186927.359825089] [mapviz]: Found mapviz plugin: mapviz_plugins/robot_image
[mapviz-1] [INFO] [1699186927.359883716] [mapviz]: Found mapviz plugin: mapviz_plugins/route
[mapviz-1] [INFO] [1699186927.360032253] [mapviz]: Found mapviz plugin: mapviz_plugins/string
[mapviz-1] [INFO] [1699186927.360129547] [mapviz]: Found mapviz plugin: mapviz_plugins/textured_marker
[mapviz-1] [INFO] [1699186927.360264900] [mapviz]: Found mapviz plugin: mapviz_plugins/tf_frame
[mapviz-1] [INFO] [1699186927.360550336] [mapviz]: Found mapviz plugin: mapviz_plugins/tile_map
[mapviz-1] [INFO] [1699186927.366819335] [mapviz]: Loading configuration from /home/kevin/kev_ws/install/nav2_gps_waypoint_follower_demo/share/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
[mapviz-1] [INFO] [1699186927.367797404] [mapviz]: fixed frame selected: map
[mapviz-1] [INFO] [1699186927.367930217] [mapviz]: Target frame selected: map
[mapviz-1] [INFO] [1699186927.371868189] [mapviz]: creating: mapviz_plugins/tile_map
[mapviz-1] [WARN] [1699186927.537189075] [mapviz]: [transform_manager]: No transformer from 'wgs84' to 'map'. If 'map' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [ERROR] [1699186927.537354140] [mapviz]: Error: No transform between wgs84 and map
[mapviz-1] [INFO] [1699186927.537915991] [mapviz]: Reorder displays
[mapviz-1] [INFO] [1699186927.543621743] [mapviz]: creating: mapviz_plugins/point_click_publisher
[mapviz-1] [INFO] [1699186928.373131851] [mapviz]: Publishing points to topic: clicked_point
[mapviz-1] [INFO] [1699186928.383323504] [mapviz]: Ready.
[mapviz-1] [INFO] [1699186928.429633950] [mapviz]: Reorder displays
[mapviz-1] [INFO] [1699186928.429773572] [mapviz]: Publishing points to topic: clicked_point
[mapviz-1] [INFO] [1699186928.450993260] [mapviz]: creating: mapviz_plugins/tf_frame
[mapviz-1] [INFO] [1699186928.493739732] [mapviz]: Reorder displays
[mapviz-1] [WARN] [1699186928.496914266] [mapviz]: Waiting for transform.
[mapviz-1] [INFO] [1699186928.497490327] [mapviz]: Setting target frame to to base_link
[mapviz-1] [INFO] [1699186928.504547484] [mapviz]: creating: mapviz_plugins/navsat
[mapviz-1] [INFO] [1699186928.576091935] [mapviz]: Reorder displays
[mapviz-1] [WARN] [1699186928.581271037] [mapviz]: No messages received.
[mapviz-1] [INFO] [1699186928.622145884] [mapviz]: Subscribing to /gps/fix
[mapviz-1] [WARN] [1699186928.669973665] [mapviz]: [transform_manager]: No transformer from 'base_link' to 'map'. If 'base_link' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [INFO] [1699186928.940983553] [mapviz]: OK
[mapviz-1] [WARN] [1699186928.966091841] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186928.966204912] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.295457865] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.314493185] [mapviz]: [transform_manager]: No transformer from 'base_link' to 'tf'. If 'base_link' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.334351789] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.419833049] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [ERROR] [1699186929.432907198] [mapviz]: NETWORK ERROR: Protocol "" is unknown
[mapviz-1] [WARN] [1699186929.440230266] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.443635410] [mapviz]: [transform_manager]: No transformer from 'base_link' to 'tf'. If 'base_link' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [WARN] [1699186929.454462204] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [ERROR] [1699186929.468745364] [mapviz]: NETWORK ERROR: Protocol "" is unknown
[mapviz-1] [WARN] [1699186929.469391118] [mapviz]: [transform_manager]: No transformer from '' to 'tf'. If '' is a /tf frame, it may not have been broadcast recently.
[mapviz-1] [ERROR] [1699186929.477346367] [mapviz]: NETWORK ERROR: Protocol "" is unknown
[mapviz-1] [INFO] [1699186929.492517608] [mapviz]: OK
[mapviz-1] [INFO] [1699186929.785185251] [mapviz]: OK

  // auto node = node_.lock(); 
  auto node = node_;

def generate_launch_description():
    # Get the launch directory
    costmap_filters_demo_dir = get_package_share_directory('nav2_costmap_filters_demo')

    # Create our own temporary YAML files that include substitutions
    lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']

    # Parameters
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file_keepout = LaunchConfiguration('params_file_keepout')
    params_file_speed = LaunchConfiguration('params_file_speed')
    mask_yaml_file_keepout = LaunchConfiguration('mask_keepout')
    mask_yaml_file_speed = LaunchConfiguration('mask_speed')

    # Declare the launch arguments
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true')

    declare_autostart_cmd = DeclareLaunchArgument(
        'autostart', default_value='true',
        description='Automatically startup the nav2 stack')

    declare_params_file_keepout_cmd = DeclareLaunchArgument(
            'params_file_keepout',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'params', 'keepout_params.yaml'),
            description='Full path to the ROS2 parameters file for the Keepout filter')

    declare_params_file_speed_cmd = DeclareLaunchArgument(
            'params_file_speed',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'params', 'speed_params.yaml'),
            description='Full path to the ROS2 parameters file for the Speed filter')

    declare_mask_yaml_file_keepout_cmd = DeclareLaunchArgument(
            'mask_keepout',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'maps', 'fullmap_keepout.yaml'),
            description='Full path to filter mask yaml file to load for the Keepout filter')

    declare_mask_yaml_file_speed_cmd = DeclareLaunchArgument(
            'mask_speed',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'maps', 'speed.yaml'),
            description='Full path to filter mask yaml file to load for the Speed filter')

    # Make re-written yaml for Keepout filter
    param_substitutions_keepout = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file_keepout}

    configured_params_keepout = RewrittenYaml(
        source_file=params_file_keepout,
        root_key=namespace,
        param_rewrites=param_substitutions_keepout,
        convert_types=True)

    # Make re-written yaml for Speed filter
    param_substitutions_speed = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file_speed}

    configured_params_speed = RewrittenYaml(
        source_file=params_file_speed,
        root_key=namespace,
        param_rewrites=param_substitutions_speed,
        convert_types=True)

    # Nodes launching commands
    start_lifecycle_manager_cmd = Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_costmap_filters',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': lifecycle_nodes}])

    start_map_server_cmd = Node(
            package='nav2_map_server',
            executable='map_server',
            name='filter_mask_server',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[configured_params_keepout])

    start_costmap_filter_info_server_cmd = Node(
        package='nav2_map_server',
        executable='costmap_filter_info_server',
        name='costmap_filter_info_server_1',
        namespace=namespace,
        output='screen',
        emulate_tty=True,
        parameters=[configured_params_speed])

    start_costmap_filter_info_server_cmd2 = Node(
        package='nav2_map_server',
        executable='costmap_filter_info_server',
        name='costmap_filter_info_server_2',
        namespace=namespace,
        output='screen',
        emulate_tty=True,
        parameters=[configured_params_keepout])


    ld = LaunchDescription()

    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_params_file_keepout_cmd)
    ld.add_action(declare_params_file_speed_cmd)
    ld.add_action(declare_mask_yaml_file_keepout_cmd)
    ld.add_action(declare_mask_yaml_file_speed_cmd)

    ld.add_action(start_lifecycle_manager_cmd)
    ld.add_action(start_map_server_cmd)
    ld.add_action(start_costmap_filter_info_server_cmd)
    ld.add_action(start_costmap_filter_info_server_cmd2)

    return ld

ros2 launch nav2_costmap_filters_demo costmap_filter_info_2.launch.py                                                                                     
[INFO] [launch]: All log files can be found below /home/bot/.ros/log/2023-05-04-01-30-16-163378-bot-22263
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [lifecycle_manager-1]: process started with pid [22264]
[INFO] [map_server-2]: process started with pid [22266]
[INFO] [costmap_filter_info_server-3]: process started with pid [22268]
[INFO] [costmap_filter_info_server-4]: process started with pid [22270]
[lifecycle_manager-1] [INFO] [1683156616.663989370] [lifecycle_manager_costmap_filters]: Creating
[costmap_filter_info_server-4] [INFO] [1683156616.667593196] [costmap_filter_info_server]: This is costmap filter info publisher
[costmap_filter_info_server-3] [INFO] [1683156616.668954028] [costmap_filter_info_server]: This is costmap filter info publisher
[costmap_filter_info_server-4] [INFO] [1683156616.676255188] [costmap_filter_info_server_2]: 
[costmap_filter_info_server-4] 	costmap_filter_info_server_2 lifecycle node launched. 
[costmap_filter_info_server-4] 	Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-2] [INFO] [1683156616.687254944] [filter_mask_server]: 
[map_server-2] 	filter_mask_server lifecycle node launched. 
[map_server-2] 	Waiting on external lifecycle transitions to activate
[map_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-2] [INFO] [1683156616.687365271] [filter_mask_server]: Creating
[costmap_filter_info_server-3] [INFO] [1683156616.746267633] [costmap_filter_info_server_1]: 
[costmap_filter_info_server-3] 	costmap_filter_info_server_1 lifecycle node launched. 
[costmap_filter_info_server-3] 	Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-3] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-1] [INFO] [1683156616.773070866] [lifecycle_manager_costmap_filters]: Creating and initializing lifecycle service clients
[lifecycle_manager-1] [INFO] [1683156618.847922028] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156620.848138293] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156622.848298710] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156624.848502387] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156626.848733649] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156628.848942898] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156630.849106236] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156632.849306747] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156634.849465813] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156636.849632568] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...

[lifecycle_manager-5] [INFO] [1627288921.973608032] [lifecycle_manager_navigation]: Creating bond timer...
[bt_navigator-4] [INFO] [1627288936.097422353] [bt_navigator]: Begin navigating from current location to (1.83, -2.85)

GradientLayer::onInitialize()
{
  auto node = node_.lock(); 
  declareParameter("enabled", rclcpp::ParameterValue(true));
  node->get_parameter(name_ + "." + "enabled", enabled_);

  need_recalculation_ = false;
  current_ = true;
}

robot@robothost:~$ ros2 launch vector_description display.launch.py
[INFO] [launch]: All log files can be found below /home/robot/.ros/log/2022-02-18-14-13-30-610366-robothost-18354
[INFO] [launch]: Default logging verbosity is set to INFO
[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught exception when trying to load file of format [py]: cannot import name 'Command'

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

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='vector_description').find('vector_description')
    default_model_path = os.path.join(pkg_share, 'src/description/vector_description.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]

Starting >>> nav2_costmap_filters_demo
Starting >>> nav2_gradient_costmap_plugin
Starting >>> nav2_sms_recovery
Starting >>> nav2_straightline_planner
Starting >>> sam_bot_description
Finished <<< nav2_costmap_filters_demo [0.91s]                                  
Finished <<< sam_bot_description [1.14s]                                         
--- stderr: nav2_gradient_costmap_plugin                                             
/home/chenkel/ros2/nav2_ws/src/navigation2_tutorials/nav2_gradient_costmap_plugin/src/gradient_layer.cpp: In member function ‘virtual void nav2_gradient_costmap_plugin::GradientLayer::onInitialize()’:
/home/chenkel/ros2/nav2_ws/src/navigation2_tutorials/nav2_gradient_costmap_plugin/src/gradient_layer.cpp:70:21: error: ‘using SharedPtr = class std::shared_ptr<rclcpp_lifecycle::LifecycleNode>’ {aka ‘class std::shared_ptr<rclcpp_lifecycle::LifecycleNode>’} has no member named ‘lock’
   70 |   auto node = node_.lock();
      |                     ^~~~
make[2]: *** [CMakeFiles/nav2_gradient_costmap_plugin_core.dir/build.make:63: CMakeFiles/nav2_gradient_costmap_plugin_core.dir/src/gradient_layer.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:105: CMakeFiles/nav2_gradient_costmap_plugin_core.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< nav2_gradient_costmap_plugin [6.27s, exited with code 2]
Aborted  <<< nav2_straightline_planner [10.4s]                                         
Aborted  <<< nav2_sms_recovery [11.1s]                                 

Summary: 2 packages finished [11.3s]
  1 package failed: nav2_gradient_costmap_plugin
  2 packages aborted: nav2_sms_recovery nav2_straightline_planner
  3 packages had stderr output: nav2_gradient_costmap_plugin nav2_sms_recovery nav2_straightline_planner

[controller_server-4] [INFO] [1617794494.106441780] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794494.107058570] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794494.107080741] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 826s 621000000ns
[controller_server-4] [INFO] [1617794495.106504024] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794495.106999333] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794495.107018282] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 827s 521000000ns
[controller_server-4] [INFO] [1617794496.106503821] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794496.106976139] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794496.106996291] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 828s 621000000ns
[controller_server-4] [INFO] [1617794497.306577073] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794497.307159186] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794497.307185101] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 829s 721000000ns
[controller_server-4] [INFO] [1617794498.306491576] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794498.306999256] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794498.307018059] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 830s 721000000ns
[controller_server-4] [INFO] [1617794499.306503259] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794499.307099119] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794499.307125979] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 831s 721000000ns
[controller_server-4] [INFO] [1617794500.306443555] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794500.307062602] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794500.307088974] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 832s 721000000ns
[controller_server-4] [INFO] [1617794501.306475765] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794501.307010382] [tf_help]: Transform data too old when converting from map to odom

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    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: 10.0
    laser_min_range: 0.22
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0  
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.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: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    controller_plugins: ["FollowPath"]
   #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.1
      max_vel_y: 0.0
      max_vel_theta: 1.5
      min_speed_xy: 0.0
      max_speed_xy: .1
      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.5
      acc_lim_y: 0.0
      acc_lim_theta: 1.2
      decel_lim_x: -1.5
      decel_lim_y: 0.0
      decel_lim_theta: -1.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.02
      angular_granularity: 0.017
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.05                
      yaw_goal_tolerance: 0.034
      stateful: true
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist","ObstacleFootprint"]
      BaseObstacle.scale: 0.08
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.2
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.2
      PathDist.scale: 32.0
      GoalDist.scale: 20.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
      ObstacleFootprint.scale: 0.08


controller_server_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: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.105
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
      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"
      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: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        map_subscribe_transient_local: True
      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: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.105
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      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"
      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: /intel_realsense_r200_depth/points
          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"
      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

planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_straightline_planner/StraightLine" 
      interpolation_resolution: 0.1


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_link
    transform_timeout: 0.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower
[INFO] [1701172139.520198172] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172140.522618706] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172141.524480719] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172142.526854488] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172143.529257288] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172144.531678462] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172145.533696952] [basic_navigator]: robot_localization/get_state service not available, waiting...
[INFO] [1701172146.535800306] [basic_navigator]: robot_localization/get_state service not available, waiting...

[mapviz-1] [WARN] [1701172112.460493075] [swri_transform_util::Transformer]: Wgs84Transformer not initialized
[mapviz-1] [WARN] [1701172112.460534350] [swri_transform_util::Transformer]: Wgs84Transformer not initialized
[initialize_origin.py-2] [INFO] [1701172112.461930965] [initialize_origin]: Got NavSat message.
[initialize_origin.py-2] [INFO] [1701172112.462575108] [initialize_origin]: Origin from 'navsat' source set to 38.1614564605658, -122.45460714408775, 488.3200360890478
[initialize_origin.py-2] [INFO] [1701172112.463337905] [initialize_origin]: Successfully set origin; unsubscribing.
[mapviz-1] [INFO] [1701172112.480655151] [mapviz]: OK
[mapviz-1] [INFO] [1701172112.482309682] [mapviz]: OK
[mapviz-1] [INFO] [1701172138.485441870] [mapviz]: Point in wgs84: -122.454,38.1612

void 
SemLayer::onInitialize() {

  auto node = node_.lock();
  obj_sub_ = node->create_subscription<object_msgs::msg::Object>("/object_detection", rclcpp::SensorDataQoS(),std::bind(&SemLayer::objectCallback, this, std::placeholders::_1));
  RCLCPP_INFO(node->get_logger(), "SemLayer: subscribed to " "topic %s", obj_sub_->get_topic_name());

  declareParameter("enabled", rclcpp::ParameterValue(true));
  declareParameter("publish_occgrid", rclcpp::ParameterValue(false));
  get_parameters();

  if (publish_occgrid_) {
    grid_pub_ =
        node->create_publisher<nav_msgs::msg::OccupancyGrid>("sem_grid", 1);
    grid_pub_->on_activate();
  }
}

void 
SemLayer::get_parameters() {
  auto node = node_.lock();
  node->get_parameter(name_ + "." + "enabled", enabled_);
  node->get_parameter(name_ + "." + "publish_occgrid", publish_occgrid_);
}

void 
SemLayer::objectCallback(const object_msgs::msg::Object::SharedPtr msg) {
  obj_message_mutex_.lock();
  object = *msg;
  obj_message_mutex_.unlock();
  obj_detected_ = true;
}

void
SemLayer::updateBounds(
  double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, double * min_y, double * max_x, double * max_y)
{
  if (need_recalculation_) {
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    // For some reason when I make these -<double>::max() it does not
    // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
    // -<float>::max() instead.
    *min_x = -std::numeric_limits<float>::max();
    *min_y = -std::numeric_limits<float>::max();
    *max_x = std::numeric_limits<float>::max();
    *max_y = std::numeric_limits<float>::max();
    need_recalculation_ = false;
  } else {
    double tmp_min_x = last_min_x_;
    double tmp_min_y = last_min_y_;
    double tmp_max_x = last_max_x_;
    double tmp_max_y = last_max_y_;
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    *min_x = std::min(tmp_min_x, *min_x);
    *min_y = std::min(tmp_min_y, *min_y);
    *max_x = std::max(tmp_max_x, *max_x);
    *max_y = std::max(tmp_max_y, *max_y);
  }
}

void
SemLayer::onFootprintChanged()
{
  need_recalculation_ = true;
  RCLCPP_DEBUG(rclcpp::get_logger(
      "nav2_costmap_2d"), "SemLayer::onFootprintChanged(): num footprint points: %lu",
    layered_costmap_->getFootprint().size());
}

void 
SemLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid,int min_i, int min_j, int max_i, int max_j) {
  if (!enabled_) {
    return;
  }
  if (!obj_detected_){
    return;
  }
  // unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
  min_i = std::max(0, min_i);
  min_j = std::max(0, min_j);
  max_i = std::min(static_cast<int>(size_x), max_i);
  max_j = std::min(static_cast<int>(size_y), max_j);
  for (int x = min_i; x < max_i; x++) {
    for (int y = min_j; y < max_j; y++) {
      // int index = master_grid.getIndex(x, y);
      master_grid.setCost(x, y, 240);
    }
}
  obj_detected_ = false;
}
} // namespace nav2_sem_costmap_plugin
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_sem_costmap_plugin::SemLayer, nav2_costmap_2d::Layer)

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: 10
      height: 10
      resolution: 0.05
      robot_radius: 0.17
      plugins: ["obstacle_layer", "sem_layer"]
      # plugins: ["obstacle_layer", "inflation_layer"]
      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"
          raytrace_max_range: 8.0
          raytrace_min_range: 0.0
          obstacle_max_range: 9.5
          obstacle_min_range: 0.0
      sem_layer:
        plugin: "nav2_sem_costmap_plugin::SemLayer"
        enabled: True
        publish_occgrid: False
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 0.4
        inflation_radius: 0.32
      always_send_full_costmap: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.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"]

find_package(rosidl_default_generators REQUIRED)
find_package(action_msgs REQUIRED)