本文主要是介绍px4 avoidance笔记,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
最近在用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 fovpcl::PointCloud<pcl::PointXYZ> maxima = removeNaNAndGetMaxima(cameras_[index].untransformed_cloud_);// update point cloud FOVpcl_ros::transformPointCloud(maxima, maxima, fcu_transform);updateFOVFromMaxima(cameras_[index].fov_fcu_frame_, maxima);// transform cloud to local_origin framepcl_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要与之对应,否则会出现以下错误,容易误导人:
这篇关于px4 avoidance笔记的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!