Local Costmap is empty
I have looked at all the other posts that have answers but none of them apply to me or they don't fix the problem. This is my launch for move_base
<launch>
<remap from="map" to="/rtabmap/grid_map" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find my_mower)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find my_mower)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find my_mower)/params/local_costmap_params.yaml" command="load" />
<rosparam file="$(find my_mower)/params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find my_mower)/params/base_local_planner_params.yaml" command="load" />
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find my_mower)/params/costmap_converter_params.yaml" command="load" />
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="-1.22 -.10 0 0 0 0 1 t265_link base_link 100" />
</launch>
This is the common params
footprint: [[-0.35, 0.6], [1.32 , 0.6], [1.32, -0.6], [-0.35, -0.6]]
#robot_radius: 1
transform_tolerance: 2
static_layer:
enabled: true
inflation_layer:
inflation_radius: .5
obstacle_layer:
enabled: true
obstacle_range: 3.5
raytrrace_range: 4
max_obstacle_height: 99999.0
min_obstacle_height: -99999.0
track_unknown_space: true
observation_sources: point_cloud_sensor
point_cloud_sensor: {sensor_frame: odom,
data_type: PointCloud2,
topic: rtabmap/local_grid_obstacle, #testing different topics
marking: true, clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 99999.0}
This is the local costmap params
local_costmap:
global_frame: t265_link
robot_base_frame: t265_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05
transform_tolerance: 5
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
rosparam shows local_costmap observation source is correct. this is an echo of the observation source topic:
this is an echo of the empty local costmap being created
this is the output of launching move_base: (notice how at the end it says Recovery behavior will clear layer 'obstacles')
... logging to /home/ryley/.ros/log/7f786a8e-6f67-11eb-981b-0923652b57c3/roslaunch-ryley-GL65-9SD-151466.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://ryley-GL65-9SD:37259/
SUMMARY
========
PARAMETERS
* /move_base/TebLocalPlannerROS/acc_lim_theta: 0.5
* /move_base/TebLocalPlannerROS/acc_lim_x: 0.5
* /move_base/TebLocalPlannerROS/alternative_time_cost: False
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/cluster_max_distance: 0.4
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/cluster_min_pts: 2
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/convex_hull_min_pt_separation: 0.1
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_convert_outlier_pts: True
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_filter_remaining_outlier_pts: False
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_inlier_distance: 0.15
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_min_inliers: 10
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_no_iterations: 2000
* /move_base/TebLocalPlannerROS/costmap_converter/CostmapToPolygonsDBSMCCH/ransac_remainig_outliers: 3
* /move_base/TebLocalPlannerROS/costmap_converter_plugin: costmap_converter...
* /move_base/TebLocalPlannerROS/costmap_converter_rate: 5
* /move_base/TebLocalPlannerROS/costmap_converter_spin_thread: True
* /move_base/TebLocalPlannerROS/costmap_obstacles_behind_robot_dist: 1.0
* /move_base/TebLocalPlannerROS/dt_hysteresis: 0.1
* /move_base/TebLocalPlannerROS/dt_ref: 0.3
* /move_base/TebLocalPlannerROS/enable_homotopy_class_planning: True
* /move_base/TebLocalPlannerROS/enable_multithreading: True
* /move_base/TebLocalPlannerROS/feasibility_check_no_poses: 5
* /move_base/TebLocalPlannerROS/footprint_model/type: polygon
* /move_base/TebLocalPlannerROS/footprint_model/vertices: [[-0.35, 0.6], [1...
* /move_base/TebLocalPlannerROS/free_goal_vel: False
* /move_base/TebLocalPlannerROS/global_plan_overwrite_orientation: True
* /move_base/TebLocalPlannerROS/h_signature_prescaler: 0.5
* /move_base/TebLocalPlannerROS/h_signature_threshold: 0.1
* /move_base/TebLocalPlannerROS/include_costmap_obstacles ...
What is being published in
/scan
? Also, would you be able to attach aRviz
screenshot?Also, in the
local_costmap_params.yaml
,global_frame
androbot_base_frame
are set tot265_link
. What is that?Also, are you using a
static_map
for navigation?The solution was to correct the source frame for the observation source.
point_cloud_sensor: {sensor_frame: odom,
topoint_cloud_sensor: {sensor_frame: t265_odom,
and fix the local map frames to make sense.