wrongT-wrongR-notunderstandScale

src/time_record/src/record_time.cpp

+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include "sensor_msgs/Image.h"
+#include <iostream>
+using namespace std;
+#include "time.h"
+#include <fstream>
+
+bool first_flag=0;
+double time1;
+void Record(const sensor_msgs::ImagePtr msg)
+{
+  if(!first_flag)
+  {
+    time1=msg->header.stamp.toSec();
+    first_flag=1;
+  }
+  double time=msg->header.stamp.toSec()-time1;
+  fstream f1;
+  f1.open("time1.txt",ios::app|ios::out);
+  if(!f1.is_open()){cout<<"no file open!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;return ;}
+  f1<<time<<endl;
+  f1.close();
+  cout<<time<<endl;
+}
+int main(int argc, char **argv)
+{
+  
+
+  ros::init(argc, argv, "TimeRecord");
+
+  ros::NodeHandle n;
+  ros::Subscriber sub = n.subscribe("/camera/rgb/image_rect_color", 1, Record);
+  ROS_INFO("Record Time of Images");
+
+  ros::spin();
+
+  return 0;
+}

time2.txt

+0
+0.0643375
+0.0967281
+0.130244
+0.160492
+0.194246
+0.227762
+0.261278
+0.294794
+0.330729
+0.362397
+0.397348
+0.430864
+0.46438
+0.497896
+0.531412
+0.564928
+0.598763
+0.632279
+0.665795
+0.699311
+0.732827
+0.766343
+0.799859
+0.833056
+0.866572
+0.899201
+0.932717
+0.967423
+0.999749
+1.03327
+1.06678
+1.1003
+1.13381
+1.16836
+1.20203
+1.2354
+1.26891
+1.30151
+1.33594
+1.36946
+1.40298
+1.43557
+1.46909
+1.50261
+1.53612
+1.56964
+1.60273
+1.63624
+1.66952
+1.70304
+1.73582
+1.76933
+1.80285
+1.83635
+1.86988
+1.90459
+1.93811
+1.97135
+2.00486
+2.03838
+2.0719
+2.10569
+2.1392
+2.17272
+2.20596
+2.23975
+2.27327
+2.30651
+2.34002
+2.37354
+2.40699
+2.4405
+2.47513
+2.50865
+2.54215
+2.57556
+2.60811
+2.64259
+2.6761
+2.70962
+2.74217
+2.77569
+2.8092
+2.84272
+2.87623
+2.90975
+2.94327
+2.97652
+3.01004
+3.04335
+3.07707
+3.11052
+3.14404
+3.17755
+3.21107
+3.24547
+3.2781
+3.31338
+3.34694
+3.38041
+3.41397
+3.44749
+3.481
+3.51448
+3.54799
+3.58151
+3.61502
+3.64858
+3.6823
+3.71576
+3.74933
+3.78285
+3.8163
+3.84926
+3.88334
+3.91685
+3.95037
+3.98388
+4.0174
+4.05107
+4.08443
+4.1181
+4.15162
+4.18498
+4.21793
+4.25121
+4.28428
+4.31779
+4.35131
+4.38483
+4.41834
+4.45186
+4.48663
+4.52021
+4.55366
+4.58724
+4.6207
+4.65421
+4.68779
+4.72131
+4.75486
+4.78838
+4.82208
+4.85559
+4.88905
+4.92257
+4.95596
+4.9896
+5.02081
+5.05658
+5.0901
+5.12361
+5.15513
+5.18839
+5.22181
+5.25533
+5.28905
+5.32257
+5.35616
+5.38968
+5.42326
+5.45718
+5.4907
+5.52422
+5.55773
+5.59084
+5.62476
+5.65845
+5.69197
+5.72676
+5.75933
+5.79376
+5.82636
+5.85988
+5.8934
+5.92691
+5.96043
+5.99389
+6.0274
+6.06092
+6.09444
+6.12754
+6.16106
+6.19458
+6.22809
+6.26158
+6.2951
+6.32861
+6.36216
+6.39567
+6.42921
+6.4627
+6.49624
+6.52975
+6.56246
+6.59679
+6.63104
+6.66499
+6.69882
+6.73233
+6.76585
+6.79936
+6.83288
+6.86608
+6.89843
+6.93356
+6.96708
+6.99824
+7.03411
+7.06763
+7.10026
+7.13329
+7.16582
+7.19926
+7.23278
+7.2663
+7.29981
+7.33333
+7.36691
+7.40142
+7.43493
+7.46863
+7.50245
+7.53597
+7.56948
+7.60332
+7.6369
+7.67042
+7.70393
+7.73739
+7.77097
+7.80448
+7.83799
+7.87151
+7.90502
+7.93854
+7.972
+8.00557
+8.03894
+8.07246
+8.10598
+8.13821
+8.17172
+8.2054
+8.23892
+8.27232
+8.30579
+8.33935
+8.37298
+8.40638
+8.4399
+8.47342
+8.50686
+8.54032
+8.57389
+8.60735
+8.64093
+8.67451
+8.70796
+8.74147
+8.77493
+8.80845
+8.84082
+8.87434
+8.90785
+8.94137
+8.97488
+9.00867
+9.04234
+9.07586
+9.10937
+9.14289
+9.1764
+9.20992
+9.24344
+9.27793
+9.31256
+9.34626
+9.37978
+9.41331
+9.44683
+9.48035
+9.51366
+9.54683
+9.57981
+9.61332
+9.64684
+9.68012
+9.71387
+9.74739
+9.78145
+9.81442
+9.84759
+9.88096
+9.91432
+9.94784
+9.98135
+10.0149
+10.0484
+10.0821
+10.1154
+10.1489
+10.1827
+10.2163
+10.2498
+10.283
+10.3164
+10.3499
+10.3835
+10.4178
+10.4513
+10.4848
+10.5175
+10.551
+10.5845
+10.6181
+10.6516
+10.685
+10.7186
+10.752
+10.7852
+10.8191
+10.8523
+10.8861
+10.9193
+10.9531
+10.9863
+11.0202
+11.0537
+11.0876
+11.1211
+11.1546
+11.1881
+11.2216
+11.2553
+11.2888
+11.3225
+11.3568
+11.3895
+11.423
+11.4565
+11.4901
+11.5236
+11.5571
+11.5905
+11.624
+11.6575
+11.691
+11.7245
+11.758
+11.7915
+11.8242
+11.8578
+11.8908
+11.9243
+11.9578
+11.9914
+12.0249
+12.0585
+12.0923
+12.1271
+12.1606
+12.1942
+12.2279
+12.2612
+12.2949
+12.3285
+12.362
+12.3955
+12.4291
+12.4627
+12.4963
+12.5298
+12.5633
+12.5968
+12.6302
+12.6637
+12.6972
+12.7307
+12.7644
+12.7977
+12.8314
+12.8648
+12.8984
+12.9319
+12.9652
+12.9987
+13.0308
+13.0643
+13.0978
+13.1332
+13.1661
+13.199
+13.2326
+13.2654
+13.2996
+13.3325
+13.3666
+13.3995
+13.4336
+13.4672
+13.5007
+13.5342
+13.5675
+13.6012
+13.6345
+13.6681
+13.7016
+13.736
+13.7696
+13.8021
+13.8356
+13.8692
+13.9036
+13.9371
+13.9707
+14.0042
+14.0377
+14.0713
+14.1048
+14.1383
+14.1718
+14.2051
+14.2386
+14.2721
+14.3057
+14.3392
+14.3724
+14.4059
+14.4394
+14.4725
+14.5059
+14.5394
+14.5729
+14.6064
+14.6397
+14.6732
+14.707
+14.7405
+14.774
+14.8073
+14.841
+14.8746
+14.9081
+14.9416
+14.9751
+15.0086
+15.0421
+15.0757
+15.1092
+15.1432
+15.1768
+15.2105
+15.244
+15.2775
+15.3111
+15.3446
+15.3778
+15.4116
+15.4449
+15.4786
+15.5123
+15.5458
+15.5792
+15.6127
+15.6466
+15.6792
+15.7126
+15.7461
+15.7797
+15.8139
+15.8468
+15.8803
+15.9138
+15.9473
+15.9808
+16.0141
+16.0476
+16.0811
+16.1146
+16.1481
+16.1817
+16.2151
+16.2487
+16.2821
+16.3156
+16.3492
+16.3838
+16.4163
+16.4509
+16.4848
+16.5183
+16.5519
+16.5854
+16.6189
+16.6524
+16.6844
+16.7192
+16.7526
+16.7862
+16.8196
+16.8531
+16.8855
+16.92
+16.9531
+16.986
+17.0195
+17.053
+17.0865
+17.1201
+17.1542
+17.1881
+17.2213
+17.2548
+17.2871
+17.3218
+17.3539
+17.3888
+17.4209
+17.4545
+17.488
+17.5234
+17.555
+17.5892
+17.624
+17.6575
+17.6897
+17.7245
+17.7567
+17.7916
+17.8251
+17.8586
+17.8921
+17.9256
+17.9591
+17.9926
+18.0261
+18.0596
+18.0932
+18.1267
+18.1602
+18.1937
+18.2269
+18.2604
+18.2938
+18.3274
+18.3609
+18.393
+18.4263
+18.4599