Skip to content
This repository has been archived by the owner on Mar 10, 2023. It is now read-only.

fix no output of ndt_mapping transformation matrix #59

Closed
wants to merge 1 commit into from
Closed

fix no output of ndt_mapping transformation matrix #59

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
18 changes: 14 additions & 4 deletions lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1014,11 +1014,10 @@ int main(int argc, char** argv)
ROS_WARN("Query base_link to primary lidar frame through tf_baselink2primarylidar param failed");
}
}

double tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw;
// 3. Try getting base_link -> lidar TF from tf_* params
if (!received_tf)
{
float tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw;
if (nh.getParam("tf_x", tf_x) &&
nh.getParam("tf_y", tf_y) &&
nh.getParam("tf_z", tf_z) &&
Expand All @@ -1031,18 +1030,29 @@ int main(int argc, char** argv)
quat.setRPY(tf_roll, tf_pitch, tf_yaw);
tf_baselink2primarylidar.setOrigin(trans);
tf_baselink2primarylidar.setRotation(quat);

received_tf = true;
}
else
{
ROS_WARN("Query base_link to primary lidar frame through tf_* params failed");
}
}

if (received_tf)
{
ROS_INFO("base_link to primary lidar transform queried successfully");

tf_baselink2primarylidar.getOrigin();
tf_x = tf_baselink2primarylidar.getOrigin().getX();
tf_y = tf_baselink2primarylidar.getOrigin().getY();
tf_z = tf_baselink2primarylidar.getOrigin().getZ();
Eigen::Translation3f tl_btol(tf_x, tf_y, tf_z);// tl: translation

tf::Matrix3x3(tf_baselink2primarylidar.getRotation()).getRPY( tf_roll, tf_pitch,tf_yaw);
Eigen::AngleAxisf rot_x_btol(tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
Eigen::AngleAxisf rot_y_btol(tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(tf_yaw, Eigen::Vector3f::UnitZ());
tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
tf_ltob = tf_btol.inverse();
}
else
{
Expand Down