🚡D.22 Collision Monitor

သူကတော့ robot safety အတွက် node ပါပဲ။ တခုခုနဲ့ တိုက်မိ မသွားဘဲ ရှောင်သွားနိုင်အောင် sensor data ကိုရယူပြီးတော့ costmap, trajectory planners တို့နဲ့အလုပ်လုပ်ပါတယ်။ အတားအဆီးပေါ်လာတာနဲ့ robot ကို slow down ဒါမှမဟုတ် ရပ်တန့်လိုက်မှာဖြစ်ပါတယ်။ ဒီကောင်က industrial robots တွေမှာဆို အရေးကြီးတဲ့ အစိတ်အပိုင်းဖြစ်ပါတယ်။ industrial robot တွေသည် high torque သို့မဟုတ် high velocity နဲ့ မောင်းတာမို့ safety ဖြစ်ဖို့ အရေးကြီးပါတယ်။ သူက robot ရဲ့ base frame နဲ့ တဝိုက် နေရာ တွေကို zone အနေနဲ့သတ်မှတ်ထားပါတယ်။ အတားအဆီးတွေသည် အဲ့ဒီ zone ထဲရောက်လာခဲ့ရင် သတ်မှတ်ထားတဲ့ အလုပ်ကို လုပ်မှာဖြစ်ပါတယ်။ zone အလိုက် သတ်မှတ်ထားတဲ့ model တွေက

• Stop model:zone တစ်ခုနဲ့ threshold တစ်ခု သတ်မှတ်ပေးထားရင် အဲ့ဒီဇုံထဲမှာ min_points အရေအတွက်ထက် obstacle အရေအတွက် များလာတဲ့ အခါ obstacle ပျောက်သွားတဲ့ အထိ robot ကို ရပ်ထာထးမှာဖြစ်ပါတယ်။

• Slowdown model: obstacle သည် ဒီ zone ထဲရောက်လာရင်တော့ speed ကို လျော့ချမှာဖြစ်ပါတယ်။ slowdown_ration ထားပေးတဲ့ အတိုင်း လျော့ချမှာဖြစ်ပြီး min_points ကတော့ အပေါ်က ကောင်နဲ့ သဘောတရားအတူတူပါပဲ။

• Limit model: သူကတော့ linear_limit နဲ့ angular_limit ထားပြီး velocity ကို ကန့်သတ်ပေးမှာဖြစ်ပါတယ်။

• Approach model: သူက လက်ရှိ robot speed ကိုရယူပြီး sensor data ကရတဲ့ obstacle ကို တိုက်မိနိုင်မည့် အချိန်ကိုတွက်ပါတယ်။ အဲ့ဒီ အချိန်သည် time_before_collision ဆိုတဲ့ parameter တန်ဖိုးထက် နည်းတဲ့အခါ robot ကို အရှိန် လျော့ချသွားမှာဖြစ်ပါတယ်။ ( obstacle ဆီကို ဝရုန်းသုန်းကား မဟုတ်ပဲ ဖြေးဖြေးချင်း ချဥ်းကပ်မယ်ပေါ့။ ) အဲ့ဒီ time_before_collision seconds ကို အမြဲအသုံးပြုနေမှာဖြစ်ပါတယ်။

zone အားလုံးနဲ့ တွဲဖက် အသုံးပြုလို့ကောင်းတာကတော့ circle shape robot ပဲ။ ကျန်တဲ့ကောင်တွေကတော့ စမ်းကြည့်ရမှာပဲ။

Name	Description	Type	Default
base_frame_id	Robot base frame.	string	“base_footprint”
odom_frame_id	Which frame to use for odometry.	string	"odom"
cmd_vel_in_topic	Input cmd_vel topic with desired robot velocity.	string	"cmd_vel_raw"
cmd_vel_out_topic	Output cmd_vel topic with output produced by Collision Monitor velocities.	string	“cmd_vel”
state_topic	Output the currently activated polygon action type and name. Optional parameter. No publisher will be created if it is unspecified.	string	""
transform_tolerance	Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.	double	0.1
source_timeout	Maximum time interval in which source data is considered as valid.	double	0.2
base_shift_correction	Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time. If enabled, produces more accurate sources positioning in the robot base frame, at the cost of slower performance. This will cause average delays for ~1/(2*odom_rate) per each cmd_vel calculation cycle. However, disabling this option for better performance is not recommended for the fast moving robots, where during the typical rate of data sources, robot could move unacceptably far. Thus reasonable odometry rates are recommended (~100 hz). (current time နဲ့ latest source time ကွာခြားချက်ကို သုံးပြီး base frame movement ရဲ့ source data ကို correct လုပ်ပေးတယ်။ enable လုပ်ထားရင် performance နှေးမယ် ၊ ဒါပေမယ် တိကျမယ်။ delay တော့ ~1/(2*odom_rate) per each cmd_vel calculation cycle . Performance မြန်ချင်လို့ disable လုပ်တာကို တော့ recommend မပေးပါဘူး ။ အကယ်၍ disable လိုက်ရင် robot ရဲ့ typical odometry rate အတ္ွင်းမှာပဲ data sources တွေတွက်ချက်နေချိန်မှာ robot က တော်တော် ဝေးဝေးရောက်သွားပါလိမ့်မယ်။ အသင့်တော်ဆုံး odometry rate တွေကတော့ 100Hz ၀န်းကျင်လို့ရေးထားပါတယ်။)
stop_pub_timeout	Timeout, after which zero-velocity ceases to be published. It could be used for other overrode systems outside Nav2 are trying to bring the robot out of a state close to a collision, or to allow a standing robot to go into sleep mode. (Zero-velocity case မှာ robot ကို timeout လုပ်ဖို့အတွက်parameter ဖြစ်ပါတယ်။ဒီ param ကို Nav2 ရဲ့ အပြင်တွေမှာပါ အသုံးပြုနိုင်ပါတယ်။ robot ကို standing mode တို့ sleep mode တို့ အဖြစ်နဲ့ပေါ့။)	double	0.1
polygons	List of data sources (laser scanners, pointclouds, etc…). Causes an error, if not specialized.	vector<string>	N/A

