最近在用px4官方的avoidance代码跑硬件避障,官方介绍了只要生成符合sensor_msgs::PointCloud2点云信息就能使用,因此为了应用长基线双目,没有使用realsense的相机,但在配置过程中一定要注意有几个地方别搞错了,浪费了很多时间:
- 点云订阅名称不要搞错,官方代码中话题名称在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要与之对应,否则会出现以下错误,容易误导人:
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)