int main(int argc, char **argv)
{
ros::init(argc, argv, "ndt_matching");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
// setting parameters
private_nh.getParam("use_gnss", _use_gnss);
private_nh.getParam("queue_size", _queue_size);
if(nh.getParam("localizer", _localizer) == false){
std::cout << "localizer is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_x", _tf_x) == false){
std::cout << "tf_x is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_y", _tf_y) == false){
std::cout << "tf_y is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_z", _tf_z) == false){
std::cout << "tf_z is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_roll", _tf_roll) == false){
std::cout << "tf_roll is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_pitch", _tf_pitch) == false){
std::cout << "tf_pitch is not set." << std::endl;
return 1;
}
if(nh.getParam("tf_yaw", _tf_yaw) == false){
std::cout << "tf_yaw is not set." << std::endl;
return 1;
}
std::cout << "_localizer: " << _localizer << std::endl;
std::cout << "_tf_x: " << _tf_x << std::endl;
std::cout << "_tf_y: " << _tf_y << std::endl;
std::cout << "_tf_z: " << _tf_z << std::endl;
std::cout << "_tf_roll: " << _tf_roll << std::endl;
std::cout << "_tf_pitch: " << _tf_pitch << std::endl;
std::cout << "_tf_yaw: " << _tf_yaw << std::endl;
Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation
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();
Eigen::Translation3f tl_ltob((-1.0)*_tf_x, (-1.0)*_tf_y, (-1.0)*_tf_z); // tl: translation
Eigen::AngleAxisf rot_x_ltob((-1.0)*_tf_roll, Eigen::Vector3f::UnitX()); //rot: rotation
Eigen::AngleAxisf rot_y_ltob((-1.0)*_tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_ltob((-1.0)*_tf_yaw, Eigen::Vector3f::UnitZ());
tf_ltob = (tl_ltob * rot_z_ltob * rot_y_ltob * rot_x_ltob).matrix();
// Updated in initialpose_callback or gnss_callback
initial_pose.x = 0.0;
initial_pose.y = 0.0;
initial_pose.z = 0.0;
initial_pose.roll = 0.0;
initial_pose.pitch = 0.0;
initial_pose.yaw = 0.0;
// Publishers
predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 1000);
ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 1000);
//current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
control_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/control_pose", 1000);
localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 1000);
estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 1000);
estimated_vel_mps_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_mps", 1000);
estimated_vel_kmph_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_kmph", 1000);
estimated_vel_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/estimated_vel", 1000);
time_ndt_matching_pub = nh.advertise<std_msgs::Float32>("/time_ndt_matching", 1000);
ndt_stat_pub = nh.advertise<ndt_localizer::ndt_stat>("/ndt_stat", 1000);
ndt_reliability_pub = nh.advertise<std_msgs::Float32>("/ndt_reliability", 1000);
// Subscribers
ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback);
ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);
ros::Subscriber map_sub = nh.subscribe("points_map", 10, map_callback);
ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 1000, initialpose_callback);
ros::Subscriber scan_sub = nh.subscribe("points_raw", _queue_size, scan_callback);
ros::spin();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "icp_matching");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
// Set log file name.
char buffer[80];
std::time_t now = std::time(NULL);
std::tm *pnow = std::localtime(&now);
std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
filename = "icp_matching_" + std::string(buffer) + ".csv";
ofs.open(filename.c_str(), std::ios::app);
// setting parameters
private_nh.getParam("use_gnss", _use_gnss);
private_nh.getParam("queue_size", _queue_size);
private_nh.getParam("offset", _offset);
if (nh.getParam("localizer", _localizer) == false)
{
std::cout << "localizer is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_x", _tf_x) == false)
{
std::cout << "tf_x is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_y", _tf_y) == false)
{
std::cout << "tf_y is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_z", _tf_z) == false)
{
std::cout << "tf_z is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_roll", _tf_roll) == false)
{
std::cout << "tf_roll is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_pitch", _tf_pitch) == false)
{
std::cout << "tf_pitch is not set." << std::endl;
return 1;
}
if (nh.getParam("tf_yaw", _tf_yaw) == false)
{
std::cout << "tf_yaw is not set." << std::endl;
return 1;
}
std::cout << "-----------------------------------------------------------------" << std::endl;
std::cout << "Log file: " << filename << std::endl;
std::cout << "use_gnss: " << _use_gnss << std::endl;
std::cout << "queue_size: " << _queue_size << std::endl;
std::cout << "offset: " << _offset << std::endl;
std::cout << "localizer: " << _localizer << std::endl;
std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", "
<< _tf_roll << ", " << _tf_pitch << ", " << _tf_yaw << ")" << std::endl;
std::cout << "-----------------------------------------------------------------" << std::endl;
Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation
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();
Eigen::Translation3f tl_ltob((-1.0) * _tf_x, (-1.0) * _tf_y, (-1.0) * _tf_z); // tl: translation
Eigen::AngleAxisf rot_x_ltob((-1.0) * _tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
Eigen::AngleAxisf rot_y_ltob((-1.0) * _tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_ltob((-1.0) * _tf_yaw, Eigen::Vector3f::UnitZ());
tf_ltob = (tl_ltob * rot_z_ltob * rot_y_ltob * rot_x_ltob).matrix();
// Updated in initialpose_callback or gnss_callback
initial_pose.x = 0.0;
initial_pose.y = 0.0;
initial_pose.z = 0.0;
initial_pose.roll = 0.0;
initial_pose.pitch = 0.0;
initial_pose.yaw = 0.0;
// Publishers
predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 1000);
icp_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/icp_pose", 1000);
// current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 1000);
estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 1000);
estimated_vel_mps_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_mps", 1000);
estimated_vel_kmph_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_kmph", 1000);
estimated_vel_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/estimated_vel", 1000);
time_icp_matching_pub = nh.advertise<std_msgs::Float32>("/time_icp_matching", 1000);
icp_stat_pub = nh.advertise<icp_localizer::icp_stat>("/icp_stat", 1000);
// ndt_reliability_pub = nh.advertise<std_msgs::Float32>("/ndt_reliability", 1000);
// Subscribers
ros::Subscriber param_sub = nh.subscribe("config/icp", 10, param_callback);
ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);
ros::Subscriber map_sub = nh.subscribe("points_map", 10, map_callback);
ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 1000, initialpose_callback);
ros::Subscriber points_sub = nh.subscribe("filtered_points", _queue_size, points_callback);
ros::spin();
return 0;
}