0
$\begingroup$

I am trying to use 4 sonar sensors (front, back, left, right) in gazebo simulation along with LIDAR for navigation using NAV2.

I have included the sonar gazebo plugins as below

<gazebo reference="sonar_front">
      <sensor name="sonar_front" type="ray">
        <always_on>true</always_on>
        <visualize>true</visualize>
        <update_rate>5</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>5</samples>
              <resolution>1.000000</resolution>
              <min_angle>-0.12</min_angle>
              <max_angle>0.12</max_angle>
            </horizontal>
            <vertical>
              <samples>5</samples>
              <resolution>1.000000</resolution>
              <min_angle>-0.01</min_angle>
              <max_angle>0.01</max_angle>
            </vertical>
          </scan>
          <range>
            <min>0.02</min>
            <max>4</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <plugin name="sonar_front" filename="libgazebo_ros_ray_sensor.so">
          <ros>
              <remapping>~/out:=sonar1</remapping>
          </ros>
          <output_type>sensor_msgs/Range</output_type>
          <radiation_type>ultrasound</radiation_type>
          <frame_name>sonar_front</frame_name>
        </plugin>
      </sensor>  
    </gazebo>

Below is a snippet of the local costmap in nav2_params.yaml where I have included the range_layer plugin

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: "base_link"
      robot_radius: 0.22
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      # footprint: "[ [0.4, 0.3], [0.4, -0.3], [-0.4, -0.3], [-0.4, 0.3] ]"
      footprint_padding: 0.01
      plugins: ["voxel_layer", "inflation_layer", "range_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 0.4
      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
      range_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        topics:
          [
            "/sonar1",
            "/sonar2",
            "/sonar7",
            "/sonar8",
          ]
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

The Lidar is placed a bit above the Sonar sensors. So I have placed a small cube that is being detected by the sonar sensors but not the lidar.

But nav2 is not detecting the cube somehow and directly collides with the cube even though the sonars detect it.

enter image description here enter image description here

here is the rosgraph enter image description here

Here is a video of the simulation nav2_sonar_error

$\endgroup$

2 Answers 2

0
$\begingroup$

Try to declare the sensor layer parameters to see if work:

https://navigation.ros.org/configuration/packages/costmap-plugins/range.html I would change the .mark_threshold or clear_threshold to check the behavior, if the value is not appropriate it will define as clear area (no obstacle detection).

The topics contain data?

Hi I have still not worked directly with sonar in ros2. But I think it can be a RELIABILITY issue. I mean your sensor data is publishing in "Reliable" QoS for example and your local_costmap node is consuming in "Best_Effort".

To check this you need to issue

ros2 topic info -v /sonar1 
ros2 topic echo /sonar1

Check if your remap has worked or not echoing Data.

Then also check if this data is arriving at local_costmap node

ros2 node info -v /node_name (local costmap or range_sensor_node_name)

After inspecting the publishers and subscribers of this node you will be able to determine if it is receiving data from sonar topics in correct reliability and if is sending(publishing) data to generate the occupancy grid (local costmap).

From the video it seems the sonar is indeed working as it is reducing in size when reflect near objects. I suppose that the sensor layer your are using is not working for some reason (maybe not installed, did you install navigation2 and nav2_bringup?).

Try:

ros2 param list 

Check if this range_layer Params are being loaded. If not the range_layer is not working. The solution would select other layer such as voxel_grid or obstacle layer...

Other hint is try to declare all parameters from this range layer... And most important check on output from terminal if a log related to range_layer Exists (if was successfully loaded).

Last attempt would be use /scan sensor from your lidar as source of this range_layer. If scan sensor works, you know the issue is within the sonar sensor gazebo plugin (a portability issue from ROS1 to ROS2) .

$\endgroup$
3
  • $\begingroup$ Hi Marcus, I have checked that the range layer is initialized correctly in the log output and also I can see the range layer parameters loaded successfully. The sonar topic is correctly arriving in the local costmap node. But I am yet to verify if this is somehow a Gazebo plugin error. $\endgroup$ Commented Mar 20 at 11:41
  • $\begingroup$ Ok I hope you fix it. I am going to try this plugin as well soon, if I get some news I msg you. $\endgroup$ Commented Mar 20 at 12:01
  • $\begingroup$ Thank you, Marcus $\endgroup$ Commented Mar 20 at 12:33
0
$\begingroup$

Nilutpol I took 2 days to figure out this issue (you are lucky because I also need ultrasonic for my project hehe). I will try to guide you to reach it step-by-step (If you are still stuck):

1- The issue was previously treated by Steve and another user you can check here: Range sensor layer can't transform from odom to range_sensor_link

Okay, my first error was naming the plugin sensor's frame_id with a name different from my ultrasonic link, to which the ultrasonic sensor is attached. You need to make sure the names match. My link name:

<link name="front_left_far_sonar_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="${ultrasonic_length}" radius="${ultrasonic_radius}" />
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="${ultrasonic_length}" radius="${ultrasonic_radius}" />
      </geometry>
      <material name="light_black"/>
    </visual>
  </link>

My Gazebo Plugin:

  <!-- Gazebo Plugin for Ultrasonic Sensor -->
  <gazebo reference="front_left_far_sonar_link">
    <sensor type="ray" name="front_left_far_sonar_link">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>20</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1</samples>
            <resolution>1.00000</resolution>
            <min_angle>-0.12</min_angle>
            <max_angle>0.12</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1.000000</resolution>
            <min_angle>-0.01</min_angle>
            <max_angle>0.01</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.2</min>
          <max>5.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="gazebo_ros_ray_sensor_plugin" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <namespace>/ultrassonic</namespace>
          <remapping>~/out:=front_left</remapping>
        </ros>
        <frame_name>front_left_far_sonar_link</frame_name>
        <output_type>sensor_msgs/Range</output_type>
        <field_of_view>0.01</field_of_view>
        <radiation_type>ultrasound</radiation_type>
      </plugin>
    </sensor>
  </gazebo>

2- I have added the log info line code as they have done( good to debug purpose on the terminal) in the git issue link I sent you Log Info Time Tolerance. So update your range_sensor_layer.cpp accordingly (it is located under nav2_costmap_2d/plugins package.

void RangeSensorLayer::updateCostmap(
  sensor_msgs::msg::Range & range_message,
  bool clear_sensor_cone)
{
  max_angle_ = range_message.field_of_view / 2;

  geometry_msgs::msg::PointStamped in, out;
  in.header.stamp = range_message.header.stamp;
  in.header.frame_id = range_message.header.frame_id;

  std::string * errstr = NULL;
  if (!tf_->canTransform(
      in.header.frame_id, global_frame_,
      tf2_ros::fromMsg(in.header.stamp),
      tf2_ros::fromRclcpp(transform_tolerance_), errstr))
  {
    RCLCPP_INFO(
      logger_, "Range sensor layer can't transform from %s to %s",
      global_frame_.c_str(), in.header.frame_id.c_str());
    return;
  }

3- For this user case (Mine too) the transform_tolerance of 0.3 seconds was enough to fix the delay in the ultrasonic --> local_costmap (odom/map) transform message. You need to set this parameter in your configuration file with this tolerance time too (transform_tolerance):

My configuration has this face:

  range_sensor_layer:
    plugin: "nav2_costmap_2d::RangeSensorLayer"
    enabled: True
    transform_tolerance: 0.3
    clear_on_max_reading: true
    topics: ["/ultrassonic/front_left"]
    clear_threshold: 0.2
    mark_threshold: 0.8
    no_readings_timeout: 5.0
    clear_after_reading: true
    max_distance: 5.0
    min_distance: 0.2
    clear_on_max_reading: true
    buffer_size: 10
    observation_persistence: 2.0
    inf_is_valid: false
  static_layer:
    map_subscribe_transient_local: True
  always_send_full_costmap: True

I hope these steps fix your issue! If not please let me know!

The result:

enter image description here

$\endgroup$
2
  • $\begingroup$ Hi Marcus, Thank you for spending time on the solution. However, I am still not able to make it work. Can I share my project repo with you? $\endgroup$ Commented Mar 26 at 8:57
  • $\begingroup$ if you check my parameters configuration you will see that I don't use "range_layer" as you used. The correct name is range_sensor_layer. Therefore you need to change twice in your code. First on plugins declaration: plugins: ["voxel_layer", "inflation_layer", "range_sensor_layer"] and later when you set your params: range_sensor_layer: plugin: "nav2_costmap_2d::RangeSensorLayer" topics: [ "/sonar1", "/sonar2", $\endgroup$ Commented Mar 27 at 20:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.