void imu_callback(std::string name, std_msgs::PoseWithRatesStamped* imu, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n",
t.to_double(),
imu->header.stamp.to_double(),
imu->acc.acc.ax, imu->acc.acc.ay, imu->acc.acc.az,
imu->vel.ang_vel.vx, imu->vel.ang_vel.vy, imu->vel.ang_vel.vz);
}
void imu_callback(std::string name, imu_node::ImuData* imu, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n",
t.to_double(),
imu->header.stamp.to_double(),
imu->accel.ax, imu->accel.ay, imu->accel.az,
imu->angrate.vx, imu->angrate.vy, imu->angrate.vz);
}
void odom_callback(string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, void* n)
{
double rel_time = t.to_double();
static bool vel_init = false;
static double yaw_offset = 0;
if (!vel_init)
{
vel_init = true;
dumb_rv = dumb_tv = 0;
prev_time = rel_time;
test_log = fopen("test.txt", "w");
}
else
{
double next_th = odom->pos.th + yaw_offset;
if (fabs(next_th - prev_th) > M_PI)
{
if (next_th > prev_th)
yaw_offset -= 2 * M_PI;
else
yaw_offset += 2 * M_PI;
}
odom->pos.th += yaw_offset;
double dt = rel_time - prev_time;
double dx = odom->pos.x - prev_x;
double dy = odom->pos.y - prev_y;
dumb_rv = (odom->pos.th - prev_th) / dt;
dumb_tv = sqrt(dx*dx + dy*dy) / dt;
fprintf(test_log, "%f %f %f %f %f %f\n",
odom->pos.x, odom->pos.y, odom->pos.th, dumb_tv, dumb_rv, dt);
}
prev_x = odom->pos.x;
prev_y = odom->pos.y;
prev_th = odom->pos.th;
prev_time = rel_time;
fprintf(clog, "ODOM %f %f %f %f %f 0 %f logsetta %f\n",
odom->pos.x, odom->pos.y, odom->pos.th,
dumb_tv, dumb_rv, rel_time, rel_time);
}
void scan_callback(string name, std_msgs::LaserScan* scan, ros::Time t, void* n)
{
double rel_time = t.to_double();
const double fov = fabs(scan->angle_max - scan->angle_min);
// only make an exception for the SICK LMS2xx running in centimeter mode
const double acc = (scan->range_max >= 81 ? 0.05 : 0.005);
fprintf(clog, "ROBOTLASER1 0 %f %f %f %f %f 0 %d ",
scan->angle_min, fov, fov / scan->angle_increment,
scan->range_max, acc, scan->get_ranges_size());
for (uint32_t i = 0; i < scan->get_ranges_size(); i++)
fprintf(clog, "%.3f ", scan->ranges[scan->get_ranges_size() - i - 1]);
double laser_x = prev_x + 0.30 * cos(prev_th); // in the robot frame
double laser_y = prev_y + 0.30 * sin(prev_th);
double laser_th = prev_th;
double laser_rv = dumb_rv; // not really
double laser_tv = dumb_tv; // not really
fprintf(clog, " 0 %f %f %f %f %f %f %f %f 0.3 0.3 1000000 %f logsetta %f\n",
laser_x, laser_y, laser_th,
prev_x, prev_y, prev_th,
laser_tv, laser_rv, rel_time,
rel_time);
}