hemsinghb commented 6 months ago

If anyone made a sonar range sensor with linorobot2 , please share the code to avoid obstacles. Regars, Hem.

hippo5329 commented 5 months ago

https://github.com/hippo5329/linorobot2_hardware/blob/master/firmware/lib/range/range.cpp

hemsinghb commented 4 months ago

Thank You dear [hippo5329] !. I did the ultrasound sensor data publishes and also got range sensor cone on RVIZ. Only issue is costmap_2d is not forming when place any obstruction in front of USS. only the cone size is increasing / decreasing. what is next.

my navigation.yaml is as under, please guide further:

global_costmap: global_costmap: ros__parameters: use_sim_time: false footprint_padding: 0.03 update_frequency: 10.0 publish_frequency: 10.0 global_frame: map robot_base_frame: base_link robot_radius: 0.22 # radius set and used, so no footprint points resolution: 0.05 plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: true observation_sources: scan base_scan footprint_clearing_enabled: true max_obstacle_height: 2.0 combination_method: 1 scan: topic: /scan obstacle_max_range: 2.5 obstacle_min_range: 0.0 raytrace_max_range: 3.0 raytrace_min_range: 0.0 max_obstacle_height: 2.0 min_obstacle_height: 0.0 clearing: true marking: true data_type: "LaserScan" inf_is_valid: false base_scan: topic: /base/scan obstacle_max_range: 2.5 obstacle_min_range: 0.0 raytrace_max_range: 3.0 raytrace_min_range: 0.0 max_obstacle_height: 2.0 min_obstacle_height: 0.0 clearing: true marking: true data_type: "LaserScan" inf_is_valid: false voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: true footprint_clearing_enabled: true max_obstacle_height: 2.0 publish_voxel_map: true origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 unknown_threshold: 15 mark_threshold: 0 observation_sources: pointcloud combination_method: 1 pointcloud: # no frame set, uses frame from message topic: /camera/depth/color/points max_obstacle_height: 2.0 min_obstacle_height: 0.01 obstacle_max_range: 2.5 obstacle_min_range: 0.0 raytrace_max_range: 3.0 raytrace_min_range: 0.0 clearing: true marking: true data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: true enabled: true subscribe_to_updates: true transform_tolerance: 0.1 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true inflation_radius: 0.55 cost_scaling_factor: 1.0 inflate_unknown: false inflate_around_unknown: true always_send_full_costmap: true

local_costmap: local_costmap: ros__parameters: use_sim_time: false update_frequency: 10.0 publish_frequency: 10.0 global_frame: odom robot_base_frame: base_link rolling_window: true width: 3 height: 3 resolution: 0.05 robot_radius: 0.22 # radius set and used, so no footprint points

plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]

  plugins: ["obstacle_layer", "voxel_layer", "inflation_layer","sonar_layer"]
  obstacle_layer:
    plugin: "nav2_costmap_2d::ObstacleLayer"
    enabled: true
    observation_sources: scan base_scan
    footprint_clearing_enabled: true
    max_obstacle_height: 2.0
    combination_method: 1
    scan:
      topic: /scan
      obstacle_max_range: 2.5
      obstacle_min_range: 0.0
      raytrace_max_range: 5.0
      raytrace_min_range: 0.0
      max_obstacle_height: 2.0
      min_obstacle_height: 0.0
      clearing: false
      marking: true
      data_type: "LaserScan"
      inf_is_valid: false
      track_unknown_space: true
      inflate_unknown: true
    base_scan:
      topic: /base/scan
      obstacle_max_range: 2.5
      obstacle_min_range: 0.0
      raytrace_max_range: 3.0
      raytrace_min_range: 0.0
      max_obstacle_height: 2.0
      min_obstacle_height: 0.0
      clearing: false
      marking: true
      data_type: "LaserScan"
      inf_is_valid: false
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    enabled: true
    footprint_clearing_enabled: true
    max_obstacle_height: 2.0
    publish_voxel_map: true
    origin_z: 0.0
    z_resolution: 0.05
    z_voxels: 16
    max_obstacle_height: 2.0
    unknown_threshold: 15
    mark_threshold: 0
    observation_sources: pointcloud
    combination_method: 1
    pointcloud:  # no frame set, uses frame from message
      topic: /camera/depth/color/points
      max_obstacle_height: 2.0
      min_obstacle_height: 0.01
      obstacle_max_range: 2.5
      obstacle_min_range: 0.0
      raytrace_max_range: 3.0
      raytrace_min_range: 0.0
      clearing: true
      marking: true
      data_type: "PointCloud2"
  inflation_layer:
    plugin: "nav2_costmap_2d::InflationLayer"
    enabled: true
    inflation_radius: 0.3
    cost_scaling_factor: 1.0
    inflate_unknown: true
    inflate_around_unknown: true

  sonar_layer:
    plugin: "nav2_costmap_2d::RangeSensorLayer"
    #plugin:"range_sensor_layer::RangeSensorLayer"
    frame_id: /range_link
    topic: /ultrasound_FL
    no_readings_timeout: 0.0
    clear_on_max_reading: true
    clear_threshold: 0.2
    mark_threshold: 1.0

