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;
}
Esempio n. 2
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;
}