Junaid22's profile - activity

I am using a ray_ground_filter node from Autoware.ai. I am get the following error when I launch the node.

Invalid argument passed to lookupTransform argument target_frame in tf2 frame_ids cannot be empty

I noticed that my tf_static is empty for some odd reason.Could anyone help me in understanding this bug?

ray_ground_filter.cpp

/*
 * Copyright 2017-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************
 *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
 */
#include <iostream>
#include <algorithm>
#include <vector>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <velodyne_pointcloud/point_types.h>
#include "autoware_config_msgs/ConfigRayGroundFilter.h"

#include <opencv2/core/version.hpp>
#if (CV_MAJOR_VERSION == 3)
#include "gencolors.cpp"
#else
#include <opencv2/contrib/contrib.hpp>
#endif

#include "points_preprocessor/ray_ground_filter/ray_ground_filter.h"

void RayGroundFilter::update_config_params(const autoware_config_msgs::ConfigRayGroundFilter::ConstPtr& param)
{
  general_max_slope_ = param->general_max_slope;
  local_max_slope_ = param->local_max_slope;
  radial_divider_angle_ = param->radial_divider_angle;
  concentric_divider_distance_ = param->concentric_divider_distance;
  min_height_threshold_ = param->min_height_threshold;
  clipping_height_ = param->clipping_height;
  min_point_distance_ = param->min_point_distance;
  reclass_distance_threshold_ = param->reclass_distance_threshold;
}

bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
                                          const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
                                          const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr)
{
  if (in_target_frame == in_cloud_ptr->header.frame_id)
  {
    *out_cloud_ptr = *in_cloud_ptr;
    return true;
  }

  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
                                                   in_cloud_ptr->header.stamp, ros::Duration(1.0));
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return false;
  }
  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
  out_cloud_ptr->header.frame_id = in_target_frame;
  return true;
}

void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher,
                                    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
                                    const std_msgs::Header& in_header)
{
  sensor_msgs::PointCloud2::Ptr cloud_msg_ptr(new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr trans_cloud_msg_ptr(new sensor_msgs::PointCloud2);
  pcl::toROSMsg(*in_cloud_to_publish_ptr, *cloud_msg_ptr);
  cloud_msg_ptr->header.frame_id = base_frame_;
  cloud_msg_ptr->header.stamp = in_header.stamp;
  const bool succeeded = TransformPointCloud(in_header.frame_id, cloud_msg_ptr, trans_cloud_msg_ptr);
  if (!succeeded)
  {
    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << cloud_msg_ptr->header.frame_id << " to "
                                                           << in_header.frame_id);
    return;
  }
  in_publisher.publish(*trans_cloud_msg_ptr);
}

/*!
 *
 * @param[in] in_cloud Input Point Cloud to be organized in radial segments
 * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
 * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
 * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
 */
void RayGroundFilter::ConvertXYZIToRTZColor(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
    const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
    const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
    const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds)
{
  out_organized_points->resize(in_cloud->points.size());
  out_radial_divided_indices->clear();
  out_radial_divided_indices->resize(radial_dividers_num_);
  out_radial_ordered_clouds->resize(radial_dividers_num_);

  for (size_t i = 0; i < in_cloud->points.size(); i++)
  {
    PointXYZIRTColor new_point;
    auto radius = static_cast<float>(
        sqrt(in_cloud->points[i ...