px4 avoidance笔记

最近在用px4官方的avoidance代码跑硬件避障，官方介绍了只要生成符合sensor_msgs::PointCloud2点云信息就能使用，因此为了应用长基线双目，没有使用realsense的相机，但在配置过程中一定要注意有几个地方别搞错了，浪费了很多时间：

1. 点云订阅名称不要搞错，官方代码中话题名称在avoidance.launch中，如下：

<env name="ROSCONSOLE_CONFIG_FILE" value="$(find local_planner)/resource/custom_rosconsole.conf"/>
<arg name="pointcloud_topics" default="[/camera/depth/color/points]"/>

<!-- Launch local planner -->
<node name="local_planner_node" pkg="local_planner" type="local_planner_node" output="screen" required="true" >
  <param name="goal_x_param" value="0" />
  <param name="goal_y_param" value="0"/>
  <param name="goal_z_param" value="4" />
  <rosparam param="pointcloud_topics" subst_value="True">$(arg pointcloud_topics)</rosparam>
</node>

<!-- set or toggle rqt parameters -->
<node name="rqt_param_toggle" pkg="local_planner" type="rqt_param_toggle.sh" />

所以发布 话题要与之对应/camera/depth/color/points。 2. 发布点云信息中，帧id也不要搞错，在local_planner_nodelet.cpp代码最后的 LocalPlannerNodelet::pointCloudTransformThread(int index) 函数中，如下： void LocalPlannerNodelet::pointCloudTransformThread(int index) { while (!should_exit_) { bool waiting_on_transform = false; bool waiting_on_cloud = false; { std::lock_guard camera_lock(*(cameras_[index].camera_mutex_));

  if (cameras_[index].received_) {
    tf::StampedTransform cloud_transform;
    tf::StampedTransform fcu_transform;

    if (tf_buffer_.getTransform(cameras_[index].untransformed_cloud_.header.frame_id, "local_origin",
                                pcl_conversions::fromPCL(cameras_[index].untransformed_cloud_.header.stamp),
                                cloud_transform) &&
        tf_buffer_.getTransform(cameras_[index].untransformed_cloud_.header.frame_id, "fcu",
                                pcl_conversions::fromPCL(cameras_[index].untransformed_cloud_.header.stamp),
                                fcu_transform)) {
      // remove nan padding and compute fov
      pcl::PointCloud<pcl::PointXYZ> maxima = removeNaNAndGetMaxima(cameras_[index].untransformed_cloud_);

      // update point cloud FOV
      pcl_ros::transformPointCloud(maxima, maxima, fcu_transform);
      updateFOVFromMaxima(cameras_[index].fov_fcu_frame_, maxima);

      // transform cloud to local_origin frame
      pcl_ros::transformPointCloud(cameras_[index].untransformed_cloud_, cameras_[index].transformed_cloud_,
                                   cloud_transform);
      cameras_[index].transformed_cloud_.header.frame_id = "local_origin";
      cameras_[index].transformed_cloud_.header.stamp = cameras_[index].untransformed_cloud_.header.stamp;

      cameras_[index].transformed_ = true;
      cameras_[index].received_ = false;
      waiting_on_cloud = true;
      std::lock_guard<std::mutex> lock(transformed_cloud_mutex_);
      transformed_cloud_cv_.notify_all();
    } else {
      waiting_on_transform = true;
    }
  } else {
    waiting_on_cloud = true;
  }
}

if (should_exit_) {
  break;
}

if (waiting_on_transform) {
  std::unique_lock<std::mutex> lck(buffered_transforms_mutex_);
  tf_buffer_cv_.wait_for(lck, std::chrono::milliseconds(5000));
} else if (waiting_on_cloud) {
  std::unique_lock<std::mutex> lck(*(cameras_[index].camera_mutex_));
  cameras_[index].camera_cv_->wait_for(lck, std::chrono::milliseconds(5000));
}

}
}
}
帧id为local_origin要与之对应，否则会出现以下错误，容易误导人：