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; }