🍏C.1 Setting up Sensors

Robot အတွက် sensor ဆိုတာလည်း အရေးကြီးပါတယ်။ ဘာဖြစ်လို့လဲဆိုတော့ sensor setup လုပ်ပြီးပြီဆိုရင် mapping တို့၊ localization တို့၊ perception တို့ နဲ့ ပတ်သက်တဲ့ task တွေမှာ အသုံးပြုရမှာ ဖြစ်လို့ပါ။ အဲ့တော့ sensor data တွေကို mapping ၊ localization တို့ ဘယ်လို့အလုပ်လုပ်လဲဆိုတာ နားလည်ဖို့ လိုပါတယ်၊၊

ကျတော်တို့ဟာ Nav 2 ရဲ့ nav2_costmap_2d package ကိုလည်း သိထားဖို့လိုပါတယ်။ ဘာလို့လဲဆိုတော့ သူက costmap တွေထုတ်ပေးတာဖြစ်ပြီး ဒါကို navigation ရဲ့ path_planning ကအသုံးပြုမှာ ဖြစ်ပါတယ်။

ကျတော်တို့အသုံးပြုကြမည့် sensor တွေက lidar, RGB camera, depth_camera , IMU , GPS ... စတာတွေပဲပေါ့ ။ ROS မှာ sensor_msgs ဆိုတဲ့ package ထဲမှာ sensor data type တော်တော်များများအတွက် data type တွေပါပါတယ်။

• sensor_msgs/Range

• sensor_msgs/LaserScan

• sensor_msgs/PointCloud2

• sensor_msgs/Image

အများအားဖြင့်တော့ အပေါ်က ကောင်တွေသုံးကြတာဖြစ်ပြီးတော့ နောက်တခြား radar_msgs, vision_msgs တို့လို့ အခြား sensor_msgs type တွေလည်း အများကြီးရှိပါသေးတယ်။

source from Nav2 Documentation

နောက် gazebo မှာဆိုရင် simulation လုပ်လို့ရတယ်။ ဒါကိုမပြောတော့ဘူး။ rom2109_simulation မှာ URDF ကို သွားလေ့လာကြည့်ပေါ့။

အဲ့လို sensor ကို setup လုပ်ပြီးပြီ ဆိုရင် slam_toolbox နဲ့ Mapping စလုပ်လို့ရပါပြီ။ slam_toolbox မှာဆိုရင် mapping ဖတ်လို့လည်းရသလို localization လုပ်လို့လည်းရတယ်။ အဲ့တော့ Nav 2 ထဲမှာ slam_toolbox ရော nav2_amcl ရော နှစ်ခုစလုံးသည် robot environment ကို localization လုပ် ပေးလို့ရတယ်။ သူတို့နှစ်ခုလုံးသည် laser_sensor data ကို ရယူအသုံးပြုနိုင်ပါတယ်။အဲ့တော့ သူတို့က sensor_msgs/LaserScan ကို subscribe လုပ်သလား ၊ လုပ်တာမှန်မမှန်ကို စစ်ဆေးပေးပေါ့ ။

အဲ့မှာ သတိထားပါ topic name သည် sensor_msgs/LaserScan ဖြစ်နိုင်သလို sensor_msgs/scan လည်းဖြစ်နေနိုင်တယ် ။ အဲ့တော့ scan topic parameter ကို စစ်ဆေးပါ။

costmap 2D

costmap_2d package ကတော့ ဘာလဲဆိုတော့ ဒီ sensor information တွေကို robot environment အဖြစ် ဖော်ပြပေးတာပါပဲ။ အဲ့မှာဆိုလို့ရှိရင် သူက cell တွေနဲ့ ဖော်ပြပေးတာပေါ့နော်။ cell_grid တွေနဲ့ ၊ အဲ့မှာ 0 to 254 (0-254) ရှိတဲ့ တန်ဖိုးတွေသည် Color pixel တန်ဖိုးတွေလိုမျိုး 0 to 254 ရှိတယ်။ 0 ဆိုတာက free ပေ့ါနော် ဘာအတာဆီးမှမရှိဘူး။ 254 ဆိုတာက occupied အတားဆီးရှိတဲ့နေရာ၊ ဒါကို costmap_2d လို့ခေါ်တာပဲ။ algorithm တွေက ဒီ costmap ကို ကြည့်ပြီး လမ်းကြောင်းဆွဲတာဗျ။