---regards, Hem

hippo5329 commented 4 months ago

Indentation is very important in YAML. Please check your indentation. To show your code here, please fence your code blocks with three backticks (```) or three tildes (~~~) on the lines before and after the code block.

hippo5329 commented 4 months ago

mark_threshold: 0.8 , which is the default , probability.

hippo5329 commented 4 months ago

local_costmap.plugins: move sonar_layer before inflation_layer global_costmap.plugins: add sonar_layer before inflation_layer

hippo5329 commented 4 months ago

  sonar_layer:
    plugin: "nav2_costmap_2d::RangeSensorLayer"
    topics: ["/ultrasound_FL"]
    clear_on_max_reading: true

hemsinghb commented 4 months ago

hi dear Friends and Sirs.

From the above advises I just edited my .yaml file as follows:

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: false
      footprint_padding: 0.03
      update_frequency: 10.0
      publish_frequency: 10.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22 # radius set and used, so no footprint points
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer","sonar_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan base_scan
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        combination_method: 1
        scan:
          topic: /scan
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          inf_is_valid: false
        base_scan:
          topic: /base/scan
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          inf_is_valid: false
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        unknown_threshold: 15
        mark_threshold: 0
        observation_sources: pointcloud
        combination_method: 1
        pointcloud:  # no frame set, uses frame from message
          topic: /camera/depth/color/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.01
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          clearing: true
          marking: true
          data_type: "PointCloud2"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
        enabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      sonar_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        enabled: True
        topics: ["ultrasound_FL"]
        no_readings_timeout: 0.0
        clear_on_max_reading: true
        clear_threshold: 0.2
        mark_threshold: 0.8
        inflate_cone: 0.99         
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: true
        inflation_radius: 0.3
        cost_scaling_factor: 1.0
        inflate_unknown: false
        inflate_around_unknown: true
      always_send_full_costmap: true

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: false
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22 # radius set and used, so no footprint points
      resolution: 0.05
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer","sonar_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan base_scan
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        combination_method: 1
        scan:
          topic: /scan
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: false
          marking: true
          data_type: "LaserScan"
          inf_is_valid: false
        base_scan:
          topic: /base/scan
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: false
          marking: true
          data_type: "LaserScan"
          inf_is_valid: false
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        unknown_threshold: 15
        mark_threshold: 0
        observation_sources: pointcloud
        combination_method: 1
        pointcloud:  # no frame set, uses frame from message
          topic: /camera/depth/color/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.01
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          clearing: true
          marking: true
          data_type: "PointCloud2"
      sonar_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        enabled: True
        topics: ["ultrasound_FL"]
        no_readings_timeout: 0.0
        clear_on_max_reading: true
        clear_threshold: 0.2
        mark_threshold: 0.8
        inflate_cone: 0.99           
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: true
        inflation_radius: 0.3
        cost_scaling_factor: 1.0
        inflate_unknown: false
        inflate_around_unknown: true

But the costmap is not forming on rviz with USS (RangeSensorLayer).

I need your help to complete my project please..., Thank you.

---regards., Hem.

hippo5329 commented 4 months ago

The order of plugins is important.

global plugins: ["static_layer", "obstacle_layer", "voxel_layer", "sonar_layer", "inflation_layer"]

local plugins: ["obstacle_layer", "voxel_layer","sonar_layer", "inflation_layer"]

hemsinghb commented 4 months ago

Dear sir, Thank you for your response .

I'm getting this warning on terminal continuously.:

[global_costmap.global_costmap]: No range readings received for 33.50 seconds, while expected at least every 1.00 seconds.

      sonar_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        enabled: True
        topics: ['ultrasound_FL']
        no_readings_timeout: 1.0       #
        clear_on_max_reading: true
        clear_threshold: 0.2
        mark_threshold: 0.8
        inflate_cone: 1.0
        clear_on_max_reading: true
        input_sensor_type: ALL

On Fri, Jul 5, 2024 at 11:09 PM Thomas Chou @.***> wrote:

The order of plugins is important.

global plugins: ["static_layer", "obstacle_layer", "voxel_layer", "sonar_layer", "inflation_layer"]

local plugins: ["obstacle_layer", "voxel_layer","sonar_layer", "inflation_layer"]

— Reply to this email directly, view it on GitHub https://github.com/linorobot/linorobot2/issues/123#issuecomment-2211197621, or unsubscribe https://github.com/notifications/unsubscribe-auth/A246PI2OJ7FST3HFSC6PIU3ZK3K3TAVCNFSM6AAAAABIRLEVNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDEMJRGE4TONRSGE . You are receiving this because you authored the thread.Message ID: @.***>

hemsinghb commented 4 months ago

the sequence is changed, however the message continuously on terminal is :

"No range readings received for --- seconds, while expected at least every 1.00 seconds" as in my code "no_readings_timeout: 1.0 " is there.

Further help please .

hippo5329 commented 4 months ago

The topic must not be relative. topics: ["/ultrasound_FL"]

Please revert to "no_readings_timeout: 0.0".

And check "ros2 topic hz /ultrasound_FL".

hemsinghb commented 4 months ago

Thanks for the tip!

I did : no_readings_timeout: 0.0

every thing is ok, no messages, but no costmap for sonar obstruction.

May I ask you please, your "sonar.launch.py" and sonar.node / package file /urdf file etc. etc .

Regards, Hem

[global_costmap.global_costmap]: No range readings received for 34.22 seconds, while expected at least every 1.00 seconds

On Sat, Jul 6, 2024 at 1:36 AM Thomas Chou @.***> wrote:

The topic must not be relative. topics: ["/ultrasound_FL"]

Please revert to "no_readings_timeout: 0.0".

And check "ros2 topic hz /ultrasound_FL".

— Reply to this email directly, view it on GitHub https://github.com/linorobot/linorobot2/issues/123#issuecomment-2211361589, or unsubscribe https://github.com/notifications/unsubscribe-auth/A246PIZVVI36SQT6PGIH5XLZK34E3AVCNFSM6AAAAABIRLEVNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDEMJRGM3DCNJYHE . You are receiving this because you authored the thread.Message ID: @.***>

hippo5329 commented 4 months ago

What is the rate of "ros2 topic hz /ultrasound_FL" ?

hemsinghb commented 4 months ago

Hi, Dear Thomas sir,

bringup.launch.py: Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0.0', '0.0', '0.0', '0', '0.0', '0.0', 'base_link', 'range_link' , '20' ]

range directory which is in "linorobot2_hardware". and firmware.ino in src/firmware/ : attached here.
my ping sensors are on teensy4.1
do you suggest to put on jetson ?
bringup node is as above.
no sonar.urdf file in project
No sonar.launch.py in my project.

if you permit I will post my hardware and ws directory.

thank you.

Hem.

On Sat, Jul 6, 2024 at 12:51 PM Thomas Chou @.***> wrote:

What is the rate of "ros2 topic hz /ultrasound_FL" ?

— Reply to this email directly, view it on GitHub https://github.com/linorobot/linorobot2/issues/123#issuecomment-2211691036, or unsubscribe https://github.com/notifications/unsubscribe-auth/A246PI5Y7ANMT6UEPM5AELTZK6LHDAVCNFSM6AAAAABIRLEVNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDEMJRGY4TCMBTGY . You are receiving this because you authored the thread.Message ID: @.***>

hemsinghb commented 4 months ago

Sr, I tried with this also : Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0.0', '0.0', '0.0', '0', '0.0', '0.0', 'base_link', 'range_link']

On Sat, Jul 6, 2024 at 3:06 PM hemsingh Banoth @.***> wrote:

Hi, Dear Thomas sir,

bringup.launch.py: Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0.0', '0.0', '0.0', '0', '0.0', '0.0', 'base_link', 'range_link' , '20' ]

range directory which is in "linorobot2_hardware". and firmware.ino in src/firmware/ : attached here.

my ping sensors are on teensy4.1

do you suggest to put on jetson ?

bringup node is as above.

no sonar.urdf file in project

No sonar.launch.py in my project.

if you permit I will post my hardware and ws directory.

thank you.

Hem.

On Sat, Jul 6, 2024 at 12:51 PM Thomas Chou @.***> wrote:

What is the rate of "ros2 topic hz /ultrasound_FL" ?

— Reply to this email directly, view it on GitHub https://github.com/linorobot/linorobot2/issues/123#issuecomment-2211691036, or unsubscribe https://github.com/notifications/unsubscribe-auth/A246PI5Y7ANMT6UEPM5AELTZK6LHDAVCNFSM6AAAAABIRLEVNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDEMJRGY4TCMBTGY . You are receiving this because you authored the thread.Message ID: @.***>

hemsinghb commented 4 months ago

ros2 topic hz /utrasound_FL

Result:

min: 0.020s max: 0.027s std dev: 0.00057s window: 516 average rate: 46.156 min: 0.020s max: 0.027s std dev: 0.00060s window: 563 average rate: 46.164 min: 0.020s max: 0.027s std dev: 0.00059s window: 610 average rate: 46.168 min: 0.020s max: 0.027s std dev: 0.00058s window: 657 average rate: 46.165 min: 0.019s max: 0.027s std dev: 0.00058s window: 704 average rate: 46.153 min: 0.019s max: 0.027s std dev: 0.00058s window: 751 average rate: 46.160 min: 0.019s max: 0.027s std dev: 0.00057s window: 798

On Sat, Jul 6, 2024 at 3:36 PM hemsingh Banoth @.***> wrote:

Sr, I tried with this also : Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0.0', '0.0', '0.0', '0', '0.0', '0.0', 'base_link', 'range_link']

On Sat, Jul 6, 2024 at 3:06 PM hemsingh Banoth @.***> wrote:

Hi, Dear Thomas sir,

bringup.launch.py: Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0.0', '0.0', '0.0', '0', '0.0', '0.0', 'base_link', 'range_link' , '20' ]

range directory which is in "linorobot2_hardware". and firmware.ino in src/firmware/ : attached here.

my ping sensors are on teensy4.1

do you suggest to put on jetson ?

bringup node is as above.

no sonar.urdf file in project

No sonar.launch.py in my project.

if you permit I will post my hardware and ws directory.

thank you.

Hem.

On Sat, Jul 6, 2024 at 12:51 PM Thomas Chou @.***> wrote:

What is the rate of "ros2 topic hz /ultrasound_FL" ?

— Reply to this email directly, view it on GitHub https://github.com/linorobot/linorobot2/issues/123#issuecomment-2211691036, or unsubscribe https://github.com/notifications/unsubscribe-auth/A246PI5Y7ANMT6UEPM5AELTZK6LHDAVCNFSM6AAAAABIRLEVNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDEMJRGY4TCMBTGY . You are receiving this because you authored the thread.Message ID: @.***>

hippo5329 commented 4 months ago

It might be a bug. I will look into this later. Thanks.

hemsinghb commented 4 months ago

Hi ,

The bug is located on my .ino file .

changed Frame_id : "/range_link" to Frame_id : "range_link" in .ino hardware file, Now the costmap is working.

Thank you.

linorobot / linorobot2

linorobot2 : when moves in real environment hitting objects which are left /right side which are not in same height of LIDAR plane #123

plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]