From e0bc0af29fb4030d628cc19377e56e535cd28486 Mon Sep 17 00:00:00 2001 From: uas Date: Mon, 16 May 2022 19:35:43 -0400 Subject: [PATCH 1/2] uav cams with gpsd trig without sync msgs --- include/spinnaker_sdk_camera_driver/capture.h | 1 + launch/acquisition.launch | 8 +-- launch/acquisition_external_trigger.launch | 4 +- .../stereo_17197554_17197553.yaml | 41 +++++++++++++ .../stereo_17197555_17197558.yaml | 40 +++++++++++++ .../stereo_17197556_17197557.yaml | 40 +++++++++++++ .../stereo_17197560_17197550.yaml | 40 +++++++++++++ params/external_trigger_params.yaml | 6 +- params/stereo_camera_example.yaml | 8 +-- src/capture.cpp | 58 ++++++++++++------- 10 files changed, 213 insertions(+), 33 deletions(-) create mode 100644 params/calibration_files/stereo_17197554_17197553.yaml create mode 100644 params/calibration_files/stereo_17197555_17197558.yaml create mode 100644 params/calibration_files/stereo_17197556_17197557.yaml create mode 100644 params/calibration_files/stereo_17197560_17197550.yaml diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index a71436d..7be41f9 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -131,6 +131,7 @@ namespace acquisition { bool TIME_BENCHMARK_; bool MASTER_TIMESTAMP_FOR_ALL_; bool EXTERNAL_TRIGGER_; + bool USE_IMU_TS_EXTERNAL_TRIGGER_; bool SAVE_; bool SAVE_BIN_; bool MANUAL_TRIGGER_; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 33a5093..05ce033 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -6,7 +6,7 @@ - + @@ -21,7 +21,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -69,4 +69,4 @@ - + diff --git a/launch/acquisition_external_trigger.launch b/launch/acquisition_external_trigger.launch index ae636d3..daaad12 100644 --- a/launch/acquisition_external_trigger.launch +++ b/launch/acquisition_external_trigger.launch @@ -24,7 +24,8 @@ - + + @@ -63,6 +64,7 @@ + diff --git a/params/calibration_files/stereo_17197554_17197553.yaml b/params/calibration_files/stereo_17197554_17197553.yaml new file mode 100644 index 0000000..bd7647d --- /dev/null +++ b/params/calibration_files/stereo_17197554_17197553.yaml @@ -0,0 +1,41 @@ +cam_ids: +- 17197553 +- 17197554 +cam_aliases: +- cam0 +- cam1 + +#master_cam: 17197553 +skip: 20 +delay: 1.0 + +#Camera info message details +distortion_model: plumb_bob +image_height: 1024 +image_width: 1280 + +distortion_coeffs: +- [-0.20450164833719758, 0.14743367885666747, 1.3691017237135784e-05, -0.00029960779673081914] +- [-0.19543806353197976, 0.12250750911745852, -0.0008848056641066484, -0.0021017501699217554] + + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1275.5155246873633, 0.0, 656.6967116430126, 0.0, 1276.6709737523597, 506.5433446279619, 0.0, 0.0, 1.0] +- [1252.4067462338985, 0.0, 629.4308621564376, 0.0, 1250.9809134660086, 513.5942922153134, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [0.9665822150177287, 0.0568881655489388, 0.24996511402973415, -0.056811130425504716, 0.9983565526264057, -0.007529228891154418, -0.2499826335392076, -0.006923181955981874, 0.9682255690077631] +- [0.9627203218217302, 0.03938618350914482, 0.26761597579376367, -0.039468734667831934, 0.9992079277965503, -0.005073067340799848, -0.2676038133793774, -0.005678518918518859, 0.96351230064162] + +projection_coeffs: +- [1445.214023875022, 0.0, 193.84015655517578, 0.0, 0.0, 1445.214023875022, 520.7913436889648, 0.0, 0.0, 0.0, 1.0, 0.0] +- [1445.214023875022, 0.0, 193.84015655517578, 296.6206626773398, 0.0, 1445.214023875022, 520.7913436889648, 0.0, 0.0, 0.0, 1.0, 0.0] + +flip_horizontal: +- false +- false + +flip_vertical: +- false +- false diff --git a/params/calibration_files/stereo_17197555_17197558.yaml b/params/calibration_files/stereo_17197555_17197558.yaml new file mode 100644 index 0000000..5b3c226 --- /dev/null +++ b/params/calibration_files/stereo_17197555_17197558.yaml @@ -0,0 +1,40 @@ +cam_ids: +- 17197555 +- 17197558 +cam_aliases: +- cam0 +- cam1 + +#master_cam: 17197555 +skip: 20 +delay: 1.0 + +#Camera info message details +distortion_model: plumb_bob +image_height: 1024 +image_width: 1280 + +distortion_coeffs: +- [-0.21133156450551877, 0.16554701015049214, 0.0006371203200430035, -0.0010939572806040086] +- [-0.19951712659266974, 0.14189320394813731, -0.002057441273332286, -0.00279936870291979] + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1255.7661285167007, 0.0, 633.6643372214643, 0.0, 1253.2470339107972, 521.0000311047382, 0.0, 0.0, 1.0] +- [1257.0217332659643, 0.0, 632.5753725217256, 0.0, 1256.1189388208545, 492.01565714778764, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [0.9957523783024181, -0.0281670698120689, -0.08765738578853412, 0.02769368284386981, 0.9995945878086233, -0.006612106642487861, 0.08780809208507502, 0.004156465073896091, 0.9961287380466786] +- [0.9956833380506718, -0.06025833095412615, -0.07059478648522698, 0.06063837181798097, 0.9981545070530874, 0.0032508326429390596, 0.07026861455540498, -0.007517552808667829, 0.997499778550468] + +projection_coeffs: +- [1388.2617969436703, 0.0, 759.2249526977539, 0.0, 0.0, 1388.2617969436703, 507.6747169494629, 0.0, 0.0, 0.0, 1.0, 0.0] +- [1388.2617969436703, 0.0, 759.2249526977539, 278.81369985863324, 0.0, 1388.2617969436703, 507.6747169494629, 0.0, 0.0, 0.0, 1.0, 0.0] + +flip_horizontal: +- true +- true + +flip_vertical: +- true +- true diff --git a/params/calibration_files/stereo_17197556_17197557.yaml b/params/calibration_files/stereo_17197556_17197557.yaml new file mode 100644 index 0000000..57d6b02 --- /dev/null +++ b/params/calibration_files/stereo_17197556_17197557.yaml @@ -0,0 +1,40 @@ +cam_ids: +- 17197556 +- 17197557 +cam_aliases: +- cam0 +- cam1 + +#master_cam: 17197556 +skip: 20 +delay: 1.0 + +#Camera info message details +distortion_model: plumb_bob +image_height: 1024 +image_width: 1280 + +distortion_coeffs: +- [-0.20704574734171718, 0.1378606052753204, -0.00016623127312903865, -0.0014678552308168902] +- [-0.20315604562055115, 0.13925984473430503, 0.0011700633742295359, -0.002631592980875036] + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1253.702985566858, 0.0, 626.0038426699172, 0.0, 1253.5611530315684, 521.6826648653606, 0.0, 0.0, 1.0] +- [1249.0625547880688, 0.0, 633.4407842588579, 0.0, 1249.0308397534995, 535.9834578202574, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [0.999870166919909, 0.015624103543014101, -0.00394166107506912, -0.015616951626054152, 0.9998763572767314, 0.0018387443878286702, 0.0039699024500641535, -0.001776948927645647, 0.9999905411187876] +- [0.9999919133838011, 0.0028219174708584696, -0.002865300820562143, -0.0028270930622141884, 0.9999943768034167, -0.001803858742772549, 0.002860194367911154, 0.0018119446277301825, 0.9999942680559943] + +projection_coeffs: +- [1242.8692771645083, 0.0, 630.979866027832, 0.0, 0.0, 1242.8692771645083, 531.3879089355469, 0.0, 0.0, 0.0, 1.0, 0.0] +- [1242.8692771645083, 0.0, 630.979866027832, 253.03796302310053, 0.0, 1242.8692771645083, 531.3879089355469, 0.0, 0.0, 0.0, 1.0, 0.0] + +flip_horizontal: +- true +- true + +flip_vertical: +- true +- true diff --git a/params/calibration_files/stereo_17197560_17197550.yaml b/params/calibration_files/stereo_17197560_17197550.yaml new file mode 100644 index 0000000..1437483 --- /dev/null +++ b/params/calibration_files/stereo_17197560_17197550.yaml @@ -0,0 +1,40 @@ +cam_ids: +- 17197560 +- 17197550 +cam_aliases: +- cam0 +- cam1 + +#master_cam: 17197560 +skip: 20 +delay: 1.0 + +#Camera info message details +distortion_model: plumb_bob +image_height: 1024 +image_width: 1280 + +distortion_coeffs: +- [-0.19646860778761444, 0.11966919829956081, 0.0007669487706857788, -0.0013965801592123956] +- [-0.1914413046332957, 0.11346023268077468, 0.0019526799897460604, -0.002542153300965656] + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1278.623621111693, 0.0, 634.8138812505675, 0.0, 1276.7310814743978, 535.3629837510302, 0.0, 0.0, 1.0] +- [1273.2817308511162, 0.0, 629.495280191018, 0.0, 1273.4616558529785, 537.4608865156434, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [0.999951055794313, -0.008506807285265669, 0.0050517566895154475, 0.008499995333857072, 0.999962938127486, 0.001368375229905995, -0.00506320996632779, -0.0013253683475786214, 0.9999863035579938] +- [0.9995596958074359, -0.029351275291735073, -0.004349385714674004, 0.02934538988112363, 0.9995683356052899, -0.001410867816356254, 0.004388919009400241, 0.001282612185801062, 0.9999895460933128] + +projection_coeffs: +- [1251.7378592347225, 0.0, 627.0195465087891, 0.0, 0.0, 1251.7378592347225, 540.562442779541, 0.0, 0.0, 0.0, 1.0, 0.0] +- [1251.7378592347225, 0.0, 627.0195465087891, 256.12061596895546, 0.0, 1251.7378592347225, 540.562442779541, 0.0, 0.0, 0.0, 1.0, 0.0] + +flip_horizontal: +- true +- true + +flip_vertical: +- true +- true diff --git a/params/external_trigger_params.yaml b/params/external_trigger_params.yaml index 232d9cc..a00d48a 100644 --- a/params/external_trigger_params.yaml +++ b/params/external_trigger_params.yaml @@ -1,15 +1,19 @@ cam_ids: -- 17197559 +- 17197557 +- 17197555 cam_aliases: - cam0 +- cam1 skip: 20 delay: 1.0 flip_horizontal: - false +- false flip_vertical: - false +- false # Assign all the follwing via launch file to prevent confusion and conflict diff --git a/params/stereo_camera_example.yaml b/params/stereo_camera_example.yaml index a588b41..b1260dc 100644 --- a/params/stereo_camera_example.yaml +++ b/params/stereo_camera_example.yaml @@ -1,11 +1,10 @@ cam_ids: -<<<<<<< HEAD -- 17197554 +- 17197557 - 17197556 cam_aliases: - cam0 - cam1 -master_cam: 17197554 +master_cam: 17197557 skip: 20 delay: 1.0 @@ -42,7 +41,6 @@ rectification_coeffs: projection_coeffs: - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] -<<<<<<< HEAD flip_horizontal: - false @@ -51,5 +49,3 @@ flip_horizontal: flip_vertical: - false - false -======= ->>>>>>> master diff --git a/src/capture.cpp b/src/capture.cpp index c3f6e44..eae58f3 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -87,6 +87,7 @@ void acquisition::Capture::init_variables_register_to_ros() { TIME_BENCHMARK_ = false; MASTER_TIMESTAMP_FOR_ALL_ = true; EXTERNAL_TRIGGER_ = false; + USE_IMU_TS_EXTERNAL_TRIGGER_ = false; EXPORT_TO_ROS_ = false; PUBLISH_CAM_INFO_ = false; SAVE_ = false; @@ -156,8 +157,8 @@ void acquisition::Capture::init_variables_register_to_ros() { #ifdef trigger_msgs_FOUND // initiliazing the trigger subscriber - if (EXTERNAL_TRIGGER_){ - timeStamp_sub = nh_.subscribe("/imu/sync_trigger", 1000, &acquisition::Capture::assignTimeStampCallback,this); + if (EXTERNAL_TRIGGER_ && !USE_IMU_TS_EXTERNAL_TRIGGER_){ + timeStamp_sub = nh_.subscribe("imu/sync_trigger", 1000, &acquisition::Capture::assignTimeStampCallback,this); for ( int i=0;i sync_message_queue; @@ -362,6 +363,9 @@ void acquisition::Capture::read_parameters() { if (nh_pvt_.getParam("external_trigger", EXTERNAL_TRIGGER_)){ ROS_INFO(" External trigger: %s",EXTERNAL_TRIGGER_?"true":"false"); + if(nh_pvt_.getParam("use_imu_ts", USE_IMU_TS_EXTERNAL_TRIGGER_)){ + ROS_INFO_STREAM(" External trigger, use_imu_ts "< 1 ){ - ROS_WARN("Difference in trigger count more than 1, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); - } - - else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ - double wait_time_start = ros::Time::now().toSec(); - ROS_WARN("Difference in trigger count zero, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); - while(latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ - ros::Duration(0.0001).sleep(); + if (USE_IMU_TS_EXTERNAL_TRIGGER_){ + if (latest_imu_trigger_count_ - prev_imu_trigger_count_ > 1 ){ + ROS_WARN("Difference in trigger count more than 1, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); + } + else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ + double wait_time_start = ros::Time::now().toSec(); + ROS_WARN("Difference in trigger count zero, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); + while(latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ + ros::Duration(0.0001).sleep(); + } + ROS_INFO_STREAM("Time gap for sync messages: "< Date: Mon, 16 May 2022 20:16:36 -0400 Subject: [PATCH 2/2] add use_imu_ts flag to all launch files --- launch/acquisition.launch | 6 ++++-- launch/acquisition_external_trigger.launch | 8 ++++---- launch/node_acquisition.launch | 6 ++++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 05ce033..1d7a29b 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -7,6 +7,8 @@ + + @@ -24,7 +26,6 @@ - @@ -50,6 +51,8 @@ + + @@ -62,7 +65,6 @@ - diff --git a/launch/acquisition_external_trigger.launch b/launch/acquisition_external_trigger.launch index daaad12..fc2fc36 100644 --- a/launch/acquisition_external_trigger.launch +++ b/launch/acquisition_external_trigger.launch @@ -6,6 +6,8 @@ + + - - @@ -51,6 +51,8 @@ + + @@ -63,8 +65,6 @@ - - diff --git a/launch/node_acquisition.launch b/launch/node_acquisition.launch index 83d3aec..4461895 100644 --- a/launch/node_acquisition.launch +++ b/launch/node_acquisition.launch @@ -8,6 +8,8 @@ + + @@ -19,7 +21,6 @@ - @@ -37,6 +38,8 @@ + + @@ -48,7 +51,6 @@ -