double error (double f, double step)
{
return f - undiscretize(discretize(f, step), step);
}
int main (int argc, char **argv)
{
static const char opts[] = "dues:";
double step = 0.001;
function_selector f = invalid_fun;
int opt = getopt(argc, argv, opts);
while (opt != -1)
{
switch (opt)
{
case 'd':
f = discretize_fun;
break;
case 'u':
f = undiscretize_fun;
break;
case 'e':
f = extract_coordinates_fun;
break;
case 's':
sscanf(optarg, "%lf", &step);
break;
default:
usage();
exit(1);
break;
}
opt = getopt(argc, argv, opts);
}
FILE *in = stdin;
FILE *out = stdout;
if (optind < argc)
{
in = fopen(argv[optind], "r");
out = fopen(argv[optind + 1], "w");
if (!in || !out)
{
puts("Cannot find file.");
exit(1);
}
}
static const char data_input_fmt[] = "%s %d %d %d %lf %lf %lf %lf %lf";
static const char float_input_fmt[] = "%lf\t%lf\t%lf";
static const char float_output_fmt[] = "%lf\t%lf\t%lf\n";
static const char int_input_fmt[] = "%d\t%d\t%d\n";
static const char *int_output_fmt = int_input_fmt;
int count = 0;
if (f == extract_coordinates_fun)
{
char first_line[100];
fgets(first_line, 100, in);
char type[100];
int i, j, k;
double x, y, z;
double u, v;
while (true)
{
fscanf(in, data_input_fmt, type, &i, &j, &k, &x, &y, &z, &u, &v);
if (feof(in))
{
break;
}
fprintf(out, float_output_fmt, x, y, z);
}
}
else if (f == discretize_fun)
{
double square_error_sum = 0;
double avg_error_x = 0;
double avg_error_y = 0;
double avg_error_z = 0;
double x, y, z;
while (true)
{
fscanf(in, float_input_fmt, &x, &y, &z);
if (feof(in))
{
break;
}
fprintf(out, int_output_fmt, discretize(x, step),
discretize(y, step), discretize(z, step));
avg_error_x += abs(error(x, step));
avg_error_y += abs(error(y, step));
avg_error_z += abs(error(z, step));
square_error_sum += square_error(x, step) + square_error(y, step) +
square_error(z, step);
count++;
}
double avg_error = sqrt(square_error_sum) / count;
avg_error_x /= count;
avg_error_y /= count;
avg_error_z /= count;
fprintf(stderr, "Average error: %.14lf\n", avg_error);
fprintf(stderr, "Average error per axis: (%.14lf, %.14lf, %.14lf)\n",
avg_error_x, avg_error_y, avg_error_z);
}
else if (f = undiscretize_fun)
{
int x, y, z;
while (true)
{
fscanf(in, int_input_fmt, &x, &y, &z);
if (feof(in))
{
break;
}
fprintf(out, float_output_fmt, undiscretize(x, step),
undiscretize(y, step), undiscretize(z, step));
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "eser4");
ros::NodeHandle n;
ros::Subscriber read_position, read_laser;
goal_x = atof(argv[1]);
goal_y = atof(argv[2]);
letturaMappa("/home/tonish/Desktop/mappa.pgm");
aggiornaMappaVertici();
salvaMappa("/home/tonish/Desktop/vertici.pgm", true);
read_position = n.subscribe("/base_pose_ground_truth", 10, subPose);
read_laser = n.subscribe("/base_scan", 10, laserCallBack);
ros::Publisher twist_pub;
geometry_msgs::Twist msg;
twist_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
p_node path = NULL;
p_node best_node = NULL;
p_node final_goal = NULL;
final_goal = insertNode(final_goal, (int)round(fabs(3*goal_x)), (int)round(fabs(3*goal_y)), 0, NULL);
ros::Rate loop_rate(10);
while(check)
ros::spinOnce();
while(ros::ok()) {
if (fabs(robot_pose_x-goal_x) < 0.5 && fabs(robot_pose_y-goal_y) < 0.5) {
printf("GOAL RAGGIUNTO!!\n");
ros::shutdown();
}
if (robot_pose_orientation > M_PI)
robot_pose_orientation -= 2*M_PI;
if (f_tot < 100) {
msg.linear.x = LINEAR_V;
msg.angular.z = 0.0;
} else {
if (linear_movement) {
msg.linear.x = LINEAR_V;
msg.angular.z = 0.0;
}
if (angular_movement_left) {
msg.linear.x = 0.0;
msg.angular.z = ANGULAR_V;
}
if (angular_movement_right) {
msg.linear.x = 0.0;
msg.angular.z = -ANGULAR_V;
}
}
if (stop && one_time) {
msg.linear.x = 0.0;
msg.angular.z = 0.0;
one_time = false;
}
if (stop) {
p_node temp = AStar();
path = insertInHead(path, final_goal);
while (temp->parent) {
path = insertInHead(path, temp);
temp = temp->parent;
}
goal_f_x = undiscretize(-path->x);
goal_f_y = undiscretize(path->y);
printf("coordinate del primo vertice= x:%f, y:%f \n", goal_f_x, goal_f_y);
stop = false;
go = true;
control = false;
}
if (fabs(robot_pose_x-goal_f_x) < 0.1 && fabs(robot_pose_y-goal_f_y) < 0.1) {
if (path->next) {
path = path->next;
goal_f_x = undiscretize(-path->x);
goal_f_y = undiscretize(path->y);
printf("coordinate del vertice corrente= x:%f, y:%f \n", goal_f_x, goal_f_y);
}
}
twist_pub.publish(msg);
loop_rate.sleep();
ros::spinOnce();
}
}
