int main(int argc, char* argv[]) {
gengetopt_args_info args;
cmdline_parser(argc,argv,&args);
ros::init(argc, argv, "localizer");
geometry_msgs::PoseWithCovariance msg;
Localizer localizer;
localizer.setConeRadii(args.coneRadii_arg);
float alphas = 0.01;
localizer.setAlpha(alphas);
localizer.setUpdateRate(updateFreq);
ros::NodeHandle n;
ros::Publisher posWCov = n.advertise<geometry_msgs::PoseWithCovariance>("/pos", 1);
ros::Subscriber cntrl = n.subscribe("/cmd_vel_mux/input/navi", 1000, &Localizer::cmdUpdate, &localizer); // update the command velocities
//ros::Subscriber cntrl = n.subscribe("/odom", 1000, &Localizer::cmdUpdate, &localizer);
ros::Subscriber beams = n.subscribe("/scan", 1000, &Localizer::handleScans, &localizer);
ros::Rate loop_rate(updateFreq);
while (ros::ok()) {
if (localizer.scans.size() != 0) {
localizer.findFeature();
localizer.EKF();
}
msg.pose.position.x = localizer.getx();
msg.pose.position.y = localizer.gety();
msg.pose.orientation.x = localizer.getQuatx();
msg.pose.orientation.y = localizer.getQuaty();
msg.pose.orientation.z = localizer.getQuatz();
msg.pose.orientation.w = localizer.getQuatw();
Eigen::Matrix3f Covariance = localizer.getSigma();
int index = 0;
for (int i = 0; i < Covariance.rows(); i++) {
for (int k = 0; k < Covariance.cols(); k++) {
msg.covariance[index] = Covariance(i,k);
index++;
}
}
ros::spinOnce();
posWCov.publish(msg);
loop_rate.sleep();
}
return 0;
}
