how can i use this package?

[kyuuzou1125]

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!

[tsengapola]

Your setup is confused.

1. If you have simulation environment that publish point cloud, you dont need imu_tf_node.
2. Your costmap launch file provides static_transform_publisher from odom->map, which I cant see in your tf tree, that means something wrong in setup/launch file/your command. Also it should be map->odom not odom->map
3. Your TF tree shows your RS is on d435_link, you have to set it up correctly in new_costmap_depth_camera.yaml

[kyuuzou1125]

Hello! I 've edit my launch file and I got this map in rviz.

It seems there is no costmap updated. Is it because the point cloud output from the realsense camera in the simulation environment is relatively sparse? And I found that the filtered point cloud topic no longer has point cloud output.

Thank you very much!

[tsengapola]

Provide me following things:

1. rostopic bw /your_point_cloud_topic
2. new_costmap_depth_camera.yaml

[kyuuzou1125]

And yaml contents are as follows:

costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 5
  static_map: false
  rolling_window: true
  width: 20
  height: 20
  resolution: 0.05  
  transform_tolerance: 1.0
  footprint: [[0.46,0.25], [0.36,0.37], [0.33,0.39], [-0.39,0.4], [-0.48,0], [-0.39,-0.4], [0.33,-0.39], [0.36,-0.37], [0.46,-0.25], [0.49,0]]

  plugins:
    - {name: static, type: 'costmap_2d::StaticLayer'}
    - {name: 3DPerception, type: 'costmap_depth_camera::DepthCameraObstacleLayer'}
    - {name: inflation, type: 'costmap_2d::InflationLayer'}

  static:
    map_topic: /map

  inflation:
    inflation_radius: 1.0
    cost_scaling_factor: 0.5

  3DPerception:
    use_global_frame_to_mark: true
    forced_clearing_distance: 0.1
    ec_seg_distance: 0.2
    ec_cluster_min_size: 5
    size_of_cluster_rejection: 5
    voxel_resolution: 0.01
    check_radius: 0.1
    number_clearing_threshold: 2
    enable_near_blocked_protection: false
    number_points_considered_as_blocked: 5
    observation_sources: d435 #depth_cam_left
    depth_cam: {sensor_frame: d435_depth_frame, topic: /d435/depth/color/points, expected_update_rate: 0.3, FOV_W: 1.0, FOV_V: 0.9, min_detect_distance: 0.15, max_detect_distance: 2.0, min_obstacle_height: 0.08}
    #depth_cam_left: {sensor_frame: camera_link_left, topic: /camera_left/depth/color/points, expected_update_rate: 0.3, FOV_W: 1.0, FOV_V: 0.9, min_detect_distance: 0.15, max_detect_distance: 2.0, min_obstacle_height: 0.08}

Thank you very much!

[tsengapola]

1. Your point cloud is too huge, use noetic-voxelized-pc-before-euc branch should solve your problem.
2. In your yaml:
   depth_cam: {sensor_frame: d435_depth_frame, ...
   shoulde be:
   depth_cam: {sensor_frame: d435_link, ...

[kyuuzou1125]

Thanks, I 've modified my yaml and applied voxel filtering to the point cloud.


I set up a dynamic obstacle bounding box and use a test map.yaml, but there is no RViz output. I mean there is no real-time costmap output when the obstacle box is moving. When the small obstacle box enters the camera's field of view, there is no output from the costmap, and when the box exits the camera's field of view, the corresponding costmap is not updated either. Could it be an issue with the RViz topic choose?
And when I switch to the pointcloud topic "clustered_pc", "marking_pc", there is no output. Could it be an issue in this section?
Thank you very much!

[tsengapola]

Look like more information is required.
Can you use ros1-debug branch to run the same setup, I put some ROS_INFO in the code for debugging.
When the system is launched, provide me the msg in the terminal.

[kyuuzou1125]

Hello! I switched to ros1-debug branch.
The results are as follow:


No update in costmap.
This is what in terminal:


The upper-left corner is Gazebo, the upper-right corner is Costmap Depth Camera, the lower-left corner is RViz, and the lower-left corner on the right is Voxel Filtering.



I cannot see any new ROS_INFO msg, I think the system didn't go into the updateBounds function, must be something wrong in the pointcloud related function.
Thank you very much again!

[tsengapola]

I think the issue is here in your yaml:
observation_sources: d435 #depth_cam_left
d435: {sensor_frame: d435_depth_frame, topic: /d435/depth/color/points, expected_update_rate: 0.3, FOV_W: 1.0, FOV_V: 0.9, min_detect_distance: 0.15, max_detect_distance: 2.0, min_obstacle_height: 0.08}

[kyuuzou1125]

It works! Thank you so much!

Like you said, the key questions are the huge pointcloud data and yaml. In yaml, I forget to keep the sources name and the name below be the same. Thank you!

By the way, when I turned into real environment, new problem turned out:

Why the buffer doesnot update?

I run your launch file and only change imu publisher and tf_transfer. Thank you!

[tsengapola]

I think the plugin did not get your point cloud.
Provide me the tf tree ad also check your topic and frame setup in your yaml.

[kyuuzou1125]

Thank you, I check my yaml and edit camera_frame inside and it works. Thank you!

The tf_tree I used to meet are all map->base_link->camera_link, so the reversed order won't cause any issues, right?

[tsengapola]

The base_link->map connection is not commonly used. I suggest to make it as map->base_link->camera_link.
You might meet some issue if you use base_link->map.