အဲ့ဒီအပြင် costmap တွေမှာ layer တွေ ပါ၀င်ပါသေးတယ်။ ဘယ်လိုလဲဆိုတော့ ဥပမာအားဖြင့် ပြောရမယ်ဆိုရင်

• static layer

• inflation layer

• range layer

• obstacle layer

• voxel grid layer ဒါမျိုးတွေပေါ့။

static layer မှာ ဘာပါမလဲဆိုတော့ slam က ထုတ်ပေးတဲ့ map topic က လာတဲ့ data တွေကို costmap ပေါ်မှာဖော်ပြပေးတာပေါ့။

obstacle layer ကျတော့ sensor ကနေပြီးတော့ LaserScan တို့ PointCloud2 တို့ ဒီကနေလာတဲ့ data တွေကို ဖော်ပြေပေးတယ် အဲ့ဒါ obstacle layer ပေါ့။

voxel layer ကတော့ 3D data ကိုဖေါ်ပြပေးနိုင်တဲ့ကောင်။ ကျွန်တော်တို့ဆီမှာ 3D camera ရှိတယ်ဆိုရင် ဒီ PointCloud2 data တွေကို ဖော်ပြပေးလို့ရတယ်။ ဒါမှမဟုတ် 3D lidar ရှိတယ်ဆိုရင်လည်း သုံးလို့ရမယ်။

ranger layer ကတော့ sonar တို့ infrared sensors တို့ဆီက ကောင် ပေ့ါ။

နောက် inflation layer ကတော့ obstacleအတားဆီနဲ့ robot သွားလာနိုင်မယ့် လမ်းကြောင်းအတွက် cost ပေါ့နော်။ ဒါတွေကို ဖော်ပြပေးတဲ့ layer ဖြစ်တယ်။ color စုံဖြစ် နေတဲ့ layer တွေက inflation layer ဖြစ်တယ်။ အဲ့တော့ အဲ့မှာအရေးကြီးတာက inflation radius အဲ့ဒီကောင်တွေကအရေးကြီးတယ်။ cost_map အတွက် deep concept ကို နားလည်ချင်ရင် ROS 1 က costmap_2d documentation ကို သွားကြည့်လို့ ပြောထားတာရှိပါတယ်။

နမူနာ ပုံစံ

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: false
      rolling_window: false
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: 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
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      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"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

နမူနာ configuration မှာဆိုရင်

• global cost_map နဲ့

• local cost_map ကိုတွေ့မှာပေါ့။

global cost_map သည် global path အတွက် long term planning လုပ်မှာ ဖြစ်ပြီး local cost_map က short term planning အနေနဲ့ collusion avoidance (obstacle avoidance) အတွက်ဖြစ်ပါတယ်။

သူ့မှာဆိုရင် ဒီမှာ နမူနာပြထားတဲ့ ဒီ lines no တွေမှာဆိုရင် 15 , 18 ,32 ,52 ,68 တွေမှာ သူက plugin ထည့်ထားတာတွေ့ရတယ်။ plugin ပါဖို့လိုပါတယ်။ ဘာလို့လဲဆိုတော့ ဒီကောင်တွေက driver တွေပဲဆိုပါတော့။

static layer အတွက်ဆိုရင် (lines 14-16) မှာ map_subscribe_transient_local ကို True ပေးဖို့ လိုတယ်။ အဲ့ကောင်မထည့်ရင် မြေပုံမပေါ်ဘူး။ သူက ထုတ်လွှင့်ပြီးသား topic ရဲ့ latest ကို အမြဲရနေအောင် QOS setting အရပေးရတာ။

map_subscribe_transient_local: True

obstacle layer မှာဆိုရင် (lines 20-30) ထိ အဲ့မှာ observation_sources က scan ၊ အဲ့ဒီ scan ဆိုတာက အောက်မှာ ရေးထားတဲ့ definition ပေ့ါနော်။ နောက် topic က LaserScan topic ကတော့ /scan ပဲပေါ့။ သိရမှာက obstacle layer ကော voxel layer နှစ်ခုလုံးက LaserScan , PointCloud2 data တွေကို ရယူနိုင်ပါတယ်။ နမူနာပြထားသလို နှစ်ခုလုံးနဲ့ အလုပ်လုပ်နိုင်တယ်။

