(6/9 時点)

流れ

• 半径 x [m] 以上の点群をフィルタリングする

→現時点では足以外の物体が検出されると追跡できないため検出範囲を限定する

• 取得した点群データの平均の座標を求める
• 算出した座標を線で繋いでいく

主な処理

半径 x [m] 以上の点群をフィルタリング

for (size_t i = 0; i < msg->ranges.size(); ++i)
        {
            if (msg->ranges[i] >= x) // 半径x[m]以上の点をフィルタリング
            {
                filtered_msg.ranges[i] = std::numeric_limits<float>::infinity();
            }
            else
            {
                float angle = msg->angle_min + i * msg->angle_increment;
                float x = msg->ranges[i] * cos(angle);
                float y = msg->ranges[i] * sin(angle);
                sum_x += x;
                sum_y += y;
                count++;
            }

点群の平均座標を計算

float avg_x = sum_x / count;
float avg_y = sum_y / count;

RvizのMarkerを用いて、算出した平均座標を線で繋いで可視化する

if (has_last_point_ && start_drawing_)
            {
                visualization_msgs::msg::Marker line;
                line.header.frame_id = "laser";
                line.header.stamp = this->get_clock()->now();
                line.ns = "trajectory";
                line.id = marker_id_++;
                line.type = visualization_msgs::msg::Marker::LINE_STRIP;
                line.action = visualization_msgs::msg::Marker::ADD;
                line.points.push_back(createPoint(last_avg_x_, last_avg_y_));
                line.points.push_back(createPoint(avg_x, avg_y));
                line.scale.x = 0.01; //線の太さ
                line.color.a = 1.0; //線の色
                line.color.r = 0.0;
                line.color.g = 1.0;
                line.color.b = 0.0;
                line.lifetime = rclcpp::Duration::from_seconds(n); //引いた線をn秒後に消す
                marker_publisher_->publish(line);
            }

結果

Rvizで足の追従を可視化した様子（写真）を以下に示す。

赤が点群・青が座標を繋いだ線であり、足の重心あたり(点群の平均座標)を追従し軌跡を描くことができた。