From f6070add560f538d6b0e38c74ac62e5bb0d930ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 31 Aug 2020 11:13:00 +0200 Subject: [PATCH 01/32] Added dependency on wol_ros_node --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index c2ab136..d46df9c 100644 --- a/package.xml +++ b/package.xml @@ -52,6 +52,7 @@ roscpp rospy std_msgs + wolf_ros_node roscpp rospy std_msgs -- GitLab From 1425322bc9f385a5573726df4fb4ac2128033ed8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 13:10:30 +0200 Subject: [PATCH 02/32] Changed rviz config file to show trajectory and factors --- launch/online.rviz | 73 +++++++++++++++++++++++++++++----------------- 1 file changed, 47 insertions(+), 26 deletions(-) diff --git a/launch/online.rviz b/launch/online.rviz index 033541a..a5a7293 100644 --- a/launch/online.rviz +++ b/launch/online.rviz @@ -4,14 +4,14 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - /Status1 - /Grid1 - - /TF1 - - /Camera1 + - /Pose1 - /MarkerArray1 + - /MarkerArray2 + - /MarkerArray3 Splitter Ratio: 0.5 - Tree Height: 582 + Tree Height: 778 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -71,7 +71,7 @@ Visualization Manager: Name: TF Show Arrows: false Show Axes: true - Show Names: false + Show Names: true Tree: map: odom: @@ -80,7 +80,7 @@ Visualization Manager: Update Interval: 0 Value: true - Class: rviz/Camera - Enabled: true + Enabled: false Image Rendering: background and overlay Image Topic: /camera_simu/image_raw Name: Camera @@ -88,7 +88,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false Visibility: Grid: true MarkerArray: true @@ -96,14 +96,6 @@ Visualization Manager: TF: true Value: true Zoom Factor: 1 - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /wolf_apriltag_vio/marker_array - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 @@ -119,6 +111,35 @@ Visualization Manager: Topic: /wolf_apriltag_vio/estimated_pose Unreliable: false Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /wolf_ros_node/trajectory + Name: MarkerArray + Namespaces: + frames: true + frames_text: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /wolf_ros_node/landmarks + Name: MarkerArray + Namespaces: + landmarks: true + landmarks_text: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /wolf_ros_node/factors + Name: MarkerArray + Namespaces: + factors_ABS: true + factors_LMK: true + factors_MOTION: true + factors_text: true + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -147,35 +168,35 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.967782497406006 + Distance: 7.928880214691162 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.48822471499443054 - Y: -0.18343956768512726 - Z: 0.25536078214645386 + X: 0.5710559487342834 + Y: -0.1793169379234314 + Z: -1.7769840955734253 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.24979741871356964 + Pitch: 0.669797420501709 Target Frame: Value: Orbit (rviz) - Yaw: 5.988564491271973 + Yaw: 2.735382556915283 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 940 + Height: 1023 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002effc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000028b000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002d00000005e0000001800ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000048200000058fc0100000002fb0000000800540069006d00650100000000000004820000028200fffffffb0000000800540069006d0065010000000000000450000000000000000000000312000002ef00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001a000000347fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000347000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000031c0000006a0000001600ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000058fc0100000002fb0000000800540069006d00650100000000000003c0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000021a0000034700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -184,6 +205,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1154 - X: 324 - Y: 55 + Width: 960 + X: 960 + Y: 27 -- GitLab From 8dc2ccbbd439436b3ef295217acd9fbf2887cdb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 13:12:53 +0200 Subject: [PATCH 03/32] Remove noisy logs --- src/subscriber_camera.cpp | 2 +- src/subscriber_imu.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index bccf2c6..7074bbe 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -66,7 +66,7 @@ void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) WOLF_TRACE(f_list.size(), " tags, nb without ROT: ", nb_without_rot, "/", f_list.size()); } } - sensor_ptr_->getProblem()->print(4,1,1,1); + // sensor_ptr_->getProblem()->print(4,1,1,1); publishDebugImg(msg, frame); //////////////////////////// // Write diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp index b406a97..b2940ff 100644 --- a/src/subscriber_imu.cpp +++ b/src/subscriber_imu.cpp @@ -71,7 +71,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu &msg) finding_orientation_prior_ = false; - std::cout << "Norm: " << w_q_imu_init_.norm() << "\nOrientation prior found: \n" << w_q_imu_init_.coeffs().conjugate() << std::endl; + std::cout << "Norm: " << w_q_imu_init_.norm() << "\nOrientation prior found: \n" << w_q_imu_init_.coeffs().transpose() << std::endl; } else { std::cout << "\nmin_acc_std not > " << std_imu_max_ << ": " << min_acc_std << std::endl; -- GitLab From e2412bc9f15b465fa3982143127bf68ad1da6720 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 13:14:34 +0200 Subject: [PATCH 04/32] Modified params according to simu rosbag --- yaml/apriltag_vio.yaml | 29 ++++++------------- yaml/params.yaml | 29 ++++--------------- yaml/processor_tracker_landmark_apriltag.yaml | 4 +-- 3 files changed, 16 insertions(+), 46 deletions(-) diff --git a/yaml/apriltag_vio.yaml b/yaml/apriltag_vio.yaml index 08ba6ad..4fd2f96 100644 --- a/yaml/apriltag_vio.yaml +++ b/yaml/apriltag_vio.yaml @@ -7,30 +7,26 @@ config: # frame_structure: "PO" dimension: 3 prior: - mode: "fix" + mode: "factor" $state: P: [0,0,0] - O: [0,0,0,1] + O: [1,0,0,0] V: [0,0,0] $sigma: P: [0.31, 0.31, 0.31] O: [0.31, 0.31, 0.31] V: [0.31, 0.31, 0.31] - time_tolerance: 0.1 - #mode: "factor" - #state: [0,0,0] - #cov: [[3,3],.1,0,0,0,.1,0,0,0,.1] - #time_tolerance: 0.1 + time_tolerance: 0.01 solver: max_num_iterations: 100 - verbose: 1 - period: 0.3 + verbose: 0 + period: 0.5 update_immediately: false visualizer: type: "Visualizer" - period: 5 + period: 0.01 sensors: - @@ -38,10 +34,7 @@ config: name: "CAMERA" plugin: "vision" extrinsic: - # pose: [0.15,0.15,0] - # pose: [0.2,0.1, 1.57] - pose: [-0.0013,-0.092,0.02, 0,0,0,1] # compact setup (from kalibr) - # intrinsics: + pose: [0,0,0, 0,0,0,1] # simu follow: "camera_simu.yaml" # follow: "camera_simu_wolf_standard.yaml" - @@ -49,11 +42,7 @@ config: name: "IMU" plugin: "imu" extrinsic: - # pose: [0.15,0.15,0] - # pose: [0.2,0.1, 1.57] - pose: [0,0,0, 0,0,0,1] - # intrinsics: - # follow: "imu_intrinsics.yaml" + pose: [0,0,0, 0,0,0,1] # mandatory with curent imu implementation follow: "imu_intrinsics.yaml" processors: - @@ -61,7 +50,7 @@ config: name: "APRILTAG PROC" sensor_name: "CAMERA" plugin: "apriltag" - time_tolerance: 0.04 + time_tolerance: 0.01 follow: "processor_tracker_landmark_apriltag.yaml" - type: "ProcessorImu" diff --git a/yaml/params.yaml b/yaml/params.yaml index b04c214..97121ec 100644 --- a/yaml/params.yaml +++ b/yaml/params.yaml @@ -1,4 +1,4 @@ -apriltag_only: true # %% +apriltag_only: false # %% # if the IMU is used, find a prior on the original orientation of the IMU based on measured gravity finding_orientation_prior: false # %% constrained_by_motion_factor_to_solve_activated: false @@ -21,33 +21,14 @@ display_scaled_tf: 1 # to overcome the frame scale limitations of rviz, 1 to imu_raw_topic: /imu/imu_raw current_frame: current -# data flow parameters -solve_iter: 100 -max_active_KF: -1 # maximum number of active key frames, if -1, not taken into account -solve_every: 0.3 # run a factor graph solved every __ seconds -# solve_every: 5000 # !!deactivated -fast_solve_dur: 0 # solve the problem at a faster rate at the begging for __ seconds -solve_every_fast: 0.05 # run a factor graph solved every __ seconds at the beginning -broadcast_trajectory_every: 0.5 # broadcast the current trajectory and landmark estimation every __ -camera_ros_buffer_size: 1000 -imu_ros_buffer_size: 1000 - -# geometric parameters -imu_extrinsics: [0,0,0, 0,0,0,1] - -# cam_extrinsics: [0,0,0, 0,0,0,1] # simu $$ -cam_extrinsics: [-0.0013,-0.092,0.02, 0.702513,0.00349,0.0086,0.7116] # compact setup (from kalibr) -# cam_extrinsics: [0,0,0, 0.45,-0.55,-0.45,0.515] # New compact setup, rotation (from kalibr) -- WHAT? -# cam_extrinsics: [0.0168,-0.009,0.0277, 0,0,1,0] # microstrain + mvBlueFox3 (from Kalibr) $$ - # sensors -shift_imu_minus_cam: 0.006 +shift_imu_minus_cam: 0.0 acc_sat: 49 # m/s^2 gyr_sat: 3.92 # rad/s -# prior_vec: [0.0,0.0,0.0, 0,0,0,1, 0,0,0] # $$ +prior_vec: [0.0,0.0,0.0, 0,0,0,1, 0,0,0] # $$ # prior_vec: [0.0,0.0,1.7, 1,0,0,0, 0,0,0] # $$ compact setup, lying on table -prior_vec: [0,0,0, 0.963449,-0.000121,-0.066758,0.259439, 0,0,0] # On the robot, IMU with Z down +# prior_vec: [0,0,0, 0.963449,-0.000121,-0.066758,0.259439, 0,0,0] # On the robot, IMU with Z down # prior_vec: [0,0,0, 0,0.999982,0,0.00603841, 0,0,0] # $$ # prior_vec: [0.0,0.5,1.1, 1,0,0,0, -0.189,0,0] # vx for Ax = 0.3 $$ # prior_vec: [0.0,0,0.6, 0.5,0,0,0.8660254, 0,0,0] # vx for Ax = 0.3, tilted 181° on X $$ @@ -57,7 +38,7 @@ prior_vec: [0,0,0, 0.963449,-0.000121,-0.066758,0.259439, 0,0,0] # On the robot std_m: 0.001 std_deg: 5 -std_vel: 0.001 +std_vel: 0.1 # Orientation prior using IMU size_buffer_imu_init: 20 diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml index 341c4fc..aa65a48 100644 --- a/yaml/processor_tracker_landmark_apriltag.yaml +++ b/yaml/processor_tracker_landmark_apriltag.yaml @@ -36,8 +36,8 @@ std_pix: 4 # pixel error $$ keyframe_vote: voting_active: true voting_aux_active: false -nb_vote_for_every_first: 1000000000 -min_time_vote: 0.1 # s +nb_vote_for_every_first: 5 +min_time_vote: 0.2 # s # Necessary conditions max_time_vote: 0.5 # s # One of the possibilities -- GitLab From 28813f13331174ef200632e5d58a7c7fecd5d04a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 13:15:42 +0200 Subject: [PATCH 05/32] Added parameters for visualization --- yaml/visualizer.yaml | 45 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 yaml/visualizer.yaml diff --git a/yaml/visualizer.yaml b/yaml/visualizer.yaml new file mode 100644 index 0000000..2762b97 --- /dev/null +++ b/yaml/visualizer.yaml @@ -0,0 +1,45 @@ +viz_factors: true +viz_overlapped_factors: false +viz_landmarks: true +viz_trajectory: true + +# viz parameters +map_frame_id: "map" +viz_scale: 1 +factors_width: 0.02 +factors_absolute_height: 20 +landmark_text_z_offset: 0.1 + +landmark_width: 0.03 +landmark_length: 0.2 +frame_width: 0.1 +frame_length: 0.01 + +frame_color_rr: 1.0 +frame_color_gg: 0.8 +frame_color_bb: 0.0 +frame_color_aa: 1 + +factor_abs_color_rr: 0.92 +factor_abs_color_gg: 0.19 +factor_abs_color_bb: 0.6 +factor_abs_color_aa: 1 + +factor_motion_color_rr: 1.0 +factor_motion_color_gg: 1.0 +factor_motion_color_bb: 0.0 +factor_motion_color_aa: 1 + +factor_loop_color_rr: 1.0 +factor_loop_color_gg: 0.0 +factor_loop_color_bb: 0.0 +factor_loop_color_aa: 1 +factor_lmk_color_rr: 0.0 +factor_lmk_color_gg: 0.0 +factor_lmk_color_bb: 1.0 +factor_lmk_color_aa: 1 + +factor_geom_color_rr: 0.0 +factor_geom_color_gg: 1.0 +factor_geom_color_bb: 1.0 +factor_geom_color_aa: 1 \ No newline at end of file -- GitLab From 904d5cfad5e42c88b2187c3f1e9a53f8f46883ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 14:46:22 +0200 Subject: [PATCH 06/32] Added rosparam to test.launch --- launch/test.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/test.launch b/launch/test.launch index 1b875fb..1ec6da2 100644 --- a/launch/test.launch +++ b/launch/test.launch @@ -34,5 +34,6 @@ + -- GitLab From 4e5dfbcc8cb69b1e4813a578b4676fc54e74411e Mon Sep 17 00:00:00 2001 From: jcasals Date: Mon, 7 Sep 2020 15:51:23 +0200 Subject: [PATCH 07/32] Modify visualization params --- launch/online.rviz | 26 +++++++++++++------------- yaml/visualizer.yaml | 10 +++++----- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/launch/online.rviz b/launch/online.rviz index a5a7293..f0157c6 100644 --- a/launch/online.rviz +++ b/launch/online.rviz @@ -8,10 +8,13 @@ Panels: - /Grid1 - /Pose1 - /MarkerArray1 + - /MarkerArray1/Namespaces1 - /MarkerArray2 + - /MarkerArray2/Namespaces1 - /MarkerArray3 + - /MarkerArray3/Namespaces1 Splitter Ratio: 0.5 - Tree Height: 778 + Tree Height: 762 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -61,8 +64,6 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - base_footprint: - Value: true map: Value: true odom: @@ -75,8 +76,7 @@ Visualization Manager: Tree: map: odom: - base_footprint: - {} + {} Update Interval: 0 Value: true - Class: rviz/Camera @@ -117,7 +117,7 @@ Visualization Manager: Name: MarkerArray Namespaces: frames: true - frames_text: true + frames_text: false Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -126,7 +126,7 @@ Visualization Manager: Name: MarkerArray Namespaces: landmarks: true - landmarks_text: true + landmarks_text: false Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -137,7 +137,7 @@ Visualization Manager: factors_ABS: true factors_LMK: true factors_MOTION: true - factors_text: true + factors_text: false Queue Size: 100 Value: true Enabled: true @@ -193,10 +193,10 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1023 + Height: 1020 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001a000000347fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000347000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000031c0000006a0000001600ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000058fc0100000002fb0000000800540069006d00650100000000000003c0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000021a0000034700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001a00000033ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000033f000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000031c0000006a0000001800ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000058fc0100000002fb0000000800540069006d00650100000000000007800000028200fffffffb0000000800540069006d00650100000000000004500000000000000000000005da0000033f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -205,6 +205,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 960 - X: 960 - Y: 27 + Width: 1920 + X: 0 + Y: 0 diff --git a/yaml/visualizer.yaml b/yaml/visualizer.yaml index 2762b97..4be802e 100644 --- a/yaml/visualizer.yaml +++ b/yaml/visualizer.yaml @@ -6,14 +6,14 @@ viz_trajectory: true # viz parameters map_frame_id: "map" viz_scale: 1 -factors_width: 0.02 +factors_width: 0.002 factors_absolute_height: 20 landmark_text_z_offset: 0.1 -landmark_width: 0.03 +landmark_width: 0.01 landmark_length: 0.2 -frame_width: 0.1 -frame_length: 0.01 +frame_width: 0.01 +frame_length: 0.001 frame_color_rr: 1.0 frame_color_gg: 0.8 @@ -42,4 +42,4 @@ factor_lmk_color_aa: 1 factor_geom_color_rr: 0.0 factor_geom_color_gg: 1.0 factor_geom_color_bb: 1.0 -factor_geom_color_aa: 1 \ No newline at end of file +factor_geom_color_aa: 1 -- GitLab From 7869a828925a4018d8ea555bb8d9b6135ece33d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Mon, 7 Sep 2020 16:26:12 +0200 Subject: [PATCH 08/32] Removed useless odom translation --- launch/test.launch | 2 +- yaml/apriltag_vio.yaml | 2 +- yaml/processor_imu.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/test.launch b/launch/test.launch index 1ec6da2..b886218 100644 --- a/launch/test.launch +++ b/launch/test.launch @@ -9,7 +9,7 @@ + args="0 0 0.0 0 0 0 base_footprint /base_link 100"/> Date: Tue, 8 Sep 2020 14:55:30 +0200 Subject: [PATCH 09/32] 25 frame + fix first sliding window --- yaml/treeconfig.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/yaml/treeconfig.yaml b/yaml/treeconfig.yaml index f3d9d1f..da86393 100644 --- a/yaml/treeconfig.yaml +++ b/yaml/treeconfig.yaml @@ -1,4 +1,4 @@ type: "TreeManagerSlidingWindow" -n_key_frames: -1 -fix_first_key_frame: false +n_key_frames: 25 +fix_first_key_frame: true viral_remove_empty_parent: true -- GitLab From 36d666605b6f496ee7eeceac1ff8be59aceae673 Mon Sep 17 00:00:00 2001 From: jcasals Date: Wed, 23 Sep 2020 15:53:26 +0200 Subject: [PATCH 10/32] Add pose and odom publishers --- yaml/apriltag_vio.yaml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/yaml/apriltag_vio.yaml b/yaml/apriltag_vio.yaml index dae5db1..316e579 100644 --- a/yaml/apriltag_vio.yaml +++ b/yaml/apriltag_vio.yaml @@ -94,6 +94,27 @@ config: # topic: "/wolf_pose" # period: "5" + ROS publisher: + - + package: "wolf_ros_node" + type: "PublisherPose" + topic: "/wolf_pose" + period: "0.5" + pose_array_msg: true + marker_msg: false + extrinsics: false + frame_id: "base" + - + package: "wolf_ros_node" + type: "PublisherOdom" + topic: "/wolf_odom" + period: "0.5" + pose_array_msg: true + marker_msg: false + extrinsics: true + sensor: "IMU" + frame_id: "base" + follow: "params.yaml" # follow: "config.yaml" # raw_libs: "@raw_libs.yaml" -- GitLab From fc41365d49912de848936f6f5d46bb48d25288b7 Mon Sep 17 00:00:00 2001 From: jcasals Date: Wed, 23 Sep 2020 15:54:02 +0200 Subject: [PATCH 11/32] Add static odom transform (can't use IMU odom) --- launch/test.launch | 4 ++++ yaml/apriltag_vio.yaml | 3 +++ 2 files changed, 7 insertions(+) diff --git a/launch/test.launch b/launch/test.launch index b886218..cc501c8 100644 --- a/launch/test.launch +++ b/launch/test.launch @@ -26,6 +26,10 @@ launch-prefix="gdb -ex run --args" output="screen"> ?> + Date: Fri, 12 Feb 2021 19:09:32 +0100 Subject: [PATCH 12/32] Cleaning up useless files --- CMakeLists.txt | 166 +------------- include/publisher_current_state.h | 26 --- include/publisher_estimated_pose.h | 27 --- include/publisher_solve_logs.h | 28 --- include/subscriber_camera.h | 1 - include/subscriber_imu.h | 72 ------ launch/online.rviz | 210 ------------------ launch/test.launch | 43 ---- msg/SolveLogs.msg | 1 - src/publisher_current_state.cpp | 34 --- src/publisher_estimated_pose.cpp | 20 -- src/publisher_solve_logs.cpp | 20 -- src/subscriber_camera.cpp | 164 +++++++------- src/subscriber_imu.cpp | 134 ----------- yaml/apriltag_vio.yaml | 93 ++++---- yaml/camera_simu_wolf_standard.yaml | 5 - yaml/imu_intrinsics.yaml | 16 -- yaml/params.yaml | 45 ---- yaml/processor_imu.yaml | 9 - yaml/processor_tracker_landmark_apriltag.yaml | 38 ++-- yaml/simple.yaml | 34 --- yaml/treeconfig.yaml | 4 - yaml/visualizer.yaml | 45 ---- 23 files changed, 146 insertions(+), 1089 deletions(-) delete mode 100644 include/publisher_current_state.h delete mode 100644 include/publisher_estimated_pose.h delete mode 100644 include/publisher_solve_logs.h delete mode 100644 include/subscriber_imu.h delete mode 100644 launch/online.rviz delete mode 100644 launch/test.launch delete mode 100644 msg/SolveLogs.msg delete mode 100644 src/publisher_current_state.cpp delete mode 100644 src/publisher_estimated_pose.cpp delete mode 100644 src/publisher_solve_logs.cpp delete mode 100644 src/subscriber_imu.cpp delete mode 100644 yaml/camera_simu_wolf_standard.yaml delete mode 100644 yaml/imu_intrinsics.yaml delete mode 100644 yaml/params.yaml delete mode 100644 yaml/processor_imu.yaml delete mode 100644 yaml/simple.yaml delete mode 100644 yaml/treeconfig.yaml delete mode 100644 yaml/visualizer.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f21125..40bdaa0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,8 @@ project(wolf_ros_apriltag) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) +set(OpenCV_DIR /usr/share/OpenCV/) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -20,41 +22,8 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(wolfcore REQUIRED) -find_package(wolfimu REQUIRED) find_package(wolfvision REQUIRED) find_package(wolfapriltag REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder add_message_files( @@ -62,20 +31,6 @@ add_message_files( SolveLogs.msg ) -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES @@ -83,35 +38,10 @@ generate_messages( geometry_msgs ) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( # INCLUDE_DIRS include # LIBRARIES wolf_ros_apriltag @@ -140,43 +70,16 @@ include_directories( add_library(subscriber_${PROJECT_NAME} src/subscriber_camera.cpp - src/subscriber_imu.cpp ) add_library(publisher_${PROJECT_NAME} - src/publisher_estimated_pose.cpp - src/publisher_current_state.cpp - src/publisher_solve_logs.cpp - # src/publisher_debug_img.cpp + # src/publisher_estimated_pose.cpp + # src/publisher_current_state.cpp + # src/publisher_solve_logs.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/wolf_ros_apriltag_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) target_link_libraries(subscriber_${PROJECT_NAME} ${wolfcore_LIBRARIES} - ${wolfimu_LIBRARIES} ${wolfapriltag_LIBRARIES} ${wolfvision_LIBRARIES} ${vision_utils_LIBRARY} @@ -185,62 +88,7 @@ target_link_libraries(subscriber_${PROJECT_NAME} target_link_libraries(publisher_${PROJECT_NAME} ${wolfcore_LIBRARIES} - ${wolfimu_LIBRARIES} ${wolfapriltag_LIBRARIES} ${wolfvision_LIBRARIES} ${catkin_LIBRARIES} - ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_wolf_ros_apriltag.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + ) \ No newline at end of file diff --git a/include/publisher_current_state.h b/include/publisher_current_state.h deleted file mode 100644 index 5e3da37..0000000 --- a/include/publisher_current_state.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef __PUBLISHER_CURRENT_STATE_H_ -#define __PUBLISHER_CURRENT_STATE_H_ - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherCurrentState: public Publisher -{ -public: - PublisherCurrentState(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherCurrentState); - virtual ~PublisherCurrentState(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherCurrentState) -#endif // __PUBLISHER_CURRENT_STATE_H_ diff --git a/include/publisher_estimated_pose.h b/include/publisher_estimated_pose.h deleted file mode 100644 index ea8d9d4..0000000 --- a/include/publisher_estimated_pose.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef __PUBLISHER_ESTIMATED_POSE_H_ -#define __PUBLISHER_ESTIMATED_POSE_H_ - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherEstimatedPose: public Publisher -{ -public: - PublisherEstimatedPose(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherEstimatedPose); - virtual ~PublisherEstimatedPose(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherEstimatedPose); - -#endif // __PUBLISHER_ESTIMATED_POSE_H_ diff --git a/include/publisher_solve_logs.h b/include/publisher_solve_logs.h deleted file mode 100644 index 43978ae..0000000 --- a/include/publisher_solve_logs.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef __PUBLISHER_SOLVE_LOGS_H_ -#define __PUBLISHER_SOLVE_LOGS_H_ - - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherSolveLogs: public Publisher -{ -public: - PublisherSolveLogs(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherSolveLogs); - virtual ~PublisherSolveLogs(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherSolveLogs) - -#endif // __PUBLISHER_SOLVE_LOGS_H_ diff --git a/include/subscriber_camera.h b/include/subscriber_camera.h index 5bf663d..938b047 100644 --- a/include/subscriber_camera.h +++ b/include/subscriber_camera.h @@ -42,7 +42,6 @@ class SubscriberCamera : public Subscriber bool use_debug_img_; ros::Time t0_cam_, ts_cam_; ros::NodeHandle my_nh_; - int shift_imu_minus_cam_; bool first_time; public: // Constructor diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h deleted file mode 100644 index f7012d7..0000000 --- a/include/subscriber_imu.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef SUBSCRIBER_CAMERA_H_ -#define SUBSCRIBER_CAMERA_H_ -/************************** - * WOLF includes * - **************************/ -#include -#include -#include -#include -#include -#include - -/************************** - * CERES includes * - **************************/ -#include -//#include "glog/logging.h" - -/************************** - * ROS includes * - **************************/ -#include -#include -#include - -/************************** - * STD includes * - **************************/ -#include -#include -#include - -#include -#include -#include "manif/SE3.h" - -#include "subscriber.h" -#include "factory_subscriber.h" - -namespace wolf -{ -class SubscriberImu : public Subscriber -{ - bool finding_orientation_prior_; - // IMU orientation prior - int size_buffer_imu_init_; - int i_imu_buff_; - double std_imu_max_; - std::vector ax_buff_; - std::vector ay_buff_; - std::vector az_buff_; - Eigen::Quaterniond w_q_imu_init_; - bool imu_time_initialized_; - ros::Time t0_imu_; - ros::Time ts_imu_; - double acc_sat_; - double gyr_sat_; - public: - // Constructor - SubscriberImu(const std::string& _unique_name, - const ParamsServer& _server, - const SensorBasePtr _sensor_ptr); - WOLF_SUBSCRIBER_CREATE(SubscriberImu); - - virtual void initialize(ros::NodeHandle& nh, const std::string& topic); - - void callback(const sensor_msgs::Imu &msg); -}; - -WOLF_REGISTER_SUBSCRIBER(SubscriberImu); -} -#endif diff --git a/launch/online.rviz b/launch/online.rviz deleted file mode 100644 index f0157c6..0000000 --- a/launch/online.rviz +++ /dev/null @@ -1,210 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /Grid1 - - /Pose1 - - /MarkerArray1 - - /MarkerArray1/Namespaces1 - - /MarkerArray2 - - /MarkerArray2/Namespaces1 - - /MarkerArray3 - - /MarkerArray3/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 762 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - map: - Value: true - odom: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: true - Tree: - map: - odom: - {} - Update Interval: 0 - Value: true - - Class: rviz/Camera - Enabled: false - Image Rendering: background and overlay - Image Topic: /camera_simu/image_raw - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Visibility: - Grid: true - MarkerArray: true - Pose: true - TF: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /wolf_apriltag_vio/estimated_pose - Unreliable: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /wolf_ros_node/trajectory - Name: MarkerArray - Namespaces: - frames: true - frames_text: false - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /wolf_ros_node/landmarks - Name: MarkerArray - Namespaces: - landmarks: true - landmarks_text: false - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /wolf_ros_node/factors - Name: MarkerArray - Namespaces: - factors_ABS: true - factors_LMK: true - factors_MOTION: true - factors_text: false - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 7.928880214691162 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.5710559487342834 - Y: -0.1793169379234314 - Z: -1.7769840955734253 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.669797420501709 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.735382556915283 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1020 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001a00000033ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000033f000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000031c0000006a0000001800ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000058fc0100000002fb0000000800540069006d00650100000000000007800000028200fffffffb0000000800540069006d00650100000000000004500000000000000000000005da0000033f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/launch/test.launch b/launch/test.launch deleted file mode 100644 index cc501c8..0000000 --- a/launch/test.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - -?> - - - - - -?> - - - - - - - - diff --git a/msg/SolveLogs.msg b/msg/SolveLogs.msg deleted file mode 100644 index 1f09ccb..0000000 --- a/msg/SolveLogs.msg +++ /dev/null @@ -1 +0,0 @@ -float32 duration diff --git a/src/publisher_current_state.cpp b/src/publisher_current_state.cpp deleted file mode 100644 index 741a87f..0000000 --- a/src/publisher_current_state.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "publisher_current_state.h" - - -PublisherCurrentState::PublisherCurrentState(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherCurrentState::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherCurrentState::publishDerived() -{ - - // Eigen::VectorXd cstate = problem_->getCurrentState(); - // nav_msgs::Odometry odom_msg; - // odom_msg.pose.pose.position.x = cstate(0); - // odom_msg.pose.pose.position.y = cstate(1); - // odom_msg.pose.pose.position.z = cstate(2); - // odom_msg.pose.pose.orientation.x = cstate(3); - // odom_msg.pose.pose.orientation.y = cstate(4); - // odom_msg.pose.pose.orientation.z = cstate(5); - // odom_msg.pose.pose.orientation.w = cstate(6); - // if (cstate.size() > 7){ - // odom_msg.twist.twist.linear.x = cstate(7); - // odom_msg.twist.twist.linear.y = cstate(8); - // odom_msg.twist.twist.linear.z = cstate(9); - // } - // publisher_.publish(odom_msg); -} diff --git a/src/publisher_estimated_pose.cpp b/src/publisher_estimated_pose.cpp deleted file mode 100644 index 93848d0..0000000 --- a/src/publisher_estimated_pose.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "publisher_estimated_pose.h" - -#include - -PublisherEstimatedPose::PublisherEstimatedPose(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem): - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherEstimatedPose::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherEstimatedPose::publishDerived() -{ - -} diff --git a/src/publisher_solve_logs.cpp b/src/publisher_solve_logs.cpp deleted file mode 100644 index ab40662..0000000 --- a/src/publisher_solve_logs.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "publisher_solve_logs.h" - -#include - -PublisherSolveLogs::PublisherSolveLogs(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherSolveLogs::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherSolveLogs::publishDerived() -{ - -} diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index 7074bbe..dcf64a1 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -17,7 +17,6 @@ SubscriberCamera::SubscriberCamera(const std::string& _unique_name, first_image_received_ = false; use_debug_img_ = true; t0_cam_ = ts_cam_ = ros::Time(0); - shift_imu_minus_cam_ = 34; first_time = true; } @@ -30,10 +29,9 @@ void SubscriberCamera::initialize(ros::NodeHandle& nh, const std::string& topic) void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) { - // std::cout << "cam_callback ts: " << msg->header.stamp << std::endl; - ros::Time t_start = ros::Time::now(); + auto start = std::chrono::high_resolution_clock::now(); - ts_cam_ = msg->header.stamp + ros::Duration(shift_imu_minus_cam_); + ts_cam_ = msg->header.stamp; if (!first_image_received_){ t0_cam_ = ts_cam_; // Relative timestamps -> first KF at timestamp 0 @@ -63,20 +61,15 @@ void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) nb_without_rot--; } } - WOLF_TRACE(f_list.size(), " tags, nb without ROT: ", nb_without_rot, "/", f_list.size()); } } - // sensor_ptr_->getProblem()->print(4,1,1,1); + auto stopbef = std::chrono::high_resolution_clock::now(); + auto durationbef = std::chrono::duration_cast(stopbef - start); + // std::cout << "APRILTAGBEFORE " << durationbef.count() << std::endl; publishDebugImg(msg, frame); - //////////////////////////// - // Write - // if (use_debug_img_){ - // publishDebugImg(msg, frame); - // } + auto stopaft = std::chrono::high_resolution_clock::now(); + auto durationaft = std::chrono::duration_cast(stopaft - start); - // if (apriltag_only_){ - // broadcastCurrentTf(); - // } } void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg, cv::Mat& frame){ /////////////////////////////// @@ -105,79 +98,76 @@ void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg cv::line(frame, feat_april->getTagCorners()[3], feat_april->getTagCorners()[0], CV_RGB(255, 0, 255), 2); } - /////////////////////////////// - // Draw tag location from estimated keyframes - // Eigen::VectorXd frame_state_bis = last_cap_cam_frame->getState(); // state estimation at the keyframe associated to the capture "last": late and jumping - VectorComposite frame_state_ts = problem_->getState(img_msg->header.stamp.toSec()); // state estimation at the ts of the captured frame: should match exactly the measurements - // Eigen::VectorXd current_state = problem_->getCurrentState(); // processor motion estimated state: in advance to the frame capture - Eigen::Isometry3d wTb = Eigen::Translation3d(frame_state_ts['P']) * Eigen::Quaternions(frame_state_ts['O'].data()); - - Eigen::Matrix6d Qb; - - - // Relative corner poses assuming an apriltag frame at the center of the tag with same oriention than the camera - // when they face each other - // bottom left to top left, direct order - Eigen::Vector3d lXP1(-1, 1,0); - Eigen::Vector3d lXP2( 1, 1,0); - Eigen::Vector3d lXP3( 1,-1,0); - Eigen::Vector3d lXP4(-1,-1,0); - std::vector lXP_vec = {lXP1, lXP2, lXP3, lXP4}; - std::vector cXp_vec; cXp_vec.resize(4); - - auto pos = sensor_ptr_->getP()->getState(); - auto ori = sensor_ptr_->getO()->getState(); - Eigen::Vector7d cam_extrinsics_; - cam_extrinsics_ << pos(0), pos(1), pos(2), ori(0), ori(1), ori(2), ori(3); - Eigen::Isometry3d bTc = Eigen::Translation3d(cam_extrinsics_.head(3)) * Eigen::Quaternions(cam_extrinsics_.segment<4>(3).data()); - - auto sensor_cam_ptr_ = std::dynamic_pointer_cast(sensor_ptr_); - auto cam_kvec_ = sensor_cam_ptr_->getIntrinsic()->getState(); // initialize pinhole camera intrinsics [cx cy fx fy] - auto cam_dvec_ = sensor_cam_ptr_->getDistortionVector(); // initialize pinhole camera intrinsics [cx cy fx fy] - - auto w_ = sensor_cam_ptr_->getImgWidth(); - auto h_ = sensor_cam_ptr_->getImgHeight(); - - for (auto lmk: problem_->getMap()->getLandmarkList()){ - LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); // IF all of the landmarks of this type - VectorComposite w_pose_lmk = lmk_april->getState(); - Eigen::Isometry3d wTl = Eigen::Translation3d(w_pose_lmk['P']) * Eigen::Quaternions(w_pose_lmk['O'].data()); - Eigen::Isometry3d cTl = (wTb * bTc).inverse() * wTl; - double s = lmk_april->getTagWidth()/2; // should not be very important -> proportional points in cam coord project at the same place... - cXp_vec[0] = cTl * (lXP1 * s); // if no parenthesis, not the same result -> why? - cXp_vec[1] = cTl * (lXP2 * s); - cXp_vec[2] = cTl * (lXP3 * s); - cXp_vec[3] = cTl * (lXP4 * s); - - - for (int i=0; i < cXp_vec.size(); i++){ - // drawCornerWithUncertainties(cXp_vec[i], cTl, (wTb * bTc).inverse(), frame); - ////////////////////////// - ////////////////////////// - // Draw corners with uncertainty - ////////////////////////// - - // Compute projected point and abort if not visible (Z>O and in the img boundaries) - if (!(cXp_vec[i][2] > 0)){ - return; - } - Eigen::Vector2d px; - Eigen::Matrix J_x_cXp; - pinhole::projectPoint(cam_kvec_, cam_dvec_, cXp_vec[i], px, J_x_cXp); - if (!pinhole::isInImage(px, w_, h_)){ - return; - } - - ///////////////////////// - // Draw projected estimated point projection - // std::cout << "--No Problem " << lmk_april->getTagId() << ": " << pcorn.transpose() << std::endl; - // visible_corners_vec.push_back() - cv::Point2d cv_corner; - cv_corner.x = px[0]; - cv_corner.y = px[1]; - cv::line(frame, cv_corner, cv_corner, CV_RGB(255,165,0), 4); - } - } + // /////////////////////////////// + // // Draw tag location from estimated keyframes + // // Eigen::VectorXd frame_state_bis = last_cap_cam_frame->getState(); // state estimation at the keyframe associated to the capture "last": late and jumping + // VectorComposite frame_state_ts = problem_->getState(img_msg->header.stamp.toSec()); // state estimation at the ts of the captured frame: should match exactly the measurements + // // Eigen::VectorXd current_state = problem_->getCurrentState(); // processor motion estimated state: in advance to the frame capture + // Eigen::Isometry3d wTb = Eigen::Translation3d(frame_state_ts['P']) * Eigen::Quaternions(frame_state_ts['O'].data()); + + // // Relative corner poses assuming an apriltag frame at the center of the tag with same oriention than the camera + // // when they face each other + // // bottom left to top left, direct order + // Eigen::Vector3d lXP1(-1, 1,0); + // Eigen::Vector3d lXP2( 1, 1,0); + // Eigen::Vector3d lXP3( 1,-1,0); + // Eigen::Vector3d lXP4(-1,-1,0); + // std::vector lXP_vec = {lXP1, lXP2, lXP3, lXP4}; + // std::vector cXp_vec; cXp_vec.resize(4); + + // auto pos = sensor_ptr_->getP()->getState(); + // auto ori = sensor_ptr_->getO()->getState(); + // Eigen::Vector7d cam_extrinsics_; + // cam_extrinsics_ << pos(0), pos(1), pos(2), ori(0), ori(1), ori(2), ori(3); + // Eigen::Isometry3d bTc = Eigen::Translation3d(cam_extrinsics_.head(3)) * Eigen::Quaternions(cam_extrinsics_.segment<4>(3).data()); + + // auto sensor_cam_ptr_ = std::dynamic_pointer_cast(sensor_ptr_); + // auto cam_kvec_ = sensor_cam_ptr_->getIntrinsic()->getState(); // initialize pinhole camera intrinsics [cx cy fx fy] + // auto cam_dvec_ = sensor_cam_ptr_->getDistortionVector(); // initialize pinhole camera intrinsics [cx cy fx fy] + + // auto w_ = sensor_cam_ptr_->getImgWidth(); + // auto h_ = sensor_cam_ptr_->getImgHeight(); + + // for (auto lmk: problem_->getMap()->getLandmarkList()){ + // LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); // IF all of the landmarks of this type + // VectorComposite w_pose_lmk = lmk_april->getState(); + // Eigen::Isometry3d wTl = Eigen::Translation3d(w_pose_lmk['P']) * Eigen::Quaternions(w_pose_lmk['O'].data()); + // Eigen::Isometry3d cTl = (wTb * bTc).inverse() * wTl; + // double s = lmk_april->getTagWidth()/2; // should not be very important -> proportional points in cam coord project at the same place... + // cXp_vec[0] = cTl * (lXP1 * s); // if no parenthesis, not the same result -> why? + // cXp_vec[1] = cTl * (lXP2 * s); + // cXp_vec[2] = cTl * (lXP3 * s); + // cXp_vec[3] = cTl * (lXP4 * s); + + + // for (int i=0; i < cXp_vec.size(); i++){ + // // drawCornerWithUncertainties(cXp_vec[i], cTl, (wTb * bTc).inverse(), frame); + // ////////////////////////// + // ////////////////////////// + // // Draw corners with uncertainty + // ////////////////////////// + + // // Compute projected point and abort if not visible (Z>O and in the img boundaries) + // if (!(cXp_vec[i][2] > 0)){ + // return; + // } + // Eigen::Vector2d px; + // Eigen::Matrix J_x_cXp; + // pinhole::projectPoint(cam_kvec_, cam_dvec_, cXp_vec[i], px, J_x_cXp); + // if (!pinhole::isInImage(px, w_, h_)){ + // return; + // } + + // ///////////////////////// + // // Draw projected estimated point projection + // // std::cout << "--No Problem " << lmk_april->getTagId() << ": " << pcorn.transpose() << std::endl; + // // visible_corners_vec.push_back() + // cv::Point2d cv_corner; + // cv_corner.x = px[0]; + // cv_corner.y = px[1]; + // cv::line(frame, cv_corner, cv_corner, CV_RGB(255,165,0), 4); + // } + // } cv_bridge::CvImage out_msg; out_msg.header = img_msg->header; diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp deleted file mode 100644 index b2940ff..0000000 --- a/src/subscriber_imu.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "subscriber_imu.h" -#include - -namespace wolf -{ -// Constructor -SubscriberImu::SubscriberImu(const std::string& _unique_name, - const ParamsServer& _server, - const SensorBasePtr _sensor_ptr) - : Subscriber(_unique_name, _server, _sensor_ptr) -{ - finding_orientation_prior_ = true; - i_imu_buff_ = 0; - size_buffer_imu_init_ = 100; - imu_time_initialized_ = false; - acc_sat_ = 3; - gyr_sat_ = 3; - ax_buff_ = std::vector(size_buffer_imu_init_); - ay_buff_ = std::vector(size_buffer_imu_init_); - az_buff_ = std::vector(size_buffer_imu_init_); -} - - -void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - sub_ = nh.subscribe(topic, 100, &SubscriberImu::callback, this); -} - -void SubscriberImu::callback(const sensor_msgs::Imu &msg) -{ - auto sen_imu_ptr = std::static_pointer_cast(sensor_ptr_); - ////////////// - // INIT ORIENTATION - if (finding_orientation_prior_){ - if (i_imu_buff_ == size_buffer_imu_init_){ - double ax_mean = std::accumulate(ax_buff_.begin(), ax_buff_.end(), 0.0)/ax_buff_.size(); - double ay_mean = std::accumulate(ay_buff_.begin(), ay_buff_.end(), 0.0)/ay_buff_.size(); - double az_mean = std::accumulate(az_buff_.begin(), az_buff_.end(), 0.0)/az_buff_.size(); - - double ax_std = std::sqrt(std::inner_product(ax_buff_.begin(), ax_buff_.end(), ax_buff_.begin(), 0.0)/size_buffer_imu_init_ - ax_mean*ax_mean); - double ay_std = std::sqrt(std::inner_product(ay_buff_.begin(), ay_buff_.end(), ay_buff_.begin(), 0.0)/size_buffer_imu_init_ - ay_mean*ay_mean); - double az_std = std::sqrt(std::inner_product(az_buff_.begin(), az_buff_.end(), az_buff_.begin(), 0.0)/size_buffer_imu_init_ - az_mean*az_mean); - - i_imu_buff_ = 0; - - double min_acc_std = std::min({ax_std, ay_std, az_std}); - if (min_acc_std < std_imu_max_){ - std::cout << "min_acc_std < " << std_imu_max_ << ": " << min_acc_std << std::endl; - // Steady enough estimation - Eigen::Vector3d w_g_norm(0,0,1); - Eigen::Vector3d imu_g(ax_mean, ay_mean, az_mean); - imu_g.normalize(); - double ctheta = w_g_norm.dot(imu_g); - double theta = std::acos(ctheta); - Eigen::Vector3d v = imu_g.cross(w_g_norm); - // handle parallel vector case - if (v.norm() > 1e-6){ - Eigen::AngleAxisd aa(theta, v/v.norm()); - w_q_imu_init_ = Eigen::Quaterniond(aa); - } - else{ - if (ctheta > 0){ - w_q_imu_init_ = Eigen::Quaterniond(1,0,0,0); - } - else{ - w_q_imu_init_ = Eigen::Quaterniond(0,1,0,0); - } - } - // Could put zero rotation on Z "yaw" axis - // sol: q2e then yaw=0 then e2q, but not necessary - - finding_orientation_prior_ = false; - - std::cout << "Norm: " << w_q_imu_init_.norm() << "\nOrientation prior found: \n" << w_q_imu_init_.coeffs().transpose() << std::endl; - } - else { - std::cout << "\nmin_acc_std not > " << std_imu_max_ << ": " << min_acc_std << std::endl; - std::cout << "Orientation prior not found" << std::endl; - } - - } - else { - ax_buff_[i_imu_buff_] = msg.linear_acceleration.x; - ay_buff_[i_imu_buff_] = msg.linear_acceleration.y; - az_buff_[i_imu_buff_] = msg.linear_acceleration.z; - i_imu_buff_++; - } - - - return; - } - ////////////// - - //Removed first image received check - if (!imu_time_initialized_){ - t0_imu_ = msg.header.stamp; //msg.header.stamp; - WOLF_TRACE("t0_imu_: ", t0_imu_); - imu_time_initialized_ = true; - } - // Removed priors set check - // WOLF_TRACE("msg.linear_acceleration: ", msg.linear_acceleration); - - // Process IMU - ts_imu_ = msg.header.stamp; - - Eigen::Vector6d acc_gyro_data; - Eigen::Vector3d acc; acc << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z; - Eigen::Vector3d ang_vel; ang_vel << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z; - acc_gyro_data << acc, ang_vel; - - // std::cout << "\n\n --- acc_gyro_data: " << acc_gyro_data.transpose() << std::endl; - // std::cout << "ts: " << (ts_imu_ - t0_imu_).toSec() << std::endl; - - auto noise_cov = sen_imu_ptr->getNoiseCov(); - if ((fabs(acc(0)) > acc_sat_) || (fabs(acc(1)) > acc_sat_) || (fabs(acc(2)) >acc_sat_)){ - noise_cov.block(0,0,3,3) = noise_cov.block(0,0,3,3)*10000000; - } - if ((fabs(ang_vel(0)) > gyr_sat_) || (fabs(ang_vel(1)) > gyr_sat_) || (fabs(ang_vel(2)) > gyr_sat_)){ - noise_cov.block(3,3,3,3) = noise_cov.block(3,3,3,3)*10000000; - } - wolf::CaptureImuPtr capt_imu = std::make_shared((ts_imu_ - t0_imu_).toSec(), sen_imu_ptr, acc_gyro_data, - noise_cov, sen_imu_ptr->getIntrinsic()->getState(), - nullptr); - sen_imu_ptr->process(capt_imu); - - // auto fact = problem_->getTrajectory()->getLastKeyFrame()->getCaptureOf(sen_imu_ptr)->getFeatureList().front()->getFactorList().front(); - // auto fact_imu = std::static_pointer_cast(fact); - - // std::cout << "imu_callback ts: " << msg.header.stamp << ", bsize " << bsize << std::endl; - - // std::cout << "current_state_: " << current_state_.transpose() << std::endl; - -} -} diff --git a/yaml/apriltag_vio.yaml b/yaml/apriltag_vio.yaml index 909da6c..25740ab 100644 --- a/yaml/apriltag_vio.yaml +++ b/yaml/apriltag_vio.yaml @@ -1,32 +1,37 @@ +############################ +######### EXAMPLE ########## +############################ + config: + + profiling: + enabled: true + file_name: "~/wolf_demo_apriltag_profiling.txt" problem: - tree_manager: - follow: "treeconfig.yaml" + tree_manager: + type: "none" frame_structure: "POV" - # frame_structure: "PO" dimension: 3 prior: mode: "factor" $state: P: [0,0,0] - O: [1,0,0,0] + O: [0,0,0,1] V: [0,0,0] $sigma: - P: [0.31, 0.31, 0.31] - O: [0.31, 0.31, 0.31] - V: [0.31, 0.31, 0.31] - time_tolerance: 0.01 + P: [0.001, 0.001, 0.001] + O: [0.1, 0.1, 0.1] + V: [0.01, 0.01, 0.01] + time_tolerance: 0.001 solver: max_num_iterations: 100 - verbose: 0 - period: 0.2 + verbose: 1 + period: 0.3 update_immediately: false - - visualizer: - type: "Visualizer" - period: 0.01 + n_threads: 2 + compute_cov: false sensors: - @@ -34,23 +39,23 @@ config: name: "CAMERA" plugin: "vision" extrinsic: - pose: [0,0,0, 0,0,0,1] # simu + pose: [0,0,0, 0,0,0,1] # Simu (e.g. circle.bag) follow: "camera_simu.yaml" - # follow: "camera_simu_wolf_standard.yaml" - type: "SensorImu" name: "IMU" plugin: "imu" extrinsic: - pose: [0,0,0, 0,0,0,1] # mandatory with curent imu implementation + pose: [0,0,0, 0,0,0,1] follow: "imu_intrinsics.yaml" + processors: - type: "ProcessorTrackerLandmarkApriltag" name: "APRILTAG PROC" sensor_name: "CAMERA" plugin: "apriltag" - time_tolerance: 0.01 + time_tolerance: 0.001 follow: "processor_tracker_landmark_apriltag.yaml" - type: "ProcessorImu" @@ -67,12 +72,32 @@ config: topic: "/camera_simu/image_raw" sensor_name: "CAMERA" - - package: "wolf_ros_apriltag" + package: "wolf_ros_imu" type: "SubscriberImu" topic: "/imu/imu_raw" sensor_name: "IMU" + imu_x_axis: 1 + imu_y_axis: 2 + imu_z_axis: 3 + cov_source: "sensor" + in_degrees: false + + ROS publisher: + - + type: "PublisherGraph" + topic: "graph" + package: "wolf_ros_node" + period: 0.1 + - + type: "PublisherTf" + topic: " " + package: "wolf_ros_node" + period: 0.1 + map_frame_id: "map" + odom_frame_id: "odom" + base_frame_id: "base_footprint" + publish_odom_tf: false - # ROS publisher: # - # package: "wolf_ros_apriltag" # type: "PublisherCurrentState" @@ -94,30 +119,4 @@ config: # topic: "/wolf_pose" # period: "5" - ROS publisher: - - - package: "wolf_ros_node" - type: "PublisherPose" - topic: "/wolf_pose" - period: "0.5" - pose_array_msg: true - marker_msg: false - extrinsics: false - frame_id: "base" - - - package: "wolf_ros_node" - type: "PublisherOdom" - topic: "/wolf_odom" - period: "0.5" - pose_array_msg: true - marker_msg: false - extrinsics: true - sensor: "IMU" - frame_id: "base" - - ROS settings: - read_odom_tf: true - - follow: "params.yaml" - # follow: "config.yaml" -# raw_libs: "@raw_libs.yaml" + use_debug_img: true diff --git a/yaml/camera_simu_wolf_standard.yaml b/yaml/camera_simu_wolf_standard.yaml deleted file mode 100644 index ec7d5f3..0000000 --- a/yaml/camera_simu_wolf_standard.yaml +++ /dev/null @@ -1,5 +0,0 @@ -width: 640 -height: 480 -pinhole_model_raw: [320, 240, 320, 320] -pinhole_model_rectified: [320, 240, 320, 320] -distortion: [0, 0, 0, 0] diff --git a/yaml/imu_intrinsics.yaml b/yaml/imu_intrinsics.yaml deleted file mode 100644 index d5b1ef6..0000000 --- a/yaml/imu_intrinsics.yaml +++ /dev/null @@ -1,16 +0,0 @@ -type: "imu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. -name: "IMU" # This is ignored. The name provided to the SensorFactory prevails -a_noise: 0.02 # standard deviation of Acceleration noise (same for all the axis) in m/s2 -w_noise: 0.03 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec -ab_initial_stdev: 0.00 # m/s2 - initial bias -wb_initial_stdev: 0.0 # rad/sec - initial bias -ab_rate_stdev: 0.0001 # m/s2/sqrt(s) -wb_rate_stdev: 0.0001 # rad/s/sqrt(s) - - # SIMU - # a_noise: 0.001 # standard deviation of Acceleration noise (same for all the axis) in m/s2 - # w_noise: 0.01 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec - # ab_initial_stdev: 0.00 # m/s2 - initial bias - # wb_initial_stdev: 0.00 # rad/sec - initial bias - # ab_rate_stdev: 0.001 # m/s2/sqrt(s) - # wb_rate_stdev: 0.001 # rad/s/sqrt(s) diff --git a/yaml/params.yaml b/yaml/params.yaml deleted file mode 100644 index 97121ec..0000000 --- a/yaml/params.yaml +++ /dev/null @@ -1,45 +0,0 @@ -apriltag_only: false # %% -# if the IMU is used, find a prior on the original orientation of the IMU based on measured gravity -finding_orientation_prior: false # %% -constrained_by_motion_factor_to_solve_activated: false - -# Map -# if true, uses a map with fixed landmarks -usemap: false -map_folder_path: /home/mfourmy/Documents/Phd_LAAS/tests/VIplots/test_apriltag_imu/ -map_name: map_111.000000_111.000000_5_2.yaml - - -# Display -use_debug_img: true -draw_detections: true -draw_estimations: false # to test again -draw_ellipses: false # not implemented -display_scaled_tf: 1 # to overcome the frame scale limitations of rviz, 1 to cancel - -# Topics -imu_raw_topic: /imu/imu_raw -current_frame: current - -# sensors -shift_imu_minus_cam: 0.0 -acc_sat: 49 # m/s^2 -gyr_sat: 3.92 # rad/s - -prior_vec: [0.0,0.0,0.0, 0,0,0,1, 0,0,0] # $$ -# prior_vec: [0.0,0.0,1.7, 1,0,0,0, 0,0,0] # $$ compact setup, lying on table -# prior_vec: [0,0,0, 0.963449,-0.000121,-0.066758,0.259439, 0,0,0] # On the robot, IMU with Z down -# prior_vec: [0,0,0, 0,0.999982,0,0.00603841, 0,0,0] # $$ -# prior_vec: [0.0,0.5,1.1, 1,0,0,0, -0.189,0,0] # vx for Ax = 0.3 $$ -# prior_vec: [0.0,0,0.6, 0.5,0,0,0.8660254, 0,0,0] # vx for Ax = 0.3, tilted 181° on X $$ -# prior_vec: [0.0,0.6,1.1, 1,0,0,0, -0.18,0,0] # simu with everything $$ -# prior_vec: [0,0,0, 0.7071068,0,0,0.7071068, 0,0,0] # cam looking straight $$ -# prior_vec: [0,0,0, 0.7071068,0,0.7071068,0, 0,0,0] # cam +90° rotation around its Z axis $$ - -std_m: 0.001 -std_deg: 5 -std_vel: 0.1 - -# Orientation prior using IMU -size_buffer_imu_init: 20 -std_imu_max: 0.4 # if std (m/s^2) of the data recorded below a certain level, \ No newline at end of file diff --git a/yaml/processor_imu.yaml b/yaml/processor_imu.yaml deleted file mode 100644 index c852bcf..0000000 --- a/yaml/processor_imu.yaml +++ /dev/null @@ -1,9 +0,0 @@ -time_tolerance: 0.01 # Time tolerance for joining KFs -unmeasured_perturbation_std: 0.00000001 -keyframe_vote: - voting_active: false - voting_aux_active: false - max_time_span: 1.0 # seconds - max_buff_length: 500 # motion deltas - dist_traveled: 2.0 # meters - angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml index aa65a48..dd498ce 100644 --- a/yaml/processor_tracker_landmark_apriltag.yaml +++ b/yaml/processor_tracker_landmark_apriltag.yaml @@ -1,5 +1,7 @@ -# type: "ProcessorTrackerLandmarkApriltag" -# name: "tracker landmark apriltag example" +############################ +######### EXAMPLE ########## +############################ + # detector parameters: quad_decimate: 0.0 # doing quad detection at lower resolution to speed things up (see end of file) @@ -10,12 +12,9 @@ refine_edges: true # better edge detection if quad_decimate > 1 (quite ippe_min_ratio: 0 # quite arbitrary, always > 1 (to deactive, set at something < 1) ippe_max_rep_error: 100 # to deactivate, set at something big (100) +# specific tag widths if using tags of different sizes $tag_widths: -1: 0.0 - # 0: 0.055 - # 1: 0.055 - # 2: 0.055 - # 3: 0.055 tag_family: "tag36h11" # see in Wolf processor_tracker_landmark_apriltag.cpp for available families tag_black_border: 1 # usually 1, 2 for Kalibr aprilgrid for ex @@ -25,26 +24,21 @@ tag_width_default: 0.2 # used if tag width not specified -- Simu $$ # tag_width_default: 0.166 # used if tag width not specified -- LAAS (B181) $$ # tag_width_default: 0.162 # used if tag width not specified -- LAAS, 04/07 $$ -# noise: -std_xy : 0.1 # m -std_z : 0.1 # m -# std_rpy_degrees : 5 # degrees -std_rpy: 5 # degrees +# used for covariance propagation std_pix: 4 # pixel error $$ -# std_pix: 10000000000000000 # pixel error -> "infinite" error $$ +# dummy covariances (not used but necessary for yaml parser) +std_xy : 0.1 # m +std_z : 0.1 # m +std_rpy: 0.1 # radians keyframe_vote: voting_active: true - voting_aux_active: false -nb_vote_for_every_first: 5 -min_time_vote: 0.2 # s -# Necessary conditions -max_time_vote: 0.5 # s -# One of the possibilities -enough_info_necessary: false # create kf if enough information to uniquely determine the KF position (recommended for apriltag_only slam) %% -min_features_for_keyframe: 1 # ! closely related to enough_info_necessary (see code) -max_features_diff: 4 - + nb_vote_for_every_first: 10 + min_time_vote: 0.05 # s + # Necessary conditions + max_time_vote: 0.3 # s + # One of the possibilities + min_features_for_keyframe: 1 reestimate_last_frame: false # for a better prior on the new keyframe: use only if no motion processor add_3d_cstr: false # add 3D constraints between the KF so that they do not jump when using apriltag only diff --git a/yaml/simple.yaml b/yaml/simple.yaml deleted file mode 100644 index bbc4591..0000000 --- a/yaml/simple.yaml +++ /dev/null @@ -1,34 +0,0 @@ -config: - - problem: - tree_manager: - # follow: "treeconfig.yaml" - frame_structure: "PO" - dimension: 3 - prior: - mode: "fix" - state: [0,0,0] - time_tolerance: 0.1 - #mode: "factor" - #state: [0,0,0] - #cov: [[3,3],.1,0,0,0,.1,0,0,0,.1] - #time_tolerance: 0.1 - - solver: - max_num_iterations: 100 - verbose: 1 - period: 0.3 - - sensors: - processors: - - # ROS subscriber: - # - - # package: "wolf_ros_laser" - # type: "SubscriberLaser2d" - # topic: "/base_scan" - # sensor_name: "scanner_front_left" - - follow: "params.yaml" - follow: "config.yaml" -# raw_libs: "@raw_libs.yaml" diff --git a/yaml/treeconfig.yaml b/yaml/treeconfig.yaml deleted file mode 100644 index da86393..0000000 --- a/yaml/treeconfig.yaml +++ /dev/null @@ -1,4 +0,0 @@ -type: "TreeManagerSlidingWindow" -n_key_frames: 25 -fix_first_key_frame: true -viral_remove_empty_parent: true diff --git a/yaml/visualizer.yaml b/yaml/visualizer.yaml deleted file mode 100644 index 4be802e..0000000 --- a/yaml/visualizer.yaml +++ /dev/null @@ -1,45 +0,0 @@ -viz_factors: true -viz_overlapped_factors: false -viz_landmarks: true -viz_trajectory: true - -# viz parameters -map_frame_id: "map" -viz_scale: 1 -factors_width: 0.002 -factors_absolute_height: 20 -landmark_text_z_offset: 0.1 - -landmark_width: 0.01 -landmark_length: 0.2 -frame_width: 0.01 -frame_length: 0.001 - -frame_color_rr: 1.0 -frame_color_gg: 0.8 -frame_color_bb: 0.0 -frame_color_aa: 1 - -factor_abs_color_rr: 0.92 -factor_abs_color_gg: 0.19 -factor_abs_color_bb: 0.6 -factor_abs_color_aa: 1 - -factor_motion_color_rr: 1.0 -factor_motion_color_gg: 1.0 -factor_motion_color_bb: 0.0 -factor_motion_color_aa: 1 - -factor_loop_color_rr: 1.0 -factor_loop_color_gg: 0.0 -factor_loop_color_bb: 0.0 -factor_loop_color_aa: 1 -factor_lmk_color_rr: 0.0 -factor_lmk_color_gg: 0.0 -factor_lmk_color_bb: 1.0 -factor_lmk_color_aa: 1 - -factor_geom_color_rr: 0.0 -factor_geom_color_gg: 1.0 -factor_geom_color_bb: 1.0 -factor_geom_color_aa: 1 -- GitLab From 41478e45d3b50fa7244dd0646f910640c148062c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= Date: Fri, 12 Feb 2021 19:12:51 +0100 Subject: [PATCH 13/32] Remove msg from cmake file --- CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 40bdaa0..b5ca473 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,12 +25,6 @@ find_package(wolfcore REQUIRED) find_package(wolfvision REQUIRED) find_package(wolfapriltag REQUIRED) -## Generate messages in the 'msg' folder -add_message_files( - FILES - SolveLogs.msg -) - ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES -- GitLab From 002bd499de00ae384d415bd82803cb5f4555eb95 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Thu, 2 Dec 2021 10:54:25 +0100 Subject: [PATCH 14/32] Update .gitlab-ci.yml file --- .gitlab-ci.yml | 235 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 235 insertions(+) create mode 100644 .gitlab-ci.yml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..9669b06 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,235 @@ +stages: + - license + - build_and_test + - demos + +############ YAML ANCHORS ############ +.preliminaries_template: &preliminaries_definition + ## Install ssh-agent if not already installed, it is required by Docker. + ## (change apt-get to yum if you use an RPM-based image) + - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )' + + ## Run ssh-agent (inside the build environment) + - eval $(ssh-agent -s) + + ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store + ## We're using tr to fix line endings which makes ed25519 keys work + ## without extra base64 encoding. + ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556 + - mkdir -p ~/.ssh + - chmod 700 ~/.ssh + - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null + # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts + - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts + + # update apt + - apt-get update + + # create ci_deps folder (if not exists) + - mkdir -pv ci_deps + +.license_header_template: &license_header_definition + - cd $CI_PROJECT_DIR + + # configure git + - export CI_NEW_BRANCH=ci_processing$RANDOM + - echo creating new temporary branch... $CI_NEW_BRANCH + - git config --global user.email "${CI_EMAIL}" + - git config --global user.name "${CI_USERNAME}" + - git checkout -b $CI_NEW_BRANCH # temporary branch + + # license headers + - export CURRENT_YEAR=$( date +'%Y' ) + - echo "current year:" ${CURRENT_YEAR} + - if [ -f license_header_${CURRENT_YEAR}.txt ]; then + # add license headers to new files + - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..." + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps + - else + # update license headers of all files + - export PREV_YEAR=$(( CURRENT_YEAR-1 )) + - echo "Creating new file license_header_${CURRENT_YEAR}.txt..." + - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt + - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps + - fi + + # push changes (if any) + - if git commit -a -m "[skip ci] license headers added or modified" ; then + - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git" + - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME} + - else + - echo "No changes, nothing to commit!" + - fi + +.install_wolf_template: &install_wolf_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d wolf ]; then + - echo "directory wolf exists" + - cd wolf + - git pull + - git checkout $WOLF_CORE_BRANCH + - else + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git + - cd wolf + - git checkout $WOLF_CORE_BRANCH + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - make -j$(nproc) + - ctest -j$(nproc) + - make install + +.install_visionutils_template: &install_visionutils_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d vision_utils ]; then + - echo "directory vision_utils exists" + - cd vision_utils + - git pull + - else + - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/vision_utils.git + - cd vision_utils + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release .. + - make -j$(nproc) + - ctest -j$(nproc) + - make install + +.install_wolfvision_template: &install_wolfvision_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d vision ]; then + - echo "directory vision exists" + - cd vision + - git pull + - git checkout $WOLF_VISION_BRANCH + - else + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git + - cd vision + - git checkout $WOLF_VISION_BRANCH + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - make -j$(nproc) + - ctest -j$(nproc) + - make install + +.install_apriltag_template: &install_apriltag_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d apriltaglib ]; then + - echo "directory apriltaglib exists" + - cd apriltaglib + - git pull + - else + - git clone https://github.com/AprilRobotics/apriltag apriltaglib + - cd apriltaglib + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release .. + - make -j$(nproc) + - make install + +.install_wolfapriltag_template: &install_wolfapriltag_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d apriltag ]; then + - echo "directory apriltag exists" + - cd apriltag + - git pull + - git checkout $WOLF_APRILTAG_BRANCH + - else + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag.git + - cd apriltag + - git checkout $WOLF_APRILTAG_BRANCH + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - make -j$(nproc) + - ctest -j$(nproc) + - make install + +.clone_wolfrosnode_template: &clone_wolfrosnode_definition + - echo "TODO clone wolf_ros_node branch ${WOLF_ROS_CORE_BRANCH}" + +.build_and_test_template: &build_and_test_definition + - echo "TODO" + +############ LICENSE HEADERS ############ +license_headers: + stage: license + image: labrobotica/wolf_deps:16.04 + cache: + - key: wolf-xenial + paths: + - ci_deps/wolf/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *license_header_definition + +############ UBUNTU 16.04 TEST ############ +build_and_test:xenial: + stage: build_and_test + image: labrobotica/wolf_deps:16.04 + cache: + - key: wolf-xenial + paths: + - ci_deps/wolf/ + - key: imu-xenial + paths: + - ci_deps/imu/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + - *install_visionutils_definition + - *install_wolfvision_definition + - *install_apriltag_definition + - *install_wolfapriltag_definition + - *clone_wolfrosnode_definition + script: + - *build_and_test_definition + +############ UBUNTU 18.04 TEST ############ +build_and_test:bionic: + stage: build_and_test + image: labrobotica/wolf_deps:18.04 + cache: + - key: wolf-bionic + paths: + - ci_deps/wolf/ + - key: imu-bionic + paths: + - ci_deps/imu/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + - *install_visionutils_definition + - *install_wolfvision_definition + - *install_apriltag_definition + - *install_wolfapriltag_definition + - *clone_wolfrosnode_definition + script: + - *build_and_test_definition + +############ RUN DEMOS ############ +demo_apriltag: + stage: demos + variables: + WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH + WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH + WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH + WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH + WOLF_ROS_APRILTAG_BRANCH: $CI_COMMIT_BRANCH + trigger: + project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_apriltag -- GitLab From 5b2ce466954b4bbeb034a54421c157ba78c23cd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Thu, 2 Dec 2021 10:57:49 +0100 Subject: [PATCH 15/32] Update .gitlab-ci.yml file --- .gitlab-ci.yml | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 9669b06..6e7ce4a 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -182,9 +182,15 @@ build_and_test:xenial: - key: wolf-xenial paths: - ci_deps/wolf/ - - key: imu-xenial + - key: visionutils-xenial paths: - - ci_deps/imu/ + - ci_deps/vision_utils/ + - key: vision-xenial + paths: + - ci_deps/vision/ + - key: apriltaglib-xenial + paths: + - ci_deps/apriltaglib/ except: - master before_script: @@ -206,9 +212,15 @@ build_and_test:bionic: - key: wolf-bionic paths: - ci_deps/wolf/ - - key: imu-bionic + - key: visionutils-bionic + paths: + - ci_deps/vision_utils/ + - key: vision-bionic + paths: + - ci_deps/vision/ + - key: apriltaglib-bionic paths: - - ci_deps/imu/ + - ci_deps/apriltaglib/ except: - master before_script: -- GitLab From d81c9c17b35f6f12066b27603ab33a32977b5c12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Thu, 2 Dec 2021 14:20:27 +0100 Subject: [PATCH 16/32] Update .gitlab-ci.yml file --- .gitlab-ci.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 6e7ce4a..cac149b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -201,6 +201,7 @@ build_and_test:xenial: - *install_apriltag_definition - *install_wolfapriltag_definition - *clone_wolfrosnode_definition + - ldconfig script: - *build_and_test_definition @@ -231,6 +232,7 @@ build_and_test:bionic: - *install_apriltag_definition - *install_wolfapriltag_definition - *clone_wolfrosnode_definition + - ldconfig script: - *build_and_test_definition -- GitLab From 8341aada910a8ee2a6e1e90a02dc567f055b12e5 Mon Sep 17 00:00:00 2001 From: joan Date: Wed, 22 Dec 2021 16:47:22 +0100 Subject: [PATCH 17/32] CI:fixing and not building tests --- .gitlab-ci.yml | 68 +++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 56 insertions(+), 12 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index cac149b..3806f88 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -28,6 +28,10 @@ stages: # create ci_deps folder (if not exists) - mkdir -pv ci_deps + # manually source ros setup.bash + - source /root/catkin_ws/devel/setup.bash + - roscd # check that it works + .license_header_template: &license_header_definition - cd $CI_PROJECT_DIR @@ -67,6 +71,7 @@ stages: - if [ -d wolf ]; then - echo "directory wolf exists" - cd wolf + - git checkout devel - git pull - git checkout $WOLF_CORE_BRANCH - else @@ -76,9 +81,8 @@ stages: - fi - mkdir -pv build - cd build - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF .. - make -j$(nproc) - - ctest -j$(nproc) - make install .install_visionutils_template: &install_visionutils_definition @@ -95,7 +99,6 @@ stages: - cd build - cmake -DCMAKE_BUILD_TYPE=release .. - make -j$(nproc) - - ctest -j$(nproc) - make install .install_wolfvision_template: &install_wolfvision_definition @@ -103,6 +106,7 @@ stages: - if [ -d vision ]; then - echo "directory vision exists" - cd vision + - git checkout devel - git pull - git checkout $WOLF_VISION_BRANCH - else @@ -112,9 +116,8 @@ stages: - fi - mkdir -pv build - cd build - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF .. - make -j$(nproc) - - ctest -j$(nproc) - make install .install_apriltag_template: &install_apriltag_definition @@ -138,6 +141,7 @@ stages: - if [ -d apriltag ]; then - echo "directory apriltag exists" - cd apriltag + - git checkout devel - git pull - git checkout $WOLF_APRILTAG_BRANCH - else @@ -147,21 +151,30 @@ stages: - fi - mkdir -pv build - cd build - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF .. - make -j$(nproc) - - ctest -j$(nproc) - make install .clone_wolfrosnode_template: &clone_wolfrosnode_definition - - echo "TODO clone wolf_ros_node branch ${WOLF_ROS_CORE_BRANCH}" + - roscd + - cd ../src + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git + - cd wolf_ros_node + - git checkout $WOLF_ROS_CORE_BRANCH .build_and_test_template: &build_and_test_definition - - echo "TODO" + - roscd + - cd ../src + - git clone ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git + - cd wolf_ros_apriltag + - git checkout $CI_COMMIT_BRANCH + - cd ../.. + - catkin_make ############ LICENSE HEADERS ############ license_headers: stage: license - image: labrobotica/wolf_deps:16.04 + image: labrobotica/wolf_vision_deps_ros:16.04 cache: - key: wolf-xenial paths: @@ -177,7 +190,7 @@ license_headers: ############ UBUNTU 16.04 TEST ############ build_and_test:xenial: stage: build_and_test - image: labrobotica/wolf_deps:16.04 + image: labrobotica/wolf_vision_deps_ros:16.04 cache: - key: wolf-xenial paths: @@ -208,7 +221,7 @@ build_and_test:xenial: ############ UBUNTU 18.04 TEST ############ build_and_test:bionic: stage: build_and_test - image: labrobotica/wolf_deps:18.04 + image: labrobotica/wolf_vision_deps_ros:18.04 cache: - key: wolf-bionic paths: @@ -236,6 +249,37 @@ build_and_test:bionic: script: - *build_and_test_definition +############ UBUNTU 20.04 TEST ############ +build_and_test:focal: + stage: build_and_test + image: labrobotica/wolf_vision_deps_ros:20.04 + cache: + - key: wolf-focal + paths: + - ci_deps/wolf/ + - key: visionutils-focal + paths: + - ci_deps/vision_utils/ + - key: vision-focal + paths: + - ci_deps/vision/ + - key: apriltaglib-focal + paths: + - ci_deps/apriltaglib/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + - *install_visionutils_definition + - *install_wolfvision_definition + - *install_apriltag_definition + - *install_wolfapriltag_definition + - *clone_wolfrosnode_definition + - ldconfig + script: + - *build_and_test_definition + ############ RUN DEMOS ############ demo_apriltag: stage: demos -- GitLab From 7f8e1f13b0671a3a36dd220fd5770f6b4ab8eca3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Wed, 22 Dec 2021 18:14:27 +0100 Subject: [PATCH 18/32] Upload New File --- license_header_2021.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 license_header_2021.txt diff --git a/license_header_2021.txt b/license_header_2021.txt new file mode 100644 index 0000000..c75a6f2 --- /dev/null +++ b/license_header_2021.txt @@ -0,0 +1,17 @@ +// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -- GitLab From f6409e26448f33b6e59e5f0a9f8da78d7a2a222d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Wed, 22 Dec 2021 18:15:35 +0100 Subject: [PATCH 19/32] Add LICENSE --- LICENSE | 619 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 619 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..281d399 --- /dev/null +++ b/LICENSE @@ -0,0 +1,619 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. -- GitLab From 44a8fb010b49efd56b3052bc75a364071e7910a1 Mon Sep 17 00:00:00 2001 From: cont-integration Date: Wed, 22 Dec 2021 17:50:10 +0000 Subject: [PATCH 20/32] [skip ci] license headers added or modified --- include/publisher_debug_img.h | 21 +++++++++++++++++++++ include/subscriber_camera.h | 21 +++++++++++++++++++++ src/publisher_debug_img.cpp | 21 +++++++++++++++++++++ src/subscriber_camera.cpp | 21 +++++++++++++++++++++ 4 files changed, 84 insertions(+) diff --git a/include/publisher_debug_img.h b/include/publisher_debug_img.h index f76b620..84f2f32 100644 --- a/include/publisher_debug_img.h +++ b/include/publisher_debug_img.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #ifndef __PUBLISHER_DEBUG_IMG_H_ #define __PUBLISHER_DEBUG_IMG_H_ diff --git a/include/subscriber_camera.h b/include/subscriber_camera.h index 938b047..c312aa3 100644 --- a/include/subscriber_camera.h +++ b/include/subscriber_camera.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #ifndef SUBSCRIBER_CAMERA_H_ #define SUBSCRIBER_CAMERA_H_ /************************** diff --git a/src/publisher_debug_img.cpp b/src/publisher_debug_img.cpp index 8e480a4..0e46082 100644 --- a/src/publisher_debug_img.cpp +++ b/src/publisher_debug_img.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #include "publisher_debug_img.h" diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index dcf64a1..e6d2e7b 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #include "subscriber_camera.h" #include #include "apriltag/feature/feature_apriltag.h" -- GitLab From 449a892f3a6f4ca0152eba16ae630e172ca548dd Mon Sep 17 00:00:00 2001 From: Joan Sola Date: Wed, 22 Dec 2021 20:43:43 +0100 Subject: [PATCH 21/32] comment out buggy publisher_debug_img --- CMakeLists.txt | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b5ca473..13a0988 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(wolf_ros_apriltag) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) -set(OpenCV_DIR /usr/share/OpenCV/) +set(OpenCV_DIR /usr/local/share/OpenCV/) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -66,11 +66,12 @@ add_library(subscriber_${PROJECT_NAME} src/subscriber_camera.cpp ) -add_library(publisher_${PROJECT_NAME} - # src/publisher_estimated_pose.cpp - # src/publisher_current_state.cpp - # src/publisher_solve_logs.cpp - ) +#add_library(publisher_${PROJECT_NAME} +# src/publisher_debug_img.cpp +## src/publisher_estimated_pose.cpp +## src/publisher_current_state.cpp +## src/publisher_solve_logs.cpp +# ) target_link_libraries(subscriber_${PROJECT_NAME} ${wolfcore_LIBRARIES} @@ -80,9 +81,9 @@ target_link_libraries(subscriber_${PROJECT_NAME} ${catkin_LIBRARIES} ) -target_link_libraries(publisher_${PROJECT_NAME} - ${wolfcore_LIBRARIES} - ${wolfapriltag_LIBRARIES} - ${wolfvision_LIBRARIES} - ${catkin_LIBRARIES} - ) \ No newline at end of file +#target_link_libraries(publisher_${PROJECT_NAME} +# ${wolfcore_LIBRARIES} +# ${wolfapriltag_LIBRARIES} +# ${wolfvision_LIBRARIES} +# ${catkin_LIBRARIES} +# ) \ No newline at end of file -- GitLab From 9bd84bb95767e344a4b5610e598308a9fa0f7236 Mon Sep 17 00:00:00 2001 From: Joan Sola Date: Wed, 22 Dec 2021 22:21:53 +0100 Subject: [PATCH 22/32] ignore --- .gitignore | 2 ++ .settings/.gitignore | 1 + 2 files changed, 3 insertions(+) create mode 100644 .gitignore create mode 100644 .settings/.gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0dcfcdb --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +/.cproject +/.project diff --git a/.settings/.gitignore b/.settings/.gitignore new file mode 100644 index 0000000..d81d4c4 --- /dev/null +++ b/.settings/.gitignore @@ -0,0 +1 @@ +/language.settings.xml -- GitLab From cbd61ef8a3d735d9e359f752bd3021bbe612708a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Thu, 23 Dec 2021 17:43:46 +0100 Subject: [PATCH 23/32] CMakeLists.txt C++14 --- CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 13a0988..0791315 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(wolf_ros_apriltag) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +## Compile as C++14 +add_compile_options(-std=c++14) set(OpenCV_DIR /usr/local/share/OpenCV/) @@ -86,4 +86,4 @@ target_link_libraries(subscriber_${PROJECT_NAME} # ${wolfapriltag_LIBRARIES} # ${wolfvision_LIBRARIES} # ${catkin_LIBRARIES} -# ) \ No newline at end of file +# ) -- GitLab From 41618052f8847d13571321f38432a9fcb39c2fcd Mon Sep 17 00:00:00 2001 From: joanvallve Date: Thu, 23 Dec 2021 20:00:36 +0100 Subject: [PATCH 24/32] didnt compile with opencv in ROS --- CMakeLists.txt | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0791315..6ccd287 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(wolf_ros_apriltag) ## Compile as C++14 add_compile_options(-std=c++14) -set(OpenCV_DIR /usr/local/share/OpenCV/) +##set(OpenCV_DIR /usr/local/share/OpenCV/) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -14,10 +14,10 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs - dynamic_reconfigure +# dynamic_reconfigure tf wolf_ros_node - OpenCV +# OpenCV image_transport ) @@ -26,11 +26,10 @@ find_package(wolfvision REQUIRED) find_package(wolfapriltag REQUIRED) ## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) +# generate_messages( +# DEPENDENCIES +# sensor_msgs# std_msgs +# ) ################################### ## catkin specific configuration ## -- GitLab From 1c9ba8227baf2753502e3a31da6eb5dc5d0a7126 Mon Sep 17 00:00:00 2001 From: joanvallve Date: Thu, 23 Dec 2021 20:00:50 +0100 Subject: [PATCH 25/32] added update on last stamp for subscriber base --- src/subscriber_camera.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index e6d2e7b..9cf153f 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -50,6 +50,8 @@ void SubscriberCamera::initialize(ros::NodeHandle& nh, const std::string& topic) void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) { + setLastStamp(msg->header.stamp); + auto start = std::chrono::high_resolution_clock::now(); ts_cam_ = msg->header.stamp; -- GitLab From 46839725b0f58c450549bfb8243e5425a9585480 Mon Sep 17 00:00:00 2001 From: joanvallve Date: Fri, 24 Dec 2021 12:20:08 +0100 Subject: [PATCH 26/32] adapted to new subscriber API --- src/subscriber_camera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index 9cf153f..1969ad8 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -50,7 +50,7 @@ void SubscriberCamera::initialize(ros::NodeHandle& nh, const std::string& topic) void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) { - setLastStamp(msg->header.stamp); + updateLastHeader(msg->header); auto start = std::chrono::high_resolution_clock::now(); -- GitLab From fc304208806c60381f636819de517c16b119bf25 Mon Sep 17 00:00:00 2001 From: Joan Sola Date: Sat, 25 Dec 2021 12:05:52 +0100 Subject: [PATCH 27/32] Add tag Ids to debug_img image --- src/subscriber_camera.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index 1969ad8..6b5bb94 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -119,6 +119,7 @@ void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg cv::line(frame, feat_april->getTagCorners()[1], feat_april->getTagCorners()[2], CV_RGB(0, 255, 0), 2); cv::line(frame, feat_april->getTagCorners()[2], feat_april->getTagCorners()[3], CV_RGB(0, 0, 255), 2); cv::line(frame, feat_april->getTagCorners()[3], feat_april->getTagCorners()[0], CV_RGB(255, 0, 255), 2); + cv::putText(frame, cv::String(std::to_string(feat_april->getDetection().id)), feat_april->getTagCorners()[0], cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,220,0),1.5); } // /////////////////////////////// -- GitLab From 1dea401caa90a8b314fa23284fc67eec103dfb6d Mon Sep 17 00:00:00 2001 From: cont-integration Date: Tue, 4 Jan 2022 14:47:49 +0000 Subject: [PATCH 28/32] [skip ci] license headers added or modified --- include/publisher_debug_img.h | 2 +- include/subscriber_camera.h | 2 +- license_header_2021.txt => license_header_2022.txt | 2 +- src/publisher_debug_img.cpp | 2 +- src/subscriber_camera.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) rename license_header_2021.txt => license_header_2022.txt (89%) diff --git a/include/publisher_debug_img.h b/include/publisher_debug_img.h index 84f2f32..d20e838 100644 --- a/include/publisher_debug_img.h +++ b/include/publisher_debug_img.h @@ -1,6 +1,6 @@ //--------LICENSE_START-------- // -// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) // All rights reserved. // diff --git a/include/subscriber_camera.h b/include/subscriber_camera.h index c312aa3..67896dc 100644 --- a/include/subscriber_camera.h +++ b/include/subscriber_camera.h @@ -1,6 +1,6 @@ //--------LICENSE_START-------- // -// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) // All rights reserved. // diff --git a/license_header_2021.txt b/license_header_2022.txt similarity index 89% rename from license_header_2021.txt rename to license_header_2022.txt index c75a6f2..0c98702 100644 --- a/license_header_2021.txt +++ b/license_header_2022.txt @@ -1,4 +1,4 @@ -// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) // All rights reserved. // diff --git a/src/publisher_debug_img.cpp b/src/publisher_debug_img.cpp index 0e46082..afdd19f 100644 --- a/src/publisher_debug_img.cpp +++ b/src/publisher_debug_img.cpp @@ -1,6 +1,6 @@ //--------LICENSE_START-------- // -// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) // All rights reserved. // diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index 6b5bb94..ea59397 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -1,6 +1,6 @@ //--------LICENSE_START-------- // -// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) // All rights reserved. // -- GitLab From 1394f2016de0f7c164bee8b159f41761a2aaadb5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Mon, 24 Jan 2022 09:20:49 +0100 Subject: [PATCH 29/32] [skip ci] Update .gitlab-ci.yml file --- .gitlab-ci.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 3806f88..58fc8eb 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -74,10 +74,10 @@ stages: - git checkout devel - git pull - git checkout $WOLF_CORE_BRANCH + - git pull - else - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git + - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git - cd wolf - - git checkout $WOLF_CORE_BRANCH - fi - mkdir -pv build - cd build @@ -109,10 +109,10 @@ stages: - git checkout devel - git pull - git checkout $WOLF_VISION_BRANCH + - git pull - else - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git + - git clone -b $WOLF_VISION_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git - cd vision - - git checkout $WOLF_VISION_BRANCH - fi - mkdir -pv build - cd build @@ -144,10 +144,10 @@ stages: - git checkout devel - git pull - git checkout $WOLF_APRILTAG_BRANCH + - git pull - else - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag.git + - git clone -b $WOLF_APRILTAG_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag.git - cd apriltag - - git checkout $WOLF_APRILTAG_BRANCH - fi - mkdir -pv build - cd build -- GitLab From 6d5ddb3ea39d7fbc7bc69a4b8e6a5229d47c7510 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Mon, 31 Jan 2022 12:38:03 +0100 Subject: [PATCH 30/32] [skip ci] ci removed ubuntu 16.04 --- .gitlab-ci.yml | 38 ++------------------------------------ 1 file changed, 2 insertions(+), 36 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 58fc8eb..3ae5b08 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -174,11 +174,8 @@ stages: ############ LICENSE HEADERS ############ license_headers: stage: license - image: labrobotica/wolf_vision_deps_ros:16.04 - cache: - - key: wolf-xenial - paths: - - ci_deps/wolf/ + image: labrobotica/wolf_vision_deps_ros:20.04 + cache: [] except: - master before_script: @@ -187,37 +184,6 @@ license_headers: script: - *license_header_definition -############ UBUNTU 16.04 TEST ############ -build_and_test:xenial: - stage: build_and_test - image: labrobotica/wolf_vision_deps_ros:16.04 - cache: - - key: wolf-xenial - paths: - - ci_deps/wolf/ - - key: visionutils-xenial - paths: - - ci_deps/vision_utils/ - - key: vision-xenial - paths: - - ci_deps/vision/ - - key: apriltaglib-xenial - paths: - - ci_deps/apriltaglib/ - except: - - master - before_script: - - *preliminaries_definition - - *install_wolf_definition - - *install_visionutils_definition - - *install_wolfvision_definition - - *install_apriltag_definition - - *install_wolfapriltag_definition - - *clone_wolfrosnode_definition - - ldconfig - script: - - *build_and_test_definition - ############ UBUNTU 18.04 TEST ############ build_and_test:bionic: stage: build_and_test -- GitLab From a8b0994307b54214862db100d51f0dce0d96efc7 Mon Sep 17 00:00:00 2001 From: Mederic Fourmy Date: Thu, 10 Feb 2022 11:32:17 +0100 Subject: [PATCH 31/32] Removed yaml folder --- yaml/apriltag_vio.yaml | 122 ------------------ yaml/camera_simu.yaml | 21 --- yaml/processor_tracker_landmark_apriltag.yaml | 84 ------------ 3 files changed, 227 deletions(-) delete mode 100644 yaml/apriltag_vio.yaml delete mode 100644 yaml/camera_simu.yaml delete mode 100644 yaml/processor_tracker_landmark_apriltag.yaml diff --git a/yaml/apriltag_vio.yaml b/yaml/apriltag_vio.yaml deleted file mode 100644 index 25740ab..0000000 --- a/yaml/apriltag_vio.yaml +++ /dev/null @@ -1,122 +0,0 @@ -############################ -######### EXAMPLE ########## -############################ - -config: - - profiling: - enabled: true - file_name: "~/wolf_demo_apriltag_profiling.txt" - - problem: - tree_manager: - type: "none" - frame_structure: "POV" - dimension: 3 - prior: - mode: "factor" - $state: - P: [0,0,0] - O: [0,0,0,1] - V: [0,0,0] - $sigma: - P: [0.001, 0.001, 0.001] - O: [0.1, 0.1, 0.1] - V: [0.01, 0.01, 0.01] - time_tolerance: 0.001 - - solver: - max_num_iterations: 100 - verbose: 1 - period: 0.3 - update_immediately: false - n_threads: 2 - compute_cov: false - - sensors: - - - type: "SensorCamera" - name: "CAMERA" - plugin: "vision" - extrinsic: - pose: [0,0,0, 0,0,0,1] # Simu (e.g. circle.bag) - follow: "camera_simu.yaml" - - - type: "SensorImu" - name: "IMU" - plugin: "imu" - extrinsic: - pose: [0,0,0, 0,0,0,1] - follow: "imu_intrinsics.yaml" - - processors: - - - type: "ProcessorTrackerLandmarkApriltag" - name: "APRILTAG PROC" - sensor_name: "CAMERA" - plugin: "apriltag" - time_tolerance: 0.001 - follow: "processor_tracker_landmark_apriltag.yaml" - - - type: "ProcessorImu" - name: "IMU PROC" - sensor_name: "IMU" - plugin: "imu" - apply_loss_function: true - follow: "processor_imu.yaml" - - ROS subscriber: - - - package: "wolf_ros_apriltag" - type: "SubscriberCamera" - topic: "/camera_simu/image_raw" - sensor_name: "CAMERA" - - - package: "wolf_ros_imu" - type: "SubscriberImu" - topic: "/imu/imu_raw" - sensor_name: "IMU" - imu_x_axis: 1 - imu_y_axis: 2 - imu_z_axis: 3 - cov_source: "sensor" - in_degrees: false - - ROS publisher: - - - type: "PublisherGraph" - topic: "graph" - package: "wolf_ros_node" - period: 0.1 - - - type: "PublisherTf" - topic: " " - package: "wolf_ros_node" - period: 0.1 - map_frame_id: "map" - odom_frame_id: "odom" - base_frame_id: "base_footprint" - publish_odom_tf: false - - # - - # package: "wolf_ros_apriltag" - # type: "PublisherCurrentState" - # topic: "/current_state" - # period: "5" - # - - # package: "wolf_ros_apriltag" - # type: "PublisherEstimatedPose" - # topic: "/estimated_pose" - # period: "5" - # - - # package: "wolf_ros_apriltag" - # type: "PublisherSolveLogs" - # topic: "/solve_logs" - # period: "5" - # - - # package: "wolf_ros_node" - # type: "PublisherPose" - # topic: "/wolf_pose" - # period: "5" - - use_debug_img: true diff --git a/yaml/camera_simu.yaml b/yaml/camera_simu.yaml deleted file mode 100644 index 4e62e69..0000000 --- a/yaml/camera_simu.yaml +++ /dev/null @@ -1,21 +0,0 @@ -width: 640 -height: 480 -camera_name: camera -camera_matrix: - rows: 3 - cols: 3 - data: [320, 0.000000, 320, 0.000000, 320, 240, 0.000000, 0.000000, 1.000000] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [0.0, 0.0, 0.0, 0.0, 0.0] # tangential distortion not supported by wolf - # data: [-0.117928, 0.092265, -0.002119, -0.002615, 0.000000] -rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] -projection_matrix: - rows: 3 - cols: 4 - data: [320, 0.000000, 320, 0.000000, 0.000000, 320, 240, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml deleted file mode 100644 index dd498ce..0000000 --- a/yaml/processor_tracker_landmark_apriltag.yaml +++ /dev/null @@ -1,84 +0,0 @@ -############################ -######### EXAMPLE ########## -############################ - - -# detector parameters: -quad_decimate: 0.0 # doing quad detection at lower resolution to speed things up (see end of file) -quad_sigma: 0.0 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file) -nthreads: 8 # how many thread during tag detection (does not change much?) -debug: false # write some debugging images -refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff) -ippe_min_ratio: 0 # quite arbitrary, always > 1 (to deactive, set at something < 1) -ippe_max_rep_error: 100 # to deactivate, set at something big (100) - -# specific tag widths if using tags of different sizes -$tag_widths: - -1: 0.0 - -tag_family: "tag36h11" # see in Wolf processor_tracker_landmark_apriltag.cpp for available families -tag_black_border: 1 # usually 1, 2 for Kalibr aprilgrid for ex -# tag_width_default: 0.165 # used if tag width not specified -- IRI $$ -tag_width_default: 0.2 # used if tag width not specified -- Simu $$ -# tag_width_default: 0.055 # used if tag width not specified -- Calibration $$ -# tag_width_default: 0.166 # used if tag width not specified -- LAAS (B181) $$ -# tag_width_default: 0.162 # used if tag width not specified -- LAAS, 04/07 $$ - -# used for covariance propagation -std_pix: 4 # pixel error $$ -# dummy covariances (not used but necessary for yaml parser) -std_xy : 0.1 # m -std_z : 0.1 # m -std_rpy: 0.1 # radians - -keyframe_vote: - voting_active: true - nb_vote_for_every_first: 10 - min_time_vote: 0.05 # s - # Necessary conditions - max_time_vote: 0.3 # s - # One of the possibilities - min_features_for_keyframe: 1 -reestimate_last_frame: false # for a better prior on the new keyframe: use only if no motion processor -add_3d_cstr: false # add 3D constraints between the KF so that they do not jump when using apriltag only - -max_new_features: -1 # max number of features detected in detectNewFeatures() (-1: unlimited) -apply_loss_function: true - -############### -# Apriltag settings -############### -# int nthreads; -# How many threads should be used? - -# float quad_decimate; -# detection of quads can be done on a lower-resolution image, -# improving speed at a cost of pose accuracy and a slight -# decrease in detection rate. Decoding the binary payload is -# still done at full resolution. . -# ! Special optimized value of 1.5 - -# float quad_sigma; -# What Gaussian blur should be applied to the segmented image -# (used for quad detection?) Parameter is the standard deviation -# in pixels. Very noisy images benefit from non-zero values -# (e.g. 0.8). - -# int refine_edges; -# When non-zero, the edges of the each quad are adjusted to "snap -# to" strong gradients nearby. This is useful when decimation is -# employed, as it can increase the quality of the initial quad -# estimate substantially. Generally recommended to be on (1). -# Very computationally inexpensive. Option is ignored if -# quad_decimate = 1. - -# double decode_sharpening; -# How much sharpening should be done to decoded images? This -# can help decode small tags but may or may not help in odd -# lighting conditions or low light conditions. -# The default value is 0.25. - -# int debug; -# When non-zero, write a variety of debugging images to the -# current working directory at various stages through the -# detection process. (Somewhat slow). -- GitLab From 3156cd9b2285addfac9973153f71fb6882e28363 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Wed, 9 Mar 2022 11:48:18 +0100 Subject: [PATCH 32/32] CI: update apriltag demo new name --- .gitlab-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 3ae5b08..c646173 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -256,4 +256,4 @@ demo_apriltag: WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH WOLF_ROS_APRILTAG_BRANCH: $CI_COMMIT_BRANCH trigger: - project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_apriltag + project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_apriltag_imu -- GitLab