အဲ့မှာ max_obstacle_height ဆိုတဲ့ ကောင်ရှိတယ် သူ့ကို 0 ( Zero ) ထားထားရင် အဲ့တာ ဘာလဲဆိုတော့ ကျွန်တော်တို့ voxel grid အသုံးမပြုသေးဘူးဆိုတဲ့ သဘောပေါ့။ ဒီ obstacle ရဲ့ height ကို ဒီ cost_map ပေါ်မှာ လာပြမှာဆိုတော့ အဲ့ကောင်ရဲ့ အမြင့်က 0 ထားထားတယ်ဆိုတာ ဒါသည် အသုံးမပြုဘူးဆိုတဲ့ သဘောပါ။

clearing ဆိုတာကတော့ အဲ့ဒီ obstacle တွေကို အဲ့ costmap အပေါ်ကနေ ဖျောက်ပစ်မှာလား မဖျောက်ပစ်ဘူးလားဆိုတာပဲ၊ ဒါအရေးပါတယ်လို့ထင်တယ် ဘာလို့လဲဆိုတော့ cost_map တွေရှုပ်နေလို့ရင် recovery အရ clear လုပ်ပေးဖို့လိုတယ်။အဲ့ clear ကိုမှ maximum နဲ့ miniman ထားချင်ရင်

• raytrace_max_range

• raytarce_min_range ဒီကောင်တွေကို သုံးတာမျိုးရှိမယ်။

marking ဆိုတာကတော့ costmap ပေါ်မှာ obstacle ဒါမှမဟုတ် marker တခုခု ထည့်တာကိုမထည့်တာကို လုပ်လို့ရတာဖြစ်ပါလိမ့်မယ်။ သူ့မှာလည်း obstacle_max_range , obstacle_min_range ရှိတယ်။ ( အားမှ marker topic ဖန်တီးကြည့်ပေါ့ )

နောက်တစ်ခု inflation layer အတွက် line နံပါတ် 31 ကနေ 34 ထိ အောက်ပါတိုင်းတွေ့ရသလို

inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

နောက်တခု လိုင်းနံပါတ် 67 ကနေ 70 ထိမှာ လည်း ဒီအတိုင်းပါပဲ။

inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

ဒီ inflation radius ကို exponential decay ဖြစ်သွားစေမယ့် cost_scaling_factor ဆိုတဲ့ parameter ကို တန်ဖိုး တခုခု ပေးလို့ရတယ်။ ( တကယ်တော့ ဒါသည် Algorithm ကို နားလည်ရင် ပိုကောင်းတယ်။ )

နောက်တခုက inflation radius ပါ။ သူ့ကို အရင် ROS 1 စာအုပ် မှာ အောက်ကအတိုင်းဖေါ်ပြထားပါတယ်။

ပုံအရ Figure 15 သည် path သည် အလယ်ကနေ သွားတဲ့အတွက် ပိုပြီးတော့ ကောင်းတယ်လို့ဆိုရမှာပဲဖြစ်ပါတယ်။

ဟုတ်ပြီ ထားလိုက်ပါတော့ နောက်တစ်ခုက voxel layer ။

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"

3D voxel grid ကို enable လုပ်ဖို့ publish_voxel_map ကို True ပေးရမယ်။ သူ့ရဲ့ resolution ကိုတော့ z_resolution ဆိုတာကတော့ တော့ height အတွက် ဖြစ်ပြီး voxel အရေအတွက်ကတော့ z_voxels ဆိုတဲ့ ကောင်ပါ။ observation_sources ကတေ့ာ laser ကို သုံးမယ်ဆိုရင် scan ပဲပေါ့။

ကျွန်တော့်တို့ robot မှာတော့ distance sensor သုံးမထားတဲ့အတွက် range layer ကို မသုံးထားဘူး။ နောက်မှ ultrasonic နဲ့ distance အတွက်အသုံးပြုကြည့်မယ်ဗျာ။