I created a C++ ROS2 node that uses sensor_msgs IMU
. Included the sensor_msg in the header file, also declared the IMU sensor message. Still nor really clear the error that comes. When build it I got error:
error: ‘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;
The ROS node is:
.....
namespace robot_perception {
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));
}
}
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);
}
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.
*/
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;
....
I also included #include <sensor_msgs/msg/imu.hpp>
in the header file. Still dont understand where the error is from. Any help?