tsengapola / costmap_depth_camera Goto Github PK

Hello Is ROS2 Humble working? I couldn't see your readme in ROS2 branch. Thanks

Hello, author! I want to use it together with navigation, but it dosen't work. When i use it alone, it can indeed display obstacle information and inflation radius.However, i add related plugins to move_base params including local_costmap_params and costmap_common_params, rviz can dispaly obstacle information but can not display inflation. When i set a goal which path include obstacles, the robot did not avoid obstacles.Sincerely request the author's help!

Hello, thank you very much for your nice works, should I add a touch operation after the statement (*it_3d_map).second.erase(it) in ClearMarkingbyKdtree? For example,
if (is_in_FRUSTUM && clear_all_marking_in_this_frame)
{
/// Nothing is detected, clear all markings.
// RCLCPP_WARN(logger_,"Clear all markinging in this frame.");
(*it_3d_map).second.erase(it);
touch(searchPoint.x, searchPoint.y, min_x, min_y, max_x, max_y);
}
like raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); of the obstacle_layer.cpp in navigation2.

Hi, thanks for this nice plugin.

However, we noticed that obstacles might not be marked correctly according to min_obstacle_height and max_obstacle_height in configuration. Said we set max_obstacle_height to 1.2 (unit: meter) and setup a D435i facing to a cubicle partition (height = 1.2m), there would be only part of the partition marked as obstacle. While measuring it with a tape measure and through RViz, it seems everything above 1m height is ignored.

After checking with the code, it seems the problem might be resulted from L488 and L504 in plugins/depth_camera_obstacle_layer.cpp.
Consider this part: h_ind > (int)marking_height_above_ground_/voxel_resolution_, the parentheses are missing to prioritize the calculation of marking_height_above_ground_/voxel_resolution_ before casting it to int.
So that given max_obstacle_height = 1.2 would be cast to 1 before being divided by voxel_resolution, and that's should be the cause of missing obstacle markers for objects above 1 meter height.

Therefore, those 2 lines should be revised as below:

- if(mx<0 || my<0 || h_ind>(int)marking_height_above_ground_/voxel_resolution_ || h_ind<(int)marking_height_under_ground_/voxel_resolution_)
+ if(mx<0 || my<0 || h_ind>(int)(marking_height_above_ground_/voxel_resolution_) || h_ind<(int)(marking_height_under_ground_/voxel_resolution_))

Hello, I use this package and launch a turtlebot3 file and also with this costmap_depth_camera.launch file. However, I can't see anything. My turtlebot3 launch file is as follows:

  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="-8.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>

  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/new.world"/>
    <!-- <arg name="world_name" value="/home/ncs/zyw_ws/src/sim_demo/box_house.world"/> -->
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

  <!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/> -->

</launch>

And my costmap launch file is as follows:

<launch>

  <arg name="map_file" default="$(find costmap_depth_camera)/map/gallery.yaml"/>
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" output="screen">
        <rosparam file="$(arg map_file)" command="load" />
    </node>

  <!-- Run the costmap node, load the yaml in our directory -->
  <node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" output="screen">
    <rosparam file="$(find costmap_depth_camera)/launch/new_costmap_depth_camera.yaml" command="load"/>
  </node>

  <!-- 
    Publish map to base_link tf using gyro data; 
    This is just for testing, you can remove this node (imu_tf_node) if you have your own tf
  -->
  <node name="imu_tf_node" pkg="costmap_depth_camera" type="imu_tf_moving.py" output="screen" />
  
  <!-- Provide full connection from map to camera_link -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="baselink2camera" args="0.0 0.0 0 0 0 0 odom map" output="screen" /> 
  
</launch>

This is my tf tree.

Can you give some detailed imformation of using your package? Thank you very much!

Hello, we want to use your repo for obstacle avoidance. Does this project only depend on cost_map_2d? In the actual test, we use the camera node of real_sense to send point cloud data, and run this demo launch file directly. We did not succeed in visualizing the costmap in rviz. Do I need to use the whole move_base package？

hello, i use the humble version of the plugin in the nav2_costmap_2d without inflation_layer. There are some holes in point cloud updates, as shown in the figure below:

The green dots represent the point clouds in pc_3d_map_global_, and the purple dots represent point clouds of the latest frame.
In addition, the purple dots is not be put in pc_3d_map_global_ completely, especially the point clouds near the robot.

Hello,
There is a spelling mistake in your plugin file "depth_camera_obstacle_layer.cpp":
In line 131: source_node.param("max_detect_distanxe", max_detect_distance, 2.5); The param name should be "distance".

Hello, can this plug-in be applied to a depth camera that is worn obliquely?For example, a depth camera that scans the ground.

Hello. Have you encountered such an error before? I get my topic from my d435i camera and when I start nav2 I get an error like this.

