예제 #1
0
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);
}
예제 #2
0
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);
}
예제 #3
0
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);
}
예제 #4
0
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);
}