我创建了一个使用
sensor_msgs IMUerror: ‘using element_type = const struct sensor_msgs::msg::Imu_<std::allocator<void> >’ {aka ‘const struct sensor_msgs::msg::Imu_<std::allocator<void> >’} has no member named ‘imu’
199 | imu_q.w() = imu_msg->imu.orientation.w;
ROS节点为：
#include <backhoe_perception/plane_segmentation.hpp>
#include <limits>
#include <vector>
#include <memory>
#include <string>
namespace backhoe_perception {
PlaneSegmentation::PlaneSegmentation(
const rclcpp::NodeOptions &options,
const std::string node_name):
XNode(node_name, options),
buffer_(5) {
/* message filter buffer size */
this->buffer_ = this->getParameterValue(
"buffer", this->buffer_);
// === parameters related to pub/sub ==
// whether the input pcd is a pcd with normals
this->is_pcd_with_normal_ = this->getParameterValue(
"is_pcd_with_normal", false);
// for speed, publish only ground plane coeffs
this->publish_only_gd_plane_coeffs_ = this->getParameterValue(
"publish_only_coeffs", true);
// === parameters related to iterative RANSAC ==
// minimum number of inliers for a planar segment
// to be considered a ground plane candidate
this->min_inliers_ = this->getParameterValue(
"min_inliers", 300);
// min number of iterative RANSAC trials
this->min_iterative_ransac_trials_ = this->getParameterValue(
"min_iterative_ransac_trials", 2);
// max number of iterative RANSAC trials
this->max_iterative_ransac_trials_ = this->getParameterValue(
"max_iterative_ransac_trials", 5);
// min number of points to perform RANSAC on
this->min_points_iterative_ransac_ = this->getParameterValue(
"min_points_iterative_ransac", 1000);
// min ratio of inliers:input pcd to perform early stopping
// on iterative RANSAC
this->iterative_ransac_early_stopping_inlier_ratio_ =
this->getParameterValue(
"iterative_ransac_early_stopping_inlier_ratio", 0.5f);
// == RANSAC settings (per-iteration) ==
this->use_normal_with_ransac_ = this->getParameterValue(
"use_normal_with_ransac", true);
this->ransac_num_threads_ = this->getParameterValue(
"ransac_num_threads", 1);
this->ransac_max_iterations_ = this->getParameterValue(
"ransac_max_iterations", 50);
this->ransac_outlier_threshold_ = this->getParameterValue(
"ransac_outlier_threshold", 0.3);
this->ransac_normal_distance_weight_ = this->getParameterValue(
"ransac_normal_distance_weight", 0.3);
this->tree_ = pcl::search::KdTree<PointC>::Ptr(
new pcl::search::KdTree<PointC>);
this->kdtree_ = pcl::KdTreeFLANN<PointC>::Ptr(
new pcl::KdTreeFLANN<PointC>);
this->ne_ = pcl::NormalEstimationOMP<PointC, PointN>::Ptr(
new pcl::NormalEstimationOMP<PointC, PointN>);
this->poke();
}
void PlaneSegmentation::onInit() {
rclcpp::QoS qos(rclcpp::KeepLast(3));
this->pub_plane_coeffs_ = this->create_publisher<PlaneCoefficients>(
"output_plane_coefficients", qos);
if (!this->publish_only_gd_plane_coeffs_) {
this->pub_gd = this->create_publisher<PointCloud2>(
"output_gd_points", qos);
this->pub_non_gd = this->create_publisher<PointCloud2>(
"output_non_gd_points", qos);
this->pub_indices_ = this->create_publisher<ClusterPointIndices>(
"output_indices", qos);
}
}
void PlaneSegmentation::subscribe() {
// if our pointcloud comes with normals
if (this->is_pcd_with_normal_) {
this->sub_cloud2_ = std::make_shared<
message_filters::Subscriber<PointCloud2>>(
this, "input_points");
this->sub_imu2_ = std::make_shared<
message_filters::Subscriber<Imu>>(this, "input_imu");
this->sync_joint_ = std::make_unique<
message_filters::Synchronizer<SyncPolicyJoint>>(this->buffer_);
this->sync_joint_->connectInput(*sub_cloud2_, *sub_imu2_);
this->sync_joint_->registerCallback(
std::bind(&PlaneSegmentation::jointPcdCallback,
this, ph::_1, ph::_2));
} else { // if we get the pcd and normals separately
this->sub_cloud_ = std::make_shared<
message_filters::Subscriber<PointCloud2>>(this,
"input_points");
this->sub_normal_ = std::make_shared<
message_filters::Subscriber<PointCloud2>>(this,
"input_normals");
this->sub_imu_ = std::make_shared<message_filters::Subscriber<
Imu>>(this, "input_imu");
this->sync_separate_ = std::make_unique<
message_filters::Synchronizer<SyncPolicySeparate>>(this->buffer_);
this->sync_separate_->connectInput(*sub_cloud_, *sub_normal_,
*sub_imu_);
this->sync_separate_->registerCallback(
std::bind(&PlaneSegmentation::separatePcdCallback,
this, ph::_1, ph::_2, ph::_3));
}
}
/**
* Joint callback for the plane segmentation node.
*
* If the input pcd already contains normals, this callback will be used
* to split the pcd into separate point+normal pcds for processing.
*
* @param cloud_msg PointCloud2 msg with normals.
* @param imu_msg sensor_msgs/Imu msg containing excavator imu.
*/
void PlaneSegmentation::jointPcdCallback(
const PointCloud2::ConstSharedPtr &cloud_msg,
const Imu::ConstSharedPtr &imu_msg) {
std::lock_guard<std::mutex> lock(this->mutex_);
PointCloudCN::Ptr point_normals(new PointCloudCN);
pcl::fromROSMsg(*cloud_msg, *point_normals);
// split PointCloudCN pcd into separate pts pcd & normals pcd
PointCloudC::Ptr cloud(new PointCloudC);
PointCloudN::Ptr normals(new PointCloudN);
backhoe::pcl_utils::splitPointsAndNormals(cloud, normals, point_normals);
this->extractGroundPlane(cloud, normals, imu_msg, cloud_msg->header);
}
/**
* Separate callback for the plane segmentation node.
*
* If the pcd and pcd normals input to the node come as separate
* messages, this callback will be used.
*
* @param cloud_msg PointCloud2 msg containing pcd points.
* @param normal_msg PointCloud2 msg containing pcd normals.
* @param imu_msg sensor_msgs/Imu msg containing excavator imu.
*/
void PlaneSegmentation::separatePcdCallback(
const PointCloud2::ConstSharedPtr &cloud_msg,
const PointCloud2::ConstSharedPtr &normal_msg,
const Imu::ConstSharedPtr &imu_msg) {
std::lock_guard<std::mutex> lock(this->mutex_);
// convert cloud and normals from ROS msgs
PointCloudC::Ptr cloud(new PointCloudC);
pcl::fromROSMsg(*cloud_msg, *cloud);
PointCloudN::Ptr normals(new PointCloudN);
pcl::fromROSMsg(*normal_msg, *normals);
this->extractGroundPlane(cloud, normals, imu_msg, cloud_msg->header);
}
/**
* Extract ground plane from input pcd+normals, based on the input imu.
*
* This is the primary routine for calculating and publishing the
* ground plane in the input pcd+normals. The ground plane candidates
* are compared with the input imu, and the plane with normal
* most similar to the imu is chosen as the ground plane.
* Planar segmentation is performed using iterative RANSAC.
*
* @param cloud input pcd
* @param normals normals of the input pcd
* @param imu_msg sensor_msgs/Imu msg containing excavator imu.
* @param header header to use for publishing output topics
*/
void PlaneSegmentation::extractGroundPlane(
const PointCloudC::Ptr cloud,
const PointCloudN::Ptr normals,
const Imu::ConstSharedPtr &imu_msg,
const Header header) {
RCLCPP_INFO(this->get_logger(), "Starting ground plane extraction");
// convert imu message to eigen quaternion
Eigen::Quaternionf imu_q;
Imu imu;
imu_q.w() = imu_msg->imu.orientation.w;
imu_q.x() = imu_msg->imu.orientation.x;
imu_q.y() = imu_msg->imu.orientation.y;
imu_q.z() = imu_msg->imu.orientation.z;
....
我还在头文件中包含了
#include <sensor_msgs/msg/imu.hpp>报错说IMU报文不包含
imu相反，
sensor_msgs::msg::Imuorientationstatic void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
RCLCPP_INFO(
rclcpp::get_logger("rclcpp"), "Got IMU msg with WXYZ: [%f, %f, %f, %f]",
msg->orientation.w, msg->orientation.x, msg->orientation.y,
msg->orientation.z);
}