tf2 transform and pose publish with ROS2 and humble
Hi
I like to publish a pose using IMU and Monocular Image from ORB_SLAM3 in ROS2 and humble. I made a function. But some how not familiar with tf2 in ROS2 . So I got some errors. I include the tf2 header and also declared in CMakeList.txt dependencies. But still not working
Here the code
void ImageGrabber::SyncWithImu()
{
//while(1)
while (rclcpp::ok())
{
cv::Mat im;
double tIm = 0;
if (!img0Buf.empty()&&!mpImuGb->imuBuf.empty())
{
tIm = img0Buf.front()->header.stamp.sec + img0Buf.front()->header.stamp.nanosec * 1e-9;
if (tIm > mpImuGb->imuBuf.back()->header.stamp.sec + mpImuGb->imuBuf.back()->header.stamp.nanosec * 1e-9)
continue;
{
this->mBufMutex.lock();
im = GetImage(img0Buf.front());
img0Buf.pop();
this->mBufMutex.unlock();
}
vector<ORB_SLAM3::IMU::Point> vImuMeas;
mpImuGb->mBufMutex.lock();
if(!mpImuGb->imuBuf.empty())
{
// Load imu measurements from buffer
vImuMeas.clear();
while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.sec +
mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9 <= tIm)
{
double t = mpImuGb->imuBuf.front()->header.stamp.sec + mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9;
cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
mpImuGb->imuBuf.pop();
}
}
mpImuGb->mBufMutex.unlock();
if(mbClahe)
mClahe->apply(im,im);
cv::Mat T_, R_, t_ ;
//stores return variable of TrackMonocular
mpSLAM->TrackMonocular(im, tIm, vImuMeas);
if (pub_tf || pub_pose)
{
if (!(T_.empty())) {
cv::Size s = T_.size();
if ((s.height >= 3) && (s.width >= 3)) {
R_ = T_.rowRange(0,3).colRange(0,3).t();
t_ = -R_*T_.rowRange(0,3).col(3);
std::vector<float> q = ORB_SLAM3::Converter::toQuaternion(R_);
float scale_factor=1.0;
//tf2::Transform transform;
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.header.stamp = rclcpp::Time(tIm);
tf_msg.header.frame_id = "world";
tf_msg.child_frame_id = "ORB_SLAM3_MONO_INERTIAL";
tf2::Transform tf_msg;
tf_msg.transform.translation.x = t_.at<float>(0, 0) * scale_factor;
tf_msg.transform.translation.y = t_.at<float>(0, 1) * scale_factor;
tf_msg.transform.translation.z = t_.at<float>(0, 2) * scale_factor;
tf_msg.transform.rotation.x = q[0];
tf_msg.transform.rotation.y = q[1];
tf_msg.transform.rotation.z = q[2];
tf_msg.transform.rotation.w = q[3];
//transform = tf2::Transform(tf_msg.transform);
//tf_broadcaster_->sendTransform(tf_msg.transform);
/*
if (pub_tf)
{
static tf::TransformBroadcaster br_;
br_.sendTransform(tf::StampedTransform(transform, ros::Time(tIm), "world", "ORB_SLAM3_MONO_INERTIAL"));
}
*/
if (pub_pose)
{
geometry_msgs::msg::PoseStamped pose;
//pose.header.stamp = img0Buf.front()->header.stamp;
pose.header.frame_id ="ORB_SLAM3_MONO_INERTIAL";
tf_msg::poseTFToMsg(transform, pose.pose);
orb_pub->publish(pose);
}
}
}
}
}
std::chrono::milliseconds tSleep(1);
std::this_thread::sleep_for(tSleep);
}
}
void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::SharedPtr imu_msg)
{
mBufMutex.lock();
imuBuf.push(imu_msg);
mBufMutex.unlock();
return;
}
So i got errors in
tf2::Transform tf_msg;
and
tf_msg::poseTFToMsg(transform, pose.pose);
error: ‘tf2’ has not been declared
245 | tf2::Transform tf_msg;error: ‘tfmsg’ is not a class, namespace, or enumeration 269 |
tfmsg::poseTFToMsg(transform, pose.pose);
Any help? Thanks
Asked by Astronaut on 2023-07-25 03:31:55 UTC
Comments