Trailer Bounding Box Remove 인수인계

ROS1 Trailer Bounding Box Remove

• /detection/shape_estimation/src/node.cpp
  • 선행 조건 node.hpp혹은 node.cpp에서 trailer_base_link_x, trailer_base_link_y, trailer_distance_threshold 변수 선언 필요.
  • 코드 설명 Bounding Box의 position.x, position.y 좌표와 trailer_base_link_x, trailer_base_link_y 좌표 거리 값이 trailer_distance_threshold 보다 작으면서 Bounding Box의 크기가 길이 4.6, 너비 1.8 이내일때 필터링
• 기존 코드

  #include "shape_estimation/node.hpp"
  #include "shape_estimation/shape_estimator.hpp"
  #include <tf2/LinearMath/Matrix3x3.h>
   
  ShapeEstimationNode::ShapeEstimationNode() : nh_(""), pnh_("~")
  {
    sub_ = nh_.subscribe("input", 1, &ShapeEstimationNode::callback, this);
    ///perception/object_recognition/detection/labeled_clusters
    
    pub_ = nh_.advertise<autoware_perception_msgs::DynamicObjectWithFeatureArray>("objects", 1, true);
    
    bool use_corrector;
    double l_shape_fitting_search_angle_range;
    bool orientation_reliable;
    
    pnh_.param<bool>("use_corrector", use_corrector, true);
    pnh_.param<double>("l_shape_fitting_search_angle_range", l_shape_fitting_search_angle_range, 3);
    pnh_.param<bool>("orientation_reliable", orientation_reliable, true);
    estimator_ = std::make_unique<ShapeEstimator>(l_shape_fitting_search_angle_range, use_corrector, orientation_reliable);
  }
   
  void ShapeEstimationNode::callback(
    const autoware_perception_msgs::DynamicObjectWithFeatureArray::ConstPtr & input_msg)
  {
    ///perception/object_recognition/detection/labeled_clusters callback
    // Guard
    if (pub_.getNumSubscribers() < 1) return;
    
    // Create output msg
    autoware_perception_msgs::DynamicObjectWithFeatureArray output_msg;
    output_msg.header = input_msg->header;
    
    // Estimate shape for each object and pack msg
    for (const auto & feature_object : input_msg->feature_objects) {
      // convert ros to pcl
      pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::fromROSMsg(feature_object.feature.cluster, *cluster);
      
      // estimate shape and pose
      autoware_perception_msgs::Shape shape;
      geometry_msgs::Pose pose;
      double roll, pitch, yaw;
      double trailer_distance;
      if (!estimator_->getShapeAndPose(
             feature_object.object.semantic.type, *cluster,
             feature_object.object.state, shape, pose))
        continue;
        
      //filter nan nan filter shape estimation msg (ljh)
      if(fabs(feature_object.object.state.pose_covariance.pose.position.x) > 100000 || fabs(feature_object.object.shape.dimensions.x) > 1000|| fabs(feature_object.object.shape.dimensions.y) > 1000|| fabs(feature_object.object.shape.dimensions.z) > 1000){
        ROS_WARN("Threshold shape estimation msg publsih");
        continue;
      }
      if(isnan(feature_object.object.state.pose_covariance.pose.position.x) != 0 || isnan(feature_object.object.state.pose_covariance.pose.position.y) != 0 || isnan(feature_object.object.state.pose_covariance.pose.position.z) != 0  || isnan(feature_object.object.state.twist_covariance.twist.linear.x) != 0 ||  isnan(feature_object.object.state.twist_covariance.twist.linear.y) != 0 || isnan(feature_object.object.state.twist_covariance.twist.linear.z) != 0 || isnan(feature_object.object.shape.dimensions.x) != 0|| isnan(feature_object.object.shape.dimensions.y) != 0|| isnan(feature_object.object.shape.dimensions.z) != 0){
        ROS_WARN("NAN shape estimation msg publsih");
        continue;
      }

• 추가 코드

    //trailer 1
    if(feature_object.object.semantic.type==1 || feature_object.object.semantic.type==2 || feature_object.object.semantic.type==3){
      //CAR=1 TRUCK=2 BUS=3 BICYCLE=4 MOTORBIKE=5 PEDESTRIAN=6
      
      //in node.hpp
      //double trailer_base_link_x: -3.06
      //double trailer_base_link_y: 0.0
      //double trailer_distance_threshold: 1.5
      
      trailer_distance = sqrt(pow(pose.position.x-trailer_base_link_x,2)+pow(pose.position.y-trailer_base_link_y,2)); //BEV
       ROS_WARN("shape [%d]: %f",feature_object.object.semantic.type,shape.dimensions.x);
      if(trailer_distance < trailer_distance_threshold){
        //ROS_WARN("----------start-----------");
        //ROS_WARN("td: %f",trailer_distance);
        //ROS_WARN("pose: %f %f %f",pose.position.x,pose.position.y,pose.position.z);
        //ROS_WARN("shape: %f %f %f",shape.dimensions.x,shape.dimensions.y,shape.dimensions.z);
        //ROS_WARN("----------end-----------");
        if(shape.dimensions.x < 4.6){
          if(shape.dimensions.y < 1.8){
            continue;
          }
        }
      }