lidar scanのフィルタ処理

#ロボットが自身の体を検知して停止してしまうのを解消したい

ロボットが自分の機体をscanし、障害物として認識してしまう場合は、センサデータにマスク処理をする必要がある。

##ira_laser_tools/laserscan_multi_merger使用時

ROSwiki
source: code
paper

このツールは便利だが、複数の位置から取得されたスキャンデータをある１つのLiDARスキャンに統合するので、以下のような欠点がある。(上記論文の一部抜粋)

1. 統合元のLiDARデータに含まれているものの、統合先LiDARからは見えないはずの障害物が、間違って表示されてしまう可能性がある。
2. 統合するので、統合後のデータは、あたかも最初から１つのLiDARから取得されたかのように見えてしまうため、実際にはありえないscanデータが取得されてしまう。
3. 例) 本来ならocculusionが生じるはずの座標にあるscannerに統合するのに、便宜的にその座標のスキャンデータに統合してるだけなので、occulusionが生じなかった場合のデータが取得できる。(人によってはこれは嬉しいかもしれない)

また、multi_mergerではlaunch fileでpublishするセンサデータの座標系を

 <param name="destination_frame" value="desired_frame"/>

で指定している。
統合先・元のtopic名は

<param name="scan_destination_topic" value="/scan"/>
<param name="laserscan_topics" value ="/scan1 /scan2 /scan3" /> <!-- 何こでも可 -->

で指定できる。

###LiDARセンサデータのpub/sub
センサデータはパブリッシャである
"LaserscanMerger::laser_scan_publisher_"
によって、
sensor_msgs::LaserScanPtr型の変数"output"
がpublishされる。

laser_scan_multi_merger.cpp(l.209)

laser_scan_publisher_.publish(output);

###余談1　urgの使い方
https://blog.browniealice.net/post/how_to_use_urg/
https://at-wat.github.io/ROS-quick-start-up/ros-urg.html

###余談2　sensor_msgs/LaserScanとは
公式ドキュメントを見ると
float32[] ranges, float32[] intensitiesでそれぞれ距離、反射強度を配列形式で保持していることがわかる。それぞれのデータに方向は紐付けられていないみたいなので、
float32 angle_min, angle_max, angle_incrementから自分で計算する必要がありそう。

##マスク処理

1. 今回は、multi_mergerノードがscan dataをpubするトピック名を/raw_scanとする。

  <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="log">
    <!--省略-->
    <param name="scan_destination_topic" value="/raw_scan"/> 
    <!--省略-->
  </node>

1. scan_filterノードで/raw_scanトピックをsubすると同時に、マスク処理したscanデータをpubする。
   具体的には、pub/subを同時にできるクラスを作成する。
   下のページを参考にした。
   Publishing to a topic via subscriber callback function

class ScanFilter
{
  public:
  ScanFilter(ros::NodeHandle n_)
  {
    scan_sub_ = n_.subscribe<sensor_msgs::LaserScan> ("unmasked_scan", 100, &ScanFilter::scan_callback, this);
    masked_scan_publisher = n_.advertise<sensor_msgs::LaserScan> ("scan", 1);
  }

  void scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
  {
    sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
    output->header = msg->header;
    output->header.frame_id = msg->header.frame_id;
    output->header.stamp = msg->header.stamp; 
    output->angle_min = msg->angle_min;
    output->angle_max = msg->angle_max;
    output->angle_increment = msg->angle_increment;
    output->time_increment = msg->time_increment;
    output->scan_time = msg->scan_time;
    output->range_min = msg->range_min;
    output->range_max = msg->range_max;
    output->ranges = msg->ranges;

    footprint_filter(output);

    masked_scan_publisher.publish(output);
  }

  private:
  ros::Subscriber scan_sub_;
  ros::Publisher masked_scan_publisher;
};

1. マスクしたいデータを下のように正の無限の値を代入することで、何も検知していないことにする。

void footprint_filter(sensor_msgs::LaserScanPtr& output){
  size_t size = output->ranges.size();
  for(int i = 0; i < size; i++)
  {
    float angle = output->angle_min + output->angle_increment * i;
    float x = output->ranges[i] * cos(angle);
    float y = output->ranges[i] * sin(angle);

    //this if statement assumes footprint being square.
    //modify the condition for other footprint shapes
    if(min_x <= x && x <= max_x && min_y <= y && y <= max_y){
      output->ranges[i] = std::numeric_limits<float>::infinity();
    }
  }
}

min_x, min_yなどは以下のように定義することで、cost_param.yamlで定義したロボットのfootprint内にあるセンサデータを排除できる。
footprintのload, その他処理などはcost2dmap_ros.cppやfootprint.cppファイルを参考にすると良い。

for(int i=0; i < current_footprint.size(); i++){
    min_x = std::min(min_x, (float)current_footprint[i].x);
    min_y = std::min(min_y, (float)current_footprint[i].y);
    max_x = std::max(max_x, (float)current_footprint[i].x);
    max_y = std::max(max_y, (float)current_footprint[i].y);
  }

###footprint周り

static std::string actuator_state;
static std::vector<geometry_msgs::Point> extended_footprint;
static std::vector<geometry_msgs::Point> original_footprint;
static std::vector<geometry_msgs::Point> current_footprint;

//current footprint params
static float max_x;
static float min_x=std::numeric_limits<float>::infinity();
static float max_y;
static float min_y=std::numeric_limits<float>::infinity();

//  中略

void load_footprints(ros::NodeHandle& nh, std::vector<geometry_msgs::Point>& extended_footprint, std::vector<geometry_msgs::Point>& original_footprint)
{
  XmlRpc::XmlRpcValue footprint_xmlrpc1, footprint_xmlrpc2;
  nh.getParam("extended_footprint", footprint_xmlrpc1);
  nh.getParam("footprint", footprint_xmlrpc2);

  extended_footprint = return_footprint(footprint_xmlrpc1); 
  original_footprint = return_footprint(footprint_xmlrpc2);
}

std::vector<geometry_msgs::Point> return_footprint(XmlRpc::XmlRpcValue footprint_xmlrpc)
{
  std::string full_param_name;
  std::vector<geometry_msgs::Point> points;

  if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
      footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
  {
    if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
    {
      return points;
    }
  }
  else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
  {
    points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
    return points;
  }
  else
  {
    return points;
  }

}

###launchファイル

  <node name="scan_filter" pkg="scan_filter" type="scan_filter" output="log" >
    <rosparam file="$(find path_to_launch)/config/costmap_common_params.yaml" command="load" />
  </node>

##その他
ROSのsensor_msgs:PointCloud2データの構造と扱い方
この記事を見る限り、sensor_msgs::PointCloud2型のデータは１データごとの(x,y,z)座標(uint8[] data)とその他メタデータ(Pointfield[] fieldsなど)を有していると考えられる。

講師区ドキュメントのbig_endianとはなんそやというのは
Big Endian and Little Endianをみると、
big endian order: 小さいアドレスに一番の大きい値、大きいアドレスに一番小さい値が入るような順番
little endian order: 逆