[planner_server-6] [ERROR] [1703502100.699044300] [global_costmap.global_costmap]: Raw cloud size 150497 is larger than 20000 points. Exiting..
[planner_server-6] [ERROR] [1703502101.010950008] [global_costmap.global_costmap]: Raw cloud size 150168 is larger than 20000 points. Exiting..
[ERROR] [planner_server-6]: process has died [pid 16063, exit code -11, cmd '/home/mustafa/ros2_ws/install/nav2_planner/lib/nav2_planner/planner_server --ros-args --log-level info --ros-args -r __node:=planner_server --params-file /tmp/tmp97b_ghha -r /tf:=tf -r /tf_static:=tf_static -r cmd_vel:=/cmd_vel_unstamped'].

Hello good work, I can get the package to work properly. But I have a problem, the obstacle that appears in the inflation layer does not inflate around it. Therefore it is moving too close to the target. What do you suggest for this? Also, is there a problem if I add it to both local and global costmap? I think you only added it to global_costmap. Below are the parameters and an example image:

global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_footprint
use_sim_time: True
footprint: "[[-0.599, -0.389], [-0.599, 0.389], [0.599, 0.389], [0.599, -0.389]]"
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "depth_camera", "obstacle_layer", "voxel_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
depth_camera:
plugin: "nav2_costmap_2d::DepthCameraObstacleLayer"
use_global_frame_to_mark: True
forced_clearing_distance: 0.3 #0.1
ec_seg_distance: 0.1 # 0.2
ec_cluster_min_size: 6 # 5
size_of_cluster_rejection: 6 # 5
voxel_resolution: 0.2
check_radius: 0.2 # 0.1
number_clearing_threshold: 2 #2
enable_near_blocked_protection: False #False
number_points_considered_as_blocked: 6 # 5
use_voxelized_observation: False
observation_sources: top_depth_cam bottom_depth_cam rear_depth_cam
top_depth_cam:
sensor_frame: front_top_link #front_top_bottom_screw_frame
topic: /front_top/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.3962 #FOV_W: 1.3962
FOV_V: 0.9599 #FOV_V: 0.9599
min_detect_distance: 0.15 # 0.15
max_detect_distance: 2.5 # 4.0
min_obstacle_height: 0.1 # 0.2
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: False
marking: True
bottom_depth_cam:
sensor_frame: front_bottom_link
topic: /front_bottom/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.0
FOV_V: 0.9
min_detect_distance: 0.05
max_detect_distance: 3.0
min_obstacle_height: 0.085
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: True
marking: True
rear_depth_cam:
sensor_frame: rear_link #front_top_bottom_screw_frame
topic: /rear/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.3962 #FOV_W: 1.3962
FOV_V: 0.9599 #FOV_V: 0.9599
min_detect_distance: 0.15 # 0.15
max_detect_distance: 2.5 # 4.0
min_obstacle_height: 0.1 # 0.2
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: False
marking: True
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: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
# scan:
# topic: /depth_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: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 7.58 # 5.0 , 6.58
inflation_radius: 0.45
inflate_around_unknown: True
inflate_unknown: true
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False

planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.2
use_astar: false
allow_unknown: true

local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 10
height: 10
resolution: 0.05
footprint: "[[-0.599, -0.389], [-0.599, 0.389], [0.599, 0.389], [0.599, -0.389]]"
plugins: ["depth_camera", "obstacle_layer", "voxel_layer", "inflation_layer"]
depth_camera:
plugin: "nav2_costmap_2d::DepthCameraObstacleLayer"
use_global_frame_to_mark: False
forced_clearing_distance: 0.3 #0.1
ec_seg_distance: 0.1 # 0.2
ec_cluster_min_size: 6 # 5
size_of_cluster_rejection: 6 # 5
voxel_resolution: 0.2
check_radius: 0.2 # 0.1
number_clearing_threshold: 2 #2
enable_near_blocked_protection: False #False
number_points_considered_as_blocked: 6 # 5
use_voxelized_observation: False
observation_sources: top_depth_cam bottom_depth_cam rear_depth_cam
top_depth_cam:
sensor_frame: front_top_link #front_top_bottom_screw_frame
topic: /front_top/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.3962 #FOV_W: 1.3962
FOV_V: 0.9599 #FOV_V: 0.9599
min_detect_distance: 0.15 # 0.15
max_detect_distance: 2.5 # 4.0
min_obstacle_height: 0.1 # 0.2
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: True
marking: True
bottom_depth_cam:
sensor_frame: front_bottom_link
topic: /front_bottom/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.0
FOV_V: 0.9
min_detect_distance: 0.05
max_detect_distance: 3.0
min_obstacle_height: 0.085
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: True
marking: True
rear_depth_cam:
sensor_frame: rear_link #front_top_bottom_screw_frame
topic: /rear/depth/color/points/voxel_grid
expected_update_rate: 20.0
FOV_W: 1.3962 #FOV_W: 1.3962
FOV_V: 0.9599 #FOV_V: 0.9599
min_detect_distance: 0.15 # 0.15
max_detect_distance: 2.5 # 4.0
min_obstacle_height: 0.1 # 0.2
obstacle_range: 4.0
raytrace_range: 4.0
clearing: True
inf_is_valid: False
marking: True
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"
# scan:
# topic: /depth_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: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 6.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.3 # 0.3 , 0.2
cost_scaling_factor: 7.58 # 5.0 , 6.58
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