diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index 59e8dcc0..88f197f9 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -24,15 +24,11 @@ message("CMAKE_BUILD_INCLUDE_DIR: ${CMAKE_BUILD_INCLUDE_DIR}") find_package(Eigen3 REQUIRED) find_package(GTSAM REQUIRED) find_package(GTSAM_UNSTABLE REQUIRED) -find_package(Python3 REQUIRED COMPONENTS Development) -find_package(tf2_eigen REQUIRED) message("Eigen Version:: ${EIGEN3_VERSION_STRING}") message("Eigen Path:: ${EIGEN3_DIR}") message("GTSAM Include Path:: ${GTSAM_INCLUDE_DIRS}") -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - # Color settings for terminal output if(NOT WIN32) string(ASCII 27 Esc) @@ -41,17 +37,10 @@ if(NOT WIN32) set(Magenta "${Esc}[35m") endif () -# Eigen -find_package(Eigen3 REQUIRED COMPONENTS) -# GTSAM -find_package(GTSAM REQUIRED) -find_package(GTSAM_UNSTABLE REQUIRED) -message("GTSAM Version:" ${GTSAM_VERSION}) -message("GTSAM Path:" ${GTSAM_DIR}) - ########### ## Build ## ########### + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -76,6 +65,8 @@ add_library(${PROJECT_NAME} SHARED src/lib/TrajectoryAlignmentHandler.cpp src/lib/NavState.cpp ) +# Exported alias target +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) # Link GTSAM and other dependencies to the graph_msf library target_link_libraries(${PROJECT_NAME} diff --git a/graph_msf/include/graph_msf/config/StaticTransforms.h b/graph_msf/include/graph_msf/config/StaticTransforms.h index 45697469..3d50d14f 100644 --- a/graph_msf/include/graph_msf/config/StaticTransforms.h +++ b/graph_msf/include/graph_msf/config/StaticTransforms.h @@ -38,7 +38,7 @@ class StaticTransforms : public TransformsDictionary { const std::string& getInitializationFrame() { return initializationFrame_; } // Functionality ------------------------------------------------------------ - virtual void findTransformations() = 0; + virtual bool findTransformations() = 0; protected: // Required frames diff --git a/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h b/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h index 4944b28a..d2723817 100644 --- a/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h +++ b/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h @@ -33,8 +33,8 @@ class GmsfUnaryExpressionLocalVelocity3 final : public GmsfUnaryExpressionLocal< // Check whether we can find an IMU measurement corresponding to the velocity measurement double imuTimestamp; graph_msf::ImuMeasurement imuMeasurement = graph_msf::ImuMeasurement(); - if (!imuBufferPtr->getClosestImuMeasurement(imuTimestamp, imuMeasurement, 0.02, velocityUnaryMeasurementPtr->timeK())) { - REGULAR_COUT << RED_START << "No IMU Measurement (in time interval) found, assuming 0 angular velocity for the factor." << COLOR_END + if (!imuBufferPtr->getClosestImuMeasurement(imuTimestamp, imuMeasurement, 0.1, velocityUnaryMeasurementPtr->timeK())) { // previously 0.02 + REGULAR_COUT << RED_START << " No IMU Measurement (in time interval) found, assuming 0 angular velocity for the factor." << COLOR_END << std::endl; } diff --git a/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml b/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml deleted file mode 100644 index 3bfdebc8..00000000 --- a/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml +++ /dev/null @@ -1,109 +0,0 @@ -#Common -loamVerbosity: 0 #Debug Output, 0-disable, higher for more verbose output -scanPeriod: 0.1 #Expected scan period for input pointcloud in seconds. Used of Distortion correction -ioRatio: 2 #Ratio of publish rate of LaserOdometry w.r.t input PCL rate. LO is calculated for each PCL but published at slower rate to Mapping, default 2 -lidarFrame: rslidar #LiDAR frame name - used for LiDAR-to-Sensor transform lookup -rotateInputCloud: false #Flag to rotate input cloud before estimating odometry/map so produced resuts are ROS frame aligned irrespective of LiDAR mounting orientation -inputCloudRotation: [0.0, 0.0, 0.0] #Rotation applied to input cloud - ORDER YPR(radians) #90 deg = 1.5707963268 rad - -#External Prior/Transform Input -forceExtPriorUseMapping : false #Flag to force use of FULL external prior instead of LaserOdometry TRANSLATION Only Estimate -extPriorAvailable: true #Flag to check if 'Primary' external prior is available -extOdomFrame: imu #External Prior odometry frame name -extFixedFrame: odom #External Prior fixed frame name -extSensorFrame: imu_link #External Prior sensor frame name -extOdomTimeOffset : 0.0 #Timeoffset (seconds) between LiDAR pointcloud and external source -fallbackExtPriorAvailable: false #Flag to check if 'Fallback ' external prior is available -fallbackExtOdomFrame: vio_imu #Fallback External Prior odometry frame name -fallbackExtFixedFrame: vio_imu_init #Fallback External Prior fixed frame name -fallbackExtSensorFrame: imu_sensor_frame #Fallback External Prior sensor frame name -fallbackExtOdomTimeOffset: 0.00 #Timeoffset (seconds) between LiDAR pointcloud and Fallbackexternal source - -#MultiScanRegistration -lidar: VLP-16 #Choose LiDAR type - options: VLP-16 HDL-32 HDL-64E O1-16 O1-64 Bperl-32 -useCloudIntensityandRingFields : false #Flag to use input pointcloud intensity or ring fields. Converts to custom PointXYZIR instead of PointXYZ. True for Bpearl -uniformLidarRingDistance: true # Whether the LiDAR rings are uniformly distributed in elevation, does not hold for e.g. Helios -ringAngleTolerance: 0.5 # [deg], Tolerance up to which angle rays are considered to belong to same ring, only impact if argument before is false -imuHistorySize: 800 #IMU Message Buffer Size , default: 200 -minRange: 0.1 #Minimum Range of useful points, default: 0.1 -maxRange: 80.0 #Maximum Range of useful points, default: 80 -featureRegions: 6 #Number of Azimuth Regions Pointcloud is divided, default:6 -curvatureRegion: 5 #Number of neigboring points on a scan line on each side of a point used for calculating curvature of that point, default 5 -maxCornerSharp: 2 #Number of Sharp Features per scan line in each curvatureRegion, default:2 -maxCornerLessSharp: 20 #Number of Less Sharp Features per scan line in each curvatureRegion, default:10*maxCornerLessSharp -maxSurfaceFlat: 4 #Number of FlatFeatures per scan line in each curvatureRegion, default:4 -surfaceCurvatureThreshold: 0.1 #Threshold above which feature is categorized Sharp, default 0.1 -lessFlatFilterSize: 0.2 #Leaf Size for downsampling remaing pointcloud after feature selection, default 0.2 -checkInvalidFeatureRegions: false -publishDebugArrowsToRemovedFeatures: false -azimuthBoundaries: [1.1,1.5,1.7,2.2] #ANYmal - -#LaserOdometry -undistortInputCloud: true #If true External Prior or Motion Model will be used for LiDAR Ego Motion Compensation of input cloud -odomMaxIterations: 25 #Maximum Number of LO optimization iterations, default 25 -odomMinIterations: 1 #Minimum Number of LO optimization iterations, default 4, (set 0 to perform only first iteration at minimum) -odomDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.1 (m) -odomDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.1 (deg) -odomDegenEigVal: 30 #Minimum eignevalue threshold for determining degeneracy of LO optimization, default 30 -odomRelativeTranslationMax: 0.8 #Max translation threshold between two pointclouds for external odometry to be considered valid input. Determined w.r.t set max robot movement speed -odomRelativeRotationMax: 0.1 #Max rotation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed - -#LaserMapping -mapMaxIterations: 10 #Maximum Number of LM optimization iterations, default 10 -mapMinIterations: 1 #Minimum Number of LM optimization iterations, default 1, (set 0 to perform only first iteration at minimum) -mapDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.05 (m) -mapDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.05 (deg) -cornerFilterSize: 0.2 #Leaf Size for downsampling current CORNER pointcloud before merging in map, default 0.2 -surfaceFilterSize: 0.2 #Leaf Size for downsampling current FLAT/SURFACE pointcloud before merging in map, default 0.2 -mapVisFilterSize: 0.4 #Leaf Size for downsampling current visualization Map pointcloud, default 0.5 -rndSampleMapVisNoOfSamples: 100000 #Number of Samples for random sampling of the visualized map cloud, default 100000 -mapCubeSize: 10.0 #Size of Cube/Voxel used for saving internal map (meters), default 50 -mapDimensionsInCubes: [101,21,101] #WxHxD of internal map in CUBE UNITS, default:[21,11,21] #Width,Height,Depth -mapStartLocationInCubes: [50,10,50] #Robot start position in internal map in CUBE UNITS, default:[10,5,10] #Width,Height,Depth -numNeighborSubmapCubes: 4 #Number of Neigboring cubes in +/- direction along each axis to build submap for matching -mapDegenEigVal: 10 #Minimum eignevalue threshold for determining degeneracy of LM optimization, default 30 -mapPriorNormThresh: 0.5 #Max translation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed -useSavedSubmapsForInitialization: false #Use saved submap on disc for initialization -submapLocalizationInitGuess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Initialization guess of robot location in submap - ORDER: XYZ(meters) - YPR(radians) -waitAndUseExternalSubmapsForInit: false #Wait for external submaps before initlization. Trigger external submaps through service call -submapsRequestServiceCall: "" #Name of service call to request submaps from other node (xyz/share_submaps) -useExternalSubmapOriginForInit: false #Use origin of external submaps as initlization guess for co-localization instead of current external pose (default: false) -forcePathPublishing: true #Force ROS path message publishing for mapping odometry -mapPublishRateDivider: 20 #Publishes full map at 1.0/(scanPeriod * ioRatio * mapPublishRateDivider), default: 20(0.25 Hz) -useExtRotationAlignment: true #Use external fixed & map frame to detemine gravity misalignment, default: true -submapVisCropRange: 0 #Range of cropping for submap visualization (set 0 to disable) - -#Factor Graph -imuFrame: imu_link #Frame of IMU used in integrator - Used for lookup transform with lidarFrame -imuRate: 400 #Rate of IMU input (Hz) used for initialization of gravity, initial Roll/Pitch and gyro bias -imuTimeForInit: 0.5 #Time interval of IMU measurements used for initialization -imuTimeOffset: 0.0 #seconds #Offset between IMU and LiDAR Measurements - Depending on LiDAR timestamp first(+0.05) or last(-0.05) -initGraphRollPitchFromIMU: true #Initialize graph roll/pitch from IMU -globalZaxisPointsUp: true #Z-axis of global coordinate system pointing up -accNoiseDensity: 7.84e-04 #Continuous-time "Covariance" of accelerometer -accBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing accelerometer bias random walk (biasAccCovariance) -gyrNoiseDensity: 3.4906585e-5 #Continuous-time "Covariance" of gyroscope measurements -gyrBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing gyroscope bias random walk (biasOmegaCovariance) -imuIntegrationCovariance: 1.0e-08 #Continuous-time "Covariance" describing integration uncertainty -imuBiasAccOmegaInt: 1.0 #Covariance of bias used for pre-integration -accBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of accelerometer bias -gyrBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of gyroscope bias -smootherLag: 3.0 #Lag of fixed lag smoother[seconds] -additonalIterations: 3 #Additional iterations of graph optimizer after update with new factors -positionReLinTh: 0.05 #Position linearization threshold -rotationReLinTh: 0.05 #Rotation linearization threshold -velocityReLinTh: 0.1 #Linear Velocity linearization threshold -accBiasReLinTh: 0.1 #Accelerometer bias linearization threshold -gyrBiasReLinTh: 0.1 #Gyroscope bias linearization threshold -relinearizeSkip: 1 -enableRelinearization: true -evaluateNonlinearError: false -cacheLinearizedFactors: true -findUnusedFactorSlots: false -enablePartialRelinearizationCheck: true -enableDetailedResults: false -poseBetweenNoise: [0.5, 0.5, 0.05, 0.1, 0.1, 0.1] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) -zeroMotionDetection: false #Detect and Add Zero Motion Factors(Zero delta Pose and Velocity) -zeroMotionThreshold: 0.01 #Zero motion threshold in meters (Currently only motion detection is translation only) -minZeroMoitionDetections: 10 #Minimum number of consective zero motions detected before factors are added -gravityRollPitchFactors: false #Add Gravity-aligned Roll-Pitch from IMU when in Zero motion (only works if Zero-Motion Factors are added) diff --git a/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml b/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml deleted file mode 100644 index 403a3779..00000000 --- a/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml +++ /dev/null @@ -1,523 +0,0 @@ -control: - joint_state_controller: - publish_rate: 50 - type: joint_state_controller/JointStateController - joy_teleop: - joy_node: - autorepeat_rate: 20 - deadzone: 0.1 - dev: /dev/input/js0 - teleop_twist_joy: - axis_angular: 0 - axis_linear: 1 - enable_button: 4 - enable_turbo_button: 5 - scale_angular: 1.2 - scale_angular_turbo: 1.2 - scale_linear: 1.6 - scale_linear_turbo: 1.0 - lowlevel_controller: - LF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - LF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - LF_WHEEL_velocity_controller: - joint: LF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - RF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - RF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - RF_WHEEL_velocity_controller: - joint: RF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - WHEEL_JOINT_ff_param: - ff_general: 25 - ff_pure_rotation: 100 - robot_joint_publisher: - publish_rate: 50 - type: joint_state_controller/JointStateController - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_diff_drive: - angular: - z: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - base_frame_id: base_link - cmd_vel_timeout: 0.25 - enable_odom_tf: false - estimate_velocity_from_position: false - left_wheel: - - LF_WHEEL_JOINT - left_wheel_radius_multiplier: 1.0 - linear: - x: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - pose_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - publish_rate: 50.0 - right_wheel: - - RF_WHEEL_JOINT - right_wheel_radius_multiplier: 1.0 - twist_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - type: diff_drive_controller/DiffDriveController - velocity_rolling_window_size: 2 - wheel_radius_multiplier: 1.0 - wheel_separation_multiplier: 1.0 - smb_lowlevel_controller: - command_smb: true - control_namespace: /control - controller_namespace: lowlevel_controller - port: /dev/ttyUSB0 - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_robot_state_publisher: - publish_frequency: 50 - use_tf_static: true - twist_mux: - locks: - - name: e_stop - priority: 255 - timeout: 0.0 - topic: joy_teleop/e_stop - - name: e_stop_opc - priority: 200 - timeout: 0.0 - topic: opc/e_stop - topics: - - name: RC - priority: 15 - timeout: 0.5 - topic: smb_lowlevel_controller/rc_twist - - name: joy - priority: 10 - timeout: 0.5 - topic: joy_teleop/cmd_vel - - name: keyboard - priority: 9 - timeout: 0.5 - topic: keyboard_teleop/cmd_vel - - name: joy_opc - priority: 8 - timeout: 0.5 - topic: opc/cmd_vel - - name: mpc_command_twist - priority: 5 - timeout: 0.2 - topic: smb_mpc/command_twist - - name: teb_planner_twist - priority: 3 - timeout: 1.0 - topic: teb_planner_twist -odometry: - tracking_camera_odometry_conversion: - in_odom_frame: tracking_camera_pose_frame - in_sensor_frame: tracking_camera_pose_frame - out_odom_frame: tracking_camera_odom - out_sensor_frame: base_link -rosdistro: 'noetic - - ' -roslaunch: - uris: - host_smb_263_nuc__35755: http://smb-263-nuc:35755/ -rosserial_python: - baud: 250000 - port: /dev/versavis -rosversion: '1.16.0 - - ' -run_id: d353b820-09d9-11ee-9f1a-13a7083ee06f -serial_node: - baud: 115200 - port: /dev/smb-power -simulation: false -smb_description: "\n\n\n\n\n\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ Gazebo/Grey\n \n \n Gazebo/Red\n \n \n \n \n\ - \ \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n Gazebo/Orange\n \n \n true\n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n Gazebo/Red\n \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n Gazebo/Grey\n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n Gazebo/Black\n\ - \ \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n\n\n" -tracking_camera: - realsense2_camera: - accel_fps: 0 - accel_frame_id: tracking_camera_accel_frame - accel_optical_frame_id: tracking_camera_accel_optical_frame - align_depth: false - aligned_depth_to_color_frame_id: tracking_camera_aligned_depth_to_color_frame - aligned_depth_to_fisheye1_frame_id: tracking_camera_aligned_depth_to_fisheye1_frame - aligned_depth_to_fisheye2_frame_id: tracking_camera_aligned_depth_to_fisheye2_frame - aligned_depth_to_fisheye_frame_id: tracking_camera_aligned_depth_to_fisheye_frame - aligned_depth_to_infra1_frame_id: tracking_camera_aligned_depth_to_infra1_frame - aligned_depth_to_infra2_frame_id: tracking_camera_aligned_depth_to_infra2_frame - allow_no_texture_points: false - base_frame_id: tracking_camera_link - calib_odom_file: /home/robotx/catkin_workspaces/smb_dev/src/core/smb/config/tracking_camera_config.json - clip_distance: -1.0 - color_fps: 30 - color_frame_id: tracking_camera_color_frame - color_height: 480 - color_optical_frame_id: tracking_camera_color_optical_frame - color_width: 640 - confidence_fps: 30 - confidence_height: 480 - confidence_width: 640 - depth_fps: 30 - depth_frame_id: tracking_camera_depth_frame - depth_height: 480 - depth_optical_frame_id: tracking_camera_depth_optical_frame - depth_width: 640 - device_type: t265 - enable_accel: true - enable_color: true - enable_confidence: true - enable_depth: true - enable_fisheye: false - enable_fisheye1: false - enable_fisheye2: false - enable_gyro: true - enable_infra: false - enable_infra1: false - enable_infra2: false - enable_pointcloud: false - enable_pose: true - enable_sync: false - filters: '' - fisheye1_frame_id: tracking_camera_fisheye1_frame - fisheye1_optical_frame_id: tracking_camera_fisheye1_optical_frame - fisheye2_frame_id: tracking_camera_fisheye2_frame - fisheye2_optical_frame_id: tracking_camera_fisheye2_optical_frame - fisheye_fps: 30 - fisheye_frame_id: tracking_camera_fisheye_frame - fisheye_height: 0 - fisheye_optical_frame_id: tracking_camera_fisheye_optical_frame - fisheye_width: 0 - gyro_fps: 0 - gyro_frame_id: tracking_camera_gyro_frame - gyro_optical_frame_id: tracking_camera_gyro_optical_frame - imu_optical_frame_id: tracking_camera_imu_optical_frame - infra1_frame_id: tracking_camera_infra1_frame - infra1_optical_frame_id: tracking_camera_infra1_optical_frame - infra2_frame_id: tracking_camera_infra2_frame - infra2_optical_frame_id: tracking_camera_infra2_optical_frame - infra_fps: 30 - infra_height: 480 - infra_rgb: false - infra_width: 640 - initial_reset: false - json_file_path: '' - linear_accel_cov: 0.01 - odom_frame_id: tracking_camera_odom_frame - ordered_pc: false - pointcloud_texture_index: 0 - pointcloud_texture_stream: RS2_STREAM_COLOR - pose_frame_id: tracking_camera_pose_frame - pose_optical_frame_id: tracking_camera_pose_optical_frame - publish_odom_tf: false - publish_tf: false - reconnect_timeout: 6.0 - rosbag_filename: '' - serial_no: '' - stereo_module: - exposure: - '1': 7500 - '2': 1 - gain: - '1': 16 - '2': 16 - tf_publish_rate: 0.0 - topic_odom_in: /control/smb_diff_drive/odom - unite_imu_method: linear_interpolation - usb_port_id: '' - wait_for_device_timeout: -1.0 - tracking_module: - enable_mapping: false - enable_pose_jumping: false - enable_relocalization: false -versavis_imu_receiver: - imu_acceleration_covariance: 0.043864908 - imu_accelerator_sensitivity: 0.000833 - imu_gyro_covariance: 6e-9 - imu_gyro_sensitivity: 0.04 - imu_pub_topic: /imu - imu_sub_topic: /versavis/imu_micro diff --git a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt new file mode 100644 index 00000000..11d3d545 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt @@ -0,0 +1,141 @@ +cmake_minimum_required(VERSION 3.16) +project(smb_estimator_graph_ros2) + +## Compile as C++17 +add_compile_options(-std=c++17) +set(CMAKE_CXX_FLAGS_RELEASE "-O3") + +# For FindGlog.cmake and FindGflags.cmake +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") + +# Find Dependencies --------------------------------------------------------------------------------------------------- +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Glog REQUIRED) +find_package(graph_msf_ros2_msgs REQUIRED + COMPONENTS rosidl_generator_c rosidl_typesupport_cpp +) + + +# Check if the target from graph_msf is already defined +if(NOT TARGET graph_msf::graph_msf) + find_package(graph_msf REQUIRED) +endif() +if(NOT TARGET graph_msf_ros2::graph_msf_ros2) + find_package(graph_msf_ros2 REQUIRED) +endif() + +# Non propagated ros dependencies +find_package(std_srvs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) + +# Display Eigen version and path +message("Eigen Version: ${EIGEN3_VERSION_STRING}") +message("Eigen Path: ${Eigen3_DIR}") + +# Color +if(NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif() + +# Define include directories +include_directories( + include + ${std_srvs_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} +) + +# Library +add_library(${PROJECT_NAME} + src/lib/SmbEstimator.cpp + src/lib/readParams.cpp + src/lib/SmbStaticTransforms.cpp +) + +# Link dependencies +target_link_libraries(${PROJECT_NAME} + graph_msf_ros2::graph_msf_ros2 + ${GLOG_LIBRARIES} + ${gflags_LIBRARIES} +) + +# Print the include directories for debugging +get_target_property(INC_DIR graph_msf::graph_msf INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf::graph_msf: ${INC_DIR}") +get_target_property(INC_DIR graph_msf_ros2::graph_msf_ros2 INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf_ros2::graph_msf_ros2: ${INC_DIR}") + +# Ament dependencies +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + Glog + gflags + graph_msf_ros2_msgs +) + +# Executable +add_executable(${PROJECT_NAME}_node src/smb_estimator_node.cpp) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros +) + +# Add clang tooling +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) + add_clang_tooling( + TARGET ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_FIX + ) +endif() + +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# Mark other files for installation +install(DIRECTORY launch rviz config + DESTINATION share/${PROJECT_NAME} +) + +# Export include directories +ament_export_include_directories(include) + +# Finalize ament package +ament_package() diff --git a/ros2/examples/smb_estimator_graph_ros2/FindGflags.cmake b/ros2/examples/smb_estimator_graph_ros2/FindGflags.cmake new file mode 100644 index 00000000..7ad5b202 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/FindGflags.cmake @@ -0,0 +1,591 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGflags.cmake - Find Google gflags logging library. +# +# This module will attempt to find gflags, either via an exported CMake +# configuration (generated by gflags >= 2.1 which are built with CMake), or +# by performing a standard search for all gflags components. The order of +# precedence for these two methods of finding gflags is controlled by: +# GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION. +# +# This module defines the following variables: +# +# GFLAGS_FOUND: TRUE iff gflags is found. +# GFLAGS_INCLUDE_DIRS: Include directories for gflags. +# GFLAGS_LIBRARIES: Libraries required to link gflags. +# GFLAGS_NAMESPACE: The namespace in which gflags is defined. In versions of +# gflags < 2.1, this was google, for versions >= 2.1 it is +# by default gflags, although can be configured when building +# gflags to be something else (i.e. google for legacy +# compatibility). +# FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION: True iff the version of gflags +# found was built & installed / +# exported as a CMake package. +# +# The following variables control the behaviour of this module when an exported +# gflags CMake configuration is not found. +# +# GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by gflags >= 2.1 over searching for the +# gflags components manually. Otherwise (FALSE) +# ignore any exported gflags CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GFLAGS_INCLUDE_DIR_HINTS or GFLAGS_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GFLAGS_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for gflags includes, e.g: /timbuktu/include. +# GFLAGS_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for gflags libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GFLAGS_INCLUDE_DIR: Include directory for gflags, not including the +# include directory of any dependencies. +# GFLAGS_LIBRARY: gflags library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when FindGflags was +# invoked, necessary for MSVC. +macro(GFLAGS_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif() +endmacro(GFLAGS_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find gflags or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(GFLAGS_REPORT_NOT_FOUND REASON_MSG) + unset(GFLAGS_FOUND) + unset(GFLAGS_INCLUDE_DIRS) + unset(GFLAGS_LIBRARIES) + # Do not use unset, as we want to keep GFLAGS_NAMESPACE in the cache, + # but simply clear its value. + set(GFLAGS_NAMESPACE "" CACHE STRING + "gflags namespace (google or gflags)" FORCE) + + # Make results of search visible in the CMake GUI if gflags has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GFLAGS_INCLUDE_DIR + GFLAGS_LIBRARY + GFLAGS_NAMESPACE) + + gflags_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Gflags_FIND_QUIETLY) + message(STATUS "Failed to find gflags - " ${REASON_MSG} ${ARGN}) + elseif (Gflags_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find gflags - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find gflags - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GFLAGS_REPORT_NOT_FOUND) + +# Verify that all variable names passed as arguments are defined (can be empty +# but must be defined) or raise a fatal error. +macro(GFLAGS_CHECK_VARS_DEFINED) + foreach(CHECK_VAR ${ARGN}) + if (NOT DEFINED ${CHECK_VAR}) + message(FATAL_ERROR "open3d_slam Bug: ${CHECK_VAR} is not defined.") + endif() + endforeach() +endmacro(GFLAGS_CHECK_VARS_DEFINED) + +# Use check_cxx_source_compiles() to compile trivial test programs to determine +# the gflags namespace. This works on all OSs except Windows. If using Visual +# Studio, it fails because msbuild forces check_cxx_source_compiles() to use +# CMAKE_BUILD_TYPE=Debug for the test project, which usually breaks detection +# because MSVC requires that the test project use the same build type as gflags, +# which would normally be built in Release. +# +# Defines: GFLAGS_NAMESPACE in the caller's scope with the detected namespace, +# which is blank (empty string, will test FALSE is CMake conditionals) +# if detection failed. +function(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE) + # Verify that all required variables are defined. + gflags_check_vars_defined( + GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY) + # Ensure that GFLAGS_NAMESPACE is always unset on completion unless + # we explicitly set if after having the correct namespace. + set(GFLAGS_NAMESPACE "" PARENT_SCOPE) + + include(CheckCXXSourceCompiles) + # Setup include path & link library for gflags for CHECK_CXX_SOURCE_COMPILES. + set(CMAKE_REQUIRED_INCLUDES ${GFLAGS_INCLUDE_DIR}) + set(CMAKE_REQUIRED_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) + # First try the (older) google namespace. Note that the output variable + # MUST be unique to the build type as otherwise the test is not repeated as + # it is assumed to have already been performed. + check_cxx_source_compiles( + "#include + int main(int argc, char * argv[]) { + google::ParseCommandLineFlags(&argc, &argv, true); + return 0; + }" + GFLAGS_IN_GOOGLE_NAMESPACE) + if (GFLAGS_IN_GOOGLE_NAMESPACE) + set(GFLAGS_NAMESPACE google PARENT_SCOPE) + return() + endif() + + # Try (newer) gflags namespace instead. Note that the output variable + # MUST be unique to the build type as otherwise the test is not repeated as + # it is assumed to have already been performed. + set(CMAKE_REQUIRED_INCLUDES ${GFLAGS_INCLUDE_DIR}) + set(CMAKE_REQUIRED_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) + check_cxx_source_compiles( + "#include + int main(int argc, char * argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + return 0; + }" + GFLAGS_IN_GFLAGS_NAMESPACE) + if (GFLAGS_IN_GFLAGS_NAMESPACE) + set(GFLAGS_NAMESPACE gflags PARENT_SCOPE) + return() + endif (GFLAGS_IN_GFLAGS_NAMESPACE) +endfunction(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE) + +# Use regex on the gflags headers to attempt to determine the gflags namespace. +# Checks both gflags.h (contained namespace on versions < 2.1.2) and +# gflags_declare.h, which contains the namespace on versions >= 2.1.2. +# In general, this method should only be used when +# GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE() cannot be used, or has +# failed. +# +# Defines: GFLAGS_NAMESPACE in the caller's scope with the detected namespace, +# which is blank (empty string, will test FALSE is CMake conditionals) +# if detection failed. +function(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_REGEX) + # Verify that all required variables are defined. + gflags_check_vars_defined(GFLAGS_INCLUDE_DIR) + # Ensure that GFLAGS_NAMESPACE is always undefined on completion unless + # we explicitly set if after having the correct namespace. + set(GFLAGS_NAMESPACE "" PARENT_SCOPE) + + # Scan gflags.h to identify what namespace gflags was built with. On + # versions of gflags < 2.1.2, gflags.h was configured with the namespace + # directly, on >= 2.1.2, gflags.h uses the GFLAGS_NAMESPACE #define which + # is defined in gflags_declare.h, we try each location in turn. + set(GFLAGS_HEADER_FILE ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + if (NOT EXISTS ${GFLAGS_HEADER_FILE}) + gflags_report_not_found( + "Could not find file: ${GFLAGS_HEADER_FILE} " + "containing namespace information in gflags install located at: " + "${GFLAGS_INCLUDE_DIR}.") + endif() + file(READ ${GFLAGS_HEADER_FILE} GFLAGS_HEADER_FILE_CONTENTS) + + string(REGEX MATCH "namespace [A-Za-z]+" + GFLAGS_NAMESPACE "${GFLAGS_HEADER_FILE_CONTENTS}") + string(REGEX REPLACE "namespace ([A-Za-z]+)" "\\1" + GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}") + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to extract gflags namespace from header file: " + "${GFLAGS_HEADER_FILE}.") + endif (NOT GFLAGS_NAMESPACE) + + if (GFLAGS_NAMESPACE STREQUAL "google" OR + GFLAGS_NAMESPACE STREQUAL "gflags") + # Found valid gflags namespace from gflags.h. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" PARENT_SCOPE) + return() + endif() + + # Failed to find gflags namespace from gflags.h, gflags is likely a new + # version, check gflags_declare.h, which in newer versions (>= 2.1.2) contains + # the GFLAGS_NAMESPACE #define, which is then referenced in gflags.h. + set(GFLAGS_DECLARE_FILE ${GFLAGS_INCLUDE_DIR}/gflags/gflags_declare.h) + if (NOT EXISTS ${GFLAGS_DECLARE_FILE}) + gflags_report_not_found( + "Could not find file: ${GFLAGS_DECLARE_FILE} " + "containing namespace information in gflags install located at: " + "${GFLAGS_INCLUDE_DIR}.") + endif() + file(READ ${GFLAGS_DECLARE_FILE} GFLAGS_DECLARE_FILE_CONTENTS) + + string(REGEX MATCH "#define GFLAGS_NAMESPACE [A-Za-z]+" + GFLAGS_NAMESPACE "${GFLAGS_DECLARE_FILE_CONTENTS}") + string(REGEX REPLACE "#define GFLAGS_NAMESPACE ([A-Za-z]+)" "\\1" + GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}") + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to extract gflags namespace from declare file: " + "${GFLAGS_DECLARE_FILE}.") + endif (NOT GFLAGS_NAMESPACE) + + if (GFLAGS_NAMESPACE STREQUAL "google" OR + GFLAGS_NAMESPACE STREQUAL "gflags") + # Found valid gflags namespace from gflags.h. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" PARENT_SCOPE) + return() + endif() +endfunction(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_REGEX) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GFLAGS_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GFLAGS_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# gflags CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a gflags exported configuration if available. +if (NOT DEFINED GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION + AND NOT GFLAGS_INCLUDE_DIR_HINTS + AND NOT GFLAGS_LIBRARY_DIR_HINTS) + message(STATUS "No preference for use of exported gflags CMake configuration " + "set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported gflags CMake configuration " + "if available.") + set(GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION TRUE) +endif() + +if (GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for gflags, as generated by + # gflags versions >= 2.1. + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(gflags QUIET + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (gflags_FOUND) + message(STATUS "Found installed version of gflags: ${gflags_DIR}") + else(gflags_FOUND) + # Failed to find an installed version of gflags, repeat search allowing + # exported build directories. + message(STATUS "Failed to find installed gflags CMake configuration, " + "searching for gflags build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that gflags is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(gflags QUIET + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (gflags_FOUND) + message(STATUS "Found exported gflags build directory: ${gflags_DIR}") + endif(gflags_FOUND) + endif(gflags_FOUND) + + set(FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION ${gflags_FOUND}) + + # gflags v2.1 - 2.1.2 shipped with a bug in their gflags-config.cmake [1] + # whereby gflags_LIBRARIES = "gflags", but there was no imported target + # called "gflags", they were called: gflags[_nothreads]-[static/shared]. + # As this causes linker errors when gflags is not installed in a location + # on the current library paths, detect if this problem is present and + # fix it. + # + # [1] https://github.com/gflags/gflags/issues/110 + if (gflags_FOUND) + # NOTE: This is not written as additional conditions in the outer + # if (gflags_FOUND) as the NOT TARGET "${gflags_LIBRARIES}" + # condition causes problems if gflags is not found. + if (${gflags_VERSION} VERSION_LESS 2.1.3 AND + NOT TARGET "${gflags_LIBRARIES}") + message(STATUS "Detected broken gflags install in: ${gflags_DIR}, " + "version: ${gflags_VERSION} <= 2.1.2 which defines gflags_LIBRARIES = " + "${gflags_LIBRARIES} which is not an imported CMake target, see: " + "https://github.com/gflags/gflags/issues/110. Attempting to fix by " + "detecting correct gflags target.") + # Ordering here expresses preference for detection, specifically we do not + # want to use the _nothreads variants if the full library is available. + list(APPEND CHECK_GFLAGS_IMPORTED_TARGET_NAMES + gflags-shared gflags-static + gflags_nothreads-shared gflags_nothreads-static) + foreach(CHECK_GFLAGS_TARGET ${CHECK_GFLAGS_IMPORTED_TARGET_NAMES}) + if (TARGET ${CHECK_GFLAGS_TARGET}) + message(STATUS "Found valid gflags target: ${CHECK_GFLAGS_TARGET}, " + "updating gflags_LIBRARIES.") + set(gflags_LIBRARIES ${CHECK_GFLAGS_TARGET}) + break() + endif() + endforeach() + if (NOT TARGET ${gflags_LIBRARIES}) + message(STATUS "Failed to fix detected broken gflags install in: " + "${gflags_DIR}, version: ${gflags_VERSION} <= 2.1.2, none of the " + "imported targets for gflags: ${CHECK_GFLAGS_IMPORTED_TARGET_NAMES} " + "are defined. Will continue with a manual search for gflags " + "components. We recommend you build/install a version of gflags > " + "2.1.2 (or master).") + set(FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION FALSE) + endif() + endif() + endif() + + if (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) + message(STATUS "Detected gflags version: ${gflags_VERSION}") + set(GFLAGS_FOUND ${gflags_FOUND}) + set(GFLAGS_INCLUDE_DIR ${gflags_INCLUDE_DIR}) + set(GFLAGS_LIBRARY ${gflags_LIBRARIES}) + + # gflags does not export the namespace in their CMake configuration, so + # use our function to determine what it should be, as it can be either + # gflags or google dependent upon version & configuration. + # + # NOTE: We use the regex method to determine the namespace here, as + # check_cxx_source_compiles() will not use imported targets, which + # is what gflags will be in this case. + gflags_check_gflags_namespace_using_regex() + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to determine gflags namespace using regex for gflags " + "version: ${gflags_VERSION} exported here: ${gflags_DIR} using CMake.") + endif (NOT GFLAGS_NAMESPACE) + else (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) + message(STATUS "Failed to find an installed/exported CMake configuration " + "for gflags, will perform search for installed gflags components.") + endif (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) +endif(GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION) + +if (NOT GFLAGS_FOUND) + # Either failed to find an exported gflags CMake configuration, or user + # told us not to use one. Perform a manual search for all gflags components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GFLAGS_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GFLAGS_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + list(APPEND GFLAGS_CHECK_PATH_SUFFIXES + gflags/include # Windows (for C:/Program Files prefix). + gflags/Include ) # Windows (for C:/Program Files prefix). + + list(APPEND GFLAGS_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + list(APPEND GFLAGS_CHECK_LIBRARY_SUFFIXES + gflags/lib # Windows (for C:/Program Files prefix). + gflags/Lib ) # Windows (for C:/Program Files prefix). + + # Search supplied hint directories first if supplied. + find_path(GFLAGS_INCLUDE_DIR + NAMES gflags/gflags.h + HINTS ${GFLAGS_INCLUDE_DIR_HINTS} + PATHS ${GFLAGS_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GFLAGS_CHECK_PATH_SUFFIXES}) + if (NOT GFLAGS_INCLUDE_DIR OR + NOT EXISTS ${GFLAGS_INCLUDE_DIR}) + gflags_report_not_found( + "Could not find gflags include directory, set GFLAGS_INCLUDE_DIR " + "to directory containing gflags/gflags.h") + endif (NOT GFLAGS_INCLUDE_DIR OR + NOT EXISTS ${GFLAGS_INCLUDE_DIR}) + + find_library(GFLAGS_LIBRARY NAMES gflags + HINTS ${GFLAGS_LIBRARY_DIR_HINTS} + PATHS ${GFLAGS_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GFLAGS_CHECK_LIBRARY_SUFFIXES}) + if (NOT GFLAGS_LIBRARY OR + NOT EXISTS ${GFLAGS_LIBRARY}) + gflags_report_not_found( + "Could not find gflags library, set GFLAGS_LIBRARY " + "to full path to libgflags.") + endif (NOT GFLAGS_LIBRARY OR + NOT EXISTS ${GFLAGS_LIBRARY}) + + # gflags typically requires a threading library (which is OS dependent), note + # that this defines the CMAKE_THREAD_LIBS_INIT variable. If we are able to + # detect threads, we assume that gflags requires it. + find_package(Threads QUIET) + set(GFLAGS_LINK_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) + # On Windows (including MinGW), the Shlwapi library is used by gflags if + # available. + if (WIN32) + include(CheckIncludeFileCXX) + check_include_file_cxx("shlwapi.h" HAVE_SHLWAPI) + if (HAVE_SHLWAPI) + list(APPEND GFLAGS_LINK_LIBRARIES shlwapi.lib) + endif(HAVE_SHLWAPI) + endif (WIN32) + + # Mark internally as found, then verify. GFLAGS_REPORT_NOT_FOUND() unsets + # if called. + set(GFLAGS_FOUND TRUE) + + # Identify what namespace gflags was built with. + if (GFLAGS_INCLUDE_DIR AND NOT GFLAGS_NAMESPACE) + # To handle Windows peculiarities / CMake bugs on MSVC we try two approaches + # to detect the gflags namespace: + # + # 1) Try to use check_cxx_source_compiles() to compile a trivial program + # with the two choices for the gflags namespace. + # + # 2) [In the event 1) fails] Use regex on the gflags headers to try to + # determine the gflags namespace. Whilst this is less robust than 1), + # it does avoid any interaction with msbuild. + gflags_check_gflags_namespace_using_try_compile() + + if (NOT GFLAGS_NAMESPACE) + # Failed to determine gflags namespace using check_cxx_source_compiles() + # method, try and obtain it using regex on the gflags headers instead. + message(STATUS "Failed to find gflags namespace using using " + "check_cxx_source_compiles(), trying namespace regex instead, " + "this is expected on Windows.") + gflags_check_gflags_namespace_using_regex() + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to determine gflags namespace either by " + "check_cxx_source_compiles(), or namespace regex.") + endif (NOT GFLAGS_NAMESPACE) + endif (NOT GFLAGS_NAMESPACE) + endif (GFLAGS_INCLUDE_DIR AND NOT GFLAGS_NAMESPACE) + + # Make the GFLAGS_NAMESPACE a cache variable s/t the user can view it, and could + # overwrite it in the CMake GUI. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" CACHE STRING + "gflags namespace (google or gflags)" FORCE) + + # gflags does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GFLAGS_NAMESPACE in the cache / GUI + # with an invalid value. + if (GFLAGS_NAMESPACE AND + NOT GFLAGS_NAMESPACE STREQUAL "google" AND + NOT GFLAGS_NAMESPACE STREQUAL "gflags") + gflags_report_not_found( + "Caller defined GFLAGS_NAMESPACE:" + " ${GFLAGS_NAMESPACE} is not valid, not google or gflags.") + endif () + # Catch case when caller has set GFLAGS_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GFLAGS_INCLUDE_DIR AND + NOT EXISTS ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + gflags_report_not_found( + "Caller defined GFLAGS_INCLUDE_DIR:" + " ${GFLAGS_INCLUDE_DIR} does not contain gflags/gflags.h header.") + endif (GFLAGS_INCLUDE_DIR AND + NOT EXISTS ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + # TODO: This regex for gflags library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GFLAGS_LIBRARY}" LOWERCASE_GFLAGS_LIBRARY) + if (GFLAGS_LIBRARY AND + NOT "${LOWERCASE_GFLAGS_LIBRARY}" MATCHES ".*gflags[^/]*") + gflags_report_not_found( + "Caller defined GFLAGS_LIBRARY: " + "${GFLAGS_LIBRARY} does not match gflags.") + endif (GFLAGS_LIBRARY AND + NOT "${LOWERCASE_GFLAGS_LIBRARY}" MATCHES ".*gflags[^/]*") + + gflags_reset_find_library_prefix() + +endif(NOT GFLAGS_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GFLAGS_FOUND) + set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR}) + set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) +endif (GFLAGS_FOUND) + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Gflags DEFAULT_MSG + GFLAGS_INCLUDE_DIRS GFLAGS_LIBRARIES GFLAGS_NAMESPACE) + +# Only mark internal variables as advanced if we found gflags, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GFLAGS_FOUND) + mark_as_advanced(FORCE GFLAGS_INCLUDE_DIR + GFLAGS_LIBRARY + GFLAGS_NAMESPACE + gflags_DIR) # Autogenerated by find_package(gflags) +endif (GFLAGS_FOUND) diff --git a/ros2/examples/smb_estimator_graph_ros2/FindGlog.cmake b/ros2/examples/smb_estimator_graph_ros2/FindGlog.cmake new file mode 100644 index 00000000..979dceda --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/FindGlog.cmake @@ -0,0 +1,346 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGlog.cmake - Find Google glog logging library. +# +# This module defines the following variables: +# +# GLOG_FOUND: TRUE iff glog is found. +# GLOG_INCLUDE_DIRS: Include directories for glog. +# GLOG_LIBRARIES: Libraries required to link glog. +# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found +# was built & installed / exported +# as a CMake package. +# +# The following variables control the behaviour of this module: +# +# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by glog > 0.3.4 over searching for the +# glog components manually. Otherwise (FALSE) +# ignore any exported glog CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for glog includes, e.g: /timbuktu/include. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for glog libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GLOG_INCLUDE_DIR: Include directory for glog, not including the +# include directory of any dependencies. +# GLOG_LIBRARY: glog library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindGlog was invoked. +macro(GLOG_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif() +endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find glog or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(GLOG_REPORT_NOT_FOUND REASON_MSG) + unset(GLOG_FOUND) + unset(GLOG_INCLUDE_DIRS) + unset(GLOG_LIBRARIES) + # Make results of search visible in the CMake GUI if glog has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GLOG_INCLUDE_DIR + GLOG_LIBRARY) + + glog_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Glog_FIND_QUIETLY) + message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) + elseif (Glog_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GLOG_REPORT_NOT_FOUND) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GLOG_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GLOG_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# glog CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a glog exported configuration if available. +if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION + AND NOT GLOG_INCLUDE_DIR_HINTS + AND NOT GLOG_LIBRARY_DIR_HINTS) + message(STATUS "No preference for use of exported glog CMake configuration " + "set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported glog CMake configuration " + "if available.") + set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) +endif() + +if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for glog, as generated by + # glog versions > 0.3.4 + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its + # project name when built with CMake, but exports itself as just 'glog'. + # On Linux/OS X this does not break detection as the project name is + # not used as part of the install path for the CMake package files, + # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded + # in glog's CMakeLists. However, on Windows the project name *is* + # part of the install prefix: C:/Program Files/google-glog/[include,lib]. + # However, by default CMake checks: + # C:/Program Files/ which does not + # exist and thus detection fails. Thus we use the NAMES to force the + # search to use both google-glog & glog. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + message(STATUS "Found installed version of glog: ${glog_DIR}") + else() + # Failed to find an installed version of glog, repeat search allowing + # exported build directories. + message(STATUS "Failed to find installed glog CMake configuration, " + "searching for glog build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + message(STATUS "Found exported glog build directory: ${glog_DIR}") + endif(glog_FOUND) + endif(glog_FOUND) + + set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) + + if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + message(STATUS "Detected glog version: ${glog_VERSION}") + set(GLOG_FOUND ${glog_FOUND}) + # glog wraps the include directories into the exported glog::glog target. + set(GLOG_INCLUDE_DIR "") + set(GLOG_LIBRARY glog::glog) + else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + message(STATUS "Failed to find an installed/exported CMake configuration " + "for glog, will perform search for installed glog components.") + endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) +endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + +if (NOT GLOG_FOUND) + # Either failed to find an exported glog CMake configuration, or user + # told us not to use one. Perform a manual search for all glog components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GLOG_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + google-glog/include # CMake installs with project name prefix. + google-glog/Include) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + google-glog/lib # CMake installs with project name prefix. + google-glog/Lib) + + # Search supplied hint directories first if supplied. + find_path(GLOG_INCLUDE_DIR + NAMES glog/logging.h + HINTS ${GLOG_INCLUDE_DIR_HINTS} + PATHS ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) + if (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + glog_report_not_found( + "Could not find glog include directory, set GLOG_INCLUDE_DIR " + "to directory containing glog/logging.h") + endif (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + + find_library(GLOG_LIBRARY NAMES glog + HINTS ${GLOG_LIBRARY_DIR_HINTS} + PATHS ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) + if (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + glog_report_not_found( + "Could not find glog library, set GLOG_LIBRARY " + "to full path to libglog.") + endif (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + + # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets + # if called. + set(GLOG_FOUND TRUE) + + # Glog does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + glog_report_not_found( + "Caller defined GLOG_INCLUDE_DIR:" + " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") + endif (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + # TODO: This regex for glog library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) + if (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + glog_report_not_found( + "Caller defined GLOG_LIBRARY: " + "${GLOG_LIBRARY} does not match glog.") + endif (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + + glog_reset_find_library_prefix() + +endif(NOT GLOG_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) +endif (GLOG_FOUND) + +# If we are using an exported CMake glog target, the include directories are +# wrapped into the target itself, and do not have to be (and are not) +# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS +# to the list of required variables in order that glog be reported as found. +if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) +else() + set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) +endif() + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Glog DEFAULT_MSG + ${GLOG_REQUIRED_VARIABLES}) + +# Only mark internal variables as advanced if we found glog, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GLOG_FOUND) + mark_as_advanced(FORCE GLOG_INCLUDE_DIR + GLOG_LIBRARY + glog_DIR) # Autogenerated by find_package(glog) +endif (GLOG_FOUND) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml new file mode 100644 index 00000000..8a042bf7 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml @@ -0,0 +1,16 @@ +/smb_estimator_node: + ros__parameters: + #External Prior/Transform Input + extrinsics: + # Published by GMSF + worldFrame: "world_graph_msf" + odomFrame: "odom_graph_msf" + # Used for estimation + imuFrame: "imu_link" + baseLinkFrame: "base_link" + initializeZeroYawAndPositionOfFrame: "base_link" # Initialize the yaw and position of the base frame to zero + # this is just meant to avoid any jump after getting the first true measurements + + name_ids: + referenceFrameAligned: "_graph_msf_aligned" # Specifies what is the suffix of the aligned published fixed frames + sensorFrameCorrected: "_graph_msf_corrected" # Specifies what is the suffix of the corrected published sensor frames diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_config.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_config.yaml new file mode 100644 index 00000000..79c42f89 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_config.yaml @@ -0,0 +1,8 @@ +/smb_estimator_node: + ros__parameters: + #Common + common_params: + verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors + odomNotJumpAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) + logRealTimeStateToMemory: false # Whether the real-time state should be logged to memory (for logging purposes) + logLatencyAndUpdateDurationToMemory: false # Whether the latency and update duration should be logged to memory (for logging purposes) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml new file mode 100644 index 00000000..e3ef892a --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml @@ -0,0 +1,81 @@ +/smb_estimator_node: + ros__parameters: + # Sensor Params + sensor_params: + imuRate: 200.0 # Rate of IMU input (Hz) + createStateEveryNthImuMeasurement: 5 # Create a new state every n-th IMU measurement + useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements + imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter + imuBufferLength: 800 + imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot + + # Initialization + initialization_params: + estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU + gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity + + # Factor Graph + graph_params: + realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] + realTimeSmootherUseIsam: true + realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable + useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother + slowBatchSmootherUseIsam: false # if false, then levenberg-marquardt is used for the slow batch smoother, NO EFFECT YET + slowBatchSmootherUseCholeskyFactorization: false # CHOLESKY faster, QR numerically more stable + # Optimizer Config + gaussNewtonWildfireThreshold: 0.001 # Threshold for the wildfire in the Gauss-Newton optimization + minOptimizationFrequency: 1.0 # Minimum optimization frequency [Hz], makes sure that optimization is triggered at least every x Hz + maxOptimizationFrequency: 10.0 # Maximum optimization frequency [Hz], can be used to control computational load + additionalOptimizationIterations: 0 # Additional iterations of graph optimizer after update with new factors + findUnusedFactorSlots: true + enableDetailedResults: false + usingBiasForPreIntegration: true # If true, the bias is used during pre-integration + useWindowForMarginalsComputation: true # If true, the window is used for marginal computation + windowSizeSecondsForMarginalsComputation: 300.0 # Size of the window for marginal computation, unit: seconds + # Alignment Parameters + optimizeReferenceFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world + referenceFramePosesResetThreshold: 8.0 # Reset T_M_W if distance between initial guess and optimization are too big, [SE(3) distance] + centerMeasurementsAtKeyframePositionBeforeAlignment: true # If true, the measurements are centered before alignment + createReferenceAlignmentKeyframeEveryNSeconds: 10.0 # Create a new keyframe for alignment every n seconds + # Extrinsic Calibration + optimizeExtrinsicSensorToSensorCorrectedOffset: false # If true, the extrinsic calibration is optimized + + # Noise Parameters + noise_params: + ## IMU + ### Position + accNoiseDensity: 2.2555e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) + integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) + use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect + ### Rotation + gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) + omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 + ### Bias + accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) + gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) + biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) + accBiasPrior: 0.0 # Prior/starting value of accelerometer bias + gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias + ## Initial State + initialPositionNoiseDensity: 1.0e-04 # Prior/starting value of position + initialOrientationNoiseDensity: 1.0e-03 # Prior/starting value of orientation + initialVelocityNoiseDensity: 1.0e-02 # Prior/starting value of velocity + initialAccBiasNoiseDensity: 1.0e-03 # Prior/starting value of accelerometer bias + initialGyroBiasNoiseDensity: 1.0e-03 # Prior/starting value of gyroscope bias + + # Relinearization + relinearization_params: + positionReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Position linearization threshold + rotationReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Rotation linearization threshold + velocityReLinTh: 1.0e-03 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold + accBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold + gyrBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold + referenceFrameReLinTh: 1.0e-03 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld + calibrationReLinTh: 1.0e-03 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + displacementReLinTh: 1.0e-03 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + landmarkReLinTh: 1.0e-03 # Letter "l" in GTSAM variables, Landmark linearization threshold + relinearizeSkip: 10 # Re-linearization is skipped every n-th step, default: 10 + enableRelinearization: true # Whether to use relinearization, default: true + evaluateNonlinearError: true # Whether to evaluate the nonlinear error before and after the update, default: false + cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true + enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml new file mode 100644 index 00000000..765ec560 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml @@ -0,0 +1,9 @@ +/smb_estimator_node: + ros__parameters: + #External Prior/Transform Input + extrinsics: + lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame + wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame + vioOdometryFrame: "base_link" #VIO odometry frame name - used to lookup VIO-to-ExternalSensor Frame diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml new file mode 100644 index 00000000..01da7795 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -0,0 +1,32 @@ +/smb_estimator_node: + ros__parameters: + # Sensor Params + sensor_params: + ## Config + useLioOdometry: true + useWheelOdometryBetween: false + useWheelLinearVelocities: false + useVioOdometry: false + ## Rates + lioOdometryRate: 10 + wheelOdometryBetweenRate: 100 + wheelLinearVelocitiesRate: 100 + vioOdometryRate: 50 + # Wheel Radius + wheelRadius: 0.195 # meters + + # Alignment Parameters + alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioSe3AlignmentRandomWalk: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + + # Noise Parameters + noise_params: + ## LiDAR + lioPoseUnaryNoiseDensity: [ 0.02, 0.02, 0.02, 0.002, 0.002, 0.002 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + ## Wheel + wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + wheelLinearVelocitiesNoiseDensity: [ 0.2, 0.1, 0.01 ] # StdDev, ORDER VxVyVz(meters/sec) + ## VIO + vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h new file mode 100644 index 00000000..da8ea593 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h @@ -0,0 +1,120 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once + +// std +#include +#include + +// ROS 2 +#include +#include +#include +#include +#include +#include +#include + +// Workspace +#include "graph_msf_ros2/GraphMsfRos2.h" + +// Defined Macros +#define ROS_QUEUE_SIZE 100 +#define NUM_GNSS_CALLBACKS_UNTIL_START 20 // 0 + +namespace smb_se { + +class SmbEstimator : public graph_msf::GraphMsfRos2 { + public: + explicit SmbEstimator(std::shared_ptr& node); + + ~SmbEstimator() override = default; + + void setup(); + + protected: + // Virtual Functions + void readParams(); + void initializePublishers(); + void initializeSubscribers(); + void initializeMessages(); + void initializeServices(); + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); + + private: + // Time + std::chrono::time_point startTime_; + std::chrono::time_point currentTime_; + + // Config ------------------------------------- + + // Rates + double lioOdometryRate_ = 5.0; + double wheelOdometryBetweenRate_ = 50.0; + double wheelLinearVelocitiesRate_ = 50.0; + double vioOdometryRate_ = 50.0; + + // Alignment Parameters + Eigen::Matrix initialSe3AlignmentNoise_ = 1.0 * Eigen::Matrix::Ones(); + Eigen::Matrix lioSe3AlignmentRandomWalk_ = 0.0 * Eigen::Matrix::Ones(); + + // Noise + // LiDAR Odometry + Eigen::Matrix lioPoseUnaryNoise_; + // Wheel Odometry + // Between + Eigen::Matrix wheelPoseBetweenNoise_; + // Linear Velocities + Eigen::Matrix wheelLinearVelocitiesNoise_; + // VIO Odometry + Eigen::Matrix vioPoseBetweenNoise_; + + // ROS Related stuff ---------------------------- + + // Callbacks + // LiDAR + void lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& lidarOdomPtr); + // Wheel Between + void wheelOdometryPoseCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& wheelOdomPtr); + // Wheel Linear Velocities + void wheelLinearVelocitiesCallback_(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& wheelsSpeedsPtr); + // VIO + void vioOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& vioOdomPtr); + + // Callback Members + int wheelOdometryCallbackCounter_ = -1; + Eigen::Isometry3d T_O_Bw_km1_; + double wheelOdometryTimeKm1_ = 0.0; + int num_imu_errors_ = 0; + rclcpp::Time last_imu_timestamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + // Subscribers + rclcpp::Subscription::SharedPtr subLioOdometry_; + rclcpp::Subscription::SharedPtr subWheelOdometryBetween_; + rclcpp::Subscription::SharedPtr subWheelLinearVelocities_; + rclcpp::Subscription::SharedPtr subVioOdometry_; + + // Publishers + rclcpp::Publisher::SharedPtr pubMeasMapLioPath_; + rclcpp::Publisher::SharedPtr pubMeasMapVioPath_; + + // Messages + std::shared_ptr measLio_mapImuPathPtr_; + std::shared_ptr measVio_mapImuPathPtr_; + + // Flags + bool useLioOdometryFlag_ = true; + bool useWheelOdometryBetweenFlag_ = false; + bool useWheelLinearVelocitiesFlag_ = false; + bool useVioOdometryFlag_ = false; + + // Wheel Radius + double wheelRadiusMeter_ = 0.195; +}; + +} // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h new file mode 100644 index 00000000..78f990e4 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -0,0 +1,58 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once + +// Workspace +#include +#include +#include + +namespace smb_se { + +class SmbStaticTransforms : public graph_msf::StaticTransformsTf { + public: + SmbStaticTransforms(const std::shared_ptr& nodePtr); + + // Setters + void setLioOdometryFrame(const std::string& s) { lidarOdometryFrame_ = s; } + void setWheelOdometryBetweenFrame(const std::string& s) { wheelOdometryBetweenFrame_ = s; } + void setWheelLinearVelocityLeftFrame(const std::string& s) { wheelLinearVelocityLeftFrame_ = s; } + void setWheelLinearVelocityRightFrame(const std::string& s) { wheelLinearVelocityRightFrame_ = s; } + void setVioOdometryFrame(const std::string& s) { vioOdometryFrame_ = s; } + + // Getters + const std::string& getLioOdometryFrame() const { return lidarOdometryFrame_; } + const std::string& getWheelOdometryBetweenFrame() const { return wheelOdometryBetweenFrame_; } + const std::string& getWheelLinearVelocityLeftFrame() const { return wheelLinearVelocityLeftFrame_; } + const std::string& getWheelLinearVelocityRightFrame() const { return wheelLinearVelocityRightFrame_; } + const std::string& getVioOdometryFrame() const { return vioOdometryFrame_; } + + // Set flags + void setUseLioOdometryFlag(bool flag) { useLioOdometryFlag_ = flag; } + void setUseVioOdometryFlag(bool flag) { useVioOdometryFlag_ = flag; } + void setUseWheelOdometryBetweenFlag(bool flag) { useWheelOdometryBetweenFlag_ = flag; } + void setUseWheelLinearVelocitiesFlag(bool flag) { useWheelLinearVelocitiesFlag_ = flag; } + + private: + bool findTransformations() override; + + // Frames + std::string lidarOdometryFrame_; + std::string wheelOdometryBetweenFrame_; + std::string wheelLinearVelocityLeftFrame_; + std::string wheelLinearVelocityRightFrame_; + std::string vioOdometryFrame_; + + // Odometry flags + bool useLioOdometryFlag_ = false; + bool useVioOdometryFlag_ = false; + bool useWheelOdometryBetweenFlag_ = false; + bool useWheelLinearVelocitiesFlag_ = false; +}; + +} // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h new file mode 100644 index 00000000..f61e1f83 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h @@ -0,0 +1,15 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf/interface/Terminal.h" + +#ifndef SMB_CONSTANTS_H +#define SMB_CONSTANTS_H + +#define REGULAR_COUT std::cout << YELLOW_START << "SmbEstimator" << COLOR_END + +#endif // SMB_CONSTANTS_H diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py new file mode 100644 index 00000000..d17cf9f3 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -0,0 +1,106 @@ +from launch import LaunchDescription +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # Package name + pkg_name = "smb_estimator_graph_ros2" + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Declare launch arguments + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + imu_topic_name = LaunchConfiguration("imu_topic_name") + lidar_odometry_topic_name = LaunchConfiguration("lidar_odometry_topic_name") + wheel_odometry_topic_name = LaunchConfiguration("wheel_odometry_topic_name") + wheel_velocities_topic_name = LaunchConfiguration("wheel_velocities_topic_name") + vio_odometry_topic_name = LaunchConfiguration("vio_odometry_topic_name") + logging_dir_location = LaunchConfiguration("logging_dir_location") + + # Default config file paths + core_graph_config_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_config.yaml" + ) + core_graph_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_params.yaml" + ) + core_extrinsic_param_file = os.path.join( + pkg_dir, "config", "core", "core_extrinsic_params.yaml" + ) + smb_graph_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_graph_params.yaml" + ) + smb_extrinsic_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_extrinsic_params.yaml" + ) + + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument( + "use_sim_time", default_value=use_sim_time, description="Use simulation time" + ), + DeclareLaunchArgument( + "imu_topic_name", default_value="/imu/data_raw", description="IMU topic name" + ), + DeclareLaunchArgument( + "lidar_odometry_topic_name", + default_value="/open3d/scan2map_odometry", + description="Lidar odometry topic name", + ), + DeclareLaunchArgument( + "wheel_odometry_topic_name", + default_value="/wheel_odometry", + description="Wheel odometry topic name", + ), + DeclareLaunchArgument( + "wheel_velocities_topic_name", + default_value="/wheel_velocities", + description="Wheel velocities topic name", + ), + DeclareLaunchArgument( + "vio_odometry_topic_name", + default_value="/tracking_camera/odom/sample", + description="VIO odometry topic name", + ), + DeclareLaunchArgument( + "logging_dir_location", + default_value=os.path.join(pkg_dir, "logging"), + description="Logging directory location", + ), + # Set the /use_sim_time parameter for all nodes + launch.actions.SetLaunchConfiguration("use_sim_time", launch.substitutions.LaunchConfiguration("use_sim_time")), + # Main Node + Node( + package="smb_estimator_graph_ros2", + executable="smb_estimator_graph_ros2_node", + name="smb_estimator_node", + output="screen", + # prefix='gdb -ex run --args', + # prefix='xterm -e gdb -ex run --args', + # emulate_tty=True, + parameters=[ + {"use_sim_time": use_sim_time}, + {"launch/optimizationResultLoggingPath": logging_dir_location}, + core_graph_config_param_file, + core_graph_param_file, + core_extrinsic_param_file, + smb_graph_param_file, + smb_extrinsic_param_file, + ], + remappings=[ + ("/imu_topic", imu_topic_name), + ("/lidar_odometry_topic", lidar_odometry_topic_name), + ("/wheel_odometry_topic", wheel_odometry_topic_name), + ("/wheel_velocities_topic", wheel_velocities_topic_name), + ("/vio_odometry_topic", vio_odometry_topic_name), + ], + ), + ] + ) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py new file mode 100644 index 00000000..9cd1f390 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -0,0 +1,99 @@ +from launch import LaunchDescription +import launch +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # Package name + pkg_name = "smb_estimator_graph_ros2" + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Sim Time + use_sim_time_str = launch.substitutions.LaunchConfiguration( + "use_sim_time", default="true" + ).perform(launch.LaunchContext()) + use_sim_time = use_sim_time_str.lower() == "true" # Convert to Python bool + print(f"Use Sim Time: {use_sim_time}") + + # Declare launch arguments + imu_topic_name = LaunchConfiguration("imu_topic_name") + lidar_odometry_topic_name = LaunchConfiguration("lidar_odometry_topic_name") + core_graph_param_file = LaunchConfiguration("core_graph_param_file") + smb_graph_param_file = LaunchConfiguration("smb_graph_param_file") + + # Default config file paths + default_core_graph_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_params.yaml" + ) + default_smb_graph_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_graph_params.yaml" + ) + + return LaunchDescription( + [ + # Declare launch arguments + # A. Sim time + DeclareLaunchArgument( + "use_sim_time", + default_value=use_sim_time_str.lower(), + description="Use simulated time", + ), + # B. Topics + DeclareLaunchArgument( + "imu_topic_name", default_value="/imu/data_raw", description="IMU topic name" + ), + DeclareLaunchArgument( + "lidar_odometry_topic_name", + default_value="/open3d/scan2map_odometry", + description="Lidar odometry topic name", + ), + # C. Parameter files + DeclareLaunchArgument( + "core_graph_param_file", + default_value=default_core_graph_param_file, + description="Core graph parameter file", + ), + DeclareLaunchArgument( + "smb_graph_param_file", + default_value=default_smb_graph_param_file, + description="SMB graph parameter file", + ), + # D. Set the /use_sim_time parameter for all nodes + launch.actions.SetLaunchConfiguration( + "use_sim_time", launch.substitutions.LaunchConfiguration("use_sim_time") + ), + # E. Include the main launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(pkg_dir, "launch", "smb_estimator_graph.launch.py")] + ), + launch_arguments={ + "use_sim_time": launch.substitutions.LaunchConfiguration( + "use_sim_time" + ), + "imu_topic_name": imu_topic_name, + "lidar_odometry_topic_name": lidar_odometry_topic_name, + "core_graph_param_file": core_graph_param_file, + "smb_graph_param_file": smb_graph_param_file, + }.items(), + ), + # F. RVIZ Node + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", os.path.join(pkg_dir, "rviz", "rviz_config.rviz")], + parameters=[ + {"use_sim_time": use_sim_time}, + ], + output="screen", + ), + ] + ) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py new file mode 100644 index 00000000..4a4b71f9 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py @@ -0,0 +1,92 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Package name + pkg_name = 'smb_estimator_graph_ros2' + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Declare launch arguments + use_sim_time = LaunchConfiguration('use_sim_time') + imu_topic_name = LaunchConfiguration('imu_topic_name') + lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') + wheel_odometry_topic_name = LaunchConfiguration('wheel_odometry_topic_name') + wheel_velocities_topic_name = LaunchConfiguration('wheel_velocities_topic_name') + vio_odometry_topic_name = LaunchConfiguration('vio_odometry_topic_name') + logging_dir_location = LaunchConfiguration('logging_dir_location') + + # Default config file paths + core_graph_config_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_config.yaml') + core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') + core_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_extrinsic_params.yaml') + smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') + smb_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_extrinsic_params.yaml') + + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation time' + ), + DeclareLaunchArgument( + 'imu_topic_name', + default_value='/imu/data_raw', + description='IMU topic name' + ), + DeclareLaunchArgument( + 'lidar_odometry_topic_name', + default_value='/open3d/scan2map_odometry', + description='Lidar odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_odometry_topic_name', + default_value='/wheel_odometry', + description='Wheel odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_velocities_topic_name', + default_value='/wheel_velocities', + description='Wheel velocities topic name' + ), + DeclareLaunchArgument( + 'vio_odometry_topic_name', + default_value='/tracking_camera/odom/sample', + description='VIO odometry topic name' + ), + DeclareLaunchArgument( + 'logging_dir_location', + default_value=os.path.join(pkg_dir, 'logging'), + description='Logging directory location' + ), + + # Node + Node( + package='smb_estimator_graph_ros2', + executable='smb_estimator_graph_ros2_node', + name='smb_estimator_node', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'launch/optimizationResultLoggingPath': logging_dir_location}, + core_graph_config_param_file, + core_graph_param_file, + core_extrinsic_param_file, + smb_graph_param_file, + smb_extrinsic_param_file + ], + remappings=[ + ('/imu_topic', imu_topic_name), + ('/lidar_odometry_topic', lidar_odometry_topic_name), + ('/wheel_odometry_topic', wheel_odometry_topic_name), + ('/wheel_velocities_topic', wheel_velocities_topic_name), + ('/vio_odometry_topic', vio_odometry_topic_name) + ] + ) + ]) \ No newline at end of file diff --git a/ros2/examples/smb_estimator_graph_ros2/package.xml b/ros2/examples/smb_estimator_graph_ros2/package.xml new file mode 100644 index 00000000..6d237823 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/package.xml @@ -0,0 +1,40 @@ + + + smb_estimator_graph_ros2 + 0.0.0 + + + State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry + and GNSS estimates. + + + Julian Nubert + BSD-3-Clause + Julian Nubert + + + ament_cmake + + + graph_msf_ros2 + nav_msgs + std_msgs + sensor_msgs + tf2 + tf2_ros + rclcpp + graph_msf + glog + + + smb_description + + + ament_cmake_gtest + rosbag2 + + + + ament_cmake + + diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz new file mode 100644 index 00000000..74de00d5 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz @@ -0,0 +1,729 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /OdomImuOdometry1 + - /OdomImuOdometry1/Covariance1 + - /OdomImuOdometry1/Covariance1/Position1 + - /WorldImuOdometry1/Shape1 + - /WheelOdometry1/Shape1 + Splitter Ratio: 0.5529412031173706 + Tree Height: 1442 + - 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 + Name: Time + SyncMode: 0 + SyncSource: O3dMap +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 + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + LF_WHEEL: + Value: true + LH_WHEEL: + Value: true + RF_WHEEL: + Value: true + RH_WHEEL: + Value: true + base_inertia: + Value: true + base_link: + Value: true + imu: + Value: true + imu_link: + Value: true + lidar_mount_link: + Value: true + map_o3d: + Value: true + map_o3d_viz: + Value: true + odom: + Value: true + odom_graph_msf: + Value: true + raw_rs_o3d: + Value: true + rgb_camera_link: + Value: true + rgb_camera_optical_link: + Value: true + rslidar: + Value: true + top: + Value: true + tracking_camera_link: + Value: true + tracking_camera_pose_frame: + Value: true + world_graph_msf: + Value: true + Marker Alpha: 1 + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: + {} + lidar_mount_link: + rslidar: + map_o3d: + map_o3d_viz: + {} + raw_rs_o3d: + {} + odom_graph_msf: + world_graph_msf: + odom: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + top: + {} + tracking_camera_link: + tracking_camera_pose_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 16.27729034423828 + Min Color: 0; 0; 0 + Min Intensity: -3.2934958934783936 + Name: FilteredPointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /rslidar/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: OdomImuEstimated + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/est_path_odom_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 114; 159; 207 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: WorldImuEstimated + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/est_path_world_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: lio_MapImuMeasured + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/measLiDAR_path_map_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 193; 125; 17 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: vio_MapImuMeasured + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/measVIO_path_map_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 191; 64; 170 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: MapImuOptimized + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/opt_path_world_imu + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CompslamMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /compslam_lio/laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CompslamRegisteredCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /compslam_lio/registered_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: O3dMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /mapping/assembled_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: O3dRegisteredCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /mapping_node/raw_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 239; 41; 41 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: OdomImuOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 255; 255 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/est_odometry_odom_imu + Unreliable: false + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: WorldImuOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 114; 159; 207 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/est_odometry_world_imu + Unreliable: false + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: O3dOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 138; 226; 52 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /mapping_node/scan2map_odometry + Unreliable: false + Value: false + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: WheelOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /control/smb_diff_drive/odom + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_pose_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: smb_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: odom_graph_msf + 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002290000069afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000c00000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000069a0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000069afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e0000069a0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064f0000003efc0100000002fb0000000800540069006d006500000000000000064f0000077d00fffffffb0000000800540069006d00650100000000000004500000000000000000000009cb0000069a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3072 + X: 0 + Y: 960 diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz new file mode 100644 index 00000000..9cc7337b --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -0,0 +1,501 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + Splitter Ratio: 0.6352941393852234 + Tree Height: 1031 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_inertia: + Value: true + base_link: + Value: true + imu_link: + Value: true + imu_sensor: + Value: true + lidar_mount_link: + Value: true + odom: + Value: true + odom_graph_msf: + Value: true + odom_graph_msf_aligned: + Value: true + rgb_camera_link: + Value: true + rgb_camera_optical_link: + Value: true + rslidar: + Value: true + top: + Value: true + tracking_camera_link: + Value: true + tracking_camera_pose_frame: + Value: true + world_graph_msf: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_link: + base_footprint: + {} + base_inertia: + {} + imu_link: + imu_sensor: + {} + lidar_mount_link: + {} + odom_graph_msf: + world_graph_msf: + odom_graph_msf_aligned: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + rslidar: + {} + top: + {} + tracking_camera_link: + tracking_camera_pose_frame: + {} + Update Interval: 0 + Value: true + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: OdometryInOdom + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_odometry_odom_imu + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: PathInOdom + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_path_odom_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 85; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: PathInWorld + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_path_world_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: MeasPathLio + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/measLiDAR_path_map_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: OptimizedPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/opt_path_world_imu + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + tracking_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_pose_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/velocity_marker + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 204 + Min Color: 0; 0; 0 + Min Intensity: 14 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rslidar/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom_graph_msf + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 22.106815338134766 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1328 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000492fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000492000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000492fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000492000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007320000003efc0100000002fb0000000800540069006d00650100000000000007320000026600fffffffb0000000800540069006d00650100000000000004500000000000000000000005d60000049200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1842 + X: 3272 + Y: 29 diff --git a/ros2/examples/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py b/ros2/examples/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py new file mode 100644 index 00000000..3477d678 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import sys +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +from rosbag2_py import SequentialReader, StorageOptions, ConverterOptions +from rosbag2_py import SequentialWriter, StorageOptions as WriterStorageOptions +from tf2_msgs.msg import TFMessage +import yaml + +def filter_topics(in_bag, out_bag, frames_we_want): + # Setup reader + storage_options = StorageOptions(uri=in_bag, storage_id='sqlite3') + converter_options = ConverterOptions('', '') + reader = SequentialReader() + reader.open(storage_options, converter_options) + + # Setup writer + writer_storage_options = WriterStorageOptions(uri=out_bag, storage_id='sqlite3') + writer = SequentialWriter() + writer.open(writer_storage_options, converter_options) + + # Get topic types + topic_types = reader.get_all_topics_and_types() + type_map = {topic_info.name: topic_info.type for topic_info in topic_types} + + print("Writing to " + out_bag) + print("Reading from " + in_bag) + + while reader.has_next(): + topic_name, data, timestamp = reader.read_next() + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + + if topic_name == "/tf" and isinstance(msg, TFMessage): + transforms_to_keep = [] + for transform in msg.transforms: + if transform.header.frame_id in frames_we_want and transform.child_frame_id in frames_we_want: + transforms_to_keep.append(transform) + msg.transforms = transforms_to_keep + writer.write(topic_name, data, timestamp) + elif topic_name != '/tf': + writer.write(topic_name, data, timestamp) + +def filter_topics_2(in_bag, out_bag, frames_we_dont_want): + # Setup reader + storage_options = StorageOptions(uri=in_bag, storage_id='sqlite3') + converter_options = ConverterOptions('', '') + reader = SequentialReader() + reader.open(storage_options, converter_options) + + # Setup writer + writer_storage_options = WriterStorageOptions(uri=out_bag, storage_id='sqlite3') + writer = SequentialWriter() + writer.open(writer_storage_options, converter_options) + + # Get topic types + topic_types = reader.get_all_topics_and_types() + type_map = {topic_info.name: topic_info.type for topic_info in topic_types} + + print("Writing to " + out_bag) + print("Reading from " + in_bag) + + while reader.has_next(): + topic_name, data, timestamp = reader.read_next() + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + + if topic_name == "/tf" and isinstance(msg, TFMessage): + transforms_to_keep = [] + for transform in msg.transforms: + if transform.header.frame_id not in frames_we_dont_want and transform.child_frame_id not in frames_we_dont_want: + transforms_to_keep.append(transform) + msg.transforms = transforms_to_keep + writer.write(topic_name, data, timestamp) + elif topic_name != '/tf': + writer.write(topic_name, data, timestamp) + +if __name__ == '__main__': + print("Starting") + in_bag = sys.argv[1] + out_bag = sys.argv[2] + # filter_topics(in_bag, out_bag, ['base_link', 'odom', 'map', + # 'torso', 'Hip', 'Pelvis', 'Tibia', 'base_footprint']) + filter_topics_2(in_bag, out_bag, ['odom']) + print("Done") \ No newline at end of file diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp new file mode 100644 index 00000000..641e656b --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -0,0 +1,338 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph_ros2/SmbEstimator.h" + +// Project +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" + +// Workspace +#include "graph_msf/measurements/BinaryMeasurementXD.h" +#include "graph_msf/measurements/UnaryMeasurementXD.h" +#include "graph_msf_ros2/util/conversions.h" +#include "smb_estimator_graph_ros2/constants.h" + +namespace smb_se { + +SmbEstimator::SmbEstimator(std::shared_ptr& node) : graph_msf::GraphMsfRos2(node) { + REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; + + // Call setup after declaring parameters + SmbEstimator::setup(); +} + +void SmbEstimator::setup() { + REGULAR_COUT << GREEN_START << " SmbEstimator-Setup called." << COLOR_END << std::endl; + + // Boolean flags + node_->declare_parameter("sensor_params.useLioOdometry", false); + node_->declare_parameter("sensor_params.useWheelOdometryBetween", false); + node_->declare_parameter("sensor_params.useWheelLinearVelocities", false); + node_->declare_parameter("sensor_params.useVioOdometry", false); + + // Sensor parameters (int) + node_->declare_parameter("sensor_params.lioOdometryRate", 0); + node_->declare_parameter("sensor_params.wheelOdometryBetweenRate", 0); + node_->declare_parameter("sensor_params.wheelLinearVelocitiesRate", 0); + node_->declare_parameter("sensor_params.vioOdometryRate", 0); + + // Alignment parameters (vector of double) + node_->declare_parameter("alignment_params.initialSe3AlignmentNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("alignment_params.lioSe3AlignmentRandomWalk", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + + // Noise parameters (vectors of double) + node_->declare_parameter("noise_params.lioPoseUnaryNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.wheelPoseBetweenNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.wheelLinearVelocitiesNoiseDensity", std::vector{0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.vioPoseBetweenNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + + // Extrinsic frames (string) + node_->declare_parameter("extrinsics.lidarOdometryFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelOdometryBetweenFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelLinearVelocityLeftFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelLinearVelocityRightFrame", std::string("")); + node_->declare_parameter("extrinsics.vioOdometryFrame", std::string("")); + + // Wheel Radius (double) + node_->declare_parameter("sensor_params.wheelRadius", 0.0); + + // Create SmbStaticTransforms + staticTransformsPtr_ = std::make_shared(node_); + + SmbEstimator::readParams(); + + // Initialize ROS 2 publishers and subscribers + SmbEstimator::initializePublishers(); + SmbEstimator::initializeSubscribers(); + SmbEstimator::initializeMessages(); + SmbEstimator::initializeServices(); + + GraphMsfRos2::setup(staticTransformsPtr_); + + // Transforms --> query until returns true + bool foundTransforms = false; + while (!foundTransforms) { + foundTransforms = staticTransformsPtr_->findTransformations(); + // Sleep for 0.1 seconds to avoid busy waiting + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl; +} + +void SmbEstimator::initializePublishers() { + pubMeasMapLioPath_ = node_->create_publisher("/graph_msf/measLiDAR_path_map_imu", ROS_QUEUE_SIZE); + pubMeasMapVioPath_ = node_->create_publisher("/graph_msf/measVIO_path_map_imu", ROS_QUEUE_SIZE); +} + +void SmbEstimator::initializeSubscribers() { + if (useLioOdometryFlag_) { + subLioOdometry_ = node_->create_subscription( + "/lidar_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::lidarOdometryCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized LiDAR Odometry subscriber with topic: /lidar_odometry_topic" << std::endl; + } + + if (useWheelOdometryBetweenFlag_) { + subWheelOdometryBetween_ = node_->create_subscription( + "/wheel_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::wheelOdometryPoseCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized Wheel Odometry subscriber with topic: /wheel_odometry_topic" << std::endl; + } + + if (useWheelLinearVelocitiesFlag_) { + subWheelLinearVelocities_ = node_->create_subscription( + "/wheel_velocities_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::wheelLinearVelocitiesCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized Wheel Linear Velocities subscriber with topic: /wheel_velocities_topic" << std::endl; + } + + if (useVioOdometryFlag_) { + subVioOdometry_ = node_->create_subscription( + "/vio_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::vioOdometryCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized VIO Odometry subscriber with topic: /vio_odometry_topic" << std::endl; + } +} + +void SmbEstimator::initializeMessages() { + measLio_mapImuPathPtr_ = std::make_shared(); + measVio_mapImuPathPtr_ = std::make_shared(); +} + +void SmbEstimator::initializeServices() { + // Nothing for now +} + +void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { + const rclcpp::Time new_imu_timestamp{imuPtr->header.stamp}; +// const rclcpp::Time arrival_time = this->node_->get_clock()->now(); +// const rclcpp::Duration delay = arrival_time - new_imu_timestamp; +// if (delay < rclcpp::Duration(0, 0)) { +// REGULAR_COUT << RED_START << " IMU message arrived at " << std::fixed << delay.seconds() << " before the message " +// "timestamp. The messages should not be stamped with a time in the future." << COLOR_END << std::endl; +// } else if (delay > rclcpp::Duration(0, 100000000)) { +// REGULAR_COUT << RED_START << " IMU message arrival was delayed by " << std::fixed << delay.seconds() << "." << COLOR_END << std::endl; +// } else { +// REGULAR_COUT << " IMU message arrival was ok. Delay: " << delay.seconds() << "." << COLOR_END << std::endl; +// } + + if (graph_msf::GraphMsf::areRollAndPitchInited() && !graph_msf::GraphMsf::areYawAndPositionInited() && !useLioOdometryFlag_ && + !useWheelOdometryBetweenFlag_ && !useWheelLinearVelocitiesFlag_ && !useVioOdometryFlag_) { + REGULAR_COUT << RED_START << " IMU callback is setting global yaw and position, as no other odometry is available. Initializing..." + << COLOR_END << std::endl; + + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "IMU_init_6D", int(graphConfigPtr_->imuRate_), staticTransformsPtr_->getImuFrame(), + staticTransformsPtr_->getImuFrame() + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), + imuPtr->header.stamp.sec + imuPtr->header.stamp.nanosec * 1e-9, 1.0, Eigen::Isometry3d::Identity(), + Eigen::MatrixXd::Identity(6, 1)); + + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + graph_msf::GraphMsf::pretendFirstMeasurementReceived(); + } + // Remove if norm is larger than 100 + const double angular_velocity_norm = std::sqrt(imuPtr->angular_velocity.x * imuPtr->angular_velocity.x + + imuPtr->angular_velocity.y * imuPtr->angular_velocity.y + + imuPtr->angular_velocity.z * imuPtr->angular_velocity.z); + const double linear_acceleration_norm = std::sqrt(imuPtr->linear_acceleration.x * imuPtr->linear_acceleration.x + + imuPtr->linear_acceleration.y * imuPtr->linear_acceleration.y + + imuPtr->linear_acceleration.z * imuPtr->linear_acceleration.z); + if (angular_velocity_norm > 10) { + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU angular velocity is larger than 10 rad/s, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; + return; + } else if (linear_acceleration_norm > 100.0) { + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100 m/s^2, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; + return; + } + // Check timestamps strictly increase + if (new_imu_timestamp == last_imu_timestamp_) { + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was duplicated, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; + return; + } else if (new_imu_timestamp < last_imu_timestamp_) { + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was before last included IMU measurement " + " at time" << last_imu_timestamp_.seconds() << ", skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; + return; + } + last_imu_timestamp_ = new_imu_timestamp; + + graph_msf::GraphMsfRos2::imuCallback(imuPtr); +} + +void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& odomLidarPtr) { + static int lidarOdometryCallbackCounter__ = -1; + static double lastLidarOdometryTimeK_ = 0.0; + static constexpr double lioOdometryRate_ = 10.0; // Hz + + // Timestamp + double lidarOdometryTimeK = odomLidarPtr->header.stamp.sec + odomLidarPtr->header.stamp.nanosec * 1e-9; + + // Check whether the callback rate is not exceeded + if (lidarOdometryCallbackCounter__ >= 0 && lidarOdometryTimeK - lastLidarOdometryTimeK_ < (1.0 / lioOdometryRate_)) { + return; // Skip this callback if the rate is exceeded + } else { + lastLidarOdometryTimeK_ = lidarOdometryTimeK; // Update the last timestamp + } + + // Update the callback counter + ++lidarOdometryCallbackCounter__; + + Eigen::Isometry3d lio_T_M_Lk; + graph_msf::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix()); + + + const std::string& lioOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(); + + graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( + "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::Huber(3.0), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, + staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); + + if (lidarOdometryCallbackCounter__ <= 2) { + return; + } else if (areYawAndPositionInited()) { + this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement); + } else { + this->initYawAndPosition(unary6DMeasurement); + } + + addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id + referenceFrameAlignedNameId, odomLidarPtr->header.stamp, + (lio_T_M_Lk * staticTransformsPtr_->rv_T_frame1_frame2(lioOdometryFrame, staticTransformsPtr_->getImuFrame()).matrix()) + .block<3, 1>(0, 3), + graphConfigPtr_->imuBufferLength_ * 4); + + pubMeasMapLioPath_->publish(*measLio_mapImuPathPtr_); +} + +void SmbEstimator::wheelOdometryPoseCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& wheelOdometryKPtr) { + if (!areRollAndPitchInited()) { + return; + } + + ++wheelOdometryCallbackCounter_; + + Eigen::Isometry3d T_O_Bw_k; + graph_msf::odomMsgToEigen(*wheelOdometryKPtr, T_O_Bw_k.matrix()); + double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.sec + wheelOdometryKPtr->header.stamp.nanosec * 1e-9; + + if (wheelOdometryCallbackCounter_ == 0) { + T_O_Bw_km1_ = T_O_Bw_k; + wheelOdometryTimeKm1_ = wheelOdometryTimeK; + return; + } + + const std::string& wheelOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getWheelOdometryBetweenFrame(); + + if (!areYawAndPositionInited()) { + if (!useLioOdometryFlag_) { + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "Lidar_unary_6D", int(wheelOdometryBetweenRate_), wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::None(), wheelOdometryTimeK, 1.0, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + } + } else if (wheelOdometryCallbackCounter_ % 5 == 0 && wheelOdometryCallbackCounter_ > 0) { + Eigen::Isometry3d T_Bkm1_Bk = T_O_Bw_km1_.inverse() * T_O_Bw_k; + graph_msf::BinaryMeasurementXD delta6DMeasurement( + "Wheel_odometry_6D", int(wheelOdometryBetweenRate_ / 5), wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::Tukey(1.0), wheelOdometryTimeKm1_, wheelOdometryTimeK, T_Bkm1_Bk, wheelPoseBetweenNoise_); + this->addBinaryPose3Measurement(delta6DMeasurement); + + T_O_Bw_km1_ = T_O_Bw_k; + wheelOdometryTimeKm1_ = wheelOdometryTimeK; + } +} + +void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& wheelsSpeedsPtr) { + if (!areRollAndPitchInited()) { + return; + } + + const double timeK = wheelsSpeedsPtr->data[0]; + const double leftWheelSpeedRps = wheelsSpeedsPtr->data[1]; + const double rightWheelSpeedRps = wheelsSpeedsPtr->data[2]; + const double leftWheelSpeedMs = leftWheelSpeedRps * wheelRadiusMeter_; + const double rightWheelSpeedMs = rightWheelSpeedRps * wheelRadiusMeter_; + + const std::string& wheelLinearVelocityLeftFrame = + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityLeftFrame(); + const std::string& wheelLinearVelocityRightFrame = + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityRightFrame(); + + if (!areYawAndPositionInited()) { + if (!useLioOdometryFlag_ && !useWheelOdometryBetweenFlag_) { + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "Lidar_unary_6D", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, + Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + } + } else { + // graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( + // "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + // wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + // Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + // this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); + + // graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( + // "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, + // wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + // Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + // this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement); + + graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( + "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, + Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); + + graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( + "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, + wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, + Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement); + } +} + +void SmbEstimator::vioOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& vioOdomPtr) { + std::cout << "VIO odometry not yet stable enough for usage, disable flag." << std::endl; + + Eigen::Isometry3d vio_T_M_Ck; + graph_msf::odomMsgToEigen(*vioOdomPtr, vio_T_M_Ck.matrix()); + + addToPathMsg(measVio_mapImuPathPtr_, vioOdomPtr->header.frame_id, vioOdomPtr->header.stamp, + (vio_T_M_Ck * staticTransformsPtr_ + ->rv_T_frame1_frame2(dynamic_cast(staticTransformsPtr_.get())->getVioOdometryFrame(), + staticTransformsPtr_->getImuFrame()) + .matrix()) + .block<3, 1>(0, 3), + graphConfigPtr_->imuBufferLength_ * 4 * 10); + + pubMeasMapVioPath_->publish(*measVio_mapImuPathPtr_); +} + +} // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp new file mode 100644 index 00000000..9d21829b --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -0,0 +1,102 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" + +// ROS 2 +#include +#include +#include + +// Workspace +#include "graph_msf_ros2/util/conversions.h" +#include "smb_estimator_graph_ros2/constants.h" + +namespace smb_se { + +SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr) : graph_msf::StaticTransformsTf(nodePtr) { + REGULAR_COUT << GREEN_START << " Initializing smb static transforms..." << COLOR_END << std::endl; +} + +bool SmbStaticTransforms::findTransformations() { + // Super Method + // Need to find the transformations in the TF-tree + if (!graph_msf::StaticTransformsTf::findTransformations()) { + REGULAR_COUT << RED_START << " Failed to find transformations in TF-tree." << COLOR_END << std::endl; + return false; + } + + // Print to console + REGULAR_COUT << COLOR_END << " Looking up transforms in TF-tree." << std::endl; + REGULAR_COUT << COLOR_END << " Waiting for up to 100 seconds until they arrive..." << std::endl; + + // Temporary variable + geometry_msgs::msg::TransformStamped transform; + Eigen::Isometry3d eigenTransform; + + // Imu to LiDAR Link --- + if (useLioOdometryFlag_) { + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << lidarOdometryFrame_ << " for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, lidarOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_) = eigenTransform; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_Lidar: " << imuFrame_ << " " << lidarOdometryFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; + lv_T_frame1_frame2(lidarOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).inverse(); + } + + // Imu to VIO Link --- + if (useVioOdometryFlag_) { + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << vioOdometryFrame_ << " for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, vioOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; + lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); + } + + // Wheel Frames --- + if (useWheelOdometryBetweenFlag_ || useWheelLinearVelocitiesFlag_) { + REGULAR_COUT + << RED_START + << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." + << COLOR_END << std::endl; + + // Left Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelLeft = tf2::transformToEigen(transform.transform); + T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelLeft: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() + << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; + lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); + + // Right Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelRight = tf2::transformToEigen(transform.transform); + T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelRight: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() + << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; + lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); + } + + REGULAR_COUT << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; + return true; +} + +} // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp new file mode 100644 index 00000000..e6ce7b41 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp @@ -0,0 +1,96 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph_ros2/SmbEstimator.h" + +// Project +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" +#include "smb_estimator_graph_ros2/constants.h" + +// GraphMSF ROS2 +#include "graph_msf_ros2/ros/read_ros_params.h" + +namespace smb_se { + +void SmbEstimator::readParams() { + // Check + if (!graphConfigPtr_) { + throw std::runtime_error("SmbEstimator: graphConfigPtr must be initialized."); + } + + // Flags + useLioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useLioOdometry"); + dynamic_cast(staticTransformsPtr_.get())->setUseLioOdometryFlag(useLioOdometryFlag_); + useWheelOdometryBetweenFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelOdometryBetween"); + dynamic_cast(staticTransformsPtr_.get())->setUseWheelOdometryBetweenFlag(useWheelOdometryBetweenFlag_); + useWheelLinearVelocitiesFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelLinearVelocities"); + dynamic_cast(staticTransformsPtr_.get())->setUseWheelLinearVelocitiesFlag(useWheelLinearVelocitiesFlag_); + useVioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useVioOdometry"); + dynamic_cast(staticTransformsPtr_.get())->setUseVioOdometryFlag(useVioOdometryFlag_); + + // Sensor Params + lioOdometryRate_ = graph_msf::tryGetParam(node_, "sensor_params.lioOdometryRate"); + wheelOdometryBetweenRate_ = graph_msf::tryGetParam(node_, "sensor_params.wheelOdometryBetweenRate"); + wheelLinearVelocitiesRate_ = graph_msf::tryGetParam(node_, "sensor_params.wheelLinearVelocitiesRate"); + vioOdometryRate_ = graph_msf::tryGetParam(node_, "sensor_params.vioOdometryRate"); + + // Alignment Parameters + const auto initialSe3AlignmentNoiseDensity = + graph_msf::tryGetParam>(node_, "alignment_params.initialSe3AlignmentNoiseDensity"); + initialSe3AlignmentNoise_ << initialSe3AlignmentNoiseDensity[0], initialSe3AlignmentNoiseDensity[1], initialSe3AlignmentNoiseDensity[2], + initialSe3AlignmentNoiseDensity[3], initialSe3AlignmentNoiseDensity[4], initialSe3AlignmentNoiseDensity[5]; + const auto lioSe3AlignmentRandomWalk = + graph_msf::tryGetParam>(node_, "alignment_params.lioSe3AlignmentRandomWalk"); + lioSe3AlignmentRandomWalk_ << lioSe3AlignmentRandomWalk[0], lioSe3AlignmentRandomWalk[1], lioSe3AlignmentRandomWalk[2], + lioSe3AlignmentRandomWalk[3], lioSe3AlignmentRandomWalk[4], lioSe3AlignmentRandomWalk[5]; + + // Noise Parameters + /// LiDAR Odometry + const auto poseUnaryNoise = + graph_msf::tryGetParam>(node_, "noise_params.lioPoseUnaryNoiseDensity"); // roll,pitch,yaw,x,y,z + lioPoseUnaryNoise_ << poseUnaryNoise[0], poseUnaryNoise[1], poseUnaryNoise[2], poseUnaryNoise[3], poseUnaryNoise[4], poseUnaryNoise[5]; + /// Wheel Odometry + /// Between + const auto wheelPoseBetweenNoise = + graph_msf::tryGetParam>(node_, "noise_params.wheelPoseBetweenNoiseDensity"); // roll,pitch,yaw,x,y,z + wheelPoseBetweenNoise_ << wheelPoseBetweenNoise[0], wheelPoseBetweenNoise[1], wheelPoseBetweenNoise[2], wheelPoseBetweenNoise[3], + wheelPoseBetweenNoise[4], wheelPoseBetweenNoise[5]; + /// Linear Velocities + const auto wheelLinearVelocitiesNoise = + graph_msf::tryGetParam>(node_, "noise_params.wheelLinearVelocitiesNoiseDensity"); // left,right + wheelLinearVelocitiesNoise_ << wheelLinearVelocitiesNoise[0], wheelLinearVelocitiesNoise[1], wheelLinearVelocitiesNoise[2]; + /// VIO Odometry + const auto vioPoseBetweenNoise = + graph_msf::tryGetParam>(node_, "noise_params.vioPoseBetweenNoiseDensity"); // roll,pitch,yaw,x,y,z + vioPoseBetweenNoise_ << vioPoseBetweenNoise[0], vioPoseBetweenNoise[1], vioPoseBetweenNoise[2], vioPoseBetweenNoise[3], + vioPoseBetweenNoise[4], vioPoseBetweenNoise[5]; + + // Set frames + /// LiDAR odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setLioOdometryFrame(graph_msf::tryGetParam(node_, "extrinsics.lidarOdometryFrame")); + /// Wheel Odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelOdometryBetweenFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelOdometryBetweenFrame")); + /// Whel Linear Velocities frames + /// Left + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelLinearVelocityLeftFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelLinearVelocityLeftFrame")); + /// Right + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelLinearVelocityRightFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelLinearVelocityRightFrame")); + + /// VIO Odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setVioOdometryFrame(graph_msf::tryGetParam(node_, "extrinsics.vioOdometryFrame")); + + // Wheel Radius + wheelRadiusMeter_ = graph_msf::tryGetParam(node_, "sensor_params.wheelRadius"); +} + +} // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/src/smb_estimator_node.cpp b/ros2/examples/smb_estimator_graph_ros2/src/smb_estimator_node.cpp new file mode 100644 index 00000000..d07e95a3 --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/src/smb_estimator_node.cpp @@ -0,0 +1,40 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// ROS 2 +#include + +// Debugging +// #include + +// Local packages +#include "smb_estimator_graph_ros2/SmbEstimator.h" + +// Main node entry point +int main(int argc, char** argv) { + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Debugging + // FLAGS_alsologtostderr = true; + + // Create Node + auto privateNodePtr = std::make_shared("smb_estimator_node"); + + // Create Instance of SmbEstimator + auto smbEstimator = std::make_shared(privateNodePtr); + + // Use Multi-Threaded Executor + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + executor.add_node(privateNodePtr); + executor.spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt new file mode 100644 index 00000000..9338b635 --- /dev/null +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -0,0 +1,146 @@ +cmake_minimum_required(VERSION 3.16) +project(graph_msf_ros2) + +# Compile as C++17 +add_compile_options(-std=c++17) + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(urdf REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(std_srvs REQUIRED) +find_package(visualization_msgs REQUIRED) + +# Same Workspace -------------------- +# Graph MSF +if(NOT TARGET graph_msf::graph_msf) + find_package(graph_msf REQUIRED) +endif() + +# Color (Optional) +if (NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif () + +########### +## Build ## +########### + +# Include directories --------------------------------------------------- +include_directories( + include +) + +# Library ---------------------------------------------------------------- +add_library(${PROJECT_NAME} SHARED + src/lib/StaticTransformsTf.cpp + src/lib/GraphMsfRos2.cpp + src/lib/readParams.cpp + src/lib/conversions.cpp +) +# Exported alias target +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) + +# Link dependencies +ament_target_dependencies(${PROJECT_NAME} + PUBLIC + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + std_srvs + visualization_msgs + rosidl_default_runtime +) +target_link_libraries(${PROJECT_NAME} + PUBLIC + Eigen3::Eigen + rclcpp::rclcpp + tf2::tf2 + tf2_ros::tf2_ros + tf2_eigen::tf2_eigen + graph_msf::graph_msf + # graph_msf_ros2_msgs::graph_msf_ros2_msgs + # rosidl_typesupport_cpp::rosidl_typesupport_cpp + # rosidl_service_type_support_t::rosidl_service_type_support_t +) + +# Include directories for the library +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +# Print the include directories for debugging +get_target_property(INC_DIR graph_msf::graph_msf INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf::graph_msf: ${INC_DIR}") + +# Python Scripts ---------------------------------------------------------------- +install(PROGRAMS + ${PROJECT_NAME}_py/replay/manual_pose_files_to_tf_and_odom_bag.py + ${PROJECT_NAME}_py/plotting/plot_latest_quantitites_in_folder.py + ${PROJECT_NAME}_py/bag_filter/remove_tf_from_bag.py + DESTINATION lib/${PROJECT_NAME} +) + +############# +## Install ## +############# + +# Export the include directories and linked libraries +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets # Export the target for downstream usage + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install the include directory +install(DIRECTORY include/ + DESTINATION include +) + +# Install the export file for downstream projects +install(EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +# Version +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION 1.0.0 + COMPATIBILITY SameMajorVersion +) + +# Configure +configure_package_config_file( + "${PROJECT_NAME}Config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION share/${PROJECT_NAME}/cmake +) + +# Files +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION share/${PROJECT_NAME}/cmake +) + +# Export ament package +ament_package() diff --git a/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml b/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml new file mode 100644 index 00000000..9d33da22 --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml @@ -0,0 +1,7 @@ +#External Prior/Transform Input +extrinsics: + worldFrame: world + mapFrame: map + odomFrame: odom + imuFrame: imu_box_link # IMU_CABIN_link #Frame of IMU used in integrator - Used for lookup transform to lidarFrame + initializeZeroYawAndPositionOfFrame: imu_box_link diff --git a/ros2/graph_msf_ros2/config/core_graph_config.yaml b/ros2/graph_msf_ros2/config/core_graph_config.yaml new file mode 100644 index 00000000..3b7434b9 --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_graph_config.yaml @@ -0,0 +1,4 @@ +#Common +common_params: + verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors + reLocalizeWorldToMapAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) diff --git a/ros2/graph_msf_ros2/config/core_graph_params.yaml b/ros2/graph_msf_ros2/config/core_graph_params.yaml new file mode 100644 index 00000000..bb912c7f --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_graph_params.yaml @@ -0,0 +1,54 @@ +# Sensor Params +sensor_params: + imuRate: 100 #Rate of IMU input (Hz) + imuBufferLength: 250 + imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot + +# Initialization +initialization_params: + estimateGravityFromImu: true # If true, the gravity is estimated in the beginning using the IMU, if false, the value of 9.81 is used + +# Factor Graph +graph_params: + useIsam: true # if false, then levenberg-marquardt is used, CURRENTLY NO EFFECT + smootherLag: 2.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 + additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors + findUnusedFactorSlots: true + enableDetailedResults: false + usingFallbackGraph: true + usingCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable + usingBiasForPreIntegration: true + +# Outlier Rejection +outlier_params: + poseMotionOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements + +# Noise Parameters +noise_params: + ## IMU + ### Position + accNoiseDensity: 7.225e-08 #Continuous-time "Covariance" of accelerometer, microstrain: sigma^2=7.225e-7 + integrationNoiseDensity: 1.0e-03 #continuous-time "Covariance" describing integration uncertainty, default: 1.0e-06 + use2ndOrderCoriolis: true #Use 2nd order coriolis effect + ### Rotation + gyrNoiseDensity: 1.71e-08 #Continuous-time "Covariance" of gyroscope measurements, microstrain: sigma^2=1.71-08 + omegaCoriolis: 0.0 #Coriolis effect + ### Bias + accBiasRandomWalk: 1.0e-05 #gnss:1.0e-06 #lidar: 1e-01 #Continuous-time "Covariance" describing accelerometer bias random walk, default: 1.0e-06 + gyrBiasRandomWalk: 9.33e-08 #gnss:9.33e-08 #lidar: 9.33e-01 #Continuous-time "Covariance" describing gyroscope bias random walk, default: 9.33e-08 + biasAccOmegaInit: 1.0e-03 #covariance of bias used as initial estimate default: 1.0e-2 + accBiasPrior: 0.0 #Prior/starting value of accelerometer bias + gyrBiasPrior: 0.0 #Prior/starting value of gyroscope bias + +# Relinearization +relinearization_params: + positionReLinTh: 1.0e-02 #Position linearization threshold + rotationReLinTh: 1.0e-02 #Rotation linearization threshold + velocityReLinTh: 1.0e-02 #Linear Velocity linearization threshold + accBiasReLinTh: 1.0e-05 #Accelerometer bias linearization threshold + gyrBiasReLinTh: 1.0e-05 #Gyroscope bias linearization threshold + relinearizeSkip: 1 + enableRelinearization: true + evaluateNonlinearError: false + cacheLinearizedFactors: true + enablePartialRelinearizationCheck: false diff --git a/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in b/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in new file mode 100644 index 00000000..e5bd51d2 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in @@ -0,0 +1,9 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/graph_msf_ros2Targets.cmake") + +# Optionally set legacy-style variables (only if needed) +# set(graph_msf_INCLUDE_DIRS "@CMAKE_INSTALL_INCLUDEDIR@") + +# Set up version compatibility (optional if you use a version file) +# include("${CMAKE_CURRENT_LIST_DIR}/graph_msfConfigVersion.cmake") \ No newline at end of file diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/__init__.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py new file mode 100644 index 00000000..5281510e --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import sys +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + + +def filter_topics(in_bag, out_bag, frames_we_want): + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions(uri=in_bag, storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + reader.open(storage_options, converter_options) + topic_types = reader.get_all_topics_and_types() + + type_map = {topic.name: topic.type for topic in topic_types} + + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=out_bag, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + for topic in topic_types: + writer.create_topic(topic) + + while reader.has_next(): + (topic, data, t) = reader.read_next() + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + if topic == "/tf" and hasattr(msg, "transforms"): + transforms_to_keep = [] + for transform in msg.transforms: + if ( + transform.header.frame_id in frames_we_want + and transform.child_frame_id in frames_we_want + ): + transforms_to_keep.append(transform) + + if transforms_to_keep: + msg.transforms = transforms_to_keep + writer.write(topic, data, t) + + elif topic != "/tf": + writer.write(topic, data, t) + + +def filter_topics_2(in_bag, out_bag, frames_we_dont_want): + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions(uri=in_bag, storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + reader.open(storage_options, converter_options) + topic_types = reader.get_all_topics_and_types() + + type_map = {topic.name: topic.type for topic in topic_types} + + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=out_bag, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + for topic in topic_types: + writer.create_topic(topic) + + while reader.has_next(): + (topic, data, t) = reader.read_next() + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + if topic == "/tf" and hasattr(msg, "transforms"): + transforms_to_keep = [] + for transform in msg.transforms: + if ( + transform.header.frame_id not in frames_we_dont_want + and transform.child_frame_id not in frames_we_dont_want + ): + transforms_to_keep.append(transform) + + if transforms_to_keep: + msg.transforms = transforms_to_keep + writer.write(topic, data, t) + + elif topic != "/tf": + writer.write(topic, data, t) + + +if __name__ == "__main__": + print("Starting") + in_bag = sys.argv[1] + out_bag = sys.argv[2] + + # Use one of the filtering functions as needed + # Example: + # filter_topics(in_bag, out_bag, ['base_link', 'odom', 'map', 'torso', 'Hip', 'Pelvis', 'Tibia', 'base_footprint']) + filter_topics_2( + in_bag, out_bag, ["odom_graph_msf", "world_graph_msf", "map_o3d_gmsf"] + ) + + print("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py new file mode 100644 index 00000000..660082f1 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import sys +import rosbag2_py +from rclpy.serialization import deserialize_message, serialize_message +from rosidl_runtime_py.utilities import get_message +import logging + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +def filter_rosbag(input_bag_path, output_bag_path): + try: + # Initialize the rosbag2 reader + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions( + uri=input_bag_path, storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + reader.open(storage_options, converter_options) + + # Get all topics and types from the input bag + topic_types = reader.get_all_topics_and_types() + type_map = {topic.name: topic.type for topic in topic_types} + + # Initialize the rosbag2 writer + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=output_bag_path, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + # Create topics in the output bag based on the input bag's topics + for topic in topic_types: + writer.create_topic(topic) + + # Loop through each topic and message in the input rosbag + while reader.has_next(): + try: + topic, data, timestamp = reader.read_next() + + # Skip the /tf topic + if topic == "/tf": + continue + + # Deserialize and re-serialize the message for writing + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + serialized_msg = serialize_message(msg) + writer.write(topic, serialized_msg, timestamp) + except Exception as e: + logger.warning(f"Error processing message on topic {topic}: {str(e)}") + continue + + except Exception as e: + logger.error(f"Error processing bag: {str(e)}") + raise + +if __name__ == "__main__": + if len(sys.argv) != 3: + logger.error("Usage: python3 remove_tf_from_bag.py input_bag output_bag") + sys.exit(1) + + logger.info("Starting") + input_bag = sys.argv[1] + output_bag = sys.argv[2] + try: + filter_rosbag(input_bag_path=input_bag, output_bag_path=output_bag) + logger.info("Done") + except Exception as e: + logger.error(f"Failed to process bag: {str(e)}") + sys.exit(1) diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py new file mode 100644 index 00000000..78e2901a --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 + +# General Packages +import os +import matplotlib.pyplot as plt +import numpy as np +import sys +from ament_index_python.packages import get_package_share_directory +import logging + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +# Path to the folder containing the files +HOME_DIR = str(os.path.expanduser("~")) +dir_path = os.path.join( + HOME_DIR, + "workspaces/rsl_workspaces/graph_msf_ws/src/graph_msf_dev/examples/anymal_estimator_graph/logging", +) + +# Add the parent directory to sys.path +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +# Workspace +import utils.get_latest_time_string_in_folder as get_latest_time_string_in_folder + + +def plot_quantities_in_file(file_path: str, type: str, dir_path: str, latest_file_string: str): + try: + # Data + fields = [] + # Bias + if "bias" in type: + fields = ["t", "a_x", "a_y", "a_z", "w_x", "w_y", "w_z"] + # Covariance + elif "covariance" in type: + fields = ["t", + "cov_00", "cov_01", "cov_02", "cov_03", "cov_04", "cov_05", + "cov_10", "cov_11", "cov_12", "cov_13", "cov_14", "cov_15", + "cov_20", "cov_21", "cov_22", "cov_23", "cov_24", "cov_25", + "cov_30", "cov_31", "cov_32", "cov_33", "cov_34", "cov_35", + "cov_40", "cov_41", "cov_42", "cov_43", "cov_44", "cov_45", + "cov_50", "cov_51", "cov_52", "cov_53", "cov_54", "cov_55"] + elif "transform" in type or "pose" in type: + fields = ["t", "x", "y", "z", "qw", "qx", "qy", "qz", "roll", "pitch", "yaw"] + elif "velocity" in type: + fields = ["t", "v_x", "v_y", "v_z"] + + # Read data + data = [] + first_line = True + with open(file_path, "r") as file: + type = type.split(".")[0] # Remove everything after the dot + for line in file: + if first_line: + first_line = False + continue + parts = line.strip().split(",") + if len(parts) < len(fields): + logger.warning(f"Skipping malformed line: {line}") + continue + data.append(np.asarray(parts)) + + if not data: + logger.error(f"No valid data found in {file_path}") + return + + # Process Data + data = np.array(data, dtype=np.float64) + data[:, 0] = data[:, 0] - data[0, 0] # Subtract first timestamp + logger.info(f"Data shape: {data.shape}") + logger.info(f"Fields: {fields}") + + # Plot + plt.figure() + if "covariance" not in type: + for i, field in enumerate(fields): + if i == 0 or field in ["qw", "qx", "qy", "qz"]: + continue + + logger.info(f"Plotting field: {field}. Max: {np.max(data[:, i])}. Min: {np.min(data[:, i])}") + plt.plot(data[:, 0], data[:, i], label=field) + + # Create plot when entering last field for each type + if "bias" in type and field == "a_z": + plt.xlabel("Time [s]") + plt.ylabel("Acceleration [m/s^2]") + plt.title(f"{type} Acceleration Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_acceleration.png") + plt.savefig(plot_path) + plt.figure() + elif "bias" in type and field == "w_z": + plt.xlabel("Time [s]") + plt.ylabel("Angular Velocity [rad/s]") + plt.title(f"{type} Angular Velocity Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_angular_velocity.png") + plt.savefig(plot_path) + elif ("transform" in type or "pose" in type) and field == "z": + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.title(f"{type} Position Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_position.png") + plt.savefig(plot_path) + plt.figure() + elif ("transform" in type or "pose" in type) and field == "yaw": + plt.xlabel("Time [s]") + plt.ylabel("Orientation [rad]") + plt.title(f"{type} Orientation Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_orientation.png") + plt.savefig(plot_path) + plt.figure() + elif "velocity" in type and field == "v_z": + plt.xlabel("Time [s]") + plt.ylabel("Velocity [m/s]") + plt.title(f"{type} Velocity Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}.png") + plt.savefig(plot_path) + plt.figure() + + logger.info(f"Plot saved to: {plot_path}") + + # Covariance and x-y plots + if "covariance" in type: + plt.figure() + plt.plot(data[:, 0], data[:, 1], label="cov_xx") + plt.plot(data[:, 0], data[:, 8], label="cov_yy") + plt.plot(data[:, 0], data[:, 15], label="cov_zz") + plt.xlabel("Time [s]") + plt.ylabel("Covariance [m]") + plt.title(f"{type} Covariance Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}.png") + plt.savefig(plot_path) + logger.info(f"Covariance plot saved to: {plot_path}") + elif "transform" in type or "pose" in type: + plt.figure() + plt.plot(data[:, 1], data[:, 2], label="x-y") + plt.xlabel("x [m]") + plt.ylabel("y [m]") + plt.title(f"{type} x-y Position Over Time") + plt.axis("equal") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_x_y_position.png") + plt.savefig(plot_path) + logger.info(f"X-Y plot saved to: {plot_path}") + + except Exception as e: + logger.error(f"Error plotting {type}: {str(e)}") + raise + +def main(): + try: + if len(sys.argv) != 2: + logger.error("Usage: python3 plot_latest_quantitites_in_folder.py ") + sys.exit(1) + + package_name = sys.argv[1] + logger.info(f"Package name: {package_name}") + + # Get package directory + try: + package_dir = get_package_share_directory(package_name) + dir_path = os.path.join(package_dir, "logging") + logger.info(f"Directory path: {dir_path}") + except Exception as e: + logger.error(f"Failed to find package {package_name}: {str(e)}") + sys.exit(1) + + # Get latest directory + latest_dir_string = get_latest_time_string_in_folder.get_latest_time_string_in_folder(dir_path) + logger.info(f"Latest directory: {latest_dir_string}") + + # File types to plot + file_types = [ + "B_imu_bias", + "R_6D_transform", + "v_state_3D_velocity", + "X_state_6D_pose", + "X_state_6D_pose_covariance", + ] + + # Get files + files = os.listdir(os.path.join(dir_path, latest_dir_string)) + files = [f for f in files if f.endswith(".csv")] + logger.info(f"Files in {latest_dir_string}: {files}") + + # Plot files + for file in files: + for file_type in file_types: + if file_type in file: + logger.info(f"Plotting {file_type} from {file}") + plot_quantities_in_file( + file_path=os.path.join(dir_path, latest_dir_string, file), + type=file, + dir_path=os.path.join(dir_path, latest_dir_string), + latest_file_string="", + ) + + except Exception as e: + logger.error(f"Error in main: {str(e)}") + sys.exit(1) + +if __name__ == "__main__": + logger.info("Starting") + main() + logger.info("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/__init__.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py new file mode 100644 index 00000000..824d5a21 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 + +import os.path +import sys +import logging +from pathlib import Path +from ament_index_python.packages import get_package_share_directory + +# Add the parent directory to sys.path +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +# Workspace +from graph_msf_ros2_py.replay.transform_helpers import write_bag +from graph_msf_ros2_py.utils.get_latest_time_string_in_folder import get_latest_time_string_in_folder + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +def process_file(dir_path: str, csv_file_name: str, fixed_frame_id: str, child_frame_id: str): + try: + latest_time_string = get_latest_time_string_in_folder(folder_path=dir_path) + input_file_path = os.path.join(dir_path, latest_time_string, f"{csv_file_name}.csv") + output_bag_path = os.path.join(dir_path, latest_time_string, f"bag_{csv_file_name}.bag") + + logger.info(f"Processing file: {input_file_path}") + logger.info(f"Output bag: {output_bag_path}") + + write_bag( + input_file_path=input_file_path, + output_bag_path=output_bag_path, + fixed_frame_id=fixed_frame_id, + child_frame_id=child_frame_id + ) + logger.info(f"Successfully processed {csv_file_name}") + except Exception as e: + logger.error(f"Error processing {csv_file_name}: {str(e)}") + raise + +def main(): + try: + # Get command line arguments + if len(sys.argv) != 3: + logger.error("Usage: python3 manual_pose_files_to_tf_and_odom_bag.py ") + sys.exit(1) + + package_name = sys.argv[1] + imu_frame_name = sys.argv[2] + + # Validate IMU frame name + valid_frames = ["cpt7_imu", "imu_link"] + if imu_frame_name not in valid_frames: + logger.error(f"Invalid IMU frame name. Must be one of: {valid_frames}") + sys.exit(1) + + logger.info(f"Package name: {package_name}") + logger.info(f"IMU frame name: {imu_frame_name}") + + # Get package directory + try: + package_dir = get_package_share_directory(package_name) + dir_path = os.path.join(package_dir, "logging") + logger.info(f"Directory path: {dir_path}") + except Exception as e: + logger.error(f"Failed to find package {package_name}: {str(e)}") + sys.exit(1) + + # Process files + files_to_process = [ + "X_state_6D_pose", + "rt_world_x_state_6D_pose", + "optimized_X_state_transform" # Additional file from ROS2 version + ] + + fixed_frame_id = "gt_world" # or "offline_world_graph_msf" depending on your needs + + for file_name in files_to_process: + try: + process_file(dir_path, file_name, fixed_frame_id, imu_frame_name) + except Exception as e: + logger.warning(f"Skipping {file_name} due to error: {str(e)}") + continue + + except Exception as e: + logger.error(f"Error in main: {str(e)}") + sys.exit(1) + +if __name__ == "__main__": + logger.info("Starting") + main() + logger.info("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py new file mode 100644 index 00000000..c6b6d80d --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 + +# General Packages +from scipy.spatial.transform import Rotation as R +import copy +import sys + +# ROS 2 +import rclpy +from rclpy.time import Time +from rclpy.serialization import serialize_message +from geometry_msgs.msg import TransformStamped +from tf2_msgs.msg import TFMessage +import rosbag2_py + + +def invert_transform(tf_msg): + # Old orientation + q_old = [ + tf_msg.transform.rotation.x, + tf_msg.transform.rotation.y, + tf_msg.transform.rotation.z, + tf_msg.transform.rotation.w, + ] + # Invert the orientation + r_old = R.from_quat(q_old) + r_new = r_old.inv() + q_new = r_new.as_quat() + + # Old translation + t_old = [ + tf_msg.transform.translation.x, + tf_msg.transform.translation.y, + tf_msg.transform.translation.z, + ] + t_new = -r_new.apply(t_old) + + # Define the new transform + tf_msg_new = TransformStamped() + tf_msg_new.header = copy.deepcopy(tf_msg.header) + tf_msg_new.header.frame_id = tf_msg.child_frame_id + tf_msg_new.child_frame_id = tf_msg.header.frame_id + + # Set the new translation + tf_msg_new.transform.translation.x = t_new[0] + tf_msg_new.transform.translation.y = t_new[1] + tf_msg_new.transform.translation.z = t_new[2] + + # Set the new orientation + tf_msg_new.transform.rotation.x = q_new[0] + tf_msg_new.transform.rotation.y = q_new[1] + tf_msg_new.transform.rotation.z = q_new[2] + tf_msg_new.transform.rotation.w = q_new[3] + + return tf_msg_new + + +def create_transform_stamped( + timestamp: float, + x: float, + y: float, + z: float, + qw: float, + qx: float, + qy: float, + qz: float, + fixed_frame_id: str, + child_frame_id: str, +): + t = TransformStamped() + t.header.stamp = Time(seconds=float(timestamp)).to_msg() + t.transform.translation.x = float(x) + t.transform.translation.y = float(y) + t.transform.translation.z = float(z) + t.transform.rotation.x = float(qx) + t.transform.rotation.y = float(qy) + t.transform.rotation.z = float(qz) + t.transform.rotation.w = float(qw) + t.header.frame_id = fixed_frame_id + t.child_frame_id = child_frame_id + return t + + +def write_bag(input_file_path, output_bag_path, fixed_frame_id, child_frame_id): + first_line = True + writer = rosbag2_py.SequentialWriter() + storage_options = rosbag2_py.StorageOptions( + uri=output_bag_path, storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + writer.open(storage_options, converter_options) + + # Set the topic information for /tf + tf_topic_info = rosbag2_py.TopicMetadata( + name="/tf", type="tf2_msgs/msg/TFMessage", serialization_format="cdr" + ) + writer.create_topic(tf_topic_info) + + with open(input_file_path, "r") as file: + for line in file: + # Header line + if first_line: + first_line = False + continue + + # Assuming format: timestamp,x,y,z,qw,qx,qy,qz + parts = line.strip().split(",") + if len(parts) < 11: + continue # Skip malformed lines + timestamp, x, y, z, qx, qy, qz, qw, roll, pitch, yaw = parts + tf_msg = create_transform_stamped( + timestamp=timestamp, + x=x, + y=y, + z=z, + qw=qw, + qx=qx, + qy=qy, + qz=qz, + fixed_frame_id=fixed_frame_id, + child_frame_id=child_frame_id, + ) + + # Invert the transform + inv_tf = invert_transform(tf_msg) + + # Wrap in TFMessage + tf_message = TFMessage(transforms=[inv_tf]) + + # Write to the bag file + writer.write( + "/tf", + serialize_message(tf_message), + Time(seconds=float(timestamp)).nanoseconds, + ) + + print(f"Conversion complete. Output saved to {output_bag_path}") + + +if __name__ == "__main__": + rclpy.init() + if len(sys.argv) < 5: + print( + "Usage: python3 convert_ros2_bag_with_transform.py " + ) + sys.exit(1) + + input_file = sys.argv[1] + output_bag = sys.argv[2] + fixed_frame_id = sys.argv[3] + child_frame_id = sys.argv[4] + + write_bag(input_file, output_bag, fixed_frame_id, child_frame_id) + rclpy.shutdown() diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py new file mode 100644 index 00000000..f9383ebd --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python + +# Imports +import os + +def get_latest_time_string_in_folder(folder_path: str) -> str: + print(f"All files located at: {folder_path}:") + # Load all files in location + files = os.listdir(folder_path) + # Only get csv files + files = [f for f in files if f.endswith(".csv")] + # We want to only get the latest files + files.sort() + latest_filename = files[-1] + # Get first part of string before "_optimized" + time_string = latest_filename.split("_v")[0] + return time_string \ No newline at end of file diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h new file mode 100644 index 00000000..90b56102 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -0,0 +1,176 @@ +#ifndef GRAPHMSFROS_H +#define GRAPHMSFROS_H + +// std +#include +#include +#include +#include + +// ROS2 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Workspace +#include "graph_msf/interface/GraphMsf.h" +#include "graph_msf/interface/GraphMsfClassic.h" +#include "graph_msf/interface/GraphMsfHolistic.h" + +// Macros +#define ROS_QUEUE_SIZE 1 + +namespace graph_msf { + +class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { + public: + GraphMsfRos2(std::shared_ptr& node); + // Destructor + ~GraphMsfRos2() override = default; + + // Setup + void setup(std::shared_ptr staticTransformsPtr); + + protected: + // Functions that need implementation + virtual void initializePublishers(); + virtual void initializeSubscribers(); + virtual void initializeMessages(); + virtual void initializeServices(rclcpp::Node& node); + + // Commodity Functions to be shared ----------------------------------- + // Static + static void addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Vector3d& t, int maxBufferLength); + static void addToOdometryMsg( + const nav_msgs::msg::Odometry::SharedPtr& msgPtr, const std::string& fixedFrame, const std::string& movingFrame, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, const Eigen::Vector3d& W_v_W_F, + const Eigen::Vector3d& W_w_W_F, + const Eigen::Matrix& poseCovariance = Eigen::Matrix::Zero(), + const Eigen::Matrix& twistCovariance = Eigen::Matrix::Zero()); + static void addToPoseWithCovarianceStampedMsg( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Matrix& transformCovariance = Eigen::Matrix::Zero()); + static void extractCovariancesFromOptimizedState( + Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + static void createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker); + void createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& position, const std::string& nameSpace, const int id, + visualization_msgs::msg::Marker& marker); + + // Parameter Loading ----------------------------------- + virtual void readParams(); + + // Callbacks + virtual void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); + + // Services + bool srvOfflineSmootherOptimizeCallback( + const std::shared_ptr req, + std::shared_ptr res); + + // Publishing ----------------------------------- + virtual void publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + void publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr); + void publishOptimizedStateAndBias( + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos, const double timeStamp); + + void publishTfTreeTransform(const std::string& frameName, const std::string& childFrameName, double timeStamp, + const Eigen::Isometry3d& T_frame_childFrame) const; + void publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, + const Eigen::Matrix& twistCovarianceRos) const; + void publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, + const double timeStamp) const; + void publishVelocityMarkers(const std::shared_ptr& navStatePtr) const; + void publishImuPaths(const std::shared_ptr& navStatePtr) const; + void publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const; + + // Measure time + long secondsSinceStart(); + + // Node + std::shared_ptr node_; + + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Time + std::chrono::time_point startTime; + std::chrono::time_point currentTime; + + // Publishers + // TF + std::shared_ptr tfBroadcaster_; + + // Members + std::string referenceFrameAlignedNameId = "_graph_msf_aligned"; + std::string sensorFrameCorrectedNameId = "_graph_msf_corrected"; + std::string optimizationResultLoggingPath = ""; + + private: + // Publishers + rclcpp::Publisher::SharedPtr pubEstOdomImu_; + rclcpp::Publisher::SharedPtr pubEstWorldImu_; + rclcpp::Publisher::SharedPtr pubOptWorldImu_; + rclcpp::Publisher::SharedPtr pubEstWorldPosVariance_; + rclcpp::Publisher::SharedPtr pubEstWorldRotVariance_; + rclcpp::Publisher::SharedPtr pubVelocityMarker_; + rclcpp::Publisher::SharedPtr pubEstOdomImuPath_; + rclcpp::Publisher::SharedPtr pubEstWorldImuPath_; + rclcpp::Publisher::SharedPtr pubOptWorldImuPath_; + rclcpp::Publisher::SharedPtr pubAccelBias_; + rclcpp::Publisher::SharedPtr pubGyroBias_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssLPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssRPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldLidarPath_; + rclcpp::Publisher::SharedPtr pubAddedImuMeas_; + + std::map::SharedPtr> + pubPoseStampedByTopicMap_ = {}; + + // Subscribers + rclcpp::Subscription::SharedPtr subImu_; + + // Messages + nav_msgs::msg::Odometry::SharedPtr estOdomImuMsgPtr_; + nav_msgs::msg::Odometry::SharedPtr estWorldImuMsgPtr_; + nav_msgs::msg::Odometry::SharedPtr optWorldImuMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr estWorldPosVarianceMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr estWorldRotVarianceMsgPtr_; + nav_msgs::msg::Path::SharedPtr estOdomImuPathPtr_; + nav_msgs::msg::Path::SharedPtr estWorldImuPathPtr_; + nav_msgs::msg::Path::SharedPtr optWorldImuPathPtr_; + nav_msgs::msg::Path::SharedPtr measWorldLidarPathPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr accelBiasMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr gyroBiasMsgPtr_; + + // Services + rclcpp::Service::SharedPtr srvSmootherOptimize_; + + // Last Optimized State Timestamp + double lastOptimizedStateTimestamp_ = 0.0; + double lastIntegratedStateTimestamp_ = 0.0; + + // Mutex + std::mutex rosPublisherMutex_; +}; +} // namespace graph_msf + +#endif // end GRAPHMSFROS_H diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h new file mode 100644 index 00000000..f73f82fc --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h @@ -0,0 +1,12 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf/interface/Terminal.h" + +#pragma once + +#define REGULAR_COUT std::cout << YELLOW_START << "GraphMsfRos2" << COLOR_END diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h new file mode 100644 index 00000000..0dfc2212 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h @@ -0,0 +1,27 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ +#pragma once + +#include +#include + +namespace graph_msf +{ + + class ElementToRoot final + { + public: + /// Constructor + explicit ElementToRoot(const tf2::Transform &T, const std::string &rootName_, const std::string &elementName_) + : T_root_element(T), rootName(rootName_), elementName(elementName_) {} + + tf2::Transform T_root_element; ///< The tf2 Transform from root to element + std::string rootName; ///< The name of the root element to which this link is attached + std::string elementName; ///< The name of the element + }; + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h new file mode 100644 index 00000000..09b482cf --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h @@ -0,0 +1,33 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once +// ROS 2 +#include +#include +#include + +// Workspace +#include "graph_msf/config/StaticTransforms.h" + +namespace graph_msf { + +class StaticTransformsTf : public StaticTransforms { + public: + explicit StaticTransformsTf(const rclcpp::Node::SharedPtr& node); + + virtual ~StaticTransformsTf() = default; + + protected: + bool findTransformations() override; + + // Members + std::shared_ptr tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h new file mode 100644 index 00000000..d31357b8 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h @@ -0,0 +1,41 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#ifndef StaticTransformsUrdf_H +#define StaticTransformsUrdf_H + +#include +#include +#include +#include "graph_msf/config/StaticTransforms.h" +#include "graph_msf_ros2/extrinsics/ElementToRoot.h" +#include "rclcpp/rclcpp.hpp" +#include "urdf/model.h" + +namespace graph_msf { + +class StaticTransformsUrdf : public StaticTransforms { + public: + StaticTransformsUrdf(const std::shared_ptr privateNodePtr); + void setup(); + + protected: + std::string urdfDescriptionName_; + urdf::Model urdfModel_; + std::unique_ptr model_; + KDL::Tree tree_; + std::map segments_; + void getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName = ""); + tf2::Transform kdlToTransform(const KDL::Frame& k); + std::shared_ptr privateNode_; + + private: + virtual void findTransformations() = 0; +}; + +} // namespace graph_msf +#endif // end StaticTransformsUrdf_H diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h new file mode 100644 index 00000000..f15fe1dc --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h @@ -0,0 +1,52 @@ +#pragma once + +// ROS 2 +#include +#include +#include +#include + +// Workspace +#include "graph_msf/interface/Terminal.h" +#include "graph_msf_ros2/constants.h" + +namespace graph_msf { + +template +inline void printKey(const std::string& key, T value) { + std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key << " set to: " << value << std::endl; +} + +template <> +inline void printKey(const std::string& key, std::vector vector) { + std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key << " set to: "; + for (const auto& element : vector) { + std::cout << element << ","; + } + std::cout << std::endl; +} + +// Implementation of Templating +template +T tryGetParam(std::shared_ptr node, const std::string& key) { + T value; + + if (node->get_parameter(key, value)) { + printKey(key, value); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); + // std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key.c_str() << " set to: " << value << std::endl; + return value; + } + + if (node->get_parameter("/" + key, value)) { + printKey("/" + key, value); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); + // std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key.c_str() << " set to: " << value << std::endl; + return value; + } + + RCLCPP_ERROR(node->get_logger(), "Parameter not found: %s", key.c_str()); + throw std::runtime_error("GraphMsfRos2 - " + key + " not specified."); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h new file mode 100644 index 00000000..f94d773f --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h @@ -0,0 +1,47 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace graph_msf { + +Eigen::Matrix convertCovarianceGtsamConventionToRosConvention( + const Eigen::Matrix& covGtsam); + +void odomMsgToEigen(const nav_msgs::msg::Odometry& odomLidar, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::Pose& pose, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::PoseStamped& pose, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::PoseWithCovarianceStamped& odomLidar, Eigen::Matrix4d& T); + +void odomMsgToTf(const nav_msgs::msg::Odometry& odomLidar, tf2::Transform& T); + +tf2::Transform matrix3ToTf(const Eigen::Matrix3d& R); + +tf2::Transform matrix4ToTf(const Eigen::Matrix4d& T); + +tf2::Transform isometry3ToTf(const Eigen::Isometry3d& T); + +void tfToMatrix4(const tf2::Transform& tf_T, Eigen::Matrix4d& T); + +void tfToIsometry3(const tf2::Transform& tf_T, Eigen::Isometry3d& T); + +void transformStampedToIsometry3(const geometry_msgs::msg::Transform& transform, Eigen::Isometry3d& T); + +tf2::Transform pose3ToTf(const Eigen::Matrix3d& T); + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/package.xml b/ros2/graph_msf_ros2/package.xml new file mode 100644 index 00000000..51cc492c --- /dev/null +++ b/ros2/graph_msf_ros2/package.xml @@ -0,0 +1,41 @@ + + + graph_msf_ros2 + 0.0.0 + + State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry + and GNSS estimates. + + + Julian Nubert + BSD + Julian Nubert + + + ament_cmake + rosidl_default_generators + + rclcpp + graph_msf + kdl_parser + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + + urdf + eigen_colcon + tf2_eigen + graph_msf_ros2_msgs + + + python3 + + + ament_cmake_pytest + rosbag2_py + + rosidl_interface_packages + diff --git a/ros2/graph_msf_ros2/resource/graph_msf_ros2 b/ros2/graph_msf_ros2/resource/graph_msf_ros2 new file mode 100644 index 00000000..0519ecba --- /dev/null +++ b/ros2/graph_msf_ros2/resource/graph_msf_ros2 @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/ros2/graph_msf_ros2/rviz/vis.rviz b/ros2/graph_msf_ros2/rviz/vis.rviz new file mode 100644 index 00000000..029f6208 --- /dev/null +++ b/ros2/graph_msf_ros2/rviz/vis.rviz @@ -0,0 +1,203 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1 + - /Odometry1/Shape1 + - /Odometry2 + - /Odometry2/Shape1 + Splitter Ratio: 0.5 + Tree Height: 1079 + - 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 + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 0.5 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /crl_rzr/liopard/lio/odometry + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 238; 238; 236 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/transform_odom_lidar + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: crl_rzr/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: + Angle: -1.5699995756149292 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 94.30439758300781 + Target Frame: + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000018a000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004a8000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1869 + X: 691 + Y: 3 diff --git a/ros2/graph_msf_ros2/setup.py b/ros2/graph_msf_ros2/setup.py new file mode 100644 index 00000000..9272c5e6 --- /dev/null +++ b/ros2/graph_msf_ros2/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup +from setuptools import find_packages + +package_name = 'graph_msf_ros2' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(where='graph_msf_ros2_py'), + package_dir={'': 'graph_msf_ros2_py'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Julian Nubert', + maintainer_email='nubertj@ethz.ch', + description='State estimation based on factor graphs, utilizing GTSAM functionality', + license='BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'manual_pose_files_to_tf_and_odom_bag = graph_msf_ros2_py.replay.manual_pose_files_to_tf_and_odom_bag:main', + 'plot_latest_quantitites_in_folder = graph_msf_ros2_py.plotting.plot_latest_quantitites_in_folder:main', + 'remove_tf_from_bag = graph_msf_ros2_py.bag_filter.remove_tf_from_bag:main', + ], + }, +) \ No newline at end of file diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp new file mode 100644 index 00000000..ee091d13 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -0,0 +1,715 @@ +// Implementation +#include "graph_msf_ros2/GraphMsfRos2.h" + +// ROS2 +#include +#include +#include + +// Workspace +#include "graph_msf_ros2/constants.h" +#include "graph_msf_ros2/util/conversions.h" + +namespace graph_msf { + +GraphMsfRos2::GraphMsfRos2(std::shared_ptr& node) : node_(node) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Constructor called."); + // Configurations ---------------------------- + // Graph Config + graphConfigPtr_ = std::make_shared(); +} + +void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Setup called."); + + // Check + if (staticTransformsPtr == nullptr) { + throw std::runtime_error("Static transforms not set. Has to be set."); + } + + // Clock + clock_ = node_->get_clock(); + + // Sensor Params + node_->declare_parameter("sensor_params.imuRate", 0.0); + node_->declare_parameter("sensor_params.createStateEveryNthImuMeasurement", 25); + node_->declare_parameter("sensor_params.useImuSignalLowPassFilter", false); + node_->declare_parameter("sensor_params.imuLowPassFilterCutoffFreq", 0.0); + node_->declare_parameter("sensor_params.imuBufferLength", 800); + node_->declare_parameter("sensor_params.imuTimeOffset", 0.0); + + // Initialization Params + node_->declare_parameter("initialization_params.estimateGravityFromImu", false); + node_->declare_parameter("initialization_params.gravityMagnitude", 9.80665); + + // Graph Params + node_->declare_parameter("graph_params.realTimeSmootherUseIsam", false); + node_->declare_parameter("graph_params.realTimeSmootherLag", 0.0); + node_->declare_parameter("graph_params.useAdditionalSlowBatchSmoother", false); + node_->declare_parameter("graph_params.slowBatchSmootherUseIsam", false); + node_->declare_parameter("graph_params.gaussNewtonWildfireThreshold", 0.0); + node_->declare_parameter("graph_params.minOptimizationFrequency", 0.0); + node_->declare_parameter("graph_params.maxOptimizationFrequency", 0.0); + node_->declare_parameter("graph_params.additionalOptimizationIterations", 0); + node_->declare_parameter("graph_params.findUnusedFactorSlots", false); + node_->declare_parameter("graph_params.enableDetailedResults", false); + node_->declare_parameter("graph_params.realTimeSmootherUseCholeskyFactorization", false); + node_->declare_parameter("graph_params.slowBatchSmootherUseCholeskyFactorization", false); + node_->declare_parameter("graph_params.usingBiasForPreIntegration", false); + node_->declare_parameter("graph_params.optimizeReferenceFramePosesWrtWorld", false); + node_->declare_parameter("graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset", false); + node_->declare_parameter("graph_params.referenceFramePosesResetThreshold", 0.0); + node_->declare_parameter("graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment", false); + + node_->declare_parameter("graph_params.useWindowForMarginalsComputation", false); + node_->declare_parameter("graph_params.windowSizeSecondsForMarginalsComputation", 0.0); + node_->declare_parameter("graph_params.createReferenceAlignmentKeyframeEveryNSeconds", 0.0); + + // Noise Params + node_->declare_parameter("noise_params.accNoiseDensity", 0.0); + node_->declare_parameter("noise_params.integrationNoiseDensity", 0.0); + node_->declare_parameter("noise_params.use2ndOrderCoriolis", false); + node_->declare_parameter("noise_params.gyrNoiseDensity", 0.0); + node_->declare_parameter("noise_params.omegaCoriolis", 0.0); + node_->declare_parameter("noise_params.accBiasRandomWalkNoiseDensity", 0.0); + node_->declare_parameter("noise_params.gyrBiasRandomWalkNoiseDensity", 0.0); + node_->declare_parameter("noise_params.biasAccOmegaInit", 0.0); + node_->declare_parameter("noise_params.accBiasPrior", 0.0); + node_->declare_parameter("noise_params.gyrBiasPrior", 0.0); + node_->declare_parameter("noise_params.initialPositionNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialOrientationNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialVelocityNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialAccBiasNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialGyroBiasNoiseDensity", 0.0); + + // Re-linearization Params + node_->declare_parameter("relinearization_params.positionReLinTh", 0.0); + node_->declare_parameter("relinearization_params.rotationReLinTh", 0.0); + node_->declare_parameter("relinearization_params.velocityReLinTh", 0.0); + node_->declare_parameter("relinearization_params.accBiasReLinTh", 0.0); + node_->declare_parameter("relinearization_params.gyrBiasReLinTh", 0.0); + node_->declare_parameter("relinearization_params.referenceFrameReLinTh", 0.0); + node_->declare_parameter("relinearization_params.calibrationReLinTh", 0.0); + node_->declare_parameter("relinearization_params.displacementReLinTh", 0.0); + node_->declare_parameter("relinearization_params.landmarkReLinTh", 0.0); + node_->declare_parameter("relinearization_params.relinearizeSkip", 0); + node_->declare_parameter("relinearization_params.enableRelinearization", false); + node_->declare_parameter("relinearization_params.evaluateNonlinearError", false); + node_->declare_parameter("relinearization_params.cacheLinearizedFactors", false); + node_->declare_parameter("relinearization_params.enablePartialRelinearizationCheck", false); + + // Common Params + node_->declare_parameter("common_params.verbosity", 0); + node_->declare_parameter("common_params.odomNotJumpAtStart", false); + node_->declare_parameter("common_params.logRealTimeStateToMemory", false); + node_->declare_parameter("common_params.logLatencyAndUpdateDurationToMemory", false); + + // Extrinsic frames + node_->declare_parameter("extrinsics.worldFrame", std::string("")); + node_->declare_parameter("extrinsics.odomFrame", std::string("")); + node_->declare_parameter("extrinsics.imuFrame", std::string("")); + node_->declare_parameter("extrinsics.initializeZeroYawAndPositionOfFrame", std::string("")); + node_->declare_parameter("extrinsics.baseLinkFrame", std::string("")); + + // Name IDs + node_->declare_parameter("name_ids.referenceFrameAligned", std::string("")); + node_->declare_parameter("name_ids.sensorFrameCorrected", std::string("")); + + // Logging path + node_->declare_parameter("launch.optimizationResultLoggingPath", std::string("")); + + // Read parameters ---------------------------- + GraphMsfRos2::readParams(); + + // Call super class Setup ---------------------------- + GraphMsf::setup(graphConfigPtr_, staticTransformsPtr); + + // Publishers ---------------------------- + GraphMsfRos2::initializePublishers(); + + // Subscribers ---------------------------- + GraphMsfRos2::initializeSubscribers(); + + // Messages ---------------------------- + GraphMsfRos2::initializeMessages(); + + tfBroadcaster_ = std::make_shared(node_); + + // Services ---------------------------- + GraphMsfRos2::initializeServices(*node_); + + // Time + startTime = std::chrono::high_resolution_clock::now(); + + // Wrap up + RCLCPP_INFO(node_->get_logger(), "Set up successfully."); +} + +void GraphMsfRos2::initializePublishers() { + RCLCPP_INFO(node_->get_logger(), "Initializing publishers."); + + // Odometry + pubEstOdomImu_ = node_->create_publisher("/graph_msf/est_odometry_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImu_ = node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImu_ = node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); + + // Vector3 Variances + pubEstWorldPosVariance_ = + node_->create_publisher("/graph_msf/est_world_pos_variance", ROS_QUEUE_SIZE); + pubEstWorldRotVariance_ = + node_->create_publisher("/graph_msf/est_world_rot_variance", ROS_QUEUE_SIZE); + + // Velocity Marker + pubVelocityMarker_ = node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); + + // Paths + pubEstOdomImuPath_ = node_->create_publisher("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImuPath_ = node_->create_publisher("/graph_msf/est_path_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImuPath_ = node_->create_publisher("/graph_msf/opt_path_world_imu", ROS_QUEUE_SIZE); + + // Imu Bias + pubAccelBias_ = node_->create_publisher("/graph_msf/accel_bias", ROS_QUEUE_SIZE); + pubGyroBias_ = node_->create_publisher("/graph_msf/gyro_bias", ROS_QUEUE_SIZE); + + // Added Imu Measurements + pubAddedImuMeas_ = node_->create_publisher("/graph_msf/added_imu_meas", ROS_QUEUE_SIZE); +} + +void GraphMsfRos2::initializeSubscribers() { + RCLCPP_INFO(node_->get_logger(), "Initializing subscribers."); + + // Imu + subImu_ = node_->create_subscription("/imu_topic", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), + std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 Initialized main IMU subscriber with topic: %s", subImu_->get_topic_name()); +} + +void GraphMsfRos2::initializeMessages() { + RCLCPP_INFO(node_->get_logger(), "Initializing messages."); + + // Odometry + estOdomImuMsgPtr_ = std::make_shared(); + estWorldImuMsgPtr_ = std::make_shared(); + optWorldImuMsgPtr_ = std::make_shared(); + + // Vector3 Variances + estWorldPosVarianceMsgPtr_ = std::make_shared(); + estWorldRotVarianceMsgPtr_ = std::make_shared(); + + // Path + estOdomImuPathPtr_ = std::make_shared(); + estWorldImuPathPtr_ = std::make_shared(); + optWorldImuPathPtr_ = std::make_shared(); + + // Imu Bias + accelBiasMsgPtr_ = std::make_shared(); + gyroBiasMsgPtr_ = std::make_shared(); +} + +void GraphMsfRos2::initializeServices(rclcpp::Node& node) { + RCLCPP_INFO(node.get_logger(), "Initializing services."); + + // Trigger offline smoother optimization + srvSmootherOptimize_ = node.create_service( + "/graph_msf/trigger_offline_optimization", + std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, std::placeholders::_2)); +} + +bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback(const std::shared_ptr req, + std::shared_ptr res) { + // Hardcode max iterations (you can make this configurable via parameter if needed) + int maxIterations = 100; + + // Trigger offline smoother optimization and create response + if (optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath, false)) { + res->success = true; + res->message = "Optimization successful."; + } else { + res->success = false; + res->message = "Optimization failed."; + } + return true; +} + +void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& fixedFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& t, const int maxBufferLength) { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixedFrameName; + pose.header.stamp = stamp; + pose.pose.position.x = t(0); + pose.pose.position.y = t(1); + pose.pose.position.z = t(2); + pathPtr->header.frame_id = fixedFrameName; + pathPtr->header.stamp = stamp; + pathPtr->poses.push_back(pose); + if (pathPtr->poses.size() > maxBufferLength) { + pathPtr->poses.erase(pathPtr->poses.begin()); + } +} + +void GraphMsfRos2::addToOdometryMsg(const nav_msgs::msg::Odometry::SharedPtr& msgPtr, const std::string& fixedFrame, + const std::string& movingFrame, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Vector3d& F_v_W_F, const Eigen::Vector3d& F_w_W_F, + const Eigen::Matrix& poseCovariance, const Eigen::Matrix& twistCovariance) { + msgPtr->header.frame_id = fixedFrame; + msgPtr->child_frame_id = movingFrame; + msgPtr->header.stamp = stamp; + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Pose using tf2 + geometry_msgs::msg::Pose pose_msg; + tf2::convert(T, pose_msg); + msgPtr->pose.pose = pose_msg; + + msgPtr->twist.twist.linear.x = F_v_W_F(0); + msgPtr->twist.twist.linear.y = F_v_W_F(1); + msgPtr->twist.twist.linear.z = F_v_W_F(2); + msgPtr->twist.twist.angular.x = F_w_W_F(0); + msgPtr->twist.twist.angular.y = F_w_W_F(1); + msgPtr->twist.twist.angular.z = F_w_W_F(2); + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + msgPtr->pose.covariance[6 * i + j] = poseCovariance(i, j); + msgPtr->twist.covariance[6 * i + j] = twistCovariance(i, j); + } + } +} + +void GraphMsfRos2::addToPoseWithCovarianceStampedMsg(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, + const std::string& frameName, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Matrix& transformCovariance) { + msgPtr->header.frame_id = frameName; + msgPtr->header.stamp = stamp; + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Pose using tf2 + geometry_msgs::msg::Pose pose_msg; + tf2::convert(T, pose_msg); + msgPtr->pose.pose = pose_msg; + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + msgPtr->pose.covariance[6 * i + j] = transformCovariance(i, j); + } + } +} + +void GraphMsfRos2::extractCovariancesFromOptimizedState( + Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { + // Extract covariances from optimized state + if (optimizedStateWithCovarianceAndBiasPtr != nullptr) { + poseCovarianceRos = + graph_msf::convertCovarianceGtsamConventionToRosConvention(optimizedStateWithCovarianceAndBiasPtr->getPoseCovariance()); + twistCovarianceRos.block<3, 3>(0, 0) = optimizedStateWithCovarianceAndBiasPtr->getVelocityCovariance(); + } else { + poseCovarianceRos.setZero(); + twistCovarianceRos.setZero(); + } +} + +// Markers +void GraphMsfRos2::createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, const Eigen::Vector3d& velocity, + visualization_msgs::msg::Marker& marker) { + // Arrow + marker.header.frame_id = referenceFrameName; + marker.header.stamp = stamp; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + + // Scale and Color + const double scale = velocity.norm(); + marker.scale.x = 0.1; // shaft diameter + marker.scale.y = 0.2; // head diameter + marker.scale.z = 0.2; // head length + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + // Define Arrow through start and end point + geometry_msgs::msg::Point startPoint, endPoint; + startPoint.x = 0.0; // origin + startPoint.y = 0.0; // origin + startPoint.z = 1.0; // 1 meter above origin + endPoint.x = startPoint.x + velocity(0); + endPoint.y = startPoint.y + velocity(1); + endPoint.z = startPoint.z + velocity(2); + marker.points.push_back(startPoint); + marker.points.push_back(endPoint); + + // Quaternion for orientation + tf2::Quaternion q; + q.setRPY(0, 0, 0); + marker.pose.orientation.x = q.x(); + marker.pose.orientation.y = q.y(); + marker.pose.orientation.z = q.z(); + marker.pose.orientation.w = q.w(); +} + +void GraphMsfRos2::createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, const Eigen::Vector3d& position, + const std::string& nameSpace, const int id, visualization_msgs::msg::Marker& marker) { + // Sphere + marker.header.frame_id = referenceFrameName; + marker.header.stamp = stamp; + marker.ns = nameSpace; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + // Scale and Color + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + // Position + marker.pose.position.x = position(0); + marker.pose.position.y = position(1); + marker.pose.position.z = position(2); + + // Orientation + marker.pose.orientation.w = 1.0; +} + +long GraphMsfRos2::secondsSinceStart() { + currentTime = std::chrono::high_resolution_clock::now(); + return std::chrono::duration_cast(currentTime - startTime).count(); +} + +// Main IMU Callback +void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) { + // Convert to Eigen + Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, imuMsgPtr->linear_acceleration.z); + Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, imuMsgPtr->angular_velocity.z); + Eigen::Matrix addedImuMeasurements; // accel, gyro + + // Create pointer for carrying state + std::shared_ptr preIntegratedNavStatePtr = nullptr; + std::shared_ptr optimizedStateWithCovarianceAndBiasPtr = nullptr; + + // Add measurement and get state + if (GraphMsf::addCoreImuMeasurementAndGetState(linearAcc, angularVel, + imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, + preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { + // Encountered Delay + auto now = clock_->now(); + auto delay = now.seconds() - preIntegratedNavStatePtr->getTimeK(); + if (delay > 0.5) { + RCLCPP_WARN(node_->get_logger(), "Encountered delay of %.14f seconds.", delay); + // Print now vs chrono time + auto currentChronoTime = std::chrono::high_resolution_clock::now(); + auto chronoTimeSinceEpoch = std::chrono::duration_cast>(currentChronoTime.time_since_epoch()).count(); + RCLCPP_WARN(node_->get_logger(), "Now: %.14f, Std chrono time: %.14f", now.seconds(), chronoTimeSinceEpoch); + } + + // Publish Odometry + this->publishState(preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + + // Publish Filtered Imu Measurements (uncomment if needed) + // this->publishAddedImuMeas_(addedImuMeasurements, imuMsgPtr->header.stamp); + } else if (GraphMsf::isGraphInited()) { + RCLCPP_WARN(node_->get_logger(), "Could not add IMU measurement."); + } +} + +// Publish state --------------------------------------------------------------- +// Higher Level Functions +void GraphMsfRos2::publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { + // Covariances + Eigen::Matrix poseCovarianceRos, twistCovarianceRos; + graph_msf::GraphMsfRos2::extractCovariancesFromOptimizedState(poseCovarianceRos, twistCovarianceRos, + optimizedStateWithCovarianceAndBiasPtr); + + // Odometry messages + publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); + + // Variances (only diagonal elements) + Eigen::Vector3d positionVarianceRos = poseCovarianceRos.block<3, 3>(0, 0).diagonal(); + Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); + + // Publish non-time critical data in a separate thread + std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, twistCovarianceRos, + positionVarianceRos, orientationVarianceRos, integratedNavStatePtr, + optimizedStateWithCovarianceAndBiasPtr); + publishNonTimeCriticalDataThread.detach(); +} + +// Copy the arguments in order to be thread safe +void GraphMsfRos2::publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr) { + // Mutex for not overloading ROS + std::lock_guard lock(rosPublisherMutex_); + + // Time + const double& timeK = integratedNavStatePtr->getTimeK(); // Alias + + // Publish to TF + // B_O + Eigen::Isometry3d T_B_Ok = + staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getImuFrame()) * + integratedNavStatePtr->getT_O_Ik_gravityAligned().inverse(); + publishTfTreeTransform(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getOdomFrame(), timeK, T_B_Ok); + // O_W + Eigen::Isometry3d T_O_W = integratedNavStatePtr->getT_W_O().inverse(); + publishTfTreeTransform(staticTransformsPtr_->getOdomFrame(), staticTransformsPtr_->getWorldFrame(), timeK, T_O_W); + + // Publish Variances + publishDiagVarianceVectors(positionVarianceRos, orientationVarianceRos, timeK); + + // Publish Velocity Markers + publishVelocityMarkers(integratedNavStatePtr); + + // Publish paths + publishImuPaths(integratedNavStatePtr); + + // Optimized estimate ---------------------- + publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos, timeK); +} + +void GraphMsfRos2::publishOptimizedStateAndBias( + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos, const double timeStamp) { + // A: Publish Odometry and IMU Biases at update rate + if (optimizedStateWithCovarianceAndBiasPtr != nullptr && + optimizedStateWithCovarianceAndBiasPtr->getTimeK() - lastOptimizedStateTimestamp_ > 1e-03) { + // Time of this optimized state + lastOptimizedStateTimestamp_ = optimizedStateWithCovarianceAndBiasPtr->getTimeK(); + + // Odometry messages + // world->imu + if (pubOptWorldImu_->get_subscription_count() > 0) { + addToOdometryMsg(optWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik(), optimizedStateWithCovarianceAndBiasPtr->getI_v_W_I(), + optimizedStateWithCovarianceAndBiasPtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + pubOptWorldImu_->publish(*optWorldImuMsgPtr_); + } + + // Path + // world->imu + addToPathMsg(optWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 20); + if (pubOptWorldImuPath_->get_subscription_count() > 0) { + pubOptWorldImuPath_->publish(*optWorldImuPathPtr_); + } + + // Biases + // Publish accel bias + if (pubAccelBias_->get_subscription_count() > 0) { + Eigen::Vector3d accelBias = optimizedStateWithCovarianceAndBiasPtr->getAccelerometerBias(); + accelBiasMsgPtr_->header.stamp = rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9); + accelBiasMsgPtr_->header.frame_id = staticTransformsPtr_->getImuFrame(); + accelBiasMsgPtr_->vector.x = accelBias(0); + accelBiasMsgPtr_->vector.y = accelBias(1); + accelBiasMsgPtr_->vector.z = accelBias(2); + pubAccelBias_->publish(*accelBiasMsgPtr_); + } + // Publish gyro bias + if (pubGyroBias_->get_subscription_count() > 0) { + Eigen::Vector3d gyroBias = optimizedStateWithCovarianceAndBiasPtr->getGyroscopeBias(); + gyroBiasMsgPtr_->header.stamp = rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9); + gyroBiasMsgPtr_->header.frame_id = staticTransformsPtr_->getImuFrame(); + gyroBiasMsgPtr_->vector.x = gyroBias(0); + gyroBiasMsgPtr_->vector.y = gyroBias(1); + gyroBiasMsgPtr_->vector.z = gyroBias(2); + pubGyroBias_->publish(*gyroBiasMsgPtr_); + } + } // Publishing odometry and biases + + // B: Publish Transforms at imu rate + if (optimizedStateWithCovarianceAndBiasPtr != nullptr && timeStamp - lastIntegratedStateTimestamp_ > 1e-03) { + lastIntegratedStateTimestamp_ = timeStamp; + // TFs in Optimized State + for (const auto& framePairTransformMapIterator : + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransforms().getTransformsMap()) { + // Case 1: If includes world frame --> everything child of world -------------------------------- + if (framePairTransformMapIterator.first.first == staticTransformsPtr_->getWorldFrame() || + framePairTransformMapIterator.first.second == staticTransformsPtr_->getWorldFrame()) { + // A. Get transform + Eigen::Isometry3d T_W_M; + std::string mapFrameName; + const std::string& worldFrameName = staticTransformsPtr_->getWorldFrame(); + + // If world is second, then map is first + if (framePairTransformMapIterator.first.second == worldFrameName) { + T_W_M = framePairTransformMapIterator.second.inverse(); + mapFrameName = framePairTransformMapIterator.first.first; + } + // If world is second, then map is first, this is the case for holistic alignment + else { + T_W_M = framePairTransformMapIterator.second; + mapFrameName = framePairTransformMapIterator.first.second; + } + + // B. Publish TransformStamped for Aligned Frames + std::string transformTopic = "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; + auto poseWithCovarianceStampedMsgPtr = std::make_shared(); + addToPoseWithCovarianceStampedMsg( + poseWithCovarianceStampedMsgPtr, staticTransformsPtr_->getWorldFrame(), + rclcpp::Time(timeStamp * 1e9), T_W_M, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( + framePairTransformMapIterator.first.first, framePairTransformMapIterator.first.second)); + // Check whether publisher already exists + if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { + pubPoseStampedByTopicMap_[transformTopic] = + node_->create_publisher(transformTopic, 1); + RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); + } + if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { + pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); + } + + // C. Publish TF Tree + publishTfTreeTransform(worldFrameName, mapFrameName + referenceFrameAlignedNameId, + timeStamp, T_W_M); + } + // Case 2: Other transformations + else { + const std::string& sensorFrameName = framePairTransformMapIterator.first.first; + const std::string& sensorFrameNameCorrected = framePairTransformMapIterator.first.second; + const Eigen::Isometry3d& T_sensor_sensorCorrected = framePairTransformMapIterator.second; + + // A. Publish TransformStamped for Corrected Frames + std::string transformTopic = "/graph_msf/transform_" + sensorFrameName + "_to_" + sensorFrameNameCorrected; + auto poseWithCovarianceStampedMsgPtr = std::make_shared(); + addToPoseWithCovarianceStampedMsg( + poseWithCovarianceStampedMsgPtr, sensorFrameName, rclcpp::Time(timeStamp * 1e9), + T_sensor_sensorCorrected, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2(sensorFrameName, + sensorFrameNameCorrected)); + // Check whether publisher already exists + if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { + pubPoseStampedByTopicMap_[transformTopic] = + node_->create_publisher(transformTopic, 1); + RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); + } + if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { + pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); + } + + // B. Publish TF Tree + publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, timeStamp, + T_sensor_sensorCorrected); + } + } // for each frame pair transform + } // Publishing of Transforms +} + +// Lower Level Functions +void GraphMsfRos2::publishTfTreeTransform(const std::string& parentFrameName, const std::string& childFrameName, const double timeStamp, + const Eigen::Isometry3d& T_frame_childFrame) const { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = rclcpp::Time(timeStamp * 1e9); + transformStamped.header.frame_id = parentFrameName; + transformStamped.child_frame_id = childFrameName; + transformStamped.transform.translation.x = T_frame_childFrame.translation().x(); + transformStamped.transform.translation.y = T_frame_childFrame.translation().y(); + transformStamped.transform.translation.z = T_frame_childFrame.translation().z(); + Eigen::Quaterniond q(T_frame_childFrame.rotation()); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Transform + // tf2::convert(T_frame_childFrame, transformStamped.transform); + + tfBroadcaster_->sendTransform(transformStamped); +} + +void GraphMsfRos2::publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, + const Eigen::Matrix& twistCovarianceRos) const { + // Odom->imu + if (pubEstOdomImu_->get_subscription_count() > 0) { + addToOdometryMsg(estOdomImuMsgPtr_, staticTransformsPtr_->getOdomFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), preIntegratedNavStatePtr->getT_O_Ik_gravityAligned(), + preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + pubEstOdomImu_->publish(*estOdomImuMsgPtr_); + } + // World->imu + if (pubEstWorldImu_->get_subscription_count() > 0) { + addToOdometryMsg(estWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), preIntegratedNavStatePtr->getT_W_Ik(), + preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + pubEstWorldImu_->publish(*estWorldImuMsgPtr_); + } +} + +void GraphMsfRos2::publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, + const double timeStamp) const { + // World Position Variance + if (pubEstWorldPosVariance_->get_subscription_count() > 0) { + estWorldPosVarianceMsgPtr_->header.stamp = rclcpp::Time(timeStamp * 1e9); + estWorldPosVarianceMsgPtr_->header.frame_id = staticTransformsPtr_->getWorldFrame(); + estWorldPosVarianceMsgPtr_->vector.x = posVarianceRos(0); + estWorldPosVarianceMsgPtr_->vector.y = posVarianceRos(1); + estWorldPosVarianceMsgPtr_->vector.z = posVarianceRos(2); + pubEstWorldPosVariance_->publish(*estWorldPosVarianceMsgPtr_); + } + // World Rotation Variance + if (pubEstWorldRotVariance_->get_subscription_count() > 0) { + estWorldRotVarianceMsgPtr_->header.stamp = rclcpp::Time(timeStamp * 1e9); + estWorldRotVarianceMsgPtr_->header.frame_id = staticTransformsPtr_->getWorldFrame(); + estWorldRotVarianceMsgPtr_->vector.x = rotVarianceRos(0); + estWorldRotVarianceMsgPtr_->vector.y = rotVarianceRos(1); + estWorldRotVarianceMsgPtr_->vector.z = rotVarianceRos(2); + pubEstWorldRotVariance_->publish(*estWorldRotVarianceMsgPtr_); + } +} + +void GraphMsfRos2::publishVelocityMarkers(const std::shared_ptr& navStatePtr) const { + // Velocity in Odom Frame Marker + visualization_msgs::msg::Marker velocityMarker; + createVelocityMarker(staticTransformsPtr_->getImuFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), navStatePtr->getI_v_W_I(), + velocityMarker); + + // Publish + if (pubVelocityMarker_->get_subscription_count() > 0) { + pubVelocityMarker_->publish(velocityMarker); + } +} + +void GraphMsfRos2::publishImuPaths(const std::shared_ptr& navStatePtr) const { + // odom->imu + addToPathMsg(estOdomImuPathPtr_, staticTransformsPtr_->getOdomFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), + navStatePtr->getT_O_Ik_gravityAligned().translation(), graphConfigPtr_->imuBufferLength_ * 4); + if (pubEstOdomImuPath_->get_subscription_count() > 0) { + pubEstOdomImuPath_->publish(*estOdomImuPathPtr_); + } + // world->imu + addToPathMsg(estWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), + navStatePtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 4); + if (pubEstWorldImuPath_->get_subscription_count() > 0) { + pubEstWorldImuPath_->publish(*estWorldImuPathPtr_); + } +} + +void GraphMsfRos2::publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const { + // Publish added imu measurement + if (pubAddedImuMeas_->get_subscription_count() > 0) { + sensor_msgs::msg::Imu addedImuMeasMsg; + addedImuMeasMsg.header.stamp = stamp; + addedImuMeasMsg.header.frame_id = staticTransformsPtr_->getImuFrame(); + addedImuMeasMsg.linear_acceleration.x = addedImuMeas(0); + addedImuMeasMsg.linear_acceleration.y = addedImuMeas(1); + addedImuMeasMsg.linear_acceleration.z = addedImuMeas(2); + addedImuMeasMsg.angular_velocity.x = addedImuMeas(3); + addedImuMeasMsg.angular_velocity.y = addedImuMeas(4); + addedImuMeasMsg.angular_velocity.z = addedImuMeas(5); + pubAddedImuMeas_->publish(addedImuMeasMsg); + } +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp new file mode 100644 index 00000000..53a9b409 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp @@ -0,0 +1,61 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "graph_msf_ros2/extrinsics/StaticTransformsTf.h" + +// ROS +// #include +#include +#include +#include + +// Workspace +#include "graph_msf_ros2/constants.h" +#include "graph_msf_ros2/util/conversions.h" + +namespace graph_msf { + +StaticTransformsTf::StaticTransformsTf(const rclcpp::Node::SharedPtr& node) + : StaticTransforms(), + tf_buffer_(std::make_shared(node->get_clock())), + tf_listener_(*tf_buffer_) { + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "StaticTransformsTf - Initializing static transforms..."); +} + +bool StaticTransformsTf::findTransformations() { + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transforms in TF-tree."); + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms between the following frames are required: %s, %s", + imuFrame_.c_str(), baseLinkFrame_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Waiting for up to 100 seconds until they arrive..."); + + // Temporary variable + geometry_msgs::msg::TransformStamped transform_stamped; + + // Imu to Base --- + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transform from %s to %s", imuFrame_.c_str(), + baseLinkFrame_.c_str()); + try { + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << baseLinkFrame_ << " for 10 seconds." << std::endl; + transform_stamped = + tf_buffer_->lookupTransform(imuFrame_, baseLinkFrame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(100.0)); + Eigen::Isometry3d eigenTransform = tf2::transformToEigen(transform_stamped.transform); + lv_T_frame1_frame2(imuFrame_, baseLinkFrame_) = eigenTransform; + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_B: " << imuFrame_ << " " << baseLinkFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).translation() << std::endl; + lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse(); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR(rclcpp::get_logger("graph_msf"), "Transform lookup failed: %s", ex.what()); + REGULAR_COUT << RED_START << " Transform lookup failed: " << ex.what() << COLOR_END << std::endl; + return false; + } + // Wrap up + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms looked up successfully."); + return true; +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp new file mode 100644 index 00000000..80b7243a --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp @@ -0,0 +1,84 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf_ros2/extrinsics/StaticTransformsUrdf.h" +#include + +namespace graph_msf { + +StaticTransformsUrdf::StaticTransformsUrdf(const std::shared_ptr privateNodePtr) + : privateNode_(privateNodePtr) { + RCLCPP_INFO(privateNode_->get_logger(), "StaticTransformsUrdf Initializing..."); +} + +void StaticTransformsUrdf::setup() { + RCLCPP_INFO(privateNode_->get_logger(), "Setting up for description name '%s'", urdfDescriptionName_.c_str()); + + // Declare and retrieve the URDF parameter + privateNode_->declare_parameter(urdfDescriptionName_, "default_description"); + std::string urdfDescriptionContent; + privateNode_->get_parameter(urdfDescriptionName_, urdfDescriptionContent); + + if (urdfDescriptionContent.empty()) { + throw std::runtime_error("StaticTransformsUrdf - Could not load description, description empty."); + } + + // Initialize the urdf::Model from the URDF string content + if (!urdfModel_.initString(urdfDescriptionContent)) { + throw std::runtime_error("StaticTransformsUrdf - Failed to parse URDF description content."); + } + + // Initialize the KDL tree + if (!kdl_parser::treeFromUrdfModel(urdfModel_, tree_)) { + throw std::runtime_error("Failed to extract KDL tree from robot description"); + } + + segments_.clear(); + getRootTransformations(tree_.getRootSegment()); + + RCLCPP_INFO(privateNode_->get_logger(), "StaticTransformsUrdf Initialized."); +} + +void StaticTransformsUrdf::getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName) { + const std::string& elementName = GetTreeElementSegment(element->second).getName(); + if (rootName.empty()) { + rootName = elementName; + } + + auto children = GetTreeElementChildren(element->second); + for (const auto& child : children) { + KDL::Chain chain; + tree_.getChain(rootName, child->second.segment.getName(), chain); + + tf2::Transform T_root_element; + for (int i = 0; i < chain.getNrOfSegments(); ++i) { + KDL::Frame frameToRoot = chain.getSegment(i).getFrameToTip(); + double x, y, z, w; + frameToRoot.M.GetQuaternion(x, y, z, w); + tf2::Quaternion q(x, y, z, w); + + tf2::Vector3 t(frameToRoot.p[0], frameToRoot.p[1], frameToRoot.p[2]); + T_root_element = T_root_element * tf2::Transform(q, t); + } + + ElementToRoot elementToRoot(T_root_element, rootName, child->second.segment.getName()); + segments_.emplace(child->second.segment.getName(), elementToRoot); + getRootTransformations(child, rootName); + } +} + +tf2::Transform StaticTransformsUrdf::kdlToTransform(const KDL::Frame& k) { + tf2::Transform tf_T; + tf_T.setOrigin(tf2::Vector3(k.p.x(), k.p.y(), k.p.z())); + tf2::Quaternion q; + k.M.GetQuaternion(q[0], q[1], q[2], q[3]); + tf_T.setRotation(q); + + return tf_T; +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/conversions.cpp b/ros2/graph_msf_ros2/src/lib/conversions.cpp new file mode 100644 index 00000000..9ecc6139 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/conversions.cpp @@ -0,0 +1,98 @@ +// Implementation +#include "graph_msf_ros2/util/conversions.h" +#include + +namespace graph_msf { + +Eigen::Matrix convertCovarianceGtsamConventionToRosConvention( + const Eigen::Matrix& covGtsam) { + Eigen::Matrix covRos; + covRos.setZero(); + covRos.block<3, 3>(0, 0) = covGtsam.block<3, 3>(3, 3); + covRos.block<3, 3>(3, 3) = covGtsam.block<3, 3>(0, 0); + covRos.block<3, 3>(0, 3) = covGtsam.block<3, 3>(3, 0); + covRos.block<3, 3>(3, 0) = covGtsam.block<3, 3>(0, 3); + return covRos; +} + +void geometryPoseToEigen(const geometry_msgs::msg::Pose& pose, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + affine.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void geometryPoseToEigen(const geometry_msgs::msg::PoseStamped& pose, Eigen::Matrix4d& T) { + geometryPoseToEigen(pose.pose, T); +} + +void geometryPoseToEigen(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, Eigen::Matrix4d& T) { + geometryPoseToEigen(pose.pose.pose, T); +} + +void odomMsgToEigen(const nav_msgs::msg::Odometry& odomLidar, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + + affine.translation() = + Eigen::Vector3d(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z); + Eigen::Quaterniond q(odomLidar.pose.pose.orientation.w, odomLidar.pose.pose.orientation.x, + odomLidar.pose.pose.orientation.y, odomLidar.pose.pose.orientation.z); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void odomMsgToTf(const nav_msgs::msg::Odometry& odomLidar, tf2::Transform& T) { + T = tf2::Transform( + tf2::Quaternion(odomLidar.pose.pose.orientation.x, odomLidar.pose.pose.orientation.y, + odomLidar.pose.pose.orientation.z, odomLidar.pose.pose.orientation.w), + tf2::Vector3(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z)); +} + +tf2::Transform matrix3ToTf(const Eigen::Matrix3d& R) { + Eigen::Quaterniond q(R); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(0.0, 0.0, 0.0)); +} + +tf2::Transform matrix4ToTf(const Eigen::Matrix4d& T) { + Eigen::Quaterniond q(T.block<3, 3>(0, 0)); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(T(0, 3), T(1, 3), T(2, 3))); +} + +tf2::Transform isometry3ToTf(const Eigen::Isometry3d& T) { + Eigen::Quaterniond q(T.rotation()); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), + tf2::Vector3(T.translation().x(), T.translation().y(), T.translation().z())); +} + +#include +#include + +void tfToMatrix4(const tf2::Transform& tf_T, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + affine.translation() = Eigen::Vector3d(tf_T.getOrigin().x(), tf_T.getOrigin().y(), tf_T.getOrigin().z()); + Eigen::Quaterniond q(tf_T.getRotation().w(), tf_T.getRotation().x(), tf_T.getRotation().y(), tf_T.getRotation().z()); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void tfToIsometry3(const tf2::Transform& tf_T, Eigen::Isometry3d& T) { + tf2::Vector3 origin = tf_T.getOrigin(); + T.translation() << origin.x(), origin.y(), origin.z(); + tf2::Quaternion tf_q = tf_T.getRotation(); + Eigen::Quaterniond eigen_q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z()); + T.linear() = eigen_q.toRotationMatrix(); +} + +void transformStampedToIsometry3(const geometry_msgs::msg::Transform& transform, Eigen::Isometry3d& T) { + T.translation() = Eigen::Vector3d(transform.translation.x, transform.translation.y, transform.translation.z); + Eigen::Quaterniond q(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z); + T.linear() = q.toRotationMatrix(); +} + +tf2::Transform pose3ToTf(const Eigen::Matrix3d& T) { + Eigen::Quaterniond q(T); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(0.0, 0.0, 0.0)); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp new file mode 100644 index 00000000..fc6216f3 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -0,0 +1,176 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +node_ file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of node_ package. + */ + +// C++ +#include + +// Implementation +#include "graph_msf_ros2/GraphMsfRos2.h" + +// Workspace +#include "graph_msf_ros2/ros/read_ros_params.h" +// Constants +#include "graph_msf_ros2/constants.h" + +namespace graph_msf { + +void GraphMsfRos2::readParams() { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 - Reading parameters."); + + if (!graphConfigPtr_) { + throw std::runtime_error("GraphMsfRos2: graphConfigPtr must be initialized."); + } + + // Configuration + // Sensor Parameters + graphConfigPtr_->imuRate_ = tryGetParam(node_, "sensor_params.imuRate"); + graphConfigPtr_->createStateEveryNthImuMeasurement_ = + tryGetParam(node_, "sensor_params.createStateEveryNthImuMeasurement"); + graphConfigPtr_->useImuSignalLowPassFilter_ = tryGetParam(node_, "sensor_params.useImuSignalLowPassFilter"); + graphConfigPtr_->imuLowPassFilterCutoffFreqHz_ = + tryGetParam(node_, "sensor_params.imuLowPassFilterCutoffFreq"); + graphConfigPtr_->maxSearchDeviation_ = + 1.0 / (graphConfigPtr_->imuRate_ / graphConfigPtr_->createStateEveryNthImuMeasurement_); + graphConfigPtr_->imuBufferLength_ = tryGetParam(node_, "sensor_params.imuBufferLength"); + graphConfigPtr_->imuTimeOffset_ = tryGetParam(node_, "sensor_params.imuTimeOffset"); + + // Initialization Params + graphConfigPtr_->estimateGravityFromImuFlag_ = + tryGetParam(node_, "initialization_params.estimateGravityFromImu"); + graphConfigPtr_->gravityMagnitude_ = tryGetParam(node_, "initialization_params.gravityMagnitude"); + graphConfigPtr_->W_gravityVector_ = Eigen::Vector3d(0.0, 0.0, -1.0) * graphConfigPtr_->gravityMagnitude_; + + // Graph Params + graphConfigPtr_->realTimeSmootherUseIsamFlag_ = tryGetParam(node_, "graph_params.realTimeSmootherUseIsam"); + graphConfigPtr_->realTimeSmootherLag_ = tryGetParam(node_, "graph_params.realTimeSmootherLag"); + graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_ = + tryGetParam(node_, "graph_params.useAdditionalSlowBatchSmoother"); + graphConfigPtr_->slowBatchSmootherUseIsamFlag_ = tryGetParam(node_, "graph_params.slowBatchSmootherUseIsam"); + + // Optimizer Config + graphConfigPtr_->gaussNewtonWildfireThreshold_ = + tryGetParam(node_, "graph_params.gaussNewtonWildfireThreshold"); + graphConfigPtr_->minOptimizationFrequency_ = tryGetParam(node_, "graph_params.minOptimizationFrequency"); + graphConfigPtr_->maxOptimizationFrequency_ = tryGetParam(node_, "graph_params.maxOptimizationFrequency"); + graphConfigPtr_->additionalOptimizationIterations_ = + tryGetParam(node_, "graph_params.additionalOptimizationIterations"); + graphConfigPtr_->findUnusedFactorSlotsFlag_ = tryGetParam(node_, "graph_params.findUnusedFactorSlots"); + graphConfigPtr_->enableDetailedResultsFlag_ = tryGetParam(node_, "graph_params.enableDetailedResults"); + graphConfigPtr_->realTimeSmootherUseCholeskyFactorizationFlag_ = + tryGetParam(node_, "graph_params.realTimeSmootherUseCholeskyFactorization"); + graphConfigPtr_->slowBatchSmootherUseCholeskyFactorizationFlag_ = + tryGetParam(node_, "graph_params.slowBatchSmootherUseCholeskyFactorization"); + graphConfigPtr_->usingBiasForPreIntegrationFlag_ = + tryGetParam(node_, "graph_params.usingBiasForPreIntegration"); + graphConfigPtr_->useWindowForMarginalsComputationFlag_ = + tryGetParam(node_, "graph_params.useWindowForMarginalsComputation"); + graphConfigPtr_->windowSizeSecondsForMarginalsComputation_ = + tryGetParam(node_, "graph_params.windowSizeSecondsForMarginalsComputation"); + graphConfigPtr_->optimizeReferenceFramePosesWrtWorldFlag_ = + tryGetParam(node_, "graph_params.optimizeReferenceFramePosesWrtWorld"); + graphConfigPtr_->optimizeExtrinsicSensorToSensorCorrectedOffsetFlag_ = + tryGetParam(node_, "graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset"); + + // Alignment Parameters + graphConfigPtr_->referenceFramePosesResetThreshold_ = + tryGetParam(node_, "graph_params.referenceFramePosesResetThreshold"); + graphConfigPtr_->centerMeasurementsAtKeyframePositionBeforeAlignmentFlag_ = + tryGetParam(node_, "graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment"); + graphConfigPtr_->createReferenceAlignmentKeyframeEveryNSeconds_ = + tryGetParam(node_, "graph_params.createReferenceAlignmentKeyframeEveryNSeconds"); + + // Noise Parameters + graphConfigPtr_->accNoiseDensity_ = tryGetParam(node_, "noise_params.accNoiseDensity"); + graphConfigPtr_->integrationNoiseDensity_ = tryGetParam(node_, "noise_params.integrationNoiseDensity"); + graphConfigPtr_->use2ndOrderCoriolisFlag_ = tryGetParam(node_, "noise_params.use2ndOrderCoriolis"); + graphConfigPtr_->gyroNoiseDensity_ = tryGetParam(node_, "noise_params.gyrNoiseDensity"); + graphConfigPtr_->omegaCoriolis_ = tryGetParam(node_, "noise_params.omegaCoriolis"); + graphConfigPtr_->accBiasRandomWalkNoiseDensity_ = + tryGetParam(node_, "noise_params.accBiasRandomWalkNoiseDensity"); + graphConfigPtr_->gyroBiasRandomWalkNoiseDensity_ = + tryGetParam(node_, "noise_params.gyrBiasRandomWalkNoiseDensity"); + graphConfigPtr_->biasAccOmegaInit_ = tryGetParam(node_, "noise_params.biasAccOmegaInit"); + + const double accBiasPrior = tryGetParam(node_, "noise_params.accBiasPrior"); + graphConfigPtr_->accBiasPrior_ = Eigen::Vector3d(accBiasPrior, accBiasPrior, accBiasPrior); + + const double gyroBiasPrior = tryGetParam(node_, "noise_params.gyrBiasPrior"); + graphConfigPtr_->gyroBiasPrior_ = Eigen::Vector3d(gyroBiasPrior, gyroBiasPrior, gyroBiasPrior); + + // Initial State + graphConfigPtr_->initialPositionNoiseDensity_ = + tryGetParam(node_, "noise_params.initialPositionNoiseDensity"); + graphConfigPtr_->initialOrientationNoiseDensity_ = + tryGetParam(node_, "noise_params.initialOrientationNoiseDensity"); + graphConfigPtr_->initialVelocityNoiseDensity_ = + tryGetParam(node_, "noise_params.initialVelocityNoiseDensity"); + graphConfigPtr_->initialAccBiasNoiseDensity_ = tryGetParam(node_, "noise_params.initialAccBiasNoiseDensity"); + graphConfigPtr_->initialGyroBiasNoiseDensity_ = + tryGetParam(node_, "noise_params.initialGyroBiasNoiseDensity"); + + // Re-linearization + graphConfigPtr_->positionReLinTh_ = tryGetParam(node_, "relinearization_params.positionReLinTh"); + graphConfigPtr_->rotationReLinTh_ = tryGetParam(node_, "relinearization_params.rotationReLinTh"); + graphConfigPtr_->velocityReLinTh_ = tryGetParam(node_, "relinearization_params.velocityReLinTh"); + graphConfigPtr_->accBiasReLinTh_ = tryGetParam(node_, "relinearization_params.accBiasReLinTh"); + graphConfigPtr_->gyroBiasReLinTh_ = tryGetParam(node_, "relinearization_params.gyrBiasReLinTh"); + graphConfigPtr_->referenceFrameReLinTh_ = tryGetParam(node_, "relinearization_params.referenceFrameReLinTh"); + graphConfigPtr_->calibrationReLinTh_ = tryGetParam(node_, "relinearization_params.calibrationReLinTh"); + graphConfigPtr_->displacementReLinTh_ = tryGetParam(node_, "relinearization_params.displacementReLinTh"); + graphConfigPtr_->landmarkReLinTh_ = tryGetParam(node_, "relinearization_params.landmarkReLinTh"); + + graphConfigPtr_->relinearizeSkip_ = tryGetParam(node_, "relinearization_params.relinearizeSkip"); + graphConfigPtr_->enableRelinearizationFlag_ = + tryGetParam(node_, "relinearization_params.enableRelinearization"); + graphConfigPtr_->evaluateNonlinearErrorFlag_ = + tryGetParam(node_, "relinearization_params.evaluateNonlinearError"); + graphConfigPtr_->cacheLinearizedFactorsFlag_ = + tryGetParam(node_, "relinearization_params.cacheLinearizedFactors"); + graphConfigPtr_->enablePartialRelinearizationCheckFlag_ = + tryGetParam(node_, "relinearization_params.enablePartialRelinearizationCheck"); + + // Common Parameters + graphConfigPtr_->verboseLevel_ = tryGetParam(node_, "common_params.verbosity"); + graphConfigPtr_->odomNotJumpAtStartFlag_ = tryGetParam(node_, "common_params.odomNotJumpAtStart"); + graphConfigPtr_->logRealTimeStateToMemoryFlag_ = tryGetParam(node_, "common_params.logRealTimeStateToMemory"); + graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_ = + tryGetParam(node_, "common_params.logLatencyAndUpdateDurationToMemory"); + + // Set frames + staticTransformsPtr_->setWorldFrame(tryGetParam(node_, "extrinsics.worldFrame")); + staticTransformsPtr_->setOdomFrame(tryGetParam(node_, "extrinsics.odomFrame")); + staticTransformsPtr_->setImuFrame(tryGetParam(node_, "extrinsics.imuFrame")); + staticTransformsPtr_->setInitializationFrame( + tryGetParam(node_, "extrinsics.initializeZeroYawAndPositionOfFrame")); + staticTransformsPtr_->setBaseLinkFrame(tryGetParam(node_, "extrinsics.baseLinkFrame")); + + referenceFrameAlignedNameId = tryGetParam(node_, "name_ids.referenceFrameAligned"); + sensorFrameCorrectedNameId = tryGetParam(node_, "name_ids.sensorFrameCorrected"); + + // Logging path + if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) { + optimizationResultLoggingPath = tryGetParam(node_, "launch.optimizationResultLoggingPath"); + + if (optimizationResultLoggingPath.back() != '/') { + optimizationResultLoggingPath += "/"; + } + + if (!std::filesystem::exists(optimizationResultLoggingPath)) { + std::filesystem::create_directories(optimizationResultLoggingPath); + } + + // Print the logging path + std::cout << "-------------------------------------------------" << std::endl; + RCLCPP_INFO(node_->get_logger(), "Optimization results will be logged to: %s", + optimizationResultLoggingPath.c_str()); + REGULAR_COUT << GREEN_START << " Optimization results will be logged to: " + << optimizationResultLoggingPath << COLOR_END << std::endl; + std::cout << "-------------------------------------------------" << std::endl; + } +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2_msgs/CMakeLists.txt b/ros2/graph_msf_ros2_msgs/CMakeLists.txt new file mode 100644 index 00000000..d7645a4c --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.16) +project(graph_msf_ros2_msgs) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +# Message and Service Files +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Wgs84Coordinate.msg" + "srv/GetPathInEnu.srv" + "srv/OfflineOptimizationTrigger.srv" + DEPENDENCIES nav_msgs std_msgs +) + +########### +## Build ## +########### + +# # Create an INTERFACE library for the headers +# add_library(${PROJECT_NAME}_INTERFACE INTERFACE) +# # Exported alias target +# add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}_INTERFACE) + +# # Include directories for the library +# target_include_directories(${PROJECT_NAME}_INTERFACE +# INTERFACE +# $ +# $ +# ) + +# # Link dependencies (for plain CMake usage) +# target_link_libraries(${PROJECT_NAME}_INTERFACE +# INTERFACE +# nav_msgs::nav_msgs__rosidl_generator_cpp +# std_msgs::std_msgs__rosidl_generator_cpp +# rosidl_typesupport_cpp +# ) + +############# +## Install ## +############# + +# # Export the include directories and linked libraries +# install( +# TARGETS ${PROJECT_NAME}_INTERFACE +# EXPORT ${PROJECT_NAME}Targets # Export the target for downstream usage +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +# ) + +# Install the include directory +install(DIRECTORY include/ + DESTINATION include +) + +# # Install the export file for downstream projects +# install(EXPORT ${PROJECT_NAME}Targets +# FILE ${PROJECT_NAME}Targets.cmake +# NAMESPACE ${PROJECT_NAME}:: +# DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Version +# include(CMakePackageConfigHelpers) +# write_basic_package_version_file( +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" +# VERSION 1.0.0 +# COMPATIBILITY SameMajorVersion +# ) + +# # Configure +# configure_package_config_file( +# "${PROJECT_NAME}Config.cmake.in" +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" +# INSTALL_DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Files +# install(FILES +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" +# DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Package export and installation +# ament_export_include_directories(include) +# ament_export_targets(${PROJECT_NAME}Targets) +ament_export_dependencies(rosidl_default_runtime nav_msgs std_msgs) +ament_package() diff --git a/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in b/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in new file mode 100644 index 00000000..1902a6ed --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in @@ -0,0 +1,12 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# Find dependencies +find_dependency(nav_msgs) +find_dependency(std_msgs) + +# Include the targets file +include("${CMAKE_CURRENT_LIST_DIR}/graph_msf_ros2_msgsTargets.cmake") + +check_required_components(graph_msf_ros2_msgs) diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp new file mode 100644 index 00000000..60d1ba86 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp @@ -0,0 +1,42 @@ +// generated from rosidl_generator_cpp/resource/rosidl_generator_cpp__visibility_control.hpp.in +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ +#define GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __attribute__ ((dllexport)) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs __attribute__ ((dllimport)) + #else + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __declspec(dllexport) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs __declspec(dllimport) + #endif + #ifdef ROSIDL_GENERATOR_CPP_BUILDING_DLL_graph_msf_ros2_msgs + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs + #else + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs + #endif +#else + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __attribute__ ((visibility("default"))) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs + #if __GNUC__ >= 4 + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs __attribute__ ((visibility("default"))) + #else + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs + #endif +#endif + +#ifdef __cplusplus +} +#endif + +#endif // GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp new file mode 100644 index 00000000..1ff8aafb --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp @@ -0,0 +1,114 @@ +// generated from rosidl_generator_cpp/resource/idl__builder.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ + +#include +#include + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "rosidl_runtime_cpp/message_initialization.hpp" + + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +namespace builder +{ + +class Init_OfflineOptimizationTrigger_Request_max_optimization_iterations +{ +public: + Init_OfflineOptimizationTrigger_Request_max_optimization_iterations() + : msg_(::rosidl_runtime_cpp::MessageInitialization::SKIP) + {} + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request max_optimization_iterations(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request::_max_optimization_iterations_type arg) + { + msg_.max_optimization_iterations = std::move(arg); + return std::move(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request msg_; +}; + +} // namespace builder + +} // namespace srv + +template +auto build(); + +template<> +inline +auto build<::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request>() +{ + return graph_msf_ros2_msgs::srv::builder::Init_OfflineOptimizationTrigger_Request_max_optimization_iterations(); +} + +} // namespace graph_msf_ros2_msgs + + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +namespace builder +{ + +class Init_OfflineOptimizationTrigger_Response_message +{ +public: + explicit Init_OfflineOptimizationTrigger_Response_message(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg) + : msg_(msg) + {} + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response message(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response::_message_type arg) + { + msg_.message = std::move(arg); + return std::move(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response msg_; +}; + +class Init_OfflineOptimizationTrigger_Response_success +{ +public: + Init_OfflineOptimizationTrigger_Response_success() + : msg_(::rosidl_runtime_cpp::MessageInitialization::SKIP) + {} + Init_OfflineOptimizationTrigger_Response_message success(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response::_success_type arg) + { + msg_.success = std::move(arg); + return Init_OfflineOptimizationTrigger_Response_message(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response msg_; +}; + +} // namespace builder + +} // namespace srv + +template +auto build(); + +template<> +inline +auto build<::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response>() +{ + return graph_msf_ros2_msgs::srv::builder::Init_OfflineOptimizationTrigger_Response_success(); +} + +} // namespace graph_msf_ros2_msgs + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp new file mode 100644 index 00000000..88fae57e --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp @@ -0,0 +1,275 @@ +// generated from rosidl_generator_cpp/resource/idl__struct.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ + +#include +#include +#include +#include +#include + +#include "rosidl_runtime_cpp/bounded_vector.hpp" +#include "rosidl_runtime_cpp/message_initialization.hpp" + + +#ifndef _WIN32 +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request __attribute__((deprecated)) +#else +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request __declspec(deprecated) +#endif + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +// message struct +template +struct OfflineOptimizationTrigger_Request_ +{ + using Type = OfflineOptimizationTrigger_Request_; + + explicit OfflineOptimizationTrigger_Request_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->max_optimization_iterations = 0ll; + } + } + + explicit OfflineOptimizationTrigger_Request_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + (void)_alloc; + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->max_optimization_iterations = 0ll; + } + } + + // field types and members + using _max_optimization_iterations_type = + int64_t; + _max_optimization_iterations_type max_optimization_iterations; + + // setters for named parameter idiom + Type & set__max_optimization_iterations( + const int64_t & _arg) + { + this->max_optimization_iterations = _arg; + return *this; + } + + // constant declarations + + // pointer types + using RawPtr = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_ *; + using ConstRawPtr = + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_ *; + using SharedPtr = + std::shared_ptr>; + using ConstSharedPtr = + std::shared_ptr const>; + + template>> + using UniquePtrWithDeleter = + std::unique_ptr, Deleter>; + + using UniquePtr = UniquePtrWithDeleter<>; + + template>> + using ConstUniquePtrWithDeleter = + std::unique_ptr const, Deleter>; + using ConstUniquePtr = ConstUniquePtrWithDeleter<>; + + using WeakPtr = + std::weak_ptr>; + using ConstWeakPtr = + std::weak_ptr const>; + + // pointer types similar to ROS 1, use SharedPtr / ConstSharedPtr instead + // NOTE: Can't use 'using' here because GNU C++ can't parse attributes properly + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request + std::shared_ptr> + Ptr; + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request + std::shared_ptr const> + ConstPtr; + + // comparison operators + bool operator==(const OfflineOptimizationTrigger_Request_ & other) const + { + if (this->max_optimization_iterations != other.max_optimization_iterations) { + return false; + } + return true; + } + bool operator!=(const OfflineOptimizationTrigger_Request_ & other) const + { + return !this->operator==(other); + } +}; // struct OfflineOptimizationTrigger_Request_ + +// alias to use template instance with default allocator +using OfflineOptimizationTrigger_Request = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_>; + +// constant definitions + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + + +#ifndef _WIN32 +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response __attribute__((deprecated)) +#else +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response __declspec(deprecated) +#endif + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +// message struct +template +struct OfflineOptimizationTrigger_Response_ +{ + using Type = OfflineOptimizationTrigger_Response_; + + explicit OfflineOptimizationTrigger_Response_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->success = false; + this->message = ""; + } + } + + explicit OfflineOptimizationTrigger_Response_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + : message(_alloc) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->success = false; + this->message = ""; + } + } + + // field types and members + using _success_type = + bool; + _success_type success; + using _message_type = + std::basic_string, typename std::allocator_traits::template rebind_alloc>; + _message_type message; + + // setters for named parameter idiom + Type & set__success( + const bool & _arg) + { + this->success = _arg; + return *this; + } + Type & set__message( + const std::basic_string, typename std::allocator_traits::template rebind_alloc> & _arg) + { + this->message = _arg; + return *this; + } + + // constant declarations + + // pointer types + using RawPtr = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_ *; + using ConstRawPtr = + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_ *; + using SharedPtr = + std::shared_ptr>; + using ConstSharedPtr = + std::shared_ptr const>; + + template>> + using UniquePtrWithDeleter = + std::unique_ptr, Deleter>; + + using UniquePtr = UniquePtrWithDeleter<>; + + template>> + using ConstUniquePtrWithDeleter = + std::unique_ptr const, Deleter>; + using ConstUniquePtr = ConstUniquePtrWithDeleter<>; + + using WeakPtr = + std::weak_ptr>; + using ConstWeakPtr = + std::weak_ptr const>; + + // pointer types similar to ROS 1, use SharedPtr / ConstSharedPtr instead + // NOTE: Can't use 'using' here because GNU C++ can't parse attributes properly + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response + std::shared_ptr> + Ptr; + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response + std::shared_ptr const> + ConstPtr; + + // comparison operators + bool operator==(const OfflineOptimizationTrigger_Response_ & other) const + { + if (this->success != other.success) { + return false; + } + if (this->message != other.message) { + return false; + } + return true; + } + bool operator!=(const OfflineOptimizationTrigger_Response_ & other) const + { + return !this->operator==(other); + } +}; // struct OfflineOptimizationTrigger_Response_ + +// alias to use template instance with default allocator +using OfflineOptimizationTrigger_Response = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_>; + +// constant definitions + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +struct OfflineOptimizationTrigger +{ + using Request = graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request; + using Response = graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response; +}; + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp new file mode 100644 index 00000000..74d9c1f8 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp @@ -0,0 +1,273 @@ +// generated from rosidl_generator_cpp/resource/idl__traits.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ + +#include + +#include +#include +#include + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "rosidl_runtime_cpp/traits.hpp" + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +inline void to_flow_style_yaml( + const OfflineOptimizationTrigger_Request & msg, + std::ostream & out) +{ + out << "{"; + // member: max_optimization_iterations + { + out << "max_optimization_iterations: "; + rosidl_generator_traits::value_to_yaml(msg.max_optimization_iterations, out); + } + out << "}"; +} // NOLINT(readability/fn_size) + +inline void to_block_style_yaml( + const OfflineOptimizationTrigger_Request & msg, + std::ostream & out, size_t indentation = 0) +{ + // member: max_optimization_iterations + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "max_optimization_iterations: "; + rosidl_generator_traits::value_to_yaml(msg.max_optimization_iterations, out); + out << "\n"; + } +} // NOLINT(readability/fn_size) + +inline std::string to_yaml(const OfflineOptimizationTrigger_Request & msg, bool use_flow_style = false) +{ + std::ostringstream out; + if (use_flow_style) { + to_flow_style_yaml(msg, out); + } else { + to_block_style_yaml(msg, out); + } + return out.str(); +} + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace rosidl_generator_traits +{ + +[[deprecated("use graph_msf_ros2_msgs::srv::to_block_style_yaml() instead")]] +inline void to_yaml( + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request & msg, + std::ostream & out, size_t indentation = 0) +{ + graph_msf_ros2_msgs::srv::to_block_style_yaml(msg, out, indentation); +} + +[[deprecated("use graph_msf_ros2_msgs::srv::to_yaml() instead")]] +inline std::string to_yaml(const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request & msg) +{ + return graph_msf_ros2_msgs::srv::to_yaml(msg); +} + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger_Request"; +} + +template<> +struct has_fixed_size + : std::integral_constant {}; + +template<> +struct has_bounded_size + : std::integral_constant {}; + +template<> +struct is_message + : std::true_type {}; + +} // namespace rosidl_generator_traits + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +inline void to_flow_style_yaml( + const OfflineOptimizationTrigger_Response & msg, + std::ostream & out) +{ + out << "{"; + // member: success + { + out << "success: "; + rosidl_generator_traits::value_to_yaml(msg.success, out); + out << ", "; + } + + // member: message + { + out << "message: "; + rosidl_generator_traits::value_to_yaml(msg.message, out); + } + out << "}"; +} // NOLINT(readability/fn_size) + +inline void to_block_style_yaml( + const OfflineOptimizationTrigger_Response & msg, + std::ostream & out, size_t indentation = 0) +{ + // member: success + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "success: "; + rosidl_generator_traits::value_to_yaml(msg.success, out); + out << "\n"; + } + + // member: message + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "message: "; + rosidl_generator_traits::value_to_yaml(msg.message, out); + out << "\n"; + } +} // NOLINT(readability/fn_size) + +inline std::string to_yaml(const OfflineOptimizationTrigger_Response & msg, bool use_flow_style = false) +{ + std::ostringstream out; + if (use_flow_style) { + to_flow_style_yaml(msg, out); + } else { + to_block_style_yaml(msg, out); + } + return out.str(); +} + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace rosidl_generator_traits +{ + +[[deprecated("use graph_msf_ros2_msgs::srv::to_block_style_yaml() instead")]] +inline void to_yaml( + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg, + std::ostream & out, size_t indentation = 0) +{ + graph_msf_ros2_msgs::srv::to_block_style_yaml(msg, out, indentation); +} + +[[deprecated("use graph_msf_ros2_msgs::srv::to_yaml() instead")]] +inline std::string to_yaml(const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg) +{ + return graph_msf_ros2_msgs::srv::to_yaml(msg); +} + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger_Response"; +} + +template<> +struct has_fixed_size + : std::integral_constant {}; + +template<> +struct has_bounded_size + : std::integral_constant {}; + +template<> +struct is_message + : std::true_type {}; + +} // namespace rosidl_generator_traits + +namespace rosidl_generator_traits +{ + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger"; +} + +template<> +struct has_fixed_size + : std::integral_constant< + bool, + has_fixed_size::value && + has_fixed_size::value + > +{ +}; + +template<> +struct has_bounded_size + : std::integral_constant< + bool, + has_bounded_size::value && + has_bounded_size::value + > +{ +}; + +template<> +struct is_service + : std::true_type +{ +}; + +template<> +struct is_service_request + : std::true_type +{ +}; + +template<> +struct is_service_response + : std::true_type +{ +}; + +} // namespace rosidl_generator_traits + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp new file mode 100644 index 00000000..40008f34 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp @@ -0,0 +1,71 @@ +// generated from rosidl_generator_cpp/resource/idl__type_support.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ + +#include "rosidl_typesupport_interface/macros.h" + +#include "graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp" + +#include "rosidl_typesupport_cpp/service_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_service_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger +)(); +#ifdef __cplusplus +} +#endif + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger_Request +)(); +#ifdef __cplusplus +} +#endif + +// already included above +// #include "rosidl_typesupport_cpp/message_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger_Response +)(); +#ifdef __cplusplus +} +#endif + + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp new file mode 100644 index 00000000..5d71d40a --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp @@ -0,0 +1,12 @@ +// generated from rosidl_generator_cpp/resource/idl.hpp.em +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp" + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg b/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg new file mode 100644 index 00000000..20c74042 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg @@ -0,0 +1,3 @@ +float64 longitude +float64 latitude +float64 altitude \ No newline at end of file diff --git a/ros2/graph_msf_ros2_msgs/package.xml b/ros2/graph_msf_ros2_msgs/package.xml new file mode 100644 index 00000000..303a3450 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/package.xml @@ -0,0 +1,27 @@ + + + graph_msf_ros2_msgs + 1.0.0 + Messages used by graph_msf + + Proprietary + https://bitbucket.org/leggedrobotics/compslam_se + Gabriel Waibel + Gabriel Waibel + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + rosidl_default_runtime + nav_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv b/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv new file mode 100644 index 00000000..93177abf --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv @@ -0,0 +1,3 @@ +Wgs84Coordinate[] wgs84_coordinates +--- +nav_msgs/Path gnss_enu_path \ No newline at end of file diff --git a/ros2/graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger.srv b/ros2/graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger.srv new file mode 100644 index 00000000..34507c11 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger.srv @@ -0,0 +1,4 @@ +int64 max_optimization_iterations +--- +bool success +string message \ No newline at end of file