Polygons parameters

Name	Description	Type	Default
<polygon_name>.type	Type of polygon shape. Available values are polygon, circle. Causes an error, if not specialized.	string	N/A
<polygon_name>.points	Polygon vertexes, listed in {p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...} format (e.g. {0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25} for the square in the front). Used for polygon type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to polygon_sub_topic for points in the stop/slowdown/limit action types, or footprint subscriber to footprint_topic for approach action type. (Polygon ကို vector နဲ့ 3D အသုံးပြုဖို့ ဖြစ်နိုင်)	vector<double>	N/A
<polygon_name>.polygon_sub_topic	Topic to listen the polygon points from. Applicable only for polygon type and stop/slowdown/limit action types. Causes an error, if not specified and points are also not specified. If both points and polygon_sub_topic are specified, the static points takes priority. (polygon points topic ကို နားထောင်တယ်။ Stop/slowdown/limit စတဲ့ action types တေွအတွက်ပဲ အသုံးပြုတဲ့ polygon type ဖြစ်တယ်။ topic ကို specified လုပ်မထားရင် point တွေလည်း specified မဖြစ်ဘူး။အကယ်၍ point ကော polygon_sub_topic ကော specified ထားရင် static points ကိုပဲ ဦးစားပေး ယူပါမယ်။)	string	N/A
<polygon_name>.radius	Circle radius. Used for circle type. Causes an error, if not specialized.	double	N/A
<polygon_name>.action_type	Zone behavior model. Available values are stop, slowdown, limit, approach. Causes an error, if not specialized.	string	N/A
<polygon_name>.min_points	Minimum number of data readings within a zone to trigger the action. Former max_points parameter for Humble, that meant the maximum number of data readings within a zone to not trigger the action). min_points is equal to max_points + 1 value. (zone ကနေ action ကို ဖွင့်ဖို့ အနည်းဆုံး read မယ့် data။ maximum no of data ကို zone က action ဖွင့်မပေး။ min_points တွေက max_points+1)	int	4
<polygon_name>.slowdown_ratio	Robot slowdown (share of its actual speed). Applicable for slowdown action type.	double	0.5
<polygon_name>.linear_limit	Robot linear speed limit. Applicable for limit action type.	double	0.5
<polygon_name>.angular_limit	Robot angular speed limit. Applicable for limit action type.	double	0.5
<polygon_name>.time_before_collision	Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for approach action type.	double	0.2
<polygon_name>.simulation_time_step	Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for approach action type. (collision prediction parameter , higher values ပေးရင် အချိန်ကြာပြီး acrate ဖြစ်မယ် , performance ပိုကောင်းမယ်။	double	0.1
<polygon_name>.visualize	Whether to publish the polygon in a separate topic.	bool	FALSE
<polygon_name>.polygon_pub_topic	Topic name to publish a polygon to. Used only if visualize is true.	string	<polygon_name>

Observation sources parameters

Name	Description	Type	Default
<source name>.type	Type of polygon shape. Could be scan, pointcloud or range.	string	"scan"
<source name>.topic	Topic to listen the source data from.	string	"scan"
<source name>.min_height	Minimum height the PointCloud projection to 2D space started from. Applicable for pointcloud type.	double	0.05
<source name>.max_height	Maximum height the PointCloud projection to 2D space ended with. Applicable for pointcloud type.	double	0.5
<source name>.obstacles_angle	Angle increment (in radians) between nearby obstacle points at the range arc. Two outermost points from the field of view are not taken into account (they will always exist regardless of this value). Applicable for range type. (angle increment နီးစပ်တဲ့ obstacle's point နဲ့ range of arc နဲ့ radian ကွာဟမှု။ obstacle ရဲ့ အပြင်ဘက်points တွေကို regardလုပ်မယ်။)	double	PI/180(1 degree)

နမူနာ

collision_monitor:
  ros__parameters:
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_raw"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.5
    source_timeout: 5.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"]
    PolygonStop:
      type: "circle"
      radius: 0.3
      action_type: "stop"
      min_points: 4  # max_points: 3 for Humble
      visualize: True
      polygon_pub_topic: "polygon_stop"
    PolygonSlow:
      type: "polygon"
      points: [1.0, 1.0, 1.0, -1.0, -0.5, -1.0, -0.5, 1.0]
      action_type: "slowdown"
      min_points: 4  # max_points: 3 for Humble
      slowdown_ratio: 0.3
      visualize: True
      polygon_pub_topic: "polygon_slowdown"
    PolygonLimit:
      type: "polygon"
      points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5]
      action_type: "limit"
      min_points: 4  # max_points: 3 for Humble
      linear_limit: 0.4
      angular_limit: 0.5
      visualize: True
      polygon_pub_topic: "polygon_limit"
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 2.0
      simulation_time_step: 0.02
      min_points: 6  # max_points: 5 for Humble
      visualize: False
    observation_sources: ["scan", "pointcloud"]
    scan:
      type: "scan"
      topic: "/scan"
    pointcloud:
      type: "pointcloud"
      topic: "/intel_realsense_r200_depth/points"
      min_height: 0.1
      max_height: 0.5