Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/init on tf #40

Draft
wants to merge 3 commits into
base: master
Choose a base branch
from
Draft

Feature/init on tf #40

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
11c5d7b
Init pose on tf
nyxrobotics Jan 17, 2024
1bf4bfa
Refactoring
nyxrobotics Feb 7, 2024
0b1be75
Remove unused headers
nyxrobotics Feb 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions include/nodelet/hdl_localization_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <mutex>
#include <memory>
#include <iostream>
#include <limits>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
Expand All @@ -21,6 +22,7 @@
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <pcl/filters/voxel_grid.h>
Expand Down Expand Up @@ -58,6 +60,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet
pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const;
void publishOdometry(const ros::Time& stamp, const Eigen::Matrix4f& pose, const double fitness_score);
void publishScanMatchingStatus(const std_msgs::Header& header, pcl::PointCloud<pcl::PointXYZI>::ConstPtr aligned);
bool getOdomFromTf(nav_msgs::Odometry& odom_out);
void resetTfBuffer();

private:
// ROS
Expand Down
152 changes: 96 additions & 56 deletions src/nodelet/hdl_localization_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,11 +267,13 @@ void HdlLocalizationNodelet::pointsCallback(const sensor_msgs::PointCloud2ConstP
// transform pointcloud into base_frame_id
std::string tf_error;
pcl::PointCloud<HdlLocalizationNodelet::PointT>::Ptr cloud(new pcl::PointCloud<HdlLocalizationNodelet::PointT>());
if (this->tf_buffer_.canTransform(base_frame_id_, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tf_error))
if (this->tf_buffer_.canTransform(base_frame_id_, pcl_cloud->header.frame_id, ros::Time(0), ros::Duration(0.1),
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

stampを使わない理由は何ですか?

&tf_error))
{
if (!pcl_ros::transformPointCloud(base_frame_id_, *pcl_cloud, *cloud, this->tf_buffer_))
{
NODELET_ERROR("point cloud cannot be transformed into target frame!!");
NODELET_ERROR("point cloud cannot be transformed into target frame (%s -> %s)",
pcl_cloud->header.frame_id.c_str(), base_frame_id_.c_str());
return;
}
}
Expand Down Expand Up @@ -324,61 +326,17 @@ void HdlLocalizationNodelet::pointsCallback(const sensor_msgs::PointCloud2ConstP
}
else if (use_odom_)
{
// PointClouds + Oodometry prediction
if (odom_frame_id_.empty() || base_frame_id_.empty())
// PointClouds + Odometry prediction
nav_msgs::Odometry odom;
if (!getOdomFromTf(odom))
{
NODELET_WARN_THROTTLE(10.0, "odom_frame_id_ or base_frame_id_ is not set");
pose_estimator_->predict(stamp);
return;
}
if (tf_buffer_.canTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_,
ros::Duration(0.1)) &&
tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(0.1)))
{
// Get the amount of odometry movement since the last calculation
// Coordinate system where the front of the robot is x
geometry_msgs::TransformStamped odom_delta =
tf_buffer_.lookupTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_);
// Get the latest base_frame_ to get the time
geometry_msgs::TransformStamped odom_now =
tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
ros::Time odom_stamp = odom_now.header.stamp;
ros::Duration odom_time_diff = odom_stamp - odom_stamp_last_;
double odom_time_diff_sec = odom_time_diff.toSec();
if (odom_delta.header.stamp.isZero() || odom_time_diff_sec <= 0)
{
NODELET_WARN_THROTTLE(10.0, "Wrong timestamp detected: odom_time_diff_sec = %f", odom_time_diff_sec);
}
else
{
Eigen::Vector3f odom_travel_linear(odom_delta.transform.translation.x, odom_delta.transform.translation.y,
odom_delta.transform.translation.z);
Eigen::Vector3f odom_twist_linear = odom_travel_linear / odom_time_diff_sec;
tf::Quaternion odom_travel_angular(odom_delta.transform.rotation.x, odom_delta.transform.rotation.y,
odom_delta.transform.rotation.z, odom_delta.transform.rotation.w);
double roll, pitch, yaw;
tf::Matrix3x3(odom_travel_angular).getRPY(roll, pitch, yaw);
Eigen::Vector3f odom_twist_angular(roll / odom_time_diff_sec, pitch / odom_time_diff_sec,
yaw / odom_time_diff_sec);
pose_estimator_->predictOdom(odom_stamp, odom_twist_linear, odom_twist_angular);
}
odom_stamp_last_ = odom_stamp;
}
else
{
if (tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(0.1)))
{
NODELET_WARN_THROTTLE(10.0, "The last timestamp is wrong, skip localization");
// Get the latest base_frame_ to get the time
geometry_msgs::TransformStamped odom_now =
tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
odom_stamp_last_ = odom_now.header.stamp;
}
else
{
NODELET_WARN_STREAM("Failed to look up transform between " << cloud->header.frame_id << " and "
<< odom_frame_id_);
}
}
Eigen::Vector3f odom_twist_linear(odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z);
Eigen::Vector3f odom_twist_angular(odom.twist.twist.angular.x, odom.twist.twist.angular.y,
odom.twist.twist.angular.z);
pose_estimator_->predictOdom(odom.header.stamp, odom_twist_linear, odom_twist_angular);
}
else
{
Expand Down Expand Up @@ -529,8 +487,7 @@ void HdlLocalizationNodelet::initialposeCallback(const geometry_msgs::PoseWithCo
Eigen::Quaternionf(q_global2robot.w(), q_global2robot.x(), q_global2robot.y(), q_global2robot.z());
if (use_odom_)
{
odom_stamp_last_ = pose_msg->header.stamp;
tf_buffer_.clear();
resetTfBuffer();
}
if (use_imu_)
{
Expand Down Expand Up @@ -708,6 +665,89 @@ void HdlLocalizationNodelet::publishScanMatchingStatus(const std_msgs::Header& h
status_pub_.publish(status);
}

bool HdlLocalizationNodelet::getOdomFromTf(nav_msgs::Odometry& odom_out)
{
double initialize_timeout = 0.1;
if (odom_frame_id_.empty() || base_frame_id_.empty())
{
NODELET_WARN_THROTTLE(1.0, "odom_frame_id(%s) or base_frame_id(%s) is not set", odom_frame_id_.c_str(),
base_frame_id_.c_str());
return false;
}
if (tf_buffer_.canTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_,
ros::Duration(initialize_timeout)) &&
tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(initialize_timeout)))
{
// Get the amount of odometry movement since the last calculation
// Coordinate system where the front of the robot is x
geometry_msgs::TransformStamped odom_delta =
tf_buffer_.lookupTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_);
// Get the latest base_frame_ to get the time
geometry_msgs::TransformStamped odom_now = tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
ros::Time odom_stamp = odom_now.header.stamp;
double odom_time_diff = (odom_stamp - odom_stamp_last_).toSec();
if (odom_delta.header.stamp.isZero() || odom_time_diff < std::numeric_limits<double>::epsilon())
{
NODELET_WARN("Wrong timestamp detected: odom_time_diff = %f", odom_time_diff);
odom_stamp_last_ = odom_stamp;
return false;
}
else
{
Eigen::Vector3f odom_travel_linear(odom_delta.transform.translation.x, odom_delta.transform.translation.y,
odom_delta.transform.translation.z);
Eigen::Vector3f odom_twist_linear = odom_travel_linear / odom_time_diff;
tf::Quaternion odom_travel_angular(odom_delta.transform.rotation.x, odom_delta.transform.rotation.y,
odom_delta.transform.rotation.z, odom_delta.transform.rotation.w);
double roll, pitch, yaw;
tf::Matrix3x3(odom_travel_angular).getRPY(roll, pitch, yaw);
Eigen::Vector3f odom_twist_angular(roll / odom_time_diff, pitch / odom_time_diff, yaw / odom_time_diff);
odom_out.header.stamp = odom_stamp;
odom_out.header.frame_id = odom_frame_id_;
odom_out.child_frame_id = base_frame_id_;
odom_out.pose.pose.position.x = odom_now.transform.translation.x;
odom_out.pose.pose.position.y = odom_now.transform.translation.y;
odom_out.pose.pose.position.z = odom_now.transform.translation.z;
odom_out.pose.pose.orientation.x = odom_now.transform.rotation.x;
odom_out.pose.pose.orientation.y = odom_now.transform.rotation.y;
odom_out.pose.pose.orientation.z = odom_now.transform.rotation.z;
odom_out.pose.pose.orientation.w = odom_now.transform.rotation.w;
odom_out.twist.twist.linear.x = odom_twist_linear.x();
odom_out.twist.twist.linear.y = odom_twist_linear.y();
odom_out.twist.twist.linear.z = odom_twist_linear.z();
odom_out.twist.twist.angular.x = odom_twist_angular.x();
odom_out.twist.twist.angular.y = odom_twist_angular.y();
odom_out.twist.twist.angular.z = odom_twist_angular.z();
}
odom_stamp_last_ = odom_stamp;
return true;
}
else
{
if (tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(initialize_timeout)))
{
NODELET_INFO("Received the first odom transform");
// Get the latest base_frame_ to get the time
geometry_msgs::TransformStamped odom_now =
tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
odom_stamp_last_ = odom_now.header.stamp;
return false;
}
else
{
NODELET_WARN_STREAM("Failed to look up transform between " << odom_frame_id_ << " and " << odom_frame_id_);
return false;
}
}
return false;
}

void HdlLocalizationNodelet::resetTfBuffer()
{
tf_buffer_.clear();
odom_stamp_last_ = ros::Time(0);
}

} // namespace hdl_localization

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)