From 512004cc51653db1999f8ac460d3d53d6e422016 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 8 Feb 2025 14:12:04 +0000 Subject: [PATCH 01/74] Slight tweaks for launch files --- .../script/tmux_multi_robot_with_bags_forest.sh | 5 ++--- .../script/tmux_multi_robot_with_bags_parking_lot.sh | 2 +- .../script/tmux_single_indoor_robot.sh | 4 ++-- .../script/tmux_single_outdoor_robot.sh | 2 +- .../create_slide_slam_docker.sh | 6 +++--- docker/open_slide_slam_docker.sh | 1 + .../scan2shape_launch/config/infer_node_params.yaml | 2 +- 7 files changed, 11 insertions(+), 11 deletions(-) rename run_slide_slam_docker.sh => docker/create_slide_slam_docker.sh (66%) create mode 100755 docker/open_slide_slam_docker.sh diff --git a/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_forest.sh b/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_forest.sh index 3ee1e14..f634cac 100755 --- a/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_forest.sh +++ b/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_forest.sh @@ -2,9 +2,8 @@ SESSION_NAME=multi_robot_nodes -BAG_PLAY_RATE=2.0 -#BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests' -BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests' +BAG_PLAY_RATE=1.0 +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/processed_data/forests' CURRENT_DISPLAY=${DISPLAY} if [ -z ${DISPLAY} ]; diff --git a/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_parking_lot.sh b/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_parking_lot.sh index 431ea22..540ace4 100755 --- a/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_parking_lot.sh +++ b/backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_parking_lot.sh @@ -4,7 +4,7 @@ SESSION_NAME=multi_robot_nodes BAG_PLAY_RATE=2.0 # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/parking-lot' -BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/parking-lot' +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/processed_data/parking-lot' CURRENT_DISPLAY=${DISPLAY} if [ -z ${DISPLAY} ]; diff --git a/backend/multi_robot_utils_launch/script/tmux_single_indoor_robot.sh b/backend/multi_robot_utils_launch/script/tmux_single_indoor_robot.sh index 64b81c6..8715cbd 100755 --- a/backend/multi_robot_utils_launch/script/tmux_single_indoor_robot.sh +++ b/backend/multi_robot_utils_launch/script/tmux_single_indoor_robot.sh @@ -6,7 +6,7 @@ BAG_PLAY_RATE=0.5 #BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests' # BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests' # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/indoor' -BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/indoor' +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/raw_data/indoor/' CURRENT_DISPLAY=${DISPLAY} if [ -z ${DISPLAY} ]; @@ -51,7 +51,7 @@ tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_mo tmux select-pane -t $SESSION_NAME:1.1 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_modeller sync_semantic_measurements.launch robot_name:=robot0 odom_topic:=/dragonfly67/quadrotor_ukf/control_odom" Enter tmux select-pane -t $SESSION_NAME:1.2 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test_f250.launch enable_rviz:=true" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test.launch enable_rviz:=true" Enter tmux select-pane -t $SESSION_NAME:1.3 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch process_cloud_node_rgbd_indoor_with_ns.launch odom_topic:=/dragonfly67/quadrotor_ukf/control_odom robot_name:=robot0" Enter tmux select-pane -t $SESSION_NAME:1.4 diff --git a/backend/multi_robot_utils_launch/script/tmux_single_outdoor_robot.sh b/backend/multi_robot_utils_launch/script/tmux_single_outdoor_robot.sh index 1778092..f68e5c8 100755 --- a/backend/multi_robot_utils_launch/script/tmux_single_outdoor_robot.sh +++ b/backend/multi_robot_utils_launch/script/tmux_single_outdoor_robot.sh @@ -6,7 +6,7 @@ BAG_PLAY_RATE=0.5 #BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests' # BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests' # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/indoor' -BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/outdoor' +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/raw_data/outdoor/' CURRENT_DISPLAY=${DISPLAY} if [ -z ${DISPLAY} ]; diff --git a/run_slide_slam_docker.sh b/docker/create_slide_slam_docker.sh similarity index 66% rename from run_slide_slam_docker.sh rename to docker/create_slide_slam_docker.sh index 6945d90..e9b773f 100755 --- a/run_slide_slam_docker.sh +++ b/docker/create_slide_slam_docker.sh @@ -1,6 +1,6 @@ -SlideSlamWs="/home/sam/slideslam_docker_ws" # point to your workspace directory -SlideSlamCodeDir="/home/sam/slideslam_docker_ws/src/SLIDE_SLAM" # point to your code directory where you cloned the repository -BAGS_DIR='/home/sam/bags' # point to your bags / data directory +SlideSlamWs="/home/dbutterfield3/slideslam_docker_ws" # point to your workspace directory +SlideSlamCodeDir="/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM" # point to your code directory where you cloned the repository +BAGS_DIR='/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM' # point to your bags / data directory xhost +local:root # for the lazy and reckless docker run -it \ diff --git a/docker/open_slide_slam_docker.sh b/docker/open_slide_slam_docker.sh new file mode 100755 index 0000000..4a81909 --- /dev/null +++ b/docker/open_slide_slam_docker.sh @@ -0,0 +1 @@ +docker start slideslam_ros && docker exec -it slideslam_ros /bin/bash \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/config/infer_node_params.yaml b/frontend/scan2shape/scan2shape_launch/config/infer_node_params.yaml index 7fadec8..f752c5a 100755 --- a/frontend/scan2shape/scan2shape_launch/config/infer_node_params.yaml +++ b/frontend/scan2shape/scan2shape_launch/config/infer_node_params.yaml @@ -16,7 +16,7 @@ # pc_point_step: length of a point in bytes in the point cloud. #------------------------------------------------- namespace: /os_node -model_dir: /opt/bags/vems-slam-bags/penn_smallest/ +model_dir: /opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/rangenet++/penn_smallest/ desired_frequency: 2 num_cpu_threads: 10 pc_range_threshold: 40 From 9c4394bd9840fdd1796deb66a8e7134eb8c4d10e Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Fri, 21 Feb 2025 03:23:00 +0000 Subject: [PATCH 02/74] Untested first attempt at Relative Inter-Robot Measurements --- .gitignore | 1 + .../script/tmux_CoPeD.sh | 71 +++++++ backend/sloam/CMakeLists.txt | 3 + backend/sloam/include/core/databaseManager.h | 20 +- backend/sloam/include/core/robot.h | 63 +++--- backend/sloam/include/core/sloamNode.h | 12 ++ backend/sloam/include/factorgraph/graph.h | 4 + backend/sloam/include/helpers/definitions.h | 23 ++ backend/sloam/include/helpers/utils.h | 21 +- .../sloam/launch/decentralized_sloam.launch | 3 + backend/sloam/src/core/databaseManager.cpp | 162 +++++++------- backend/sloam/src/core/inputNode.cpp | 74 ++++++- backend/sloam/src/core/robot.cpp | 83 ++++---- backend/sloam/src/core/sloam.cpp | 2 +- backend/sloam/src/core/sloamNode.cpp | 199 ++++++++++++++++-- backend/sloam/src/factorgraph/graph.cpp | 60 ++---- .../sloam/src/factorgraph/graphWrapper.cpp | 2 +- backend/sloam_msgs/CMakeLists.txt | 1 + backend/sloam_msgs/msg/PoseMst.msg | 1 + .../msg/RelativeInterRobotMeasurement.msg | 11 + .../CoPeD/infer_node_params_wilbur.yaml | 24 +++ .../process_cloud_node_params_wilbur.yaml | 50 +++++ .../launch/infer_node.launch | 4 +- .../process_cloud_node_outdoor_with_ns.launch | 3 +- frontend/scan2shape/script/infer_node.py | 11 + 25 files changed, 702 insertions(+), 206 deletions(-) create mode 100755 backend/multi_robot_utils_launch/script/tmux_CoPeD.sh create mode 100755 backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg create mode 100755 frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml create mode 100755 frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml diff --git a/.gitignore b/.gitignore index af8d641..fe63462 100755 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,4 @@ scan2shape/script/max_scale.pkl *.npy .vscode *.png +bags/ diff --git a/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh b/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh new file mode 100755 index 0000000..42dd547 --- /dev/null +++ b/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh @@ -0,0 +1,71 @@ +#!/bin/bash + +# Set parameters +SESSION_NAME=slide_slam_nodes +BAG_PLAY_RATE=0.5 +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/' +SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" + +# Set Display Settings +CURRENT_DISPLAY=${DISPLAY} +if [ -z ${DISPLAY} ]; +then + echo "DISPLAY is not set" + CURRENT_DISPLAY=:0 +fi + +# Start Tmux Session if not already in one +if [ -z ${TMUX} ]; +then + TMUX= tmux new-session -s $SESSION_NAME -d + echo "Starting new session." +else + echo "Already in tmux, leave it first." + exit +fi + +# Make mouse useful in copy mode +tmux setw -g mouse on + +# Setup the panes for the main window +tmux new-window -t $SESSION_NAME -n "Main" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -h -t $SESSION_NAME + +# Setup commands for main window +tmux select-pane -t $SESSION_NAME:1.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch infer_node.launch param_file:=config/CoPeD/infer_node_params_wilbur.yaml" Enter +tmux select-pane -t $SESSION_NAME:1.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_modeller sync_semantic_measurements.launch" Enter +tmux select-pane -t $SESSION_NAME:1.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test_LiDAR.launch enable_rviz:=true" Enter +tmux select-pane -t $SESSION_NAME:1.3 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch process_cloud_node_outdoor_with_ns.launch param_file:=config/CoPeD/process_cloud_node_params_wilbur.yaml" Enter +tmux select-pane -t $SESSION_NAME:1.4 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wilburOusterOS1-64.yaml" Enter +tmux select-pane -t $SESSION_NAME:1.5 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux select-layout -t $SESSION_NAME tiled + +# Add window for roscore +tmux new-window -t $SESSION_NAME -n "roscore" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:2.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter +tmux select-pane -t $SESSION_NAME:2.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter + +# Add window to easily kill all processes +tmux new-window -t $SESSION_NAME -n "Kill" +tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}" + +# Go back to the first window +tmux select-window -t $SESSION_NAME:1 +tmux -2 attach-session -t $SESSION_NAME diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 1f6ee02..34a706b 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -121,6 +121,8 @@ cc_binary( DEPS sloam_base sloam_core) # TESTS +# It appears that these test cases are not up to date, and thus won't work on current code. +# # if(BUILD_TESTING) # enable_testing() # Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter) @@ -136,3 +138,4 @@ cc_binary( # NAME cube_fg_test # SRCS "src/tests/cube_factor_test.cpp" # DEPS sloam_base sloam_factorgraph GTest::GTest) +# endif() \ No newline at end of file diff --git a/backend/sloam/include/core/databaseManager.h b/backend/sloam/include/core/databaseManager.h index ece3c3b..aaa1a70 100755 --- a/backend/sloam/include/core/databaseManager.h +++ b/backend/sloam/include/core/databaseManager.h @@ -46,6 +46,8 @@ struct PoseMstPair { SE3 keyPose; // the current pose of the robot SE3 relativeRawOdomMotion; // relative motion from last keyPose to current as // measured using odometry + ros::Time stamp; // the timestamp associated with the keyPose + // measurement in robot frame std::vector cylinderMsts; std::vector cubeMsts; @@ -60,6 +62,8 @@ class robotData { // find a loop closure std::deque poseMstPacket; // all received data about robot i so far + std::deque + relativeMeasPacket; // all received relative inter-robot measurements robotData() { bookmarkFG = 0; @@ -77,7 +81,7 @@ class databaseManager { */ explicit databaseManager(const ros::NodeHandle &nh); - void publishPoseMsts(int robotID); + // void publishPoseMsts(int robotID); std::unordered_map loopClosureTf; // the transformation from other robots to the host robot std::unordered_map cylinderMap; @@ -101,6 +105,8 @@ class databaseManager { robotData &getHostRobotData() { return robotDataDict_[hostRobotID_]; } + robotData &getRobotDataByID(const int robotID) { return robotDataDict_[robotID]; } + void updateFGBookmark(int newBookmark, int robotID) { robotDataDict_[robotID].bookmarkFG = newBookmark; } @@ -176,24 +182,21 @@ class databaseManager { } } - static void - Measurement2Cube(const std::vector &cms, + static void Measurement2Cube(const std::vector &cms, std::vector &objs) { for (const auto &cm : cms) { objs.emplace_back(cm.pose, cm.scale); } } - static void - object2Measurement(const std::vector &objs, + static void object2Measurement(const std::vector &objs, std::vector &msts) { for (const auto &obj : objs) { msts.emplace_back(obj); } } - static void - object2Measurement(const std::vector &objs, + static void object2Measurement(const std::vector &objs, std::vector &msts) { for (const auto &obj : objs) { msts.emplace_back(obj); @@ -252,9 +255,10 @@ class databaseManager { SE3 priorTF2World_; bool priorTFKnown_; ros::Timer timer_; + // communication must happen at least every comm_waittime_ seconds, in other words, how long does the robot wait between two communication attempts double commWaitTime_; - ros::Time startTime_; + ros::Time lastCommTime_; std::vector poseMstVectorSub_; diff --git a/backend/sloam/include/core/robot.h b/backend/sloam/include/core/robot.h index 454d6a1..3254d5c 100755 --- a/backend/sloam/include/core/robot.h +++ b/backend/sloam/include/core/robot.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -27,8 +28,8 @@ #include #include #include +#include #include -#include #include #include @@ -56,50 +57,52 @@ class Robot { size_t maxQueueSize_; ros::NodeHandle nh_; - public: - // how many seconds max to wait for semantic measurements to arrive - double semantic_meas_delay_tolerance_ = 3.0; + public: + bool robotOdomUpdated_ = false; + bool robotObservationUpdated_ = false; + StampedSE3 robotLatestOdom_; + SE3 robotLastSLOAMKeyPose_ = SE3(); + bool robotFirstOdom_; + size_t robotOdomCounter_; + bool turn_off_rel_inter_robot_loop_closure; + + int robotId_; // ID (index) of this robot + std::string robot_ns_prefix_; // string before robotId_ in ros topics + + // Initializer + Robot(const ros::NodeHandle &nh); + + // Data structures for holding recieved data from this robot std::deque robotOdomQueue_; - std::queue robotTreePcQueue_; - std::queue robotGroundPcQueue_; - std::queue, ros::Time>> robotCubesQueue_; std::queue robotObservationQueue_; + std::deque robotRelativeMeasQueue_; + + // Used by inputNode.cpp to track if sloam has already updated a previous odometry + bool robotOdomReceived_ = false; + + // How many seconds max to wait for semantic measurements to arrive + double semantic_meas_delay_tolerance_ = 3.0; // Publishers ros::Publisher pubRobotHighFreqSLOAMPose_; ros::Publisher pubRobotHighFreqSLOAMOdom_; ros::Publisher pubSloamToVioOdom_; ros::Publisher pubRobotHighFreqSyncOdom_; - //Subscribers + + // Subscribers ros::Subscriber RobotOdomSub_; - ros::Subscriber RobotGroundPCSub_; - ros::Subscriber RobotTreePCSub_; - ros::Subscriber RobotCubeSub_; ros::Subscriber RobotObservationSub_; + ros::Subscriber RobotRelativeMeasSub_; - std::string robot_odom_topic_; - bool robotTreeCloudUpdated_ = false; - bool robotGroundCloudUpdated_ = false; - bool robotCubesUpdated_ = false; - bool robotOdomReceived_ = false; - bool robotOdomUpdated_ = false; - bool robotObservationUpdated_ = false; - // optimized key poses + // Optimized key poses std::vector robotKeyPoses_; - StampedSE3 robotLatestOdom_; - SE3 robotLastSLOAMKeyPose_ = SE3(); - bool robotFirstOdom_; - size_t robotOdomCounter_; - int robotId_; - std::string robot_ns_prefix_; - std::string odom_topic_; + // Callback functions for Subscribers void RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg); - void RobotTreePCCb(const sensor_msgs::PointCloud2ConstPtr &cloudMsg); - void RobotGroundPCCb(const sensor_msgs::PointCloud2ConstPtr &cloudMsg); - void RobotCubeCb(const visualization_msgs::MarkerArray &cuboid_msg); void RobotObservationCb(const sloam_msgs::SemanticMeasSyncOdom &observation_msg); + void RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurement &relativeMeas_msg); + + // Helper methods for converting messages to objects std::vector rosEllipsoid2EllipObj(const std::vector& msgs); std::vector rosCylinder2CylinderObj(const std::vector& rosCylinders); - Robot(const ros::NodeHandle &nh); }; diff --git a/backend/sloam/include/core/sloamNode.h b/backend/sloam/include/core/sloamNode.h index eedcf3b..87894ef 100755 --- a/backend/sloam/include/core/sloamNode.h +++ b/backend/sloam/include/core/sloamNode.h @@ -66,12 +66,14 @@ class SLOAMNode : public sloam { const std::vector &cubesBody, const std::vector &ellipsoidBody, ros::Time stamp, SE3 &outPose, const int &robotID); + void addRelativeMeasurement(RelativeMeas relativeMeas); bool isInLoopClosureRegion_ = false; SemanticFactorGraphWrapper factorGraph_; databaseManager dbManager; std::mutex dbMutex; int hostRobotID; CylinderMapManager semanticMap_; + // results analysis related variables std::vector fg_optimization_time; int total_number_of_landmarks = 0; @@ -83,6 +85,7 @@ class SLOAMNode : public sloam { std::vector inter_loop_closure_time; int num_attempts_inter_loop_closure = 0; int num_successful_inter_loop_closure = 0; + int num_successful_rel_inter_robot_factor = 0; bool save_runtime_analysis = false; std::string runtime_analysis_file; @@ -96,6 +99,7 @@ class SLOAMNode : public sloam { double inter_robot_place_recognition_frequency_; double intra_robot_place_recognition_frequency_; + double rel_inter_robot_factor_frequency_; std::vector KeyPoseTimeStamps; ros::Publisher groundPub_; @@ -113,6 +117,8 @@ class SLOAMNode : public sloam { ros::Time stamp, const int &robotID); void intraLoopClosureThread_(); void interLoopClosureThread_(); + void relInterRobotFactorThread_(); + void GetIndexClosestPoseMstPair(std::deque &poseMstPacket, ros::Time stamp, int &indexClosest, double &timeDiffClosest); std::vector extractPosition( const std::vector &candidateCylinderObs, @@ -175,6 +181,12 @@ class SLOAMNode : public sloam { std::mutex ellipsoidSemanticMapMtx_; std::mutex factorGraphMtx_; + // Relative Measurement Factor Generation + ros::Time last_rel_inter_robot_factor_stamp_; + std::thread relInterRobotFactorthread_; + std::vector feasible_relative_meas_for_factors; // All measurements that could still be used to generate a factor + std::mutex feasRelMeasVectorMtx_; + // For cuboid semantic landmarks CubeMapManager cube_semantic_map_; std::vector cube_matches_; diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index bf1478f..2ef38b9 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -92,6 +92,9 @@ class SemanticFactorGraph { void addLoopClosureFactor(const Pose3 poseRelative, const size_t fromIdx, const size_t fromRobotID, const size_t toIdx, const size_t toRobotID); + void addRelativeMeasurementFactor(const Pose3 poseRelative, const size_t fromIdx, + const size_t fromRobotID, const size_t toIdx, + const size_t toRobotID); void addPointLandmarkKey(const size_t ugvIdx, const Point3& ugv_position); // getters and setters @@ -128,6 +131,7 @@ class SemanticFactorGraph { boost::shared_ptr noise_model_gps; boost::shared_ptr noise_model_cylinder; boost::shared_ptr noise_model_cube; + boost::shared_ptr noise_model_rel_meas; // Aux attributes Pose3 odomToGPS_; diff --git a/backend/sloam/include/helpers/definitions.h b/backend/sloam/include/helpers/definitions.h index c362ce3..41b8705 100755 --- a/backend/sloam/include/helpers/definitions.h +++ b/backend/sloam/include/helpers/definitions.h @@ -96,3 +96,26 @@ struct FeatureModelParams { Scalar defaultCylinderRadius; }; + +/* + * Relative Inter-Robot Measurements + */ + +/** + * @brief This struct holds relative inter-robot + * measurements. + * + * @param stamp - the timestamp. + * @param odomPose - the synced odometry. + * @param relativePose - the relative measurement + * between the current robot and robot assigned to + * "robotIndex". + * @param robotIndex - the index assigned to the robot + * whose relative pose was measured. + */ +struct RelativeMeas { + ros::Time stamp; + SE3 odomPose; + SE3 relativePose; + int robotIndex; +}; \ No newline at end of file diff --git a/backend/sloam/include/helpers/utils.h b/backend/sloam/include/helpers/utils.h index f686313..3e4b424 100755 --- a/backend/sloam/include/helpers/utils.h +++ b/backend/sloam/include/helpers/utils.h @@ -1,10 +1,13 @@ #pragma once #include +#include #include #include #include +using namespace gtsam; + inline float pow_2(const Scalar &x) { return x * x; } inline float euclideanDist2D(const PointT &vecA, const PointT &vecB) { @@ -72,6 +75,22 @@ inline SE3 ToSE3(const geometry_msgs::Pose &transform_msg) { return se3_transform; } +/** + * @brief This function converts a Sophus::SE3d + * value to a gtsam::Pose3. + * + * @param Sophus::SE3 pose : The input pose + * @return gtsam::Pose3 : The output pose + */ +inline gtsam::Pose3 SE3ToGTSAMPose3(SE3 pose) { + Eigen::Matrix3d R = pose.rotationMatrix(); + Eigen::Vector3d t = pose.translation(); + + gtsam::Rot3 gtsam_rot(R); + gtsam::Point3 gtsam_trans(t[0], t[1], t[2]); + return gtsam::Pose3(gtsam_rot, gtsam_trans); +} + inline geometry_msgs::Pose ToRosPoseMsg(const SE3 &T){ // Create a geometry_msgs::Transform message geometry_msgs::Pose transform_msg; @@ -93,4 +112,4 @@ inline geometry_msgs::Pose ToRosPoseMsg(const SE3 &T){ transform_msg.orientation.z = quaternion.z(); transform_msg.orientation.w = quaternion.w(); return transform_msg; -} +} \ No newline at end of file diff --git a/backend/sloam/launch/decentralized_sloam.launch b/backend/sloam/launch/decentralized_sloam.launch index 3b9dcb1..4b9bb68 100755 --- a/backend/sloam/launch/decentralized_sloam.launch +++ b/backend/sloam/launch/decentralized_sloam.launch @@ -10,6 +10,7 @@ + @@ -19,6 +20,7 @@ + @@ -30,6 +32,7 @@ + diff --git a/backend/sloam/src/core/databaseManager.cpp b/backend/sloam/src/core/databaseManager.cpp index bfbbcea..319f679 100755 --- a/backend/sloam/src/core/databaseManager.cpp +++ b/backend/sloam/src/core/databaseManager.cpp @@ -35,7 +35,7 @@ databaseManager::databaseManager(const ros::NodeHandle &nh) { nh_.param("number_of_robots", numRobots, 0); Eigen::Vector3d priorTF_xyz(priorTF_x, priorTF_y, priorTF_z); - priorTF2World_ = SE3(SO3(), priorTF_xyz); + priorTF2World_ = SE3(Sophus::SO3d(), priorTF_xyz); SE3 tfWorld2Robot = priorTF2World_.inverse(); // ROS_INFO_STREAM("WE ARE USING THE REFERENCE FRAME FROM ROBOT 0 AS THE WORLD FRAME!"); for (int i = 0; i < numRobots; i++) @@ -95,71 +95,6 @@ void databaseManager::updateRobotMap(const std::vector &cylMap, robotMapDict_[robotID] = newMap; } -void databaseManager::publishPoseMsts(int robotID) { - if (robotDataDict_.find(robotID) == robotDataDict_.end()) { - printf("the robot data doesn't exist in the database! No available data to " - "publish!\n"); - } else { - std::deque poseMsts = robotDataDict_[robotID].poseMstPacket; - sloam_msgs::PoseMstBundle bundleMsg; - bundleMsg.robotID = robotID; - for (int i = 0; i < poseMsts.size(); i++) { - sloam_msgs::PoseMst singleMsg; - geometry_msgs::Pose poseMsg = ToRosPoseMsg(poseMsts[i].keyPose); - geometry_msgs::Pose relativeOdom = - ToRosPoseMsg(poseMsts[i].relativeRawOdomMotion); - singleMsg.pose = poseMsg; - singleMsg.relativeRawOdom = relativeOdom; - for (int mstIdx = 0; mstIdx < poseMsts[i].cubeMsts.size(); mstIdx++) { - sloam_msgs::ROSCube rosCubeMsg; - Cube curCube = poseMsts[i].cubeMsts[mstIdx]; - for (int j = 0; j < 3; j++) { - rosCubeMsg.dim[j] = curCube.model.scale[j]; - } - rosCubeMsg.pose = gtsamPoseToRosPose(curCube.model.pose); - rosCubeMsg.semantic_label = curCube.model.semantic_label; - singleMsg.cubes.push_back(rosCubeMsg); - } - for (int mstIdx = 0; mstIdx < poseMsts[i].cylinderMsts.size(); mstIdx++) { - sloam_msgs::ROSCylinder rosCylinderMsg; - Cylinder curCylinder = poseMsts[i].cylinderMsts[mstIdx]; - for (int j = 0; j < 3; j++) { - rosCylinderMsg.ray[j] = curCylinder.model.ray[j]; - rosCylinderMsg.root[j] = curCylinder.model.root[j]; - } - rosCylinderMsg.radius = curCylinder.model.radius; - rosCylinderMsg.semantic_label = curCylinder.model.semantic_label; - singleMsg.cylinders.push_back(rosCylinderMsg); - } - for (int mstIdx = 0; mstIdx < poseMsts[i].ellipsoidMsts.size(); - mstIdx++) { - sloam_msgs::ROSEllipsoid rosEllipsoidMsg; - Ellipsoid curEllipsoid = poseMsts[i].ellipsoidMsts[mstIdx]; - for (int j = 0; j < 3; j++) { - rosEllipsoidMsg.scale[j] = curEllipsoid.model.scale[j]; - } - rosEllipsoidMsg.pose = gtsamPoseToRosPose(curEllipsoid.model.pose); - rosEllipsoidMsg.semantic_label = curEllipsoid.model.semantic_label; - singleMsg.ellipsoids.push_back(rosEllipsoidMsg); - } - bundleMsg.poseMstPair.push_back(singleMsg); - } - for (int i = 0; i < robotMapDict_[robotID].size(); i++) { - Eigen::Vector7d curPoint = robotMapDict_[robotID][i]; - sloam_msgs::vector7d labelXYZ; - labelXYZ.labelXYZ[0] = curPoint[0]; - labelXYZ.labelXYZ[1] = curPoint[1]; - labelXYZ.labelXYZ[2] = curPoint[2]; - labelXYZ.labelXYZ[3] = curPoint[3]; - labelXYZ.labelXYZ[4] = curPoint[4]; - labelXYZ.labelXYZ[5] = curPoint[5]; - labelXYZ.labelXYZ[6] = curPoint[6]; - bundleMsg.map_of_labelXYZ.push_back(labelXYZ); - } - poseMstPub_.publish(bundleMsg); - } -} - void databaseManager::poseMstCb_(const sloam_msgs::PoseMstBundle &msgs) { int robotID = msgs.robotID; if (robotDataDict_.find(robotID) == robotDataDict_.end()) { @@ -168,18 +103,17 @@ void databaseManager::poseMstCb_(const sloam_msgs::PoseMstBundle &msgs) { size_t bundleSize = msgs.poseMstPair.size(); size_t poolSize = robotDataDict_[robotID].poseMstPacket.size(); if (bundleSize > poolSize && robotID != this->hostRobotID_) { - ROS_DEBUG_STREAM("New robot data received from robot:" - << robotID << "by robot:" << this->hostRobotID_); - ROS_DEBUG_STREAM("New robot data received "); + ROS_DEBUG_STREAM("New robot data received from robot:" << robotID << "by robot:" << this->hostRobotID_); int startIdx = poolSize; for (int i = startIdx; i < bundleSize; i++) { struct PoseMstPair poseMst; sloam_msgs::PoseMst singleMsg = msgs.poseMstPair[i]; SE3 pose = toSE3Pose(singleMsg.pose); SE3 relativeOdom = toSE3Pose(singleMsg.relativeRawOdom); - // constrcut a PoseMstPair struct + // construct a PoseMstPair struct poseMst.keyPose = pose; poseMst.relativeRawOdomMotion = relativeOdom; + poseMst.stamp = singleMsg.stamp; // convert msg to object for (int j = 0; j < singleMsg.cylinders.size(); j++) { sloam_msgs::ROSCylinder curObjMsg = singleMsg.cylinders[j]; @@ -263,6 +197,7 @@ void databaseManager::measureReceivedCommMsgSize(const sloam_msgs::PoseMstBundle for (int i = 0; i < msgs.poseMstPair.size(); i++) { cur_received_msg_size_bytes += 56; cur_received_msg_size_bytes += 56; + cur_received_msg_size_bytes += 8; // For the time stamp (theorized, not prove) cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].ellipsoids.size(); cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].cubes.size(); cur_received_msg_size_bytes += 37*msgs.poseMstPair[i].cylinders.size(); @@ -272,21 +207,35 @@ void databaseManager::measureReceivedCommMsgSize(const sloam_msgs::PoseMstBundle receivedMsgSizeMB.push_back(cur_received_msg_size_bytes/1000000); } +/** + * @brief This function, triggered by a timer every 0.2 seconds, sends + * messages to the other robots if commWaitTime_ has passed since + * the last communication was sent (at time lastCommTime_). Thus, + * effectively sends info to other robots every commWaitTime_ + * seconds. + * + * @param ros::TimerEvent &e + */ void databaseManager::runCommunication_(const ros::TimerEvent &e) { - ros::Duration time_diff = ros::Time::now() - startTime_; + ros::Duration time_diff = ros::Time::now() - lastCommTime_; if (time_diff.toSec() > commWaitTime_) { - startTime_ = ros::Time::now(); + lastCommTime_ = ros::Time::now(); ROS_INFO_THROTTLE(3.0, "TRIGGERING COMMUNICATION!"); double cur_publish_msg_size_bytes = 0; + + // Send all current data on each robot for (auto iter = getRobotDataDict().begin(); iter != getRobotDataDict().end(); iter++) { int curRobotID = iter->first; sloam_msgs::PoseMstBundle bundleMsg; bundleMsg.robotID = curRobotID; + + // Send each pose measurement pair for (int i = 0; i < iter->second.poseMstPacket.size(); i++) { struct PoseMstPair pmp = iter->second.poseMstPacket[i]; sloam_msgs::PoseMst pmMsg; pmMsg.pose = ToRosPoseMsg(pmp.keyPose); + pmMsg.stamp = pmp.stamp; pmMsg.relativeRawOdom = ToRosPoseMsg(pmp.relativeRawOdomMotion); pmMsg.cylinders = obj2RosObjMsg(pmp.cylinderMsts); pmMsg.cubes = obj2RosObjMsg(pmp.cubeMsts); @@ -297,7 +246,9 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { cur_publish_msg_size_bytes += 37*pmp.cylinderMsts.size(); cur_publish_msg_size_bytes += 56; cur_publish_msg_size_bytes += 56; + cur_publish_msg_size_bytes += 8; // From time stamp (not proven) } + for (int i = 0; i < robotMapDict_[curRobotID].size(); i++) { Eigen::Vector7d curPoint = robotMapDict_[curRobotID][i]; sloam_msgs::vector7d labelXYZ; @@ -311,6 +262,8 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { bundleMsg.map_of_labelXYZ.push_back(labelXYZ); cur_publish_msg_size_bytes += 56; } + + // Send each known transformation from other robots to this robot for (auto iter=loopClosureTf.begin(); iter!=loopClosureTf.end(); iter++){ sloam_msgs::interRobotTF interRobotTFMsg; interRobotTFMsg.hostRobotID = hostRobotID_; @@ -324,3 +277,68 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { publishMsgSizeMB.push_back(cur_publish_msg_size_bytes/1000000); } } + +// void databaseManager::publishPoseMsts(int robotID) { +// if (robotDataDict_.find(robotID) == robotDataDict_.end()) { +// printf("the robot data doesn't exist in the database! No available data to " +// "publish!\n"); +// } else { +// std::deque poseMsts = robotDataDict_[robotID].poseMstPacket; +// sloam_msgs::PoseMstBundle bundleMsg; +// bundleMsg.robotID = robotID; +// for (int i = 0; i < poseMsts.size(); i++) { +// sloam_msgs::PoseMst singleMsg; +// geometry_msgs::Pose poseMsg = ToRosPoseMsg(poseMsts[i].keyPose); +// geometry_msgs::Pose relativeOdom = +// ToRosPoseMsg(poseMsts[i].relativeRawOdomMotion); +// singleMsg.pose = poseMsg; +// singleMsg.relativeRawOdom = relativeOdom; +// for (int mstIdx = 0; mstIdx < poseMsts[i].cubeMsts.size(); mstIdx++) { +// sloam_msgs::ROSCube rosCubeMsg; +// Cube curCube = poseMsts[i].cubeMsts[mstIdx]; +// for (int j = 0; j < 3; j++) { +// rosCubeMsg.dim[j] = curCube.model.scale[j]; +// } +// rosCubeMsg.pose = gtsamPoseToRosPose(curCube.model.pose); +// rosCubeMsg.semantic_label = curCube.model.semantic_label; +// singleMsg.cubes.push_back(rosCubeMsg); +// } +// for (int mstIdx = 0; mstIdx < poseMsts[i].cylinderMsts.size(); mstIdx++) { +// sloam_msgs::ROSCylinder rosCylinderMsg; +// Cylinder curCylinder = poseMsts[i].cylinderMsts[mstIdx]; +// for (int j = 0; j < 3; j++) { +// rosCylinderMsg.ray[j] = curCylinder.model.ray[j]; +// rosCylinderMsg.root[j] = curCylinder.model.root[j]; +// } +// rosCylinderMsg.radius = curCylinder.model.radius; +// rosCylinderMsg.semantic_label = curCylinder.model.semantic_label; +// singleMsg.cylinders.push_back(rosCylinderMsg); +// } +// for (int mstIdx = 0; mstIdx < poseMsts[i].ellipsoidMsts.size(); +// mstIdx++) { +// sloam_msgs::ROSEllipsoid rosEllipsoidMsg; +// Ellipsoid curEllipsoid = poseMsts[i].ellipsoidMsts[mstIdx]; +// for (int j = 0; j < 3; j++) { +// rosEllipsoidMsg.scale[j] = curEllipsoid.model.scale[j]; +// } +// rosEllipsoidMsg.pose = gtsamPoseToRosPose(curEllipsoid.model.pose); +// rosEllipsoidMsg.semantic_label = curEllipsoid.model.semantic_label; +// singleMsg.ellipsoids.push_back(rosEllipsoidMsg); +// } +// bundleMsg.poseMstPair.push_back(singleMsg); +// } +// for (int i = 0; i < robotMapDict_[robotID].size(); i++) { +// Eigen::Vector7d curPoint = robotMapDict_[robotID][i]; +// sloam_msgs::vector7d labelXYZ; +// labelXYZ.labelXYZ[0] = curPoint[0]; +// labelXYZ.labelXYZ[1] = curPoint[1]; +// labelXYZ.labelXYZ[2] = curPoint[2]; +// labelXYZ.labelXYZ[3] = curPoint[3]; +// labelXYZ.labelXYZ[4] = curPoint[4]; +// labelXYZ.labelXYZ[5] = curPoint[5]; +// labelXYZ.labelXYZ[6] = curPoint[6]; +// bundleMsg.map_of_labelXYZ.push_back(labelXYZ); +// } +// poseMstPub_.publish(bundleMsg); +// } +// } \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 56edaaa..511497e 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -94,6 +95,7 @@ class InputManager { int hostRobotID_; bool turn_off_intra_loop_closure_; + bool turn_off_rel_inter_robot_factor; }; InputManager::InputManager(ros::NodeHandle nh) @@ -116,6 +118,8 @@ InputManager::InputManager(ros::NodeHandle nh) nh_.param(idName, hostRobotID_, 0); nh_.param(node_name + "/turn_off_intra_loop_closure", turn_off_intra_loop_closure_, false); + nh_.param(node_name+"/turn_off_rel_inter_robot_factor", + turn_off_rel_inter_robot_factor, true); auto sloam_ptr = boost::make_shared(nh_); sloam_ = std::move(sloam_ptr); @@ -130,8 +134,6 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { } // robot.robotOdomQueue_ is filled, execute the rest of the code - // ROS_INFO_STREAM_THROTTLE( - // 1, "Odom queue is already filled yet for robot , which is good..."); SE3 highFreqSLOAMPose; ros::Time odom_stamp; @@ -175,7 +177,69 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { robot.pubRobotHighFreqSLOAMOdom_.publish(odom_msg); robot.pubRobotHighFreqSyncOdom_.publish(syncOdom); - // ADDING FACTORS + // Adding Factors for Relative Inter-Robot Measurements + if(!turn_off_rel_inter_robot_factor) { + // For each received measurement + for (int i = 0; i < robot.robotRelativeMeasQueue_.size(); i++) { + // If measurement is at least robot.semantic_meas_delay_tolerance_ seconds old + if ((robot.robotRelativeMeasQueue_.back().stamp - robot.robotRelativeMeasQueue_[i].stamp).toSec() + > robot.semantic_meas_delay_tolerance_) { + + // Create the stamped pose + RelativeMeas relativeMeas = robot.robotRelativeMeasQueue_[i]; + StampedSE3 validStampedPose; + validStampedPose.pose = relativeMeas.odomPose; + validStampedPose.stamp = relativeMeas.stamp; + + // Create the observation (with only odometry, no objects) + Observation latestObservation = Observation(); + latestObservation.stampedPose = validStampedPose; + + // Use odom to estimate motion since last key frame + StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; + SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; + + // Get the Previous Key Pose + SE3 prevKeyPose; + if (robot.robotKeyPoses_.size() > 0) { + prevKeyPose = robot.robotKeyPoses_[robot.robotKeyPoses_.size() - 1]; + } else { + ROS_WARN("No previous key pose. Use identity as the previous key pose."); + prevKeyPose = SE3(); + } + + // Send the relative measurement synched odometry to SLOAM + SE3 keyPose; + bool success = sloam_->runSLOAMNode( + relativeRawOdomMotion, prevKeyPose, latestObservation.cylinders, + latestObservation.cubes, latestObservation.ellipsoids, + latestObservation.stampedPose.stamp, keyPose, hostRobotID_); + if (success) { + robot.robotKeyPoses_.push_back(keyPose); + updateLastPose(raw_vio_odom_used_for_sloam, hostRobotID_); + } + + // Extract Host Robot Data class + sloam_->dbMutex.lock(); + robotData data = sloam_->dbManager.getHostRobotData(); + + // Pass the relative inter-robot measurement to the database manager + // and factor generation thread + data.relativeMeasPacket.push_back(relativeMeas); + sloam_->addRelativeMeasurement(relativeMeas); + + // Remove this value from the queue, as it's been used + robot.robotRelativeMeasQueue_.erase(robot.robotRelativeMeasQueue_.begin() + i); + sloam_->dbMutex.unlock(); + i--; + + } + // Remaining measurements are not old enough to add, so finish + else { break; } + } + } + + // ADDING OTHER FACTORS // add odom factor only if the robot has moved enough bool add_odom_factor = false; bool valid_pose_found = false; @@ -303,10 +367,11 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { } else { ROS_INFO_STREAM_THROTTLE(3.0, "Neither the odometry nor the observation is updated for robot " << hostRobotID_); } + if (sloam_->save_runtime_analysis) { saveRuntimeCommUsage(); } -} // end of RunInputNode() +} void InputManager::updateLastPose(const StampedSE3 &odom, const int &robotID) { robot.robotOdomReceived_ = true; @@ -435,6 +500,7 @@ int main(int argc, char **argv) { ros::init(argc, argv, "sloam"); ros::NodeHandle n("sloam"); InputManager in(n); + while (ros::ok()) { ros::spin(); } diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 45e961b..efbb9cd 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -17,50 +17,41 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { nh_.param("min_robot_altitude", minSLOAMAltitude_, 0.0); maxQueueSize_ = nh_.param("max_queue_size", 100); nh_.param("robot_ns_prefix", robot_ns_prefix_, "robot"); - nh_.param("odom_topic", odom_topic_, "odom"); - - // initialization std::string node_name = ros::this_node::getName(); std::string idName = node_name + "/hostRobotID"; - robotId_ = nh_.param(idName, 0); - std::string cur_robot_odom_topic; - int robot_actual_ID; - robot_actual_ID = robotId_; + nh_.param(idName, robotId_, 0); + + // Initialize variables robotFirstOdom_ = true; robotOdomCounter_ = 0; - // initialize publisher and subscriber - std::string RobotHighFreqSLOAMPose_topic = - robot_ns_prefix_ + std::to_string(robot_actual_ID) + "/pose_high_freq"; - pubRobotHighFreqSLOAMPose_ = nh_.advertise( - RobotHighFreqSLOAMPose_topic, 10); - - std::string RobotHighFreqSLOAMOdom_topic = robot_ns_prefix_ + - std::to_string(robot_actual_ID) + - "/sloam_odom_high_freq"; - pubRobotHighFreqSLOAMOdom_ = - nh_.advertise(RobotHighFreqSLOAMOdom_topic, 20); - - std::string SloamToVioOdom_topic = - robot_ns_prefix_ + std::to_string(robot_actual_ID) + "/sloam_to_vio_odom"; - pubSloamToVioOdom_ = - nh_.advertise(SloamToVioOdom_topic, 20); - - std::string RobotHighFreqSyncOdom_topic = robot_ns_prefix_ + - std::to_string(robot_actual_ID) + - "/sync_odom_high_freq"; - pubRobotHighFreqSyncOdom_ = - nh_.advertise(RobotHighFreqSyncOdom_topic, 20); + // Initialize publishers + std::string RobotHighFreqSLOAMPose_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/pose_high_freq"; + pubRobotHighFreqSLOAMPose_ = nh_.advertise(RobotHighFreqSLOAMPose_topic, 10); + + std::string RobotHighFreqSLOAMOdom_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/sloam_odom_high_freq"; + pubRobotHighFreqSLOAMOdom_ = nh_.advertise(RobotHighFreqSLOAMOdom_topic, 20); + + std::string SloamToVioOdom_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/sloam_to_vio_odom"; + pubSloamToVioOdom_ = nh_.advertise(SloamToVioOdom_topic, 20); + + std::string RobotHighFreqSyncOdom_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/sync_odom_high_freq"; + pubRobotHighFreqSyncOdom_ = nh_.advertise(RobotHighFreqSyncOdom_topic, 20); + // Initialize Subscribers RobotOdomSub_ = nh_.subscribe("odom", 10, &Robot::RobotOdomCb, this); - std::string observationSub_topic = robot_ns_prefix_ + - std::to_string(robot_actual_ID) + - "/semantic_meas_sync_odom"; + + std::string observationSub_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/semantic_meas_sync_odom"; + RobotObservationSub_ = nh_.subscribe(observationSub_topic, 10, &Robot::RobotObservationCb, this); + + bool turn_off_rel_inter_robot_loop_closure = nh_.param(node_name+"/turn_off_rel_inter_robot_loop_closure", true); + if (!turn_off_rel_inter_robot_loop_closure) { + std::string relativeMeasSub_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/relative_inter_robot_meas"; + RobotRelativeMeasSub_ = nh_.subscribe(relativeMeasSub_topic, 10, &Robot::RobotRelativeMeasCb, this); + } - RobotObservationSub_ = - nh_.subscribe(observationSub_topic, 10, &Robot::RobotObservationCb, this); - ROS_INFO_STREAM("Robot Initialized! Robot # " << robot_actual_ID); + ROS_INFO_STREAM("Robot Initialized! Robot # " << robotId_); } void Robot::RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg) { @@ -153,7 +144,6 @@ void Robot::RobotObservationCb( Cube(pose, scale, cur_cuboid_marker.semantic_label)); } - // Cylinder cur_observation.cubes = scan_cubes_body; cur_observation.cylinders = rosCylinder2CylinderObj(observation_msg.cylinder_factors); @@ -163,6 +153,25 @@ void Robot::RobotObservationCb( robotObservationUpdated_ = true; } +/** + * @brief This method converts a + * sloam_msgs::RelativeInterRobotMeasurement into + * a RelativeMeas struct and adds it to the queue. + * inputNode will pass this onto the databaseManager + * in sloamNode. + * + * @param sloam_msgs::RelativeInterRobotMeasurement + * &relativeMeas_msg + */ +void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurement &relativeMeas_msg) { + RelativeMeas cur_measurement; + cur_measurement.stamp = relativeMeas_msg.header.stamp; + cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometry.pose.pose); + cur_measurement.relativePose = databaseManager::toSE3Pose(relativeMeas_msg.relativePose); + cur_measurement.robotIndex = relativeMeas_msg.robotId; + robotRelativeMeasQueue_.push_back(cur_measurement); +} + std::vector Robot::rosCylinder2CylinderObj( const std::vector &rosCylinders) { @@ -189,7 +198,7 @@ std::vector Robot::rosEllipsoid2EllipObj( gtsam::Point3 position(x, y, z); gtsam::Pose3 pose(rot, position); int label = m.semantic_label; - // scale of the cuboid + // scale of the ellipsoid const gtsam::Point3 scale(m.scale[0], m.scale[1], m.scale[2]); // assemble a pose from the ellipsoids.emplace_back(pose, scale, label); diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index 1bd9102..1bb0285 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -161,7 +161,7 @@ void sloam::matchEllipsoidModels(const std::vector &currObjects, std::vector &matchIndices) { if (currObjects.size() == 0) { - ROS_WARN("No Ellipsoid detected in current scan!"); + ROS_WARN_STREAM_THROTTLE(1.0, "No Ellipsoid detected in current scan!"); return; } else if (mapObjects.size() == 0) { ROS_WARN("No Ellipsoid in submap!"); diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index cfa3ca4..c05d356 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -45,6 +45,8 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) int num_of_robots = nh_.param("number_of_robots", 1); inter_robot_place_recognition_frequency_ = nh_.param("inter_robot_place_recognition_frequency", 0.1); intra_robot_place_recognition_frequency_ = nh_.param("intra_robot_place_recognition_frequency", 0.1); + rel_inter_robot_factor_frequency_ = nh_.param("rel_inter_robot_factor_frequency", 1.0); + // place_recognition_attempt_time_offset double place_recognition_attempt_time_offset = nh_.param("place_recognition_attempt_time_offset", 1.5); for (int i = 0; i < num_of_robots; i++) { @@ -60,7 +62,8 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) // initialize last_intra_loop_closure_stamp_ as current time // add offset the timestamp a bit to avoid all robots calling loop closure at the same time when running on the same machine last_intra_loop_closure_stamp_ = ros::Time::now() + ros::Duration(place_recognition_attempt_time_offset) * hostRobotID; - last_inter_loop_closure_stamp_ = ros::Time::now() + ros::Duration(place_recognition_attempt_time_offset) * hostRobotID; + last_inter_loop_closure_stamp_ = last_intra_loop_closure_stamp_; + last_rel_inter_robot_factor_stamp_ = last_intra_loop_closure_stamp_; pubMapTreeModel_ = nh_.advertise("cylinders_map", 1, true); @@ -89,41 +92,53 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) tf_listener_.reset(new tf2_ros::TransformListener(tf_buffer_)); initParams_(); - // Loop Closure + // Intra-Robot Loop Closure bool turn_off_intra_loop_closure = nh_.param(node_name+"/turn_off_intra_loop_closure", true); if (turn_off_intra_loop_closure) { - ROS_WARN("Intra Loop closure is turned off"); + ROS_WARN("Intra Loop Closure is turned off"); } else { intraLoopthread_ = std::thread(&SLOAMNode::intraLoopClosureThread_, this); - ROS_WARN("Intra Loop closure is turned on"); + ROS_WARN("Intra Loop Closure is turned on"); } - // Loop Closure + // Inter-Robot Loop Closure bool turn_off_inter_loop_closure = nh_.param(node_name+"/turn_off_inter_loop_closure", true); if (turn_off_inter_loop_closure) { - ROS_WARN("Inter Loop closure is turned off"); + ROS_WARN("Inter Loop Closure is turned off"); } else { interLoopthread_ = std::thread(&SLOAMNode::interLoopClosureThread_, this); - ROS_WARN("Inter Loop closure is turned on"); + ROS_WARN("Inter Loop Closure is turned on"); } lastLoopAttemptPose_ = -1; + // Relative Measurement Inter-Robot Loop Closure + bool turn_off_rel_inter_robot_factor = nh_.param(node_name+"/turn_off_rel_inter_robot_factor", true); + if (turn_off_rel_inter_robot_factor) { + ROS_WARN("Relative Inter-Robot Loop Closure is turned off"); + } else { + relInterRobotFactorthread_ = std::thread(&SLOAMNode::relInterRobotFactorThread_, this); + ROS_WARN("Relative Inter-Robot Loop Closure is turned on"); + } + // initialize the runtime analysis variables runtime_analysis_file = save_runtime_analysis_dir_ + "/robot"+std::to_string(hostRobotID)+"_runtime_analysis.txt"; ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file); } +/** + * @brief Make sure all three child threads terminate. + */ SLOAMNode::~SLOAMNode() { if (intraLoopthread_.joinable()) intraLoopthread_.join(); if (interLoopthread_.joinable()) interLoopthread_.join(); + if (relInterRobotFactorthread_.joinable()) + relInterRobotFactorthread_.join(); } void SLOAMNode::initParams_() { // PARAMETERS - - numRobots = nh_.param("number_of_robots", 1); semanticMap_ = CylinderMapManager(numRobots); cube_semantic_map_ = CubeMapManager(); @@ -158,7 +173,6 @@ void SLOAMNode::initParams_() { fmParams_.featuresPerTree = nh_.param("features_per_tree", 50); // NO LONGER USED fmParams_.numGroundFeatures = nh_.param("num_ground_features", 5); // NO LONGER USED - setFmParams(fmParams_); // Frame Ids @@ -355,8 +369,8 @@ void SLOAMNode::intraLoopClosureThread_() { int best_number_inliers = 0; std::vector map_objects_matched_out; std::vector detection_objects_matched_out; - ROS_DEBUG("number of measurements is: %d", measurements.size()); - ROS_DEBUG("number of object in submaps is: %d", submaps.size()); + ROS_DEBUG("number of measurements is: %lu", measurements.size()); + ROS_DEBUG("number of object in submaps is: %lu", submaps.size()); // Main function for loop closure ros::Time loop_closure_start = ros::Time::now(); @@ -571,7 +585,8 @@ void SLOAMNode::interLoopClosureThread_() { if (found_inter_loop_closure) { num_successful_inter_loop_closure++; ROS_WARN_STREAM("INTER LOOP CLOSURE FOUND BETWEEN ROBOTS: " - << query_robot_id << " AND " << dbManager.getHostRobotID()); + << std::to_string(query_robot_id) << " AND " + << std::to_string(dbManager.getHostRobotID())); ros::Time inter_loop_closure_end = ros::Time::now(); inter_loop_closure_time.push_back( (inter_loop_closure_end - inter_loop_closure_start).toSec()); @@ -610,6 +625,149 @@ void SLOAMNode::interLoopClosureThread_() { } } +/** + * @brief This function searches through the deque of + * PoseMstPair and finds the message with the + * closest timestamp. It returns the corresponding + * index. If two entries have the same time difference, + * then the first index is returned. + * + * @param std::deque &poseMstPacket : The queue to search. + * @param ros::Time stamp : The stamp we compare to. + * @return int &indexClosets : Index in postMstPacket that is closest in time. + * Returns -1 if empty. + * @return double &timeDiffClosest : The difference in time between stamp and + * the closest PoseMstPair (in seconds). + */ +void SLOAMNode::GetIndexClosestPoseMstPair(std::deque &poseMstPacket, ros::Time stamp, int &indexClosest, double &timeDiffClosest) { + indexClosest = -1; + timeDiffClosest = std::numeric_limits::max(); + + for(int i = 0; i < poseMstPacket.size(); i++) { + double currTimeDiff = abs((poseMstPacket[i].stamp - stamp).toSec()); + if (currTimeDiff < timeDiffClosest) { + indexClosest = i; + timeDiffClosest = currTimeDiff; + } + } +} + +/** + * This function handles relative inter-robot factor generation + * by iterating through each relative measurement and seeing + * if the corresponding pair of robot poses (within reasonable + * time) are found within the graph. If so, its added to the + * factor graph. Does this at a rate of + * rel_inter_robot_factor_frequency_ Hz. + * + * NOTE: Currently relative inter-robot loop closures detected + * by other robots are NOT sent to other robots, should fix + * in the future. + */ +void SLOAMNode::relInterRobotFactorThread_() { + // Setup a rate + double desired_interval = 1.0 / rel_inter_robot_factor_frequency_; + ros::Rate rate(desired_interval); + + // Maximum time difference between relative inter robot measurement and stamped pose + // in order to add measurement to the factor graph + double maxTimeDiff = 0.001; // 1 ms + + while (ros::ok()) { + + // Lock mutexes + dbMutex.lock(); + feasRelMeasVectorMtx_.lock(); + + // Retrive poses from host robot + std::deque poseMstPacket = dbManager.getHostRobotData().poseMstPacket; + + // Only run loop closures if we have relative inter-robot measurements waiting + if (feasible_relative_meas_for_factors.size() > 0) { + ROS_DEBUG("****** Starting Relative Inter Robot Factor Generation Thread *****"); + + // Prepare return variables + int indexClosestHostRobot; + int indexClosestOtherRobot; + double timeDiffClosest; + int matches_found = 0; + + // Try to add each relative measurement + for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { + RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; + + // Iterate through the other robot's poses, and make sure one exists in the factor graph + // that is close enough in time to our measurement + size_t pose_counter_other = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); + GetIndexClosestPoseMstPair(dbManager.getRobotDataByID(relativeMeas.robotIndex).poseMstPacket, + relativeMeas.stamp, indexClosestOtherRobot, timeDiffClosest); + if (indexClosestOtherRobot == -1 || timeDiffClosest > maxTimeDiff || + indexClosestOtherRobot >= pose_counter_other) { continue; } + + // Iterate through our own poses, and make sure one exists in the factor graph thats close + // enough in time to our measurement + size_t pose_counter_host = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); + GetIndexClosestPoseMstPair(poseMstPacket, relativeMeas.stamp, indexClosestHostRobot, timeDiffClosest); + if (indexClosestHostRobot == -1 || timeDiffClosest > maxTimeDiff || + indexClosestHostRobot >= pose_counter_host) { continue; } + + // We found a match, so add it to the factor graph + matches_found += 1; + factorGraphMtx_.lock(); + gtsam::Pose3 relativeMeasPose = SE3ToGTSAMPose3(relativeMeas.relativePose); + factorGraph_.addLoopClosureFactor(relativeMeasPose, indexClosestHostRobot, + hostRobotID, indexClosestOtherRobot, relativeMeas.robotIndex); + factorGraphMtx_.unlock(); + ROS_DEBUG_STREAM("A Relative Inter-Robot Measurement Factor is added between Robot #" << hostRobotID << " Pose " << indexClosestHostRobot + << "and Robot #" << relativeMeas.robotIndex << " Pose " << indexClosestOtherRobot); + + // Remove this measurement from vector, so we don't use it again + feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); + i--; + } + + // Check which factors are no longer feasible for removal + for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { + + // Get pose_counter (tracks number of poses added to factor graph) + RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; + size_t pose_counter = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); + + // If factor graph for this robot isn't empty + if (pose_counter > 0) { + + // Get timestamp of most recent pose added to factor graph from observed robot + ros::Time stamp = dbManager.getRobotDataByID(relativeMeas.robotIndex).poseMstPacket[pose_counter - 1].stamp; + + // If this timestamp is later than relative measurement timestamp + if (stamp > relativeMeas.stamp) { + // This relative measurement is no longer feasible, as other robot + // simply doesn't have a pose factor to connect this measurement to + feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); + i--; + } + } + } + + // Keep track of total added + if (matches_found > 0) { + ROS_INFO_STREAM("Success: " << matches_found << " Relative Inter Robot Measurement Factors added"); + num_successful_rel_inter_robot_factor += matches_found; + } else { + ROS_DEBUG_STREAM("Added no Relative Inter Robot Measurement Factors"); + } + } + else { + ROS_DEBUG("No Available Relative Inter Robot Measurements"); + } + + // Unlock the mutexes and sleep + dbMutex.unlock(); + feasRelMeasVectorMtx_.unlock(); + rate.sleep(); + } +} + bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, const SE3 &prevKeyPose, const std::vector &cylindersBodyIn, @@ -631,6 +789,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, struct PoseMstPair pmp; pmp.keyPose = poseEstimate; pmp.cylinderMsts = cylindersBodyIn; + pmp.stamp = stamp; pmp.relativeRawOdomMotion = relativeRawOdomMotion; pmp.cubeMsts = cubesBodyIn; pmp.ellipsoidMsts = ellipsoidBodyIn; @@ -692,6 +851,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, semanticMapMtx_.unlock(); cubeSemanticMapMtx_.unlock(); ellipsoidSemanticMapMtx_.unlock(); + dbMutex.unlock(); return false; } @@ -871,4 +1031,17 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, return success; } +/** + * @brief This method add a relative measurement to the + * sloam node so that it can use it for relative inter-robot + * factor generation. + * + * @param relativeMeas - A time-stamped relative measurement + */ +void SLOAMNode::addRelativeMeasurement(RelativeMeas relativeMeas) { + feasRelMeasVectorMtx_.lock(); + feasible_relative_meas_for_factors.push_back(relativeMeas); + feasRelMeasVectorMtx_.unlock(); +} + } // namespace sloam \ No newline at end of file diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index 4b5bc29..bbe2d21 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -46,6 +46,12 @@ SemanticFactorGraph::SemanticFactorGraph() { noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec); noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01); + + // XYZ 5mm standard deviation, Rotation 0.0175 rad standard deviation (~1 degree) + // TODO: These are currently just estimates, update! + Vector6 noise_model_rel_meas_vec; + noise_model_rel_meas_vec << 0.005, 0.005, 0.005, 0.0175, 0.0175, 0.0175; + noise_model_rel_meas = noiseModel::Diagonal::Sigmas(noise_model_rel_meas_vec); start_time_ = ros::Time::now(); } @@ -89,43 +95,6 @@ void SemanticFactorGraph::addKeyPoseAndBetween( // bool use_msckf_cov = false; // Pose3 poseRelative = poseFrom.between(poseEstimate); - // No longer used, getting cov from VIO - // -------------------------------- - ////if (cov) { - //// Matrix6 M = - //// Eigen::Map>(cov->data()); / boost::shared_ptr - ///noise_model_odom = / noiseModel::Gaussian::Covariance(M); / - ///fgraph.add(BetweenFactor(getSymbol(robotID, prevIdx), / - ///getSymbol(robotID, curIdx), poseRelative, / noise_model_odom)); - ////} else { - //// fgraph.add(BetweenFactor(getSymbol(robotID, prevIdx), - //// getSymbol(robotID, curIdx), - ///poseRelative, / noise_model_pose)); - ////} - ////// here the insert also set the initial estimate for the optimization - //// fvalues.insert(getSymbol(robotID, curIdx), poseTo); - // double mins_elapsed; - // if (start_timestamp == 0.0) { - // mins_elapsed = 0.0; - // } else { - // mins_elapsed = (ros::Time::now().toSec() - start_timestamp) / 60.0; - // } - // if (noise_model_pose_inflation > 0) { - // printf( - // "+++++++ inflating covariance with time, if you are playing a bag, " - // "make sure that /use_sim_time is set as true and play bag with " - // "--clock flag \n"); - - // printf("odom_factor_noise_std_inflation_per_min is set as: \n"); - // std::cout << noise_model_pose_inflation << std::endl; - // printf( - // "number of mins since the first " - // "odometry factor: \n"); - // std::cout << mins_elapsed << std::endl; - // } - // -------------------------------- - Vector6 cur_noise_vec; // disable the inflation of covariance @@ -334,6 +303,23 @@ void SemanticFactorGraph::addLoopClosureFactor(const Pose3 poseRelative, noise_model_closure)); } +/** + * @brief Adds a Relative Inter-Robot Measurement Factor to the factor graph. + * + * @param poseRelative - Transformation from Robot #fromRobotID to Robot #toRobotID + * @param fromIdx - + */ +void SemanticFactorGraph::addRelativeMeasurementFactor(const Pose3 poseRelative, + const size_t fromIdx, const size_t fromRobotID, + const size_t toIdx, const size_t toRobotID) { + + // TODO: Scale the noise model based on the relative distance + fgraph.add(BetweenFactor(getSymbol(fromRobotID, fromIdx), + getSymbol(toRobotID, toIdx), poseRelative, + noise_model_rel_meas)); + +} + void SemanticFactorGraph::solve() { // ROS_INFO_STREAM("START SOLVING THE FACTOR GRAPH OPTIMIZATION PROBLEM"); isam->update(fgraph, fvalues); diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index fd222df..e44f698 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -77,7 +77,7 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( // set priors for the first pose setPriors(curr_pose, robotID); ROS_WARN_STREAM( - "EROOR: Factor graph optimization is done when adding the first " + "ERROR: Factor graph optimization is done when adding the first " "pose prior, this may cause problems!!!"); } else { // if cov is not specified, deafult covariance will be used, see diff --git a/backend/sloam_msgs/CMakeLists.txt b/backend/sloam_msgs/CMakeLists.txt index 8487b8a..6e99122 100755 --- a/backend/sloam_msgs/CMakeLists.txt +++ b/backend/sloam_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ add_message_files(DIRECTORY msg FILES cylinderMap.msg MultiArrayPoseObjectEdges.msg poseObjectEdges.msg + RelativeInterRobotMeasurement.msg ROSCube.msg PoseMst.msg PoseMstBundle.msg diff --git a/backend/sloam_msgs/msg/PoseMst.msg b/backend/sloam_msgs/msg/PoseMst.msg index 0bddc77..4ea02f1 100755 --- a/backend/sloam_msgs/msg/PoseMst.msg +++ b/backend/sloam_msgs/msg/PoseMst.msg @@ -1,5 +1,6 @@ geometry_msgs/Pose pose # 56 bytes geometry_msgs/Pose relativeRawOdom # 56 bytes +time stamp # 8 bytes (theorized, but not proven) ROSCube[] cubes # 69 bytes each ROSCylinder[] cylinders # 37 bytes each ROSEllipsoid[] ellipsoids # 69 bytes each \ No newline at end of file diff --git a/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg b/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg new file mode 100755 index 0000000..79dd2c8 --- /dev/null +++ b/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg @@ -0,0 +1,11 @@ +# A message containing relative inter-robot measurements +# +# relativePose contains the relative pose difference from the +# current robot to robot with index "robotId". header contains +# the timestamp of this measurement. odometry contains the +# closest synced odometry measurement. + +std_msgs/Header header +geometry_msgs/Pose relativePose +nav_msgs/Odometry odometry +int8 robotId \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml b/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml new file mode 100755 index 0000000..5d8ef21 --- /dev/null +++ b/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml @@ -0,0 +1,24 @@ +# Description: Parameters for the infer_node.py file +#------------------------------------------------- +# namespace: namespace used by the segmented point cloud publisher to publish under + +# model_dir: path to the directory containing the trained RangeNet++ model. The path should end with a '/' +# Also make sure that all model files do not have a .zip extension. If they do, then edit the file names and just remove the .zip extension. + +# desired_frequency: frequency at which inference should be performed by the infer_node.py file. +# This can be used to skip inferences on some point clouds to speed up operations especially when running on a CPU. + +# num_cpu_threads: number of CPU threads to be used by PyTorch to perform CPU inference. + +# pc_range_threshold: threshold for the range of the point cloud in meters. If the range of the points in the point cloud is greater than this threshold, +# those points are considered to be out of range and the out_of_range_default_position parameter is used as the position of those points. + +# pc_point_step: length of a point in bytes in the point cloud. +#------------------------------------------------- +namespace: /os_node +model_dir: /opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/rangenet++/penn_smallest/ +desired_frequency: 2 +num_cpu_threads: 10 +pc_range_threshold: 200 +pc_point_step: 48 +out_of_range_default_position: [0, 0, 0] \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml b/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml new file mode 100755 index 0000000..1aa7ac7 --- /dev/null +++ b/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml @@ -0,0 +1,50 @@ +# Description: Parameters for the process_cloud_node_outdoor.py file + +#------------------Important Parameters START------------------ +# threshold for segmented lidar points range in meters +valid_range_threshold: 200.0 +# expected segmentation frequency in Hz +expected_segmentation_frequency: 2.0 +# Minimum time in seconds to track a cuboid for before cuboid fitting +time_to_initialize_cuboid: 3.0 +# TODO(ankit): Change the use_1st_layer_clustering param names to something more intuitive +# DBSCAN parameters for clustering the segmented point cloud +# The first layer of clustering is done to throw away small noisy points. Hence the parameters need to be conservative +# The second layer of clustering is done to cluster the remaining points into objects. Hence the parameters can be more aggressive +use_1st_layer_clustering: True +use_2nd_layer_clustering: True +epsilon_scan_1st_layer: 0.1 +min_samples_scan_1st_layer: 7 +epsilon_scan_2nd_layer: 0.5 +min_samples_scan_2nd_layer: 25 +# Flag to determine whether to estimate facing direction of car by comparing front and rear heights +estimate_facing_dir_car: False +# Flag to determine whether to cluster and fix cuboid orientation by taking a consensus of the orientations of the cuboids in the cluster +cluster_and_fix_cuboid_orientation: True +# Flag for running KITTI dataset benchmarking code +run_kitti: False +# input point cloud parameters +pc_width: 512 +pc_height: 64 +pc_point_step: 64 +#------------------Important Parameters END------------------ + +# Minimum number of ground points required to perform RANSAC and fit a plane +ransac_n_points: 50 +# the height above the ground plane to consider a point as an object point. All points below this height are considered as ground points and not used for object fitting +ground_median_increment: 0.25 +# use simulator or run on real robot data. SHOULD ALWAYS BE FALSE for running on real robot data +use_sim: False +# visualize all cuboid instances (keep track of all and never remove tracks), should be set as False unless debugging +track_always_visualize: False +# Cuboid dim threshold used to start initial object tracking process +fit_cuboid_dim_thresh: 0.5 +# Resolution to perform voxel downsampling for accumulated instance point cloud. -1 means no downsample +downsample_res: -1 +# only keep the most recent num_points_limit_per_instance for any segmented point cloud instance while tracking it +num_instance_point_lim: 10000 +# time to delete a track if it is not updated for a long time +time_to_delete_lost_track_cuboid: 45 +# save figures of object clustering results from DBSCAN +visualize_DBSCAN_results: False +output_dir_to_save_figs: "./" diff --git a/frontend/scan2shape/scan2shape_launch/launch/infer_node.launch b/frontend/scan2shape/scan2shape_launch/launch/infer_node.launch index 1e74e15..f21f9b7 100755 --- a/frontend/scan2shape/scan2shape_launch/launch/infer_node.launch +++ b/frontend/scan2shape/scan2shape_launch/launch/infer_node.launch @@ -1,7 +1,9 @@ + + - + \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_with_ns.launch b/frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_with_ns.launch index a1fafe8..d2d89db 100755 --- a/frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_with_ns.launch +++ b/frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_with_ns.launch @@ -3,10 +3,11 @@ + - + diff --git a/frontend/scan2shape/script/infer_node.py b/frontend/scan2shape/script/infer_node.py index 0adbad3..e396e60 100755 --- a/frontend/scan2shape/script/infer_node.py +++ b/frontend/scan2shape/script/infer_node.py @@ -147,7 +147,18 @@ def pc_cb(self, msg): # points_xyz.shape[0], -1)[:, 1].astype(np.float32) # ---------------------------------------------------------- + #undistorted_pc = None + # try: undistorted_pc = ros_numpy.numpify(msg) + # except ValueError as e: + # rospy.loginfo_throttle(10, f"Height: {msg.height}") + # rospy.loginfo_throttle(10, f"width: {msg.width}") + # rospy.loginfo_throttle(10, f"Point Step: {msg.point_step}") + # rospy.loginfo_throttle(10, f"Row Step: {msg.row_step}") + # rospy.loginfo_throttle(10, f"Buffer Size: {len(msg.data)}") + # rospy.logerr_throttle(10, f"Error with numpify: {e}") + # return + self.pc_width = undistorted_pc['x'].flatten().shape[0] self.pc_height = 1 points_xyz = np.zeros((undistorted_pc['x'].flatten().shape[0], 3)) From 07b813eb50727fd0ce020717d60cb1a8ffaa448d Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Fri, 21 Feb 2025 17:16:43 +0000 Subject: [PATCH 03/74] Fix fields order bug and pointcloud width bug --- frontend/scan2shape/script/infer_node.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/frontend/scan2shape/script/infer_node.py b/frontend/scan2shape/script/infer_node.py index 0adbad3..2a0b17f 100755 --- a/frontend/scan2shape/script/infer_node.py +++ b/frontend/scan2shape/script/infer_node.py @@ -147,6 +147,15 @@ def pc_cb(self, msg): # points_xyz.shape[0], -1)[:, 1].astype(np.float32) # ---------------------------------------------------------- + def fix_pointcloud2_order(msg: PointCloud2) -> PointCloud2: + """Reorders the fields of a PointCloud2 message by offset.""" + msg.fields = sorted(msg.fields, key=lambda f: f.offset) + return msg + + # faster-lio doesn't return fields in order of offset, so reorder + # for ros_numpy.numpify() + msg = fix_pointcloud2_order(msg) + undistorted_pc = ros_numpy.numpify(msg) self.pc_width = undistorted_pc['x'].flatten().shape[0] self.pc_height = 1 @@ -223,7 +232,7 @@ def pc_cb(self, msg): if self.gpu_: torch.cuda.synchronize() - # Output probability instead of hard classification + # RangeNet++ outputs probability instead of hard classification, so convert to labels proj_argmax = proj_output[0].argmax(dim=0) # threshold out of range points @@ -257,6 +266,7 @@ def pc_cb(self, msg): pred_np_range_image = pred_np_range_image.reshape((-1)) proj_xyz_range_image = proj_xyz.cpu().numpy() proj_xyz_range_image = proj_xyz_range_image.reshape((-1, 3)) + # threshold out of points pred_np_range_image[np.linalg.norm( proj_xyz_range_image, axis=1) > self.range_threshold] = 0 @@ -273,9 +283,9 @@ def pc_cb(self, msg): pc_msg.header.frame_id = "body" rospy.logwarn_throttle( 30, "Segmented point cloud is currently hardcoded to be published in \"body\" frame. Please change it to the correct frame if needed.") - pc_msg.width = self.pc_width + pc_msg.width = 65536 #self.pc_width pc_msg.height = self.pc_height - pc_msg.point_step = self.pc_point_step + pc_msg.point_step = 16 #self.pc_point_step pc_msg.row_step = pc_msg.width * pc_msg.point_step pc_msg.fields = self.pc_fields_ pc_msg.data = full_data_range_image.tobytes() From 4d1788c9f13400b248f8b2a8946e6af49a33fef1 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Sat, 22 Feb 2025 05:10:50 +0000 Subject: [PATCH 04/74] Relative Meas Package added, Dockerfile with AprilTag --- .../script/tmux_CoPeD.sh | 6 +- .../sloam/launch/decentralized_sloam.launch | 2 +- .../rviz/decentralized_sloam_CoPeD.rviz | 1155 +++++++++++++++++ .../single_robot_sloam_test_outdoor.launch | 2 +- backend/sloam/src/core/robot.cpp | 4 +- docker/Dockerfile | 20 + docker/build_image.sh | 1 + ...lide_slam_docker.sh => enter_container.sh} | 0 ..._slide_slam_docker.sh => run_container.sh} | 6 +- frontend/relative_meas_gen/CMakeLists.txt | 213 +++ .../bash_scripts/tmux_relative_meas.sh | 63 + .../launch/dummyRelMeas.launch | 8 + frontend/relative_meas_gen/package.xml | 82 ++ .../relative_meas_gen/scripts/dummyRelMeas.py | 28 + .../CoPeD/infer_node_params_wilbur.yaml | 2 +- .../process_cloud_node_params_wilbur.yaml | 4 +- 16 files changed, 1585 insertions(+), 11 deletions(-) create mode 100644 backend/sloam/launch/rviz/decentralized_sloam_CoPeD.rviz create mode 100644 docker/Dockerfile create mode 100644 docker/build_image.sh rename docker/{open_slide_slam_docker.sh => enter_container.sh} (100%) rename docker/{create_slide_slam_docker.sh => run_container.sh} (84%) create mode 100644 frontend/relative_meas_gen/CMakeLists.txt create mode 100755 frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh create mode 100644 frontend/relative_meas_gen/launch/dummyRelMeas.launch create mode 100644 frontend/relative_meas_gen/package.xml create mode 100755 frontend/relative_meas_gen/scripts/dummyRelMeas.py diff --git a/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh b/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh index 42dd547..e81048e 100755 --- a/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh +++ b/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh @@ -51,16 +51,20 @@ tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shap tmux select-pane -t $SESSION_NAME:1.4 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wilburOusterOS1-64.yaml" Enter tmux select-pane -t $SESSION_NAME:1.5 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore tmux new-window -t $SESSION_NAME -n "roscore" tmux split-window -h -t $SESSION_NAME +tmux split-window -v -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:2.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter tmux select-pane -t $SESSION_NAME:2.1 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter +tmux select-pane -t $SESSION_NAME:2.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; rosrun image_transport republish compressed in:=/wilbur/stereo_left/image_rect_color raw out:=/wilbur/stereo_left/image_rect_color/uncompressed _image_transport:=compressed +" Enter # Add window to easily kill all processes tmux new-window -t $SESSION_NAME -n "Kill" diff --git a/backend/sloam/launch/decentralized_sloam.launch b/backend/sloam/launch/decentralized_sloam.launch index 4b9bb68..a1bff06 100755 --- a/backend/sloam/launch/decentralized_sloam.launch +++ b/backend/sloam/launch/decentralized_sloam.launch @@ -10,7 +10,7 @@ - + diff --git a/backend/sloam/launch/rviz/decentralized_sloam_CoPeD.rviz b/backend/sloam/launch/rviz/decentralized_sloam_CoPeD.rviz new file mode 100644 index 0000000..6af628e --- /dev/null +++ b/backend/sloam/launch/rviz/decentralized_sloam_CoPeD.rviz @@ -0,0 +1,1155 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /robot01/centroid map1 + - /robot01/Odometry1 + - /robot01/MarkerArray1 + - /robot01/cylinder map1 + - /robot01/MarkerArray2 + - /robot01/MarkerArray3 + - /robot01/MarkerArray4 + - /robot01/MarkerArray5 + - /robot01/CubeMap1 + - /robot01/raw_detections1 + - /robot11/Odometry1 + - /robot11/Cylinders Map1 + - /robot11/MarkerArray1 + - /robot11/MarkerArray2 + - /robot11/MarkerArray3 + - /robot11/MarkerArray4 + - /robot11/CubeMap1 + - /robot21/MarkerArray1 + - /robot21/MarkerArray2 + - /robot21/CylinderMap1 + - /robot21/EllipMap1 + - /robot21/MarkerArray3 + - /robot21/Odometry1 + - /robot21/CubeMap1 + - /robot31/cylinder map1 + - /robot31/CubeMap1 + - /robot41/MarkerArray1 + - /robot41/MarkerArray2 + - /robot41/Semantic Map1 + - /robot41/MarkerArray3 + - /robot41/MarkerArray4 + - /robot41/MarkerArray5 + - /robot41/MarkerArray6 + - /robot41/MarkerArray7 + - /robot51/MarkerArray1 + - /robot51/MarkerArray2 + - /robot51/Semantic Map1 + - /robot51/MarkerArray3 + - /robot51/MarkerArray4 + - /robot51/MarkerArray5 + - /robot51/MarkerArray6 + - /robot51/MarkerArray7 + - /robot51/MarkerArray8 + - /robot51/MarkerArray9 + - /robot61/Odometry1 + - /robot61/MarkerArray2 + - /robot61/Semantic Map1 + - /robot61/MarkerArray3 + - /robot61/MarkerArray4 + - /robot61/MarkerArray5 + - /robot61/MarkerArray6 + - /robot61/MarkerArray7 + - /robot61/MarkerArray8 + - /robot61/MarkerArray9 + - /PointCloud22 + - /PointCloud23 + Splitter Ratio: 0.39753738045692444 + Tree Height: 1083 + - 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: PointCloud2 + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Grid1 + - /PointCloud22/Autocompute Value Bounds1 + Splitter Ratio: 0.5 + Tree Height: 275 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.30000001192092896 + 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: 1000 + Reference Frame: + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_0/sloam/optimized_point_landmarks + Name: centroid map + Namespaces: + {} + Queue Size: 100 + Value: true + - Angle Tolerance: 1 + 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: 10 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /robot0/sloam_odom_high_freq + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot0/trajectory + Name: MarkerArray + Namespaces: + points_and_lines: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_0/sloam/cylinders_map + Name: cylinder map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /sloam0/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_0/sloam/cubes_map + Name: CubeMap + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot0/car_cuboids + Name: raw_detections + Namespaces: + car: true + text: true + Queue Size: 100 + Value: true + Enabled: true + Name: robot0 + - Class: rviz/Group + Displays: + - 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: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /robot1/sloam_odom_high_freq + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_1/sloam/cylinders_map + Name: Cylinders Map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_1/sloam/optimized_point_landmarks + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam1/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam1/robot0/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam1/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_1/sloam/cubes_map + Name: CubeMap + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: robot1 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam2/robot0/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam2/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_2/sloam/cylinders_map + Name: CylinderMap + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_2/sloam/optimized_point_landmarks + Name: EllipMap + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam2/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 2 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /robot2/odom + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_2/sloam/cubes_map + Name: CubeMap + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: robot2 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_3/sloam/optimized_point_landmarks + Name: centroid map + Namespaces: + {} + Queue Size: 100 + Value: true + - Angle Tolerance: 1 + 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: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /robot3/odom + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam3/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_3/sloam/cylinders_map + Name: cylinder map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_3/sloam/cubes_map + Name: CubeMap + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /robot0/chair_cuboids + Name: raw_detections + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: false + Name: robot3 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_4/sloam/optimized_point_landmarks + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Angle Tolerance: 1 + 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: false + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /quadrotor0/sloam_odom_high_freq + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot0/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_4/sloam/cylinders_map + Name: Semantic Map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_4/sloam/cubes_map + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot5/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam4/robot6/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: robot4 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_5/sloam/optimized_point_landmarks + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Angle Tolerance: 1 + 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: false + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /quadrotor0/sloam_odom_high_freq + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot0/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_5/sloam/cylinders_map + Name: Semantic Map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_5/sloam/cubes_map + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot5/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam5/robot6/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: robot5 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /robot_6/sloam/optimized_point_landmarks + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Angle Tolerance: 1 + 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: false + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /quadrotor0/sloam_odom_high_freq + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot0/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_6/sloam/cylinders_map + Name: Semantic Map + Namespaces: + {} + Queue Size: 2 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_6/sloam/cubes_map + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot5/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam6/robot6/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: robot6 + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_7/sloam/optimized_point_landmarks + Name: centroid map + Namespaces: + {} + Queue Size: 100 + Value: true + - Angle Tolerance: 1 + 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: Odometry + Position Tolerance: 0.10000000149011612 + 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: Axes + Topic: /robot7/sloam_odom_high_freq + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam7/robot7/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_7/sloam/submap_cylinder_models + Name: cylinder map + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot2/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot1/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot3/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sloam0/robot4/trajectory + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_7/sloam/cubes_map + Name: CubeMap + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /robot0/chair_cuboids + Name: raw_detections + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: false + Name: robot7 + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: 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: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /robot0/pc_instance_segmentation_accumulated + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.6147842407226562 + Min Value: -8.741279602050781 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 136; 138; 133 + Color Transformer: Intensity + Decay Time: 10 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /robot0/tree_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: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /robot0/filtered_semantic_segmentation + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /wilbur/stereo_left/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: quadrotor/map + Frame Rate: 30 + Name: rootquadrotor/map + 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/ThirdPersonFollower + Distance: 166.09930419921875 + 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: -13.022757530212402 + Y: 3.2681148052215576 + Z: 6.961444341868628e-06 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.1902037113904953 + Target Frame: body + Yaw: 5.415400505065918 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3700 + X: 140 + Y: 54 diff --git a/backend/sloam/launch/single_robot_sloam_test_outdoor.launch b/backend/sloam/launch/single_robot_sloam_test_outdoor.launch index c82aaa4..ea234d0 100755 --- a/backend/sloam/launch/single_robot_sloam_test_outdoor.launch +++ b/backend/sloam/launch/single_robot_sloam_test_outdoor.launch @@ -2,7 +2,7 @@ - + diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index efbb9cd..854dcbe 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -44,8 +44,8 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { std::string observationSub_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/semantic_meas_sync_odom"; RobotObservationSub_ = nh_.subscribe(observationSub_topic, 10, &Robot::RobotObservationCb, this); - bool turn_off_rel_inter_robot_loop_closure = nh_.param(node_name+"/turn_off_rel_inter_robot_loop_closure", true); - if (!turn_off_rel_inter_robot_loop_closure) { + bool turn_off_rel_inter_robot_factor = nh_.param(node_name+"/turn_off_rel_inter_robot_factor", true); + if (!turn_off_rel_inter_robot_factor) { std::string relativeMeasSub_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/relative_inter_robot_meas"; RobotRelativeMeasSub_ = nh_.subscribe(relativeMeasSub_topic, 10, &Robot::RobotRelativeMeasCb, this); } diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..4ab849f --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,20 @@ +FROM xurobotics/slide-slam:latest + +# Install dependencies +RUN apt-get update && apt-get install -y \ + cmake \ + git \ + libopencv-dev \ + lsof \ + && rm -rf /var/lib/apt/lists/* + +# Clone and build AprilTag +WORKDIR /opt +RUN git clone https://github.com/AprilRobotics/apriltag.git && \ + cd apriltag && \ + cmake -B build -DCMAKE_BUILD_TYPE=Release && \ + cmake --build build --target install && \ + ldconfig + +# Set working directory back to default +WORKDIR /app diff --git a/docker/build_image.sh b/docker/build_image.sh new file mode 100644 index 0000000..e7429b7 --- /dev/null +++ b/docker/build_image.sh @@ -0,0 +1 @@ +docker build -t slideslam_ros_image . \ No newline at end of file diff --git a/docker/open_slide_slam_docker.sh b/docker/enter_container.sh similarity index 100% rename from docker/open_slide_slam_docker.sh rename to docker/enter_container.sh diff --git a/docker/create_slide_slam_docker.sh b/docker/run_container.sh similarity index 84% rename from docker/create_slide_slam_docker.sh rename to docker/run_container.sh index e9b773f..00401bb 100755 --- a/docker/create_slide_slam_docker.sh +++ b/docker/run_container.sh @@ -1,6 +1,6 @@ SlideSlamWs="/home/dbutterfield3/slideslam_docker_ws" # point to your workspace directory -SlideSlamCodeDir="/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM" # point to your code directory where you cloned the repository -BAGS_DIR='/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM' # point to your bags / data directory +SlideSlamCodeDir="/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM +BAGS_DIR='/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM/bags' # point to your bags / data directory xhost +local:root # for the lazy and reckless docker run -it \ @@ -18,6 +18,6 @@ docker run -it \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ - xurobotics/slide-slam:latest \ + slideslam_ros_image \ bash diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt new file mode 100644 index 0000000..28f52de --- /dev/null +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 3.0.2) +project(relative_meas_gen) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + message_generation + message_runtime + nav_msgs + roscpp + rospy + sloam_msgs + std_msgs + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(apriltag) +find_package(yaml-cpp) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# nav_msgs# sloam_msgs# std_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES relative_meas_gen +# CATKIN_DEPENDS apriltag gtest message_generation message_runtime nav_msgs roscpp rospy sloam_msgs std_msgs std_msgs yaml-cpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} + ${apriltag_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/relative_meas_gen.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/relative_meas_gen_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_relative_meas_gen.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh new file mode 100755 index 0000000..54e5744 --- /dev/null +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -0,0 +1,63 @@ +#!/bin/bash + +# Set parameters +SESSION_NAME=slide_slam_nodes +BAG_PLAY_RATE=0.5 +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/' +SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" + +# Set Display Settings +CURRENT_DISPLAY=${DISPLAY} +if [ -z ${DISPLAY} ]; +then + echo "DISPLAY is not set" + CURRENT_DISPLAY=:0 +fi + +# Start Tmux Session if not already in one +if [ -z ${TMUX} ]; +then + TMUX= tmux new-session -s $SESSION_NAME -d + echo "Starting new session." +else + echo "Already in tmux, leave it first." + exit +fi + +# Make mouse useful in copy mode +tmux setw -g mouse on + +# Setup the panes for the main window +tmux new-window -t $SESSION_NAME -n "Main" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -h -t $SESSION_NAME + +# Setup commands for main window +tmux select-pane -t $SESSION_NAME:1.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter +tmux select-pane -t $SESSION_NAME:1.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux select-layout -t $SESSION_NAME tiled + +# Add window for roscore +tmux new-window -t $SESSION_NAME -n "roscore" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:2.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter +tmux select-pane -t $SESSION_NAME:2.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter + +# Add window to easily kill all processes +tmux new-window -t $SESSION_NAME -n "Kill" +tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}" + +# Go back to the first window +tmux select-window -t $SESSION_NAME:1 +tmux -2 attach-session -t $SESSION_NAME diff --git a/frontend/relative_meas_gen/launch/dummyRelMeas.launch b/frontend/relative_meas_gen/launch/dummyRelMeas.launch new file mode 100644 index 0000000..65d1262 --- /dev/null +++ b/frontend/relative_meas_gen/launch/dummyRelMeas.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml new file mode 100644 index 0000000..7930008 --- /dev/null +++ b/frontend/relative_meas_gen/package.xml @@ -0,0 +1,82 @@ + + + relative_meas_gen + 0.0.0 + The relative_meas_gen package + Daniel Chase Butterfield + Cameron Jackson + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + apriltag + gtest + message_generation + nav_msgs + roscpp + rospy + sloam_msgs + std_msgs + yaml-cpp + apriltag + gtest + nav_msgs + roscpp + rospy + sloam_msgs + std_msgs + yaml-cpp + apriltag + gtest + message_runtime + nav_msgs + roscpp + rospy + sloam_msgs + std_msgs + yaml-cpp + + + + + + + + diff --git a/frontend/relative_meas_gen/scripts/dummyRelMeas.py b/frontend/relative_meas_gen/scripts/dummyRelMeas.py new file mode 100755 index 0000000..460206b --- /dev/null +++ b/frontend/relative_meas_gen/scripts/dummyRelMeas.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python + +import rospy +from sloam_msgs.msg import RelativeInterRobotMeasurement + +class DummyRelativeMeasGen: + def __init__(self): + # Publisher to the topic /relative_inter_robot_meas + self.pub = rospy.Publisher("/relative_inter_robot_meas", RelativeInterRobotMeasurement, queue_size=10) + + # Timer to call self.publish_measurement every 1 second (1 Hz) + self.timer = rospy.Timer(rospy.Duration(1), self.publish_measurement) + + def publish_measurement(self, event): + """Function that gets called periodically to publish data.""" + msg = RelativeInterRobotMeasurement() + self.pub.publish(msg) + +if __name__ == '__main__': + + # Initialize the node with given name + node_name = rospy.get_param("/dummy_rel_node_name", default="dummy_rel_meas") + rospy.init_node(node_name) + + # Run until shutdown + while not rospy.is_shutdown(): + rospy.spin() + rospy.loginfo("Shutting down {}...".format(node_name)) \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml b/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml index 5d8ef21..eb52b57 100755 --- a/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml +++ b/frontend/scan2shape/scan2shape_launch/config/CoPeD/infer_node_params_wilbur.yaml @@ -20,5 +20,5 @@ model_dir: /opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/rangenet++/penn_smallest desired_frequency: 2 num_cpu_threads: 10 pc_range_threshold: 200 -pc_point_step: 48 +pc_point_step: 16 out_of_range_default_position: [0, 0, 0] \ No newline at end of file diff --git a/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml b/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml index 1aa7ac7..9f47945 100755 --- a/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml +++ b/frontend/scan2shape/scan2shape_launch/config/CoPeD/process_cloud_node_params_wilbur.yaml @@ -24,9 +24,9 @@ cluster_and_fix_cuboid_orientation: True # Flag for running KITTI dataset benchmarking code run_kitti: False # input point cloud parameters -pc_width: 512 +pc_width: 1024 pc_height: 64 -pc_point_step: 64 +pc_point_step: 16 #------------------Important Parameters END------------------ # Minimum number of ground points required to perform RANSAC and fit a plane From 1fd12e3bda49a7a07777ab8b70d29bf74caf97ef Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Tue, 25 Feb 2025 20:42:52 +0000 Subject: [PATCH 05/74] IMU to LIDAR transform calculation --- .../scripts/calcTransforms.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 frontend/relative_meas_gen/scripts/calcTransforms.py diff --git a/frontend/relative_meas_gen/scripts/calcTransforms.py b/frontend/relative_meas_gen/scripts/calcTransforms.py new file mode 100644 index 0000000..66ac9ca --- /dev/null +++ b/frontend/relative_meas_gen/scripts/calcTransforms.py @@ -0,0 +1,32 @@ +import yaml +from scipy.spatial.transform import Rotation as R +import numpy as np + +# This function calculates the IMU-to-LIDAR transformation +# for the given extrinsics file (from CoPeD) +def main(): + with open('/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/CoPeD/calibration/wilbur/extrinsics.yaml', 'r') as file: + data = yaml.safe_load(file) + + # Construct transformation from sensor_plate_link to imu_link + sp_to_i = data['imu_joint'] + R_sp_to_i = R.from_euler('XYZ', [sp_to_i['roll'], sp_to_i['pitch'], sp_to_i['yaw']], degrees=False) + H_sp_to_i = np.eye(4) + H_sp_to_i[:3, :3] = R_sp_to_i.as_matrix() + H_sp_to_i[:3, 3] = np.array([sp_to_i['x'], sp_to_i['y'], sp_to_i['z']]) + + # Construct transformation from sensor_plate_link to ouster_center_link + sp_to_oc = data['ouster_center_joint'] + R_sp_to_oc = R.from_euler('XYZ', [sp_to_oc['roll'], sp_to_oc['pitch'], sp_to_oc['yaw']], degrees=False) + H_sp_to_oc = np.eye(4) + H_sp_to_oc[:3, :3] = R_sp_to_oc.as_matrix() + H_sp_to_oc[:3, 3] = np.array([sp_to_oc['x'], sp_to_oc['y'], sp_to_oc['z']]) + + # Construct transformation from imu_link to ouster_center_link + H_i_to_sp = np.linalg.inv(H_sp_to_i) + H_i_to_oc = H_sp_to_oc @ H_i_to_sp + print("Transformation from imu_link to ouster_center_link: ", H_i_to_oc) + + +if __name__ == "__main__": + main() \ No newline at end of file From 02618002d62081308df6e513e2c6a9bf1be726a8 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 25 Feb 2025 20:32:04 -0500 Subject: [PATCH 06/74] Initial commit for apriltag. Created slidetag .h and .cc files, created node .h and .cpp files. Skeleton code for slidetag.cc and node .cpp. Launch file created but not implemented. --- docker/build_image.sh | 0 docker/cam_desktop_run_container.sh | 23 ++++++ .../include/apriltag_meas_gen.h | 17 ++++ frontend/relative_meas_gen/include/slidetag.h | 23 ++++++ .../apriltag_measurement_generator.launch | 0 .../src/apriltag_meas_gen.cpp | 25 ++++++ frontend/relative_meas_gen/src/slidetag.cc | 80 +++++++++++++++++++ 7 files changed, 168 insertions(+) mode change 100644 => 100755 docker/build_image.sh create mode 100755 docker/cam_desktop_run_container.sh create mode 100644 frontend/relative_meas_gen/include/apriltag_meas_gen.h create mode 100644 frontend/relative_meas_gen/include/slidetag.h create mode 100644 frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch create mode 100644 frontend/relative_meas_gen/src/apriltag_meas_gen.cpp create mode 100644 frontend/relative_meas_gen/src/slidetag.cc diff --git a/docker/build_image.sh b/docker/build_image.sh old mode 100644 new mode 100755 diff --git a/docker/cam_desktop_run_container.sh b/docker/cam_desktop_run_container.sh new file mode 100755 index 0000000..6dc14f0 --- /dev/null +++ b/docker/cam_desktop_run_container.sh @@ -0,0 +1,23 @@ +SlideSlamWs="/home/cam/Documents/slideslam_docker_ws" # point to your workspace directory +SlideSlamCodeDir="/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM +BAGS_DIR='/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/bags' # point to your bags / data directory + +xhost +local:root # for the lazy and reckless +docker run -it \ + --name="slideslam_ros" \ + --net="host" \ + --privileged \ + --gpus="all" \ + --workdir="/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM" \ + --env="DISPLAY=$DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --env="XAUTHORITY=$XAUTH" \ + --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ + --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ + --volume="$BAGS_DIR:/opt/bags" \ + --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + --volume="/home/$USER/repos:/home/$USER/repos" \ + slideslam_ros_image \ + bash + diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h new file mode 100644 index 0000000..1c27fab --- /dev/null +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +class ApriltagMeasurer { + public: + explicit ApriltagMeasurer(ros::NodeHandle nh); + private: + ros::Subscriber robot_images; + ros::Publisher relative_meas; + + void imageCallback(/* Fill in message information */); + float /* Or RT matrix? */ getRelativeMeasurement(/* Some parameters (RT matrices?) */); + + ros::NodeHandle nh_; +} \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/slidetag.h new file mode 100644 index 0000000..6ac1cea --- /dev/null +++ b/frontend/relative_meas_gen/include/slidetag.h @@ -0,0 +1,23 @@ +#include "apriltag.h" +#include "tagStandard52h13.h" +#include "opencv2/opencv.hpp" +#include "opencv2/core.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" +#include +#include +#include + +class slidetag { + public: + int id; + double center[2]; + double corners[4][2]; + slidetag(int id, double center[2], double corners[4][2]); + slidetag(int id); + slidetag(); +}; + +std::map ExtractAprilTags(cv::Mat image); +cv::Mat MatFromImage(std::string path); diff --git a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch new file mode 100644 index 0000000..e69de29 diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp new file mode 100644 index 0000000..9b1ac9a --- /dev/null +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -0,0 +1,25 @@ +#include + +ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { + robot_images = nh_.subscribe(/* TOPIC NAME */, 1, &ApriltagMeasurer::imageCallback, this); + + relative_meas = nh_.advertise(/* TOPIC NAME */, 1); +} + +void ApriltagMeasurer::imageCallback(/* Fill in type */) { + + cv::Mat img = MatFromImage(/* Image Data */); + map = ExtractAprilTags(cv::Mat img /* Fill in camera parameters */); + + /* + TODO: + - Get information about which robot is looking + - Find which robot is being looked at + - Call relative measurement function + */ + +} + +float /* Or RT matrix? */ ApriltagMeasurer::getRelativeMeasurement(/* Probably RT matrices */) { + +} \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/slidetag.cc b/frontend/relative_meas_gen/src/slidetag.cc new file mode 100644 index 0000000..68f7b67 --- /dev/null +++ b/frontend/relative_meas_gen/src/slidetag.cc @@ -0,0 +1,80 @@ +#include "slidetag.h" + +slidetag::slidetag(int id) : + id (id) +{} + +slidetag::slidetag(int id, double center[2], double corners[4][2]) : + id (id) +{ + this->center[0] = center[0]; + this->center[1] = center[1]; + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 2; j++) { + this->corners[i][j] = corners[i][j]; + } + } +} + +slidetag::slidetag() {} + +std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float cx, float cy, float tag_size) { + + // Create usable image object from Mat + image_u8_t img_header = { .width = img.cols, + .height = img.rows, + .stride = img.cols, + .buf = img.data + }; + + image_u8_t * image = &img_header; + + // Create apriltag detector object and add desired tag family + apriltag_detector_t *td = apriltag_detector_create(); + apriltag_family_t *tf = tagStandard52h13_create(); + apriltag_detector_add_family(td, tf); + + // Detect apriltags + zarray_t *detections = apriltag_detector_detect(td, image); + + // Create tag object vector + std::map tags; + + // Iterate through detected apriltags + for (int i = 0; i < zarray_size(detections); i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + int id = det->id; + double center[2]; + double corners[4][2]; + + center[0] = det->c[0]; + center[1] = det->c[1]; + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 2; j++) { + corners[i][j] = det->p[i][j]; + } + } + + slidetag new_tag(id, center, corners); + tags[id] = new_tag; + } + + // Free detection objects + apriltag_detections_destroy(detections); + tagStandard52h13_destroy(tf); + apriltag_detector_destroy(td); + + return tags; +} + +cv::Mat MatFromImage(std::string path) { + //Undistort Image + cv::Mat matrix = cv::imread(path, cv::IMREAD_COLOR).clone(); + cv::Mat gray; + cv::cvtColor(matrix, gray, cv::COLOR_BGR2GRAY); + return gray; +} \ No newline at end of file From 308cbe3ecb7e71d58461344cd7e0eaa59138e051 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Wed, 26 Feb 2025 19:09:34 +0000 Subject: [PATCH 07/74] Test Case Structure for sloam, new msgs, unfinished progress on dummy publisher --- .../script/{ => CoPeD}/tmux_CoPeD.sh | 0 .../script/CoPeD/tmux_CoPeD_ground_aerial.sh | 72 ++++++++++++++ backend/sloam/CMakeLists.txt | 17 +++- backend/sloam/package.xml | 3 + .../src/tests/{ => deprecated}/core_test.cpp | 0 .../{ => deprecated}/cube_factor_test.cpp | 0 .../{ => deprecated}/cylinder_factor_test.cpp | 0 .../tests/{ => deprecated}/cylinder_test.cpp | 0 .../{ => deprecated}/loop_closure_test.cpp | 0 .../src/tests/{ => deprecated}/plane_test.cpp | 0 .../src/tests/inter_robot_factor_test.cpp | 11 +++ .../src/tests/place_recognition_test.cpp | 6 +- backend/sloam_msgs/CMakeLists.txt | 2 + backend/sloam_msgs/msg/OdometryGPSBundle.msg | 7 ++ .../msg/RelativeInterRobotMeasurement.msg | 10 +- .../msg/RelativeInterRobotMeasurementOdom.msg | 15 +++ docker/Dockerfile | 3 + frontend/relative_meas_gen/CMakeLists.txt | 3 +- .../launch/dummyRelMeas.launch | 14 ++- .../launch/odomGPSSync.launch | 17 ++++ frontend/relative_meas_gen/package.xml | 5 + frontend/relative_meas_gen/requirements.txt | 2 + .../scripts/calcTransforms.py | 32 ------- .../relative_meas_gen/scripts/dummyRelMeas.py | 95 ++++++++++++++++++- .../relative_meas_gen/scripts/odomGPSSync.py | 58 +++++++++++ 25 files changed, 321 insertions(+), 51 deletions(-) rename backend/multi_robot_utils_launch/script/{ => CoPeD}/tmux_CoPeD.sh (100%) create mode 100755 backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD_ground_aerial.sh rename backend/sloam/src/tests/{ => deprecated}/core_test.cpp (100%) rename backend/sloam/src/tests/{ => deprecated}/cube_factor_test.cpp (100%) rename backend/sloam/src/tests/{ => deprecated}/cylinder_factor_test.cpp (100%) rename backend/sloam/src/tests/{ => deprecated}/cylinder_test.cpp (100%) rename backend/sloam/src/tests/{ => deprecated}/loop_closure_test.cpp (100%) rename backend/sloam/src/tests/{ => deprecated}/plane_test.cpp (100%) create mode 100644 backend/sloam/src/tests/inter_robot_factor_test.cpp create mode 100644 backend/sloam_msgs/msg/OdometryGPSBundle.msg create mode 100755 backend/sloam_msgs/msg/RelativeInterRobotMeasurementOdom.msg create mode 100644 frontend/relative_meas_gen/launch/odomGPSSync.launch create mode 100644 frontend/relative_meas_gen/requirements.txt delete mode 100644 frontend/relative_meas_gen/scripts/calcTransforms.py create mode 100755 frontend/relative_meas_gen/scripts/odomGPSSync.py diff --git a/backend/multi_robot_utils_launch/script/tmux_CoPeD.sh b/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh similarity index 100% rename from backend/multi_robot_utils_launch/script/tmux_CoPeD.sh rename to backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh diff --git a/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD_ground_aerial.sh b/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD_ground_aerial.sh new file mode 100755 index 0000000..50e24ac --- /dev/null +++ b/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD_ground_aerial.sh @@ -0,0 +1,72 @@ +#!/bin/bash + +# Set parameters +SESSION_NAME=slide_slam_nodes +BAG_PLAY_RATE=0.5 +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/' +SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" + +# Set Display Settings +CURRENT_DISPLAY=${DISPLAY} +if [ -z ${DISPLAY} ]; +then + echo "DISPLAY is not set" + CURRENT_DISPLAY=:0 +fi + +# Start Tmux Session if not already in one +if [ -z ${TMUX} ]; +then + TMUX= tmux new-session -s $SESSION_NAME -d + echo "Starting new session." +else + echo "Already in tmux, leave it first." + exit +fi + +# Make mouse useful in copy mode +tmux setw -g mouse on + +# Setup the panes for the main window +tmux new-window -t $SESSION_NAME -n "Main" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -h -t $SESSION_NAME + +# Setup commands for main window +tmux select-pane -t $SESSION_NAME:1.0 +tmux select-pane -t $SESSION_NAME:1.1 +tmux select-pane -t $SESSION_NAME:1.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wandaOusterOS1-64.yaml odom_topic:=/robot0/Odometry" Enter +tmux select-pane -t $SESSION_NAME:1.3 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch relative_meas_gen odomGPSSync.launch gps_topic:=/wanda/gps slop:=0.1 odom_topic:=/robot0/Odometry" Enter +tmux select-pane -t $SESSION_NAME:1.4 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wanda_arl_outtdoor_2023_05_19_06_2023-05-19-16-27-46.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wanda/lidar_points /wanda/imu/data /wanda/gps /wanda/lidar_points:=/robot0/lidar_points /wanda/imu/data:=/robot0/imu/data" Enter +tmux select-pane -t $SESSION_NAME:1.5 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_race1_2023_05_19_04_29_PM_1.bag -r $BAG_PLAY_RATE -s 0" Enter +tmux select-layout -t $SESSION_NAME tiled + +# Add window for roscore +tmux new-window -t $SESSION_NAME -n "roscore" +tmux split-window -h -t $SESSION_NAME +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:2.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter +tmux select-pane -t $SESSION_NAME:2.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter +tmux select-pane -t $SESSION_NAME:2.2 + + +# Add window to easily kill all processes +tmux new-window -t $SESSION_NAME -n "Kill" +tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}" + +# Go back to the first window +tmux select-window -t $SESSION_NAME:1 +tmux -2 attach-session -t $SESSION_NAME diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 34a706b..55752c4 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -120,11 +120,18 @@ cc_binary( SRCS "src/tests/place_recognition_test.cpp" DEPS sloam_base sloam_core) -# TESTS -# It appears that these test cases are not up to date, and thus won't work on current code. + + +if(BUILD_TESTING) + enable_testing() + + cc_test( + NAME inter_robot_factor_test + SRCS "src/tests/inter_robot_factor_test.cpp" + DEPS sloam_base sloam_objects sloam_core GTest::GTest) + +# These test cases are not up to date, and thus won't work on current code. # -# if(BUILD_TESTING) -# enable_testing() # Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter) # cc_test( # NAME all_sloam_test @@ -138,4 +145,4 @@ cc_binary( # NAME cube_fg_test # SRCS "src/tests/cube_factor_test.cpp" # DEPS sloam_base sloam_factorgraph GTest::GTest) -# endif() \ No newline at end of file +endif() \ No newline at end of file diff --git a/backend/sloam/package.xml b/backend/sloam/package.xml index 6f1622c..ace73e7 100755 --- a/backend/sloam/package.xml +++ b/backend/sloam/package.xml @@ -36,6 +36,9 @@ loop_closure actionlib actionlib_msgs + rostest + gtest + diff --git a/backend/sloam/src/tests/core_test.cpp b/backend/sloam/src/tests/deprecated/core_test.cpp similarity index 100% rename from backend/sloam/src/tests/core_test.cpp rename to backend/sloam/src/tests/deprecated/core_test.cpp diff --git a/backend/sloam/src/tests/cube_factor_test.cpp b/backend/sloam/src/tests/deprecated/cube_factor_test.cpp similarity index 100% rename from backend/sloam/src/tests/cube_factor_test.cpp rename to backend/sloam/src/tests/deprecated/cube_factor_test.cpp diff --git a/backend/sloam/src/tests/cylinder_factor_test.cpp b/backend/sloam/src/tests/deprecated/cylinder_factor_test.cpp similarity index 100% rename from backend/sloam/src/tests/cylinder_factor_test.cpp rename to backend/sloam/src/tests/deprecated/cylinder_factor_test.cpp diff --git a/backend/sloam/src/tests/cylinder_test.cpp b/backend/sloam/src/tests/deprecated/cylinder_test.cpp similarity index 100% rename from backend/sloam/src/tests/cylinder_test.cpp rename to backend/sloam/src/tests/deprecated/cylinder_test.cpp diff --git a/backend/sloam/src/tests/loop_closure_test.cpp b/backend/sloam/src/tests/deprecated/loop_closure_test.cpp similarity index 100% rename from backend/sloam/src/tests/loop_closure_test.cpp rename to backend/sloam/src/tests/deprecated/loop_closure_test.cpp diff --git a/backend/sloam/src/tests/plane_test.cpp b/backend/sloam/src/tests/deprecated/plane_test.cpp similarity index 100% rename from backend/sloam/src/tests/plane_test.cpp rename to backend/sloam/src/tests/deprecated/plane_test.cpp diff --git a/backend/sloam/src/tests/inter_robot_factor_test.cpp b/backend/sloam/src/tests/inter_robot_factor_test.cpp new file mode 100644 index 0000000..93d00c2 --- /dev/null +++ b/backend/sloam/src/tests/inter_robot_factor_test.cpp @@ -0,0 +1,11 @@ +#include +#include + +TEST(InterRobotFactorTests, TestMeasurementCallback) { + Robot robot; +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/backend/sloam/src/tests/place_recognition_test.cpp b/backend/sloam/src/tests/place_recognition_test.cpp index e5b967b..b3837b9 100755 --- a/backend/sloam/src/tests/place_recognition_test.cpp +++ b/backend/sloam/src/tests/place_recognition_test.cpp @@ -1,9 +1,11 @@ #include +#include // Either use randomly generated objects or load objects from your own test data bool use_generated_objects = false; -std::string file_path_robot0 = "/home/sam/slideslam_ws/src/SLIDE_SLAM/backend/sloam/clipper_semantic_object/examples/data/robot0Map_indoor.txt"; -std::string file_path_robot1 = "/home/sam/slideslam_ws/src/SLIDE_SLAM/backend/sloam/clipper_semantic_object/examples/data/robot1Map_indoor.txt"; +std::string package_path = ros::package::getPath("sloam"); +std::string file_path_robot0 = package_path + "/clipper_semantic_object/examples/data/robot0Map_indoor.txt"; +std::string file_path_robot1 = package_path + "/clipper_semantic_object/examples/data/robot1Map_indoor.txt"; bool visualize_matching_results = true; // generate a set of 50 objects of various classes and span over a 100 m by 100 diff --git a/backend/sloam_msgs/CMakeLists.txt b/backend/sloam_msgs/CMakeLists.txt index 6e99122..ebab440 100755 --- a/backend/sloam_msgs/CMakeLists.txt +++ b/backend/sloam_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ add_message_files(DIRECTORY msg FILES ROSSubMap.msg ROSObservation.msg ObservationPair.msg + OdometryGPSBundle.msg LoopClosure.msg keyPoses.msg cubeMap.msg @@ -28,6 +29,7 @@ add_message_files(DIRECTORY msg FILES MultiArrayPoseObjectEdges.msg poseObjectEdges.msg RelativeInterRobotMeasurement.msg + RelativeInterRobotMeasurementOdom.msg ROSCube.msg PoseMst.msg PoseMstBundle.msg diff --git a/backend/sloam_msgs/msg/OdometryGPSBundle.msg b/backend/sloam_msgs/msg/OdometryGPSBundle.msg new file mode 100644 index 0000000..ef809f2 --- /dev/null +++ b/backend/sloam_msgs/msg/OdometryGPSBundle.msg @@ -0,0 +1,7 @@ +# This message combines a odometry message +# and a GPS message, syncronized with the timestamp +# in header. + +std_msgs/Header header +nav_msgs/Odometry odom +sensor_msgs/NavSatFix gps \ No newline at end of file diff --git a/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg b/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg index 79dd2c8..e088bc2 100755 --- a/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg +++ b/backend/sloam_msgs/msg/RelativeInterRobotMeasurement.msg @@ -1,11 +1,11 @@ # A message containing relative inter-robot measurements # # relativePose contains the relative pose difference from the -# current robot to robot with index "robotId". header contains -# the timestamp of this measurement. odometry contains the -# closest synced odometry measurement. +# current robot with index "robotIdObserver" to robot with +# index "robotIdObserved". header contains the timestamp of this +# measurement. std_msgs/Header header geometry_msgs/Pose relativePose -nav_msgs/Odometry odometry -int8 robotId \ No newline at end of file +int8 robotIdObserver +int8 robotIdObserved \ No newline at end of file diff --git a/backend/sloam_msgs/msg/RelativeInterRobotMeasurementOdom.msg b/backend/sloam_msgs/msg/RelativeInterRobotMeasurementOdom.msg new file mode 100755 index 0000000..f38f47f --- /dev/null +++ b/backend/sloam_msgs/msg/RelativeInterRobotMeasurementOdom.msg @@ -0,0 +1,15 @@ +# A message containing relative inter-robot measurements +# with synced odometry. +# +# relativePose contains the relative pose difference from the +# current robot with index "robotIdObserver" to robot with +# index "robotIdObserved". header contains the timestamp of this +# measurement. odometry contains the closest synced odometry +# measurement for each robot. + +std_msgs/Header header +geometry_msgs/Pose relativePose +int8 robotIdObserver +int8 robotIdObserved +nav_msgs/Odometry odometryObserver +nav_msgs/Odometry odometryObserved \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile index 4ab849f..b8f6bf9 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -16,5 +16,8 @@ RUN git clone https://github.com/AprilRobotics/apriltag.git && \ cmake --build build --target install && \ ldconfig +# Install pyproj into the python environment +RUN pip install pyproj=3.5.0 + # Set working directory back to default WORKDIR /app diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 28f52de..50d5de0 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -10,12 +10,13 @@ project(relative_meas_gen) find_package(catkin REQUIRED COMPONENTS message_generation message_runtime + message_filters nav_msgs roscpp rospy sloam_msgs std_msgs - std_msgs + sensor_msgs ) ## System dependencies are found with CMake's conventions diff --git a/frontend/relative_meas_gen/launch/dummyRelMeas.launch b/frontend/relative_meas_gen/launch/dummyRelMeas.launch index 65d1262..6ed6f3a 100644 --- a/frontend/relative_meas_gen/launch/dummyRelMeas.launch +++ b/frontend/relative_meas_gen/launch/dummyRelMeas.launch @@ -1,7 +1,19 @@ - + + + + + + + + + + + + + diff --git a/frontend/relative_meas_gen/launch/odomGPSSync.launch b/frontend/relative_meas_gen/launch/odomGPSSync.launch new file mode 100644 index 0000000..fe77f9d --- /dev/null +++ b/frontend/relative_meas_gen/launch/odomGPSSync.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 7930008..96ccc6d 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -55,6 +55,7 @@ sloam_msgs std_msgs yaml-cpp + sensor_msgs apriltag gtest nav_msgs @@ -63,6 +64,7 @@ sloam_msgs std_msgs yaml-cpp + sensor_msgs apriltag gtest message_runtime @@ -72,6 +74,9 @@ sloam_msgs std_msgs yaml-cpp + sensor_msgs + python-pyproj + message_filters diff --git a/frontend/relative_meas_gen/requirements.txt b/frontend/relative_meas_gen/requirements.txt new file mode 100644 index 0000000..7f7efa0 --- /dev/null +++ b/frontend/relative_meas_gen/requirements.txt @@ -0,0 +1,2 @@ +pyproj=3.5.0 +numpy=1.22.3 \ No newline at end of file diff --git a/frontend/relative_meas_gen/scripts/calcTransforms.py b/frontend/relative_meas_gen/scripts/calcTransforms.py deleted file mode 100644 index 66ac9ca..0000000 --- a/frontend/relative_meas_gen/scripts/calcTransforms.py +++ /dev/null @@ -1,32 +0,0 @@ -import yaml -from scipy.spatial.transform import Rotation as R -import numpy as np - -# This function calculates the IMU-to-LIDAR transformation -# for the given extrinsics file (from CoPeD) -def main(): - with open('/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/CoPeD/calibration/wilbur/extrinsics.yaml', 'r') as file: - data = yaml.safe_load(file) - - # Construct transformation from sensor_plate_link to imu_link - sp_to_i = data['imu_joint'] - R_sp_to_i = R.from_euler('XYZ', [sp_to_i['roll'], sp_to_i['pitch'], sp_to_i['yaw']], degrees=False) - H_sp_to_i = np.eye(4) - H_sp_to_i[:3, :3] = R_sp_to_i.as_matrix() - H_sp_to_i[:3, 3] = np.array([sp_to_i['x'], sp_to_i['y'], sp_to_i['z']]) - - # Construct transformation from sensor_plate_link to ouster_center_link - sp_to_oc = data['ouster_center_joint'] - R_sp_to_oc = R.from_euler('XYZ', [sp_to_oc['roll'], sp_to_oc['pitch'], sp_to_oc['yaw']], degrees=False) - H_sp_to_oc = np.eye(4) - H_sp_to_oc[:3, :3] = R_sp_to_oc.as_matrix() - H_sp_to_oc[:3, 3] = np.array([sp_to_oc['x'], sp_to_oc['y'], sp_to_oc['z']]) - - # Construct transformation from imu_link to ouster_center_link - H_i_to_sp = np.linalg.inv(H_sp_to_i) - H_i_to_oc = H_sp_to_oc @ H_i_to_sp - print("Transformation from imu_link to ouster_center_link: ", H_i_to_oc) - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/frontend/relative_meas_gen/scripts/dummyRelMeas.py b/frontend/relative_meas_gen/scripts/dummyRelMeas.py index 460206b..b1e47f6 100755 --- a/frontend/relative_meas_gen/scripts/dummyRelMeas.py +++ b/frontend/relative_meas_gen/scripts/dummyRelMeas.py @@ -1,26 +1,111 @@ #!/usr/bin/env python import rospy +import sensor_msgs.msg +import pyproj +import numpy as np +import geometry_msgs.msg +import sensor_msgs.msg +import message_filters from sloam_msgs.msg import RelativeInterRobotMeasurement class DummyRelativeMeasGen: def __init__(self): + # Get parameters + self.observer_frame = rospy.get_param("observing_robot_frame", default="wanda/base") + self.gps_topic_observer = rospy.get_param("gps_topic_observing_robot", default="/wanda/gps") + self.gps_topic_observed = rospy.get_param("gps_topic_observed_robot", default="/race1/mavros/global_position/raw/fix") + self.robot_id_observer = rospy.get_param("robot_id_observer", default=0) + self.robot_id_observed = rospy.get_param("robot_id_observed", default=1) + + # Subscribers to both GPS topics and both Odometry topics + sub_observer = message_filters.Subscriber(self.gps_topic_observer, sensor_msgs.msg.NavSatFix) + sub_observed = message_filters.Subscriber(self.gps_topic_observed, sensor_msgs.msg.NavSatFix) + self.ts = message_filters.ApproximateTimeSynchronizer([sub_observer, sub_observed], queue_size=100, slop=0.0625) + self.ts.registerCallback(self.publish_relative_meas) + # Publisher to the topic /relative_inter_robot_meas self.pub = rospy.Publisher("/relative_inter_robot_meas", RelativeInterRobotMeasurement, queue_size=10) - # Timer to call self.publish_measurement every 1 second (1 Hz) - self.timer = rospy.Timer(rospy.Duration(1), self.publish_measurement) + # Hold recieved messages + self.observer_msgs = [] + self.observed_msgs = [] + + # Keep track of messages sent + self.sequence_number = 0 + + # Calculate the relative measurement between two GPS messages, + # returning a Pose message with no orientation information + def calculate_relative_measurement(self, msg_observer: sensor_msgs.msg.NavSatFix, + msg_observed: sensor_msgs.msg.NavSatFix) -> geometry_msgs.msg.Pose: + + def geodetic_to_ecef(lat, lon, alt): + """ Convert latitude, longitude, and altitude to ECEF coordinates. """ + ecef = pyproj.Proj(proj='ecef') + geodetic = pyproj.Proj(proj='latlong', datum='WGS84') + x, y, z = pyproj.transform(geodetic, ecef, lon, lat, alt, radians=False) + return np.array([x, y, z]) + + def ecef_to_enu(ecef_ref, ecef_target, lat_ref, lon_ref): + """ Convert ECEF displacement to ENU (local XYZ) coordinates. """ + dx, dy, dz = ecef_target - ecef_ref # Compute displacement vector in ECEF + + # Convert reference lat/lon to radians + lat_ref, lon_ref = np.radians(lat_ref), np.radians(lon_ref) + + # Compute rotation matrix from ECEF to ENU + R = np.array([ + [-np.sin(lon_ref), np.cos(lon_ref), 0], + [-np.sin(lat_ref) * np.cos(lon_ref), -np.sin(lat_ref) * np.sin(lon_ref), np.cos(lat_ref)], + [np.cos(lat_ref) * np.cos(lon_ref), np.cos(lat_ref) * np.sin(lon_ref), np.sin(lat_ref)] + ]) - def publish_measurement(self, event): - """Function that gets called periodically to publish data.""" + enu = R @ np.array([dx, dy, dz]) # Apply rotation matrix + return enu # Returns (ΔX, ΔY, ΔZ) + + def gps_to_xyz(lat1, lon1, alt1, lat2, lon2, alt2): + """ Compute (X, Y, Z) transformation from GPS1 to GPS2 in meters. """ + ecef_ref = geodetic_to_ecef(lat1, lon1, alt1) + ecef_target = geodetic_to_ecef(lat2, lon2, alt2) + return ecef_to_enu(ecef_ref, ecef_target, lat1, lon1) + + xyz_transform = gps_to_xyz(msg_observer.latitude, msg_observer.longitude, msg_observer.altitude, + msg_observed.latitude, msg_observed.longitude, msg_observed.altitude) + + # Create the Pose message + msg = geometry_msgs.msg.Pose() + msg.position = geometry_msgs.msg.Point() + msg.position.x = xyz_transform[0] + msg.position.y = xyz_transform[1] + msg.position.z = xyz_transform[2] + msg.orientation = geometry_msgs.msg.Quaternion() # No orientation information is provided + return msg + + def publish_relative_meas(self, observer_msg, observed_msg): + # Check both messages to make sure the timestamps are within 0.01 seconds + observer_msg = self.observer_msgs[0] + observed_msg = self.observed_msgs[0] + timestamp_diff = observer_msg.header.stamp.to_sec() - observed_msg.header.stamp.to_sec() + rospy.logwarn("Timestamps of GPS messages are within %f", timestamp_diff) + + # Create a RelativeInterRobotMeasurement message msg = RelativeInterRobotMeasurement() + msg.header.seq = self.sequence_number + msg.header.stamp = observed_msg.header.stamp + msg.header.frame_id = self.observer_frame + msg.relativePose = self.calculate_relative_measurement(observer_msg, observed_msg) + msg.robotIdObserver = self.robot_id_observer + msg.robotIdObserved = self.robot_id_observed + + # Publish the message self.pub.publish(msg) + self.sequence_number += 1 if __name__ == '__main__': - # Initialize the node with given name node_name = rospy.get_param("/dummy_rel_node_name", default="dummy_rel_meas") rospy.init_node(node_name) + meas_gen = DummyRelativeMeasGen() # Run until shutdown while not rospy.is_shutdown(): diff --git a/frontend/relative_meas_gen/scripts/odomGPSSync.py b/frontend/relative_meas_gen/scripts/odomGPSSync.py new file mode 100755 index 0000000..01fd662 --- /dev/null +++ b/frontend/relative_meas_gen/scripts/odomGPSSync.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +import rospy +import sensor_msgs.msg +import numpy as np +import sensor_msgs.msg +import nav_msgs.msg +import message_filters +from sloam_msgs.msg import OdometryGPSBundle + +class OdomGPSSync: + def __init__(self): + # Get parameters + self.odom_topic = rospy.get_param("odom_topic", default="/Odometry") + self.gps_topic = rospy.get_param("gps_topic", default="/gps") + self.pub_topic = rospy.get_param("pub_topic", default="/odom_gps_sync") + self.slop = rospy.get_param("slop", default=0.1) + + # Synchronize the odometry and gps + sub_odom = message_filters.Subscriber(self.odom_topic, nav_msgs.msg.Odometry) + sub_gps = message_filters.Subscriber(self.gps_topic, sensor_msgs.msg.NavSatFix) + self.ts = message_filters.ApproximateTimeSynchronizer([sub_odom, sub_gps], queue_size=100, slop=self.slop) + self.ts.registerCallback(self.publish_sync) + + # Publisher + self.pub = rospy.Publisher(self.pub_topic, OdometryGPSBundle, queue_size=10) + + # Keep track of messages sent + self.sequence_number = 0 + + def publish_sync(self, msg_odom: nav_msgs.msg.Odometry, msg_gps: sensor_msgs.msg.NavSatFix): + # Create a new OdometryGPSBundle message + msg = OdometryGPSBundle() + + # Fill in the header + msg.header.seq = self.sequence_number + msg.header.stamp = msg_gps.header.stamp # Assume odometry at same timestamp as gps + msg.header.frame_id = "" + + # Fill in the message and publish + msg.odom = msg_odom + msg.gps = msg_gps + self.pub.publish(msg) + + # Increment the sequence number + self.sequence_number += 1 + + +if __name__ == '__main__': + # Initialize the node with given name + node_name = rospy.get_param("/odom_gps_sync_node_name", default="odom_gps_sync_node_name") + rospy.init_node(node_name) + sync = OdomGPSSync() + + # Run until shutdown + while not rospy.is_shutdown(): + rospy.spin() + rospy.loginfo("Shutting down {}...".format(node_name)) \ No newline at end of file From 27fccb5aae2bc36bc3c4031aeeb9b3541917acfa Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Thu, 27 Feb 2025 23:14:09 -0500 Subject: [PATCH 08/74] Implemented launch file, started config file, corrected errors in Node.cpp file --- .../include/apriltag_meas_gen.h | 1 + frontend/relative_meas_gen/include/slidetag.h | 3 +- .../apriltag_measurement_generator.launch | 11 ++++++ .../params/relative_meas.yaml | 34 +++++++++++++++++++ .../src/apriltag_meas_gen.cpp | 18 +++++----- frontend/relative_meas_gen/src/slidetag.cc | 18 ++++++++-- 6 files changed, 74 insertions(+), 11 deletions(-) create mode 100644 frontend/relative_meas_gen/params/relative_meas.yaml diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 1c27fab..631eb63 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -2,6 +2,7 @@ #include #include +#include class ApriltagMeasurer { public: diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/slidetag.h index 6ac1cea..7c0af5d 100644 --- a/frontend/relative_meas_gen/include/slidetag.h +++ b/frontend/relative_meas_gen/include/slidetag.h @@ -1,4 +1,5 @@ #include "apriltag.h" +#include "apriltag_pose.h" #include "tagStandard52h13.h" #include "opencv2/opencv.hpp" #include "opencv2/core.hpp" @@ -14,7 +15,7 @@ class slidetag { int id; double center[2]; double corners[4][2]; - slidetag(int id, double center[2], double corners[4][2]); + slidetag(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation); slidetag(int id); slidetag(); }; diff --git a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch index e69de29..3a6f9e7 100644 --- a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/params/relative_meas.yaml b/frontend/relative_meas_gen/params/relative_meas.yaml new file mode 100644 index 0000000..d233ef4 --- /dev/null +++ b/frontend/relative_meas_gen/params/relative_meas.yaml @@ -0,0 +1,34 @@ + +wilbur: + camera_zero: + image_width: 1440 + image_height: 1080 + fx: 1903.520006386324 + cx: 681.5060629940748 + fy: 1895.619818759923 + cy: 517.1128313302568 + rt: + data: [1794.791137695312, 0, 676.5970053006567, 0, 0, 1833.858642578125, 515.3424035623175, 0, 0, 0, 1, 0] + camera_one: + image_width: 1440 + image_height: 1080 + fx: 1836.368323539261 + cx: 722.5409244592852 + fy: 1827.351416645816 + cy: 524.0180259791498 + rt: + data: [1725.12744140625, 0, 723.5165065089095, 0, 0, 1763.703735351562, 522.8930017468156, 0, 0, 0, 1, 0] + tags: + [ + {id: 5, size: 0.17, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}, + {id: 1, size: 0.17, x: -0.3347, y: -0.1445, z: -0.2081, qw: 0.9995, qx: -0.0261, qy: 0.0134, qz: -0.0109}, + {id: 0, size: 0.17, x: -0.3341, y: 0.1647, z: -0.3509, qw: 0.9605, qx: -0.2777, qy: 0.0006, qz: 0.0179}, + {id: 7, size: 0.17, x: 0.5010, y: 0.0086, z: -0.0020, qw: 0.9942, qx: -0.0323, qy: -0.0954, qz: 0.0386}, + {id: 4, size: 0.17, x: -0.0021, y: 0.2142, z: -0.1197, qw: 0.9493, qx: -0.3140, qy: -0.0075, qz: 0.0082}, + {id: 2, size: 0.17, x: 0.9277, y: -0.0993, z: -0.1765, qw: 0.9916, qx: -0.0935, qy: -0.0818, qz: 0.0364}, + {id: 3, size: 0.17, x: 0.9130, y: 0.2295, z: -0.3135, qw: 0.9669, qx: -0.2477, qy: -0.0606, qz: -0.0124}, + {id: 6, size: 0.17, x: 0.4940, y: 0.2477, z: -0.0743, qw: 0.9572, qx: -0.2750, qy: -0.0895, qz: -0.0070} + ] + +wanda: + diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 9b1ac9a..373b4bf 100644 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -1,15 +1,9 @@ #include -ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { - robot_images = nh_.subscribe(/* TOPIC NAME */, 1, &ApriltagMeasurer::imageCallback, this); - - relative_meas = nh_.advertise(/* TOPIC NAME */, 1); -} - void ApriltagMeasurer::imageCallback(/* Fill in type */) { - cv::Mat img = MatFromImage(/* Image Data */); - map = ExtractAprilTags(cv::Mat img /* Fill in camera parameters */); + //cv::Mat img = MatFromImage(/* Image Data */); + //map = ExtractAprilTags(cv::Mat img /* Fill in camera parameters */); /* TODO: @@ -17,7 +11,15 @@ void ApriltagMeasurer::imageCallback(/* Fill in type */) { - Find which robot is being looked at - Call relative measurement function */ + std::cout << "I recieved an image!" << std::endl; + +} + +ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { + robot_images = nh_.subscribe("/wilbur_left/", 1, &ApriltagMeasurer::imageCallback, this); + relative_meas = nh.advertise("/relative_meas/", 10); + std::cout << "Hello World from Apriltag" << std::endl; } float /* Or RT matrix? */ ApriltagMeasurer::getRelativeMeasurement(/* Probably RT matrices */) { diff --git a/frontend/relative_meas_gen/src/slidetag.cc b/frontend/relative_meas_gen/src/slidetag.cc index 68f7b67..e6e930c 100644 --- a/frontend/relative_meas_gen/src/slidetag.cc +++ b/frontend/relative_meas_gen/src/slidetag.cc @@ -4,12 +4,15 @@ slidetag::slidetag(int id) : id (id) {} -slidetag::slidetag(int id, double center[2], double corners[4][2]) : +slidetag::slidetag(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation) : id (id) { this->center[0] = center[0]; this->center[1] = center[1]; + this->rotation = *rotation; + this->translation = *translation; + for (int i = 0; i < 4; i++) { for (int j = 0; j < 2; j++) { this->corners[i][j] = corners[i][j]; @@ -50,6 +53,17 @@ std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float double center[2]; double corners[4][2]; + apriltag_detection_info_t info; + info.det = det; + info.tagsize = tag_size; + info.fx = fx; + info.fy = fy; + info.cx = cx; + info.cy = cy; + + apriltag_pose_t pose; + estimate_tag_pose(&info, &pose); + center[0] = det->c[0]; center[1] = det->c[1]; @@ -59,7 +73,7 @@ std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float } } - slidetag new_tag(id, center, corners); + slidetag new_tag(id, center, corners, pose.R, pose.t); tags[id] = new_tag; } From 7f42bfae66db4bede9246d93097e053fd67abe44 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Fri, 28 Feb 2025 16:28:24 -0500 Subject: [PATCH 09/74] Got Forest rosbags working - ascertained that there is an issue with my launch file --- docker/cam_desktop_run_container.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/cam_desktop_run_container.sh b/docker/cam_desktop_run_container.sh index 6dc14f0..bb542a9 100755 --- a/docker/cam_desktop_run_container.sh +++ b/docker/cam_desktop_run_container.sh @@ -8,7 +8,7 @@ docker run -it \ --net="host" \ --privileged \ --gpus="all" \ - --workdir="/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM" \ + --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ --env="QT_X11_NO_MITSHM=1" \ --env="XAUTHORITY=$XAUTH" \ From c1e57b42b78908f2c192fb17f264d2bc940b82d1 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 3 Mar 2025 13:36:53 -0500 Subject: [PATCH 10/74] Successfully built and launched relative measurement node --- frontend/relative_meas_gen/CMakeLists.txt | 20 +++++++++++++++---- .../include/apriltag_meas_gen.h | 8 +++++--- frontend/relative_meas_gen/include/slidetag.h | 5 ++++- .../apriltag_measurement_generator.launch | 4 +++- .../params/relative_meas.yaml | 2 -- .../src/apriltag_meas_gen.cpp | 16 +++++++++++---- .../src/{slidetag.cc => slidetag.cpp} | 0 7 files changed, 40 insertions(+), 15 deletions(-) mode change 100644 => 100755 frontend/relative_meas_gen/src/apriltag_meas_gen.cpp rename frontend/relative_meas_gen/src/{slidetag.cc => slidetag.cpp} (100%) diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 28f52de..2660ec8 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -20,9 +20,13 @@ find_package(catkin REQUIRED COMPONENTS ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(apriltag) +find_package(PkgConfig) +pkg_search_module(apriltag REQUIRED apriltag) +find_package(OpenCV REQUIRED) find_package(yaml-cpp) +set(apriltag_INCLUDE_DIRS "${apriltag_INCLUDE_DIRS}/apriltag") + ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html @@ -108,12 +112,14 @@ find_package(yaml-cpp) ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( +# catkin_package( # INCLUDE_DIRS include -# LIBRARIES relative_meas_gen +# LIBRARIES relative_measdd_executable(apriltag_measurer src/apriltag_meas_gen.cpp) +#target_link_libraries(apriltag_measurer ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +#_gen # CATKIN_DEPENDS apriltag gtest message_generation message_runtime nav_msgs roscpp rospy sloam_msgs std_msgs std_msgs yaml-cpp # DEPENDS system_lib -) +#) ########### ## Build ## @@ -124,9 +130,15 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ${apriltag_INCLUDE_DIRS} + include ) +add_executable(ApriltagMeasurer src/apriltag_meas_gen.cpp src/slidetag.cpp) +set_target_properties(ApriltagMeasurer PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) +target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES}) + ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/relative_meas_gen.cpp diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 631eb63..05c64f2 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -1,8 +1,10 @@ #pragma once -#include +#include #include #include +#include + class ApriltagMeasurer { public: @@ -11,8 +13,8 @@ class ApriltagMeasurer { ros::Subscriber robot_images; ros::Publisher relative_meas; - void imageCallback(/* Fill in message information */); + void imageCallback(const sensor_msgs::ImageConstPtr& msg); float /* Or RT matrix? */ getRelativeMeasurement(/* Some parameters (RT matrices?) */); ros::NodeHandle nh_; -} \ No newline at end of file +}; \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/slidetag.h index 7c0af5d..eb9a602 100644 --- a/frontend/relative_meas_gen/include/slidetag.h +++ b/frontend/relative_meas_gen/include/slidetag.h @@ -15,10 +15,13 @@ class slidetag { int id; double center[2]; double corners[4][2]; + matd_t rotation; + matd_t translation; + slidetag(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation); slidetag(int id); slidetag(); }; -std::map ExtractAprilTags(cv::Mat image); +std::map ExtractAprilTags(cv::Mat image); cv::Mat MatFromImage(std::string path); diff --git a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch index 3a6f9e7..677cd73 100644 --- a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch @@ -1,11 +1,13 @@ + + - + \ No newline at end of file diff --git a/frontend/relative_meas_gen/params/relative_meas.yaml b/frontend/relative_meas_gen/params/relative_meas.yaml index d233ef4..50ba920 100644 --- a/frontend/relative_meas_gen/params/relative_meas.yaml +++ b/frontend/relative_meas_gen/params/relative_meas.yaml @@ -30,5 +30,3 @@ wilbur: {id: 6, size: 0.17, x: 0.4940, y: 0.2477, z: -0.0743, qw: 0.9572, qx: -0.2750, qy: -0.0895, qz: -0.0070} ] -wanda: - diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp old mode 100644 new mode 100755 index 373b4bf..849d9ba --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -1,6 +1,6 @@ -#include +#include "apriltag_meas_gen.h" -void ApriltagMeasurer::imageCallback(/* Fill in type */) { +void ApriltagMeasurer::imageCallback(const sensor_msgs::ImageConstPtr& msg) { //cv::Mat img = MatFromImage(/* Image Data */); //map = ExtractAprilTags(cv::Mat img /* Fill in camera parameters */); @@ -16,12 +16,20 @@ void ApriltagMeasurer::imageCallback(/* Fill in type */) { } ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { - robot_images = nh_.subscribe("/wilbur_left/", 1, &ApriltagMeasurer::imageCallback, this); + robot_images = nh_.subscribe("/wilbur_left", 1, &ApriltagMeasurer::imageCallback, this); - relative_meas = nh.advertise("/relative_meas/", 10); + relative_meas = nh_.advertise("/relative_meas/", 10); std::cout << "Hello World from Apriltag" << std::endl; } +int main(int argc, char** argv) { + ros::init(argc, argv, "apriltag_node"); + ros::NodeHandle nh; + ApriltagMeasurer measurer(nh); + ros::spin(); + return 0; +} + float /* Or RT matrix? */ ApriltagMeasurer::getRelativeMeasurement(/* Probably RT matrices */) { } \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/slidetag.cc b/frontend/relative_meas_gen/src/slidetag.cpp similarity index 100% rename from frontend/relative_meas_gen/src/slidetag.cc rename to frontend/relative_meas_gen/src/slidetag.cpp From d291d778300a06167b34b867652163f0c4c75791 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 5 Mar 2025 20:51:53 -0500 Subject: [PATCH 11/74] Implemented inter-robot measurement except for transformation calculation --- frontend/relative_meas_gen/CMakeLists.txt | 4 +- .../bash_scripts/tmux_relative_meas.sh | 2 +- .../include/apriltag_meas_gen.h | 14 +- frontend/relative_meas_gen/include/slidetag.h | 8 +- .../apriltag_measurement_generator.launch | 13 +- .../params/relative_meas.yaml | 231 +++++++++++++++++- .../src/apriltag_meas_gen.cpp | 116 +++++++-- frontend/relative_meas_gen/src/slidetag.cpp | 28 ++- 8 files changed, 369 insertions(+), 47 deletions(-) diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 2660ec8..4c3df99 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(PkgConfig) pkg_search_module(apriltag REQUIRED apriltag) find_package(OpenCV REQUIRED) find_package(yaml-cpp) +find_package(Eigen3 REQUIRED) set(apriltag_INCLUDE_DIRS "${apriltag_INCLUDE_DIRS}/apriltag") @@ -132,12 +133,13 @@ include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${apriltag_INCLUDE_DIRS} + ${EIGEN_INCLUDE_DIR} include ) add_executable(ApriltagMeasurer src/apriltag_meas_gen.cpp src/slidetag.cpp) set_target_properties(ApriltagMeasurer PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) -target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES}) +target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES} yaml-cpp) ## Declare a C++ library # add_library(${PROJECT_NAME} diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh index 54e5744..28bc497 100755 --- a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -43,7 +43,7 @@ tmux split-window -h -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:1.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r 4.0 -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 05c64f2..ebdd0a4 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -2,8 +2,10 @@ #include #include -#include -#include +#include +#include +#include +#include class ApriltagMeasurer { @@ -12,9 +14,13 @@ class ApriltagMeasurer { private: ros::Subscriber robot_images; ros::Publisher relative_meas; + std::string robot_ID; + std::string camera_ID; + float intrinsics[4]; + YAML::Node config; - void imageCallback(const sensor_msgs::ImageConstPtr& msg); - float /* Or RT matrix? */ getRelativeMeasurement(/* Some parameters (RT matrices?) */); + void imageCallback(const sensor_msgs::CompressedImage msg); + void publishRelativeMeasurement(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q); ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/slidetag.h index eb9a602..8389dcb 100644 --- a/frontend/relative_meas_gen/include/slidetag.h +++ b/frontend/relative_meas_gen/include/slidetag.h @@ -1,13 +1,13 @@ #include "apriltag.h" #include "apriltag_pose.h" -#include "tagStandard52h13.h" +#include "tag36h11.h" #include "opencv2/opencv.hpp" #include "opencv2/core.hpp" #include "opencv2/imgcodecs.hpp" #include "opencv2/highgui.hpp" #include "opencv2/imgproc.hpp" +#include #include -#include #include class slidetag { @@ -23,5 +23,5 @@ class slidetag { slidetag(); }; -std::map ExtractAprilTags(cv::Mat image); -cv::Mat MatFromImage(std::string path); +std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4]); +cv::Mat MatFromImage(const sensor_msgs::CompressedImage msg); diff --git a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch index 677cd73..9b113e7 100644 --- a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch @@ -1,13 +1,18 @@ - + + + + + - - - + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/params/relative_meas.yaml b/frontend/relative_meas_gen/params/relative_meas.yaml index 50ba920..00702f6 100644 --- a/frontend/relative_meas_gen/params/relative_meas.yaml +++ b/frontend/relative_meas_gen/params/relative_meas.yaml @@ -1,22 +1,50 @@ - -wilbur: - camera_zero: +robot0: + name: "wilbur" + camera0: image_width: 1440 image_height: 1080 fx: 1903.520006386324 cx: 681.5060629940748 fy: 1895.619818759923 cy: 517.1128313302568 - rt: + x: 0.01869175488738828 + y: 0.0193 + z: 0.28672623687109766 + pitch: -0.005559586035104693 + roll: 0.0004926383242970126 + yaw: 0.04223758430352057 + R: + - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] + - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] + - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] + distortion_model: plumb_bob + distortion_coefficients: [-0.3798306402200569, 0.1959307510959587, 0.0004038542688319737, 0.000190820656519979, 0] + projection_matrix: + rows: 3 + cols: 4 data: [1794.791137695312, 0, 676.5970053006567, 0, 0, 1833.858642578125, 515.3424035623175, 0, 0, 0, 1, 0] - camera_one: + camera1: image_width: 1440 image_height: 1080 fx: 1836.368323539261 cx: 722.5409244592852 fy: 1827.351416645816 cy: 524.0180259791498 - rt: + x: 0.01869175488738828 + y: -0.5607127105911416 + z: 0.28672623687109766 + pitch: -0.005559586035104693 + roll: 0.0004926383242970126 + yaw: 0.04223758430352057 + R: + - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] + - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] + - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] + distortion_model: plumb_bob + distortion_coefficients: [-0.3831901211400326, 0.2233720519081885, 0.0008931300229381834, 0.001068644642920918, 0] + projection_matrix: + rows: 3 + cols: 4 data: [1725.12744140625, 0, 723.5165065089095, 0, 0, 1763.703735351562, 522.8930017468156, 0, 0, 0, 1, 0] tags: [ @@ -30,3 +58,194 @@ wilbur: {id: 6, size: 0.17, x: 0.4940, y: 0.2477, z: -0.0743, qw: 0.9572, qx: -0.2750, qy: -0.0895, qz: -0.0070} ] +robot1: + name: "wanda" + camera0: + image_width: 1440 + image_height: 1080 + fx: 1192.45276 + cx: 739.3438 + fy: 1190.6091 + cy: 565.7661 + x: 0.01869175488738828 + y: 0.0193 + z: 0.28672623687109766 + pitch: -0.005559586035104693 + roll: 0.0004926383242970126 + yaw: 0.04223758430352057 + R: + - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] + - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] + - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] + distortion_model: plumb_bob + distortion_coefficients: [-0.423121, 0.173144, -0.003773, -0.003173, 0.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [ 980.67499, 0, 738.69215, 0, 0, 1076.08105, 566.97831, 0, 0, 0, 1, 0] + camera1: + image_width: 1440 + image_height: 1080 + fx: 1190.130507756544 + cx: 734.8934118917687 + fy: 1188.148243248166 + cy: 567.2677230574875 + x: 0.01869175488738828 + y: -0.5607127105911416 + z: 0.28672623687109766 + R: + - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] + - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] + - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] + pitch: -0.005559586035104693 + roll: 0.0004926383242970126 + yaw: 0.04223758430352057 + distortion_model: plumb_bob + distortion_coefficients: [-0.4284069539532191, 0.1788231827634255, -0.001863194586121232, -0.002264946112770172, 0] + projection_matrix: + rows: 3 + cols: 4 + data: [975.4818115234375, 0, 734.7269645256092, 0, 0, 1071.288208007812, 571.0164652120948, 0, 0, 0, 1, 0] + tags: + [ + {id: 8, size: 0.17, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}, + {id: 11, size: 0.17, x: 1.2491, y: -0.0277, z: -0.0171, qw: 0.9982, qx: 0.0506, qy: -0.0284, qz: -0.0142}, + {id: 12, size: 0.17, x: 0.3600, y: 0.1825, z: 0.2028, qw: 0.9979, qx: -0.0592, qy: -0.0271, qz: 0.0026}, + {id: 15, size: 0.17, x: 0.8473, y: 0.1637, z: 0.1694, qw: 0.9977, qx: -0.0551, qy: -0.0387, qz: -0.0077}, + {id: 10, size: 0.17, x: 1.2043, y: -0.3991, z: -0.0282, qw: 0.9745, qx: 0.2239, qy: -0.0158, qz: -0.0020}, + {id: 9, size: 0.17, x: -0.0364, y: -0.3277, z: -0.1005, qw: 0.9918, qx: 0.1261, qy: 0.0123, qz: -0.0167}, + {id: 14, size: 0.17, x: 0.7311, y: -0.5974, z: 0.0110, qw: 0.9379, qx: 0.3439, qy: 0.0269, qz: -0.0357}, + {id: 13, size: 0.17, x: 0.3355, y: -0.5865, z: 0.0061, qw: 0.9447, qx: 0.3228, qy: 0.0416, qz: -0.0409} + ] + +robot2: + name: "race1" + camera0: + image_width: 640 + image_height: 480 + fx: 384.81109268711907 + cx: 385.4589799319776 + fy: 322.6712393337711 + cy: 239.39991235441906 + x: -0.04402011600452833 + y: -0.0025601400106739355 + z: -0.017635245742563362 + R: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + distortion_model: radtan + distortion_coefficients: [0.0018802953849829409, -0.003652303767730166, 3.117507045293004e-05, 0.00048175416276617297] + T_cam_imu: + - [1.0, 0.0, 0.0, -0.04402011600452833] + - [0.0, 1.0, 0.0, -0.0025601400106739355] + - [0.0, 0.0, 1.0, -0.017635245742563362] + - [0.0, 0.0, 0.0, 1.0] + camera1: + image_width: 640 + image_height: 480 + fx: 384.88335095659886 + cx: 385.49905756574714 + fy: 322.59930315077474 + cy: 238.68360046050088 + x: 0.006021057482269526 + y: -0.0025213336541727834 + z: -0.01743269598887508 + R: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + distortion_model: radtan + distortion_coeffs: [0.0015836636822816295, -0.0033431633421065355, 8.541751669274196e-05, 0.0007597980170162216] + T_cam_imu: + - [1.0, 0.0, 0.0, 0.006021057482269526] + - [0.0, 1.0, 0.0, -0.0025213336541727834] + - [0.0, 0.0, 1.0, -0.01743269598887508] + - [0.0, 0.0, 0.0, 1.0] + camera2: + image_width: 640 + image_height: 360 + fx: 393.4307466416494 + cx: 393.5086802291898 + fy: 309.0745377295984 + cy: 191.40656978073883 + x: 0.017870661528472276 + y: 0.04670556113481232 + z: -0.0593976812355273 + R: + - [-0.9993877307510592, 0.01208522064239451, -0.03283460166161045] + - [-0.03321602617267533, -0.032844274749509045, 0.9989083787923088] + - [0.010993599480977004, 0.9993874128976246, 0.03322558821523644] + distortion_model: radtan + distortion_coeffs: [-0.3170375008962817, 0.08966709757315208, -0.001159278638250432, -0.0008883270498344137] + T_cam_imu: + - [-0.9993877307510592, 0.01208522064239451, -0.03283460166161045, 0.017870661528472276] + - [-0.03321602617267533, -0.032844274749509045, 0.9989083787923088, 0.04670556113481232] + - [0.010993599480977004, 0.9993874128976246, 0.03322558821523644, -0.0593976812355273] + - [0.0, 0.0, 0.0, 1.0] + +robot3: + name: "race5" + camera0: + image_width: 640 + image_height: 480 + fx: 384.81109268711907 + cx: 385.4589799319776 + fy: 322.6712393337711 + cy: 239.39991235441906 + x: -0.04402011600452833 + y: -0.0025601400106739355 + z: -0.017635245742563362 + R: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + distortion_model: radtan + distortion_coefficients: [0.0018802953849829409, -0.003652303767730166, 3.117507045293004e-05, 0.00048175416276617297] + T_cam_imu: + - [1.0, 0.0, 0.0, -0.04402011600452833] + - [0.0, 1.0, 0.0, -0.0025601400106739355] + - [0.0, 0.0, 1.0, -0.017635245742563362] + - [0.0, 0.0, 0.0, 1.0] + camera1: + image_width: 640 + image_height: 480 + fx: 384.88335095659886 + cx: 385.49905756574714 + fy: 322.59930315077474 + cy: 238.68360046050088 + x: 0.006021057482269526 + y: -0.0025213336541727834 + z: -0.01743269598887508 + R: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + distortion_model: radtan + distortion_coeffs: [0.0015836636822816295, -0.0033431633421065355, 8.541751669274196e-05, 0.0007597980170162216] + T_cam_imu: + - [1.0, 0.0, 0.0, 0.006021057482269526] + - [0.0, 1.0, 0.0, -0.0025213336541727834] + - [0.0, 0.0, 1.0, -0.01743269598887508] + - [0.0, 0.0, 0.0, 1.0] + camera2: + image_width: 640 + image_height: 360 + fx: 392.9550218529275 + cx: 392.940457525672 + fy: 360.07710092394854 + cy: 204.88425337040826 + x: 0.008269639602172082 + y: 0.06372375371998219 + z: -0.047292562071506984 + R: + - [-0.9985017232823171, -0.003349172206249803, 0.05461768621771697] + - [0.054621450353775636, -0.0010428563719976078, 0.9985065896687104] + - [-0.0032872121157798105, 0.9999938477291355, 0.0012242305279655254] + distortion_model: radtan + distortion_coeffs: [-0.3415545271161533, 0.1262106741471218, -0.00019798454412713818, -0.00048726522214063034] + T_cam_imu: + - [-0.9985017232823171, -0.003349172206249803, 0.05461768621771697, 0.008269639602172082] + - [0.054621450353775636, -0.0010428563719976078, 0.9985065896687104, 0.06372375371998219] + - [-0.0032872121157798105, 0.9999938477291355, 0.0012242305279655254, -0.047292562071506984] + - [0.0, 0.0, 0.0, 1.0] \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 849d9ba..1767e8d 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -1,25 +1,104 @@ #include "apriltag_meas_gen.h" -void ApriltagMeasurer::imageCallback(const sensor_msgs::ImageConstPtr& msg) { +void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { + + cv::Mat img = MatFromImage(msg); + if (img.empty()) { + std::cerr << "Error: Decoded image is empty!" << std::endl; + return; + } - //cv::Mat img = MatFromImage(/* Image Data */); - //map = ExtractAprilTags(cv::Mat img /* Fill in camera parameters */); + std::vector tags = ExtractAprilTags(img, this->intrinsics);\ - /* - TODO: - - Get information about which robot is looking - - Find which robot is being looked at - - Call relative measurement function - */ - std::cout << "I recieved an image!" << std::endl; + Eigen::Matrix4f bot_cam_RT; + YAML::Node R = config[robot_ID][camera_ID]["R"]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + bot_cam_RT(i, j) = R[i][j].as(); + } + } + bot_cam_RT(0, 3) = config[robot_ID][camera_ID]["x"].as(); + bot_cam_RT(1, 3) = config[robot_ID][camera_ID]["y"].as(); + bot_cam_RT(2, 3) = config[robot_ID][camera_ID]["z"].as(); + + for (slidetag t : tags) { + Eigen::Matrix4f cam_tag_RT; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + cam_tag_RT(i, j) = t.rotation.data[i + j]; + } + cam_tag_RT(3, i) = t.translation.data[i]; + } + std::cout << "Tag ID: " << t.id << std::endl; + + float x; + float y; + float z; + float qw; + float qx; + float qy; + float qz; + bool tag_on_bot = false; + for (auto it = config.begin(); it != config.end(); ++it) { + std::string key = it->first.as(); + if (config[key]["tags"].IsDefined()) { + YAML::Node tags = config[key]["tags"]; + for (auto tag = tags.begin(); tag != tags.end(); ++tag) { + if (t.id == (*tag)["id"].as()) { + x = (*tag)["x"].as(); + y = (*tag)["y"].as(); + z = (*tag)["z"].as(); + qw = (*tag)["qw"].as(); + qx = (*tag)["qx"].as(); + qy = (*tag)["qy"].as(); + qz = (*tag)["qz"].as(); + tag_on_bot = true; + break; + } + } + } + } + if (tag_on_bot) { + std::cout << "Detected tag does not belong to any robot" << std::endl; + } else { + Eigen::Vector3f bot_tag_T; + bot_tag_T << x, y, z; + Eigen::Quaternionf bot_tag_Q(qw, qx, qy, qz); + + publishRelativeMeasurement(bot_cam_RT, cam_tag_RT, bot_tag_T, bot_tag_Q); + + } + } } ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { - robot_images = nh_.subscribe("/wilbur_left", 1, &ApriltagMeasurer::imageCallback, this); + std::string image_topic; + nh_.param("apriltag_node/image_topic", image_topic, "/default_topic"); + + nh_.param("apriltag_node/host_robot", robot_ID, "robot0"); + + nh_.param("apriltag_node/camera_ID", camera_ID, "camera0"); + std::cout << "Subscribing to topic: " << image_topic << std::endl; + std::string config_file; + nh_.param("apriltag_node/config_file", config_file, "default.yaml"); + std::string return_topic; + nh_.param("apriltag_node/relative_meas", return_topic, "/default_topic"); - relative_meas = nh_.advertise("/relative_meas/", 10); - std::cout << "Hello World from Apriltag" << std::endl; + config = YAML::LoadFile(config_file); + + float fx = config[robot_ID][camera_ID]["fx"].as(); + float cx = config[robot_ID][camera_ID]["cx"].as(); + float fy = config[robot_ID][camera_ID]["fy"].as(); + float cy = config[robot_ID][camera_ID]["cy"].as(); + + intrinsics[0] = fx; + intrinsics[1] = cx; + intrinsics[2] = fy; + intrinsics[3] = cy; + + robot_images = nh_.subscribe(image_topic, 1, &ApriltagMeasurer::imageCallback, this); + relative_meas = nh_.advertise(return_topic, 10); } int main(int argc, char** argv) { @@ -30,6 +109,15 @@ int main(int argc, char** argv) { return 0; } -float /* Or RT matrix? */ ApriltagMeasurer::getRelativeMeasurement(/* Probably RT matrices */) { +void ApriltagMeasurer::publishRelativeMeasurement(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q) { + + geometry_msgs::Pose pose_msg; + + /* + Calculate pose + */ + + std::cout << "Publishing pose" << std::endl; + relative_meas.publish(pose_msg); } \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/slidetag.cpp b/frontend/relative_meas_gen/src/slidetag.cpp index e6e930c..521cbcd 100644 --- a/frontend/relative_meas_gen/src/slidetag.cpp +++ b/frontend/relative_meas_gen/src/slidetag.cpp @@ -22,7 +22,7 @@ slidetag::slidetag(int id, double center[2], double corners[4][2], matd_t *rotat slidetag::slidetag() {} -std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float cx, float cy, float tag_size) { +std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4]) {//float tag_size) { // Create usable image object from Mat image_u8_t img_header = { .width = img.cols, @@ -30,19 +30,18 @@ std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float .stride = img.cols, .buf = img.data }; - image_u8_t * image = &img_header; // Create apriltag detector object and add desired tag family apriltag_detector_t *td = apriltag_detector_create(); - apriltag_family_t *tf = tagStandard52h13_create(); + apriltag_family_t *tf = tag36h11_create(); apriltag_detector_add_family(td, tf); // Detect apriltags zarray_t *detections = apriltag_detector_detect(td, image); // Create tag object vector - std::map tags; + std::vector tags; // Iterate through detected apriltags for (int i = 0; i < zarray_size(detections); i++) { @@ -53,13 +52,15 @@ std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float double center[2]; double corners[4][2]; + float tag_size = .17; + apriltag_detection_info_t info; info.det = det; info.tagsize = tag_size; - info.fx = fx; - info.fy = fy; - info.cx = cx; - info.cy = cy; + info.fx = intrinsics[0]; + info.cx = intrinsics[1]; + info.fy = intrinsics[2]; + info.cy = intrinsics[3]; apriltag_pose_t pose; estimate_tag_pose(&info, &pose); @@ -74,21 +75,22 @@ std::map ExtractAprilTags(cv::Mat img, float fx, float fy, float } slidetag new_tag(id, center, corners, pose.R, pose.t); - tags[id] = new_tag; + tags.push_back(new_tag); } // Free detection objects apriltag_detections_destroy(detections); - tagStandard52h13_destroy(tf); apriltag_detector_destroy(td); + tag36h11_destroy(tf); return tags; } -cv::Mat MatFromImage(std::string path) { +cv::Mat MatFromImage(const sensor_msgs::CompressedImage msg) { //Undistort Image - cv::Mat matrix = cv::imread(path, cv::IMREAD_COLOR).clone(); + cv::Mat raw_data(1, msg.data.size(), CV_8UC1, (void*)msg.data.data()); + cv::Mat image = cv::imdecode(raw_data, cv::IMREAD_COLOR); cv::Mat gray; - cv::cvtColor(matrix, gray, cv::COLOR_BGR2GRAY); + cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY); return gray; } \ No newline at end of file From a8988d59e70e7e97a20197e21611e20d3cfae1a0 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Thu, 6 Mar 2025 22:22:33 -0500 Subject: [PATCH 12/74] relative measurement node for apriltags and apriltag test --- frontend/relative_meas_gen/CMakeLists.txt | 26 ++++++----- .../include/apriltag_meas_gen.h | 3 +- frontend/relative_meas_gen/include/slidetag.h | 2 +- .../launch/CoPeD_relative_meas_gen.launch | 12 +++++ .../launch/dummyRelMeas.launch | 8 ---- ... => relative_measurement_generator.launch} | 10 +++-- frontend/relative_meas_gen/package.xml | 4 ++ .../src/apriltag_meas_gen.cpp | 27 +++++++++--- frontend/relative_meas_gen/src/slidetag.cpp | 6 +-- .../relative_meas_gen/test/apriltag_test.cpp | 41 ++++++++++++++++++ .../relative_meas_gen/test/apriltag_test.test | 3 ++ .../test/apriltag_test_pic.jpg | Bin 0 -> 76710 bytes 12 files changed, 108 insertions(+), 34 deletions(-) create mode 100644 frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch delete mode 100644 frontend/relative_meas_gen/launch/dummyRelMeas.launch rename frontend/relative_meas_gen/launch/{apriltag_measurement_generator.launch => relative_measurement_generator.launch} (69%) create mode 100644 frontend/relative_meas_gen/test/apriltag_test.cpp create mode 100644 frontend/relative_meas_gen/test/apriltag_test.test create mode 100644 frontend/relative_meas_gen/test/apriltag_test_pic.jpg diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 4c3df99..b060ea3 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -13,11 +13,17 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs roscpp rospy + roslib + rostest sloam_msgs std_msgs std_msgs ) +catkin_package( + CATKIN_DEPENDS roscpp +) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(PkgConfig) @@ -134,6 +140,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ${apriltag_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIR} + ${roscpp_INCLUDE_DIRS} include ) @@ -193,16 +200,7 @@ target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apri ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" +# install(TARGEstd::string package_path = ros::package::getPath("relative_meas_gen");TCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) @@ -223,5 +221,13 @@ target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apri # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() +if (CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roscpp rostest) + + catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/slidetag.cpp) + target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) + target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES}) +endif() + ## Add folders to be run by python nosetests # catkin_add_nosetests(test) diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index ebdd0a4..8bdb1b8 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -17,10 +17,11 @@ class ApriltagMeasurer { std::string robot_ID; std::string camera_ID; float intrinsics[4]; + float tagsize; YAML::Node config; void imageCallback(const sensor_msgs::CompressedImage msg); - void publishRelativeMeasurement(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q); + void publishRelativeMeasurement(std::string, Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q); ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/slidetag.h index 8389dcb..82b8ecf 100644 --- a/frontend/relative_meas_gen/include/slidetag.h +++ b/frontend/relative_meas_gen/include/slidetag.h @@ -23,5 +23,5 @@ class slidetag { slidetag(); }; -std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4]); +std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4], float tagsize); cv::Mat MatFromImage(const sensor_msgs::CompressedImage msg); diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch new file mode 100644 index 0000000..9477eec --- /dev/null +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/dummyRelMeas.launch b/frontend/relative_meas_gen/launch/dummyRelMeas.launch deleted file mode 100644 index 65d1262..0000000 --- a/frontend/relative_meas_gen/launch/dummyRelMeas.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch similarity index 69% rename from frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch rename to frontend/relative_meas_gen/launch/relative_measurement_generator.launch index 9b113e7..5a2957c 100644 --- a/frontend/relative_meas_gen/launch/apriltag_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch @@ -3,15 +3,19 @@ - - - + + + + + + + diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 7930008..3247724 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -73,6 +73,10 @@ std_msgs yaml-cpp + rostest + gtest + + diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 1767e8d..49a8093 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -2,14 +2,16 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { + // Extract image cv::Mat img = MatFromImage(msg); if (img.empty()) { std::cerr << "Error: Decoded image is empty!" << std::endl; return; } - std::vector tags = ExtractAprilTags(img, this->intrinsics);\ + std::vector tags = ExtractAprilTags(img, this->intrinsics, this->tagsize);\ + // Load RT matrix from host bot -> camera from config Eigen::Matrix4f bot_cam_RT; YAML::Node R = config[robot_ID][camera_ID]["R"]; for (int i = 0; i < 3; i++) { @@ -21,7 +23,9 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { bot_cam_RT(1, 3) = config[robot_ID][camera_ID]["y"].as(); bot_cam_RT(2, 3) = config[robot_ID][camera_ID]["z"].as(); + // Publish transformation for each detected apriltag for (slidetag t : tags) { + // Obtain RT from camera -> detected apriltag Eigen::Matrix4f cam_tag_RT; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -38,13 +42,17 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { float qx; float qy; float qz; + std::string bot_id; bool tag_on_bot = false; + + // Obtain translation vector and quaternion from detected apriltag -> detected robot for (auto it = config.begin(); it != config.end(); ++it) { std::string key = it->first.as(); if (config[key]["tags"].IsDefined()) { YAML::Node tags = config[key]["tags"]; for (auto tag = tags.begin(); tag != tags.end(); ++tag) { if (t.id == (*tag)["id"].as()) { + bot_id = key; x = (*tag)["x"].as(); y = (*tag)["y"].as(); z = (*tag)["z"].as(); @@ -58,14 +66,15 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { } } } - if (tag_on_bot) { + if (!tag_on_bot) { std::cout << "Detected tag does not belong to any robot" << std::endl; } else { Eigen::Vector3f bot_tag_T; bot_tag_T << x, y, z; Eigen::Quaternionf bot_tag_Q(qw, qx, qy, qz); - publishRelativeMeasurement(bot_cam_RT, cam_tag_RT, bot_tag_T, bot_tag_Q); + // Calculate bot -> bot transformation and publish + publishRelativeMeasurement(bot_id, bot_cam_RT, cam_tag_RT, bot_tag_T, bot_tag_Q); } } @@ -73,11 +82,12 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { } ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { + + // Load params std::string image_topic; nh_.param("apriltag_node/image_topic", image_topic, "/default_topic"); - nh_.param("apriltag_node/host_robot", robot_ID, "robot0"); - + nh_.param("apriltag_node", tagsize, .17); nh_.param("apriltag_node/camera_ID", camera_ID, "camera0"); std::cout << "Subscribing to topic: " << image_topic << std::endl; std::string config_file; @@ -85,8 +95,10 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { std::string return_topic; nh_.param("apriltag_node/relative_meas", return_topic, "/default_topic"); + // Load config file config = YAML::LoadFile(config_file); + // Load host_robot properties float fx = config[robot_ID][camera_ID]["fx"].as(); float cx = config[robot_ID][camera_ID]["cx"].as(); float fy = config[robot_ID][camera_ID]["fy"].as(); @@ -97,6 +109,7 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { intrinsics[2] = fy; intrinsics[3] = cy; + // Instantiate sub and pub robot_images = nh_.subscribe(image_topic, 1, &ApriltagMeasurer::imageCallback, this); relative_meas = nh_.advertise(return_topic, 10); } @@ -109,7 +122,7 @@ int main(int argc, char** argv) { return 0; } -void ApriltagMeasurer::publishRelativeMeasurement(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q) { +void ApriltagMeasurer::publishRelativeMeasurement(std::string bot_id, Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q) { geometry_msgs::Pose pose_msg; @@ -117,7 +130,7 @@ void ApriltagMeasurer::publishRelativeMeasurement(Eigen::Matrix4f bot_to_cam_RT, Calculate pose */ - std::cout << "Publishing pose" << std::endl; + std::cout << "Publishing pose from host to " << bot_id << std::endl; relative_meas.publish(pose_msg); } \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/slidetag.cpp b/frontend/relative_meas_gen/src/slidetag.cpp index 521cbcd..a43949e 100644 --- a/frontend/relative_meas_gen/src/slidetag.cpp +++ b/frontend/relative_meas_gen/src/slidetag.cpp @@ -22,7 +22,7 @@ slidetag::slidetag(int id, double center[2], double corners[4][2], matd_t *rotat slidetag::slidetag() {} -std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4]) {//float tag_size) { +std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float tagsize) { // Create usable image object from Mat image_u8_t img_header = { .width = img.cols, @@ -52,11 +52,9 @@ std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4]) {//floa double center[2]; double corners[4][2]; - float tag_size = .17; - apriltag_detection_info_t info; info.det = det; - info.tagsize = tag_size; + info.tagsize = tagsize; info.fx = intrinsics[0]; info.cx = intrinsics[1]; info.fy = intrinsics[2]; diff --git a/frontend/relative_meas_gen/test/apriltag_test.cpp b/frontend/relative_meas_gen/test/apriltag_test.cpp new file mode 100644 index 0000000..19fcdd9 --- /dev/null +++ b/frontend/relative_meas_gen/test/apriltag_test.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include + +TEST(apriltag_test, Detect) { + + std::string path = "/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/frontend/relative_meas_gen/test/apriltag_test_pic.jpg"; + + //std::string package_path = ros::package::getPath("relative_meas_gen"); + //std::string path = package_path + "/test/apriltag_test_pic.jpg"; + + + std::cout << path << std::endl; + cv::Mat matrix = cv::imread(path, cv::IMREAD_COLOR).clone(); + std::cout << matrix.cols << std::endl; + cv::Mat image; + cv::cvtColor(matrix, image, cv::COLOR_BGR2GRAY); + + float intrinsics[4]; + intrinsics[0] = 1903.520006386324; + intrinsics[1] = 681.5060629940748; + intrinsics[2] = 1895.619818759923; + intrinsics[3] = 517.1128313302568; + + float tagsize = 0.17; + + std::vector tags = ExtractAprilTags(image, intrinsics, tagsize); + slidetag test_tag = tags[0]; + std::cout << "Detected tag: " << test_tag.id << std::endl; + std::cout << "Expected tag: 6" << std::endl; + + EXPECT_EQ(6, test_tag.id); + +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "apriltag_test"); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/frontend/relative_meas_gen/test/apriltag_test.test b/frontend/relative_meas_gen/test/apriltag_test.test new file mode 100644 index 0000000..53b4f37 --- /dev/null +++ b/frontend/relative_meas_gen/test/apriltag_test.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/test/apriltag_test_pic.jpg b/frontend/relative_meas_gen/test/apriltag_test_pic.jpg new file mode 100644 index 0000000000000000000000000000000000000000..4a8a3ed2395e647e03eb2813cbe0a2feb46befd9 GIT binary patch literal 76710 zcmbTcWl$YW)Gd5)cXxMpcXx+_ySuwf@Zj#w!8N#Z&>+DM65K5T0zm?~dA|GBt@^6& zue*BYPfyLNn(p4yyVqWSA%FJ)n9A}>@&G6(008P=1N{8}P{_EudU)A-d)wMjS_M)% zdDz;sdD&Y3Pv_rlKnwu({{S8y4jvv6;U5rDk^TcJ4k|k8zY7No7ZU>$7mJ(#508MH znv8^unwgG{nU|YaR@MaS|GW?T|3>h441k3QwFvDB14Rvh#)5*ug8DlFAOirPp<$r@ z`^-yP>m7n3zHVtgyNvqn?Dy^y(H|VyjYo&XxF>5& z{Lg-^{xK>60ZxXBu9&BP5j*wTew@aLZGQY4Yb2sgv z@<*B#KUyppQ6#Q5UWB98`h8{*8sS$RJIAmc`f6?MXzpZdzqtE$o}^=`9db)va2^~O zXkPNvCuarRd2FRYaaQj7t+49;o^BR}p_9d?qS7k6eoUS0(S~iJjXQlAY@Wao{`C&# z<}S|5Pse-)zdm^=xtMJ^htEO?7VwbSZuyyRBNJ#ubdNbksjCS zfb#f_qHD$!lZ-gRE&7Qp(Eiis2G#uN_{(n7TmrU!Xom*ay~WVqgLk^uiwnm_B}ZbF zGf*YvWCnCDEMZY=O}43tf39?|mRt$EGJK6SjpEC_^c~*{VG3k2glG3Z2I>p(S5JRH z8jUVOeRSIhhh(jP5B><=qpJM2b69NXqklF>k?+hBCV%-mG_#?9ztq%o75 zW@?(-$1>va;x7PUP*uWijihd;KabO*e#JnA>BPmSNOoNaAN|k!kKo;>zTfAf&+sMZ zNXcR>7GX|<$FNW&6tcO&AEhqpCk+}sml-Y%!B?SK=jU^pUO%@Py>&THn61SW@Dd#2 z;cVJi`4Q~ubl6<^)w%zp`#p{0HU5B~_$9B=7^DCTL^#|Cn!`?55;HKQUVc=ujc{&p z(q{+@KLvNL6GLilem3>}tbFI+G$a~UT*uEUo4o9N`7UMuwb|`2pd&W(?LW${sn1^? z7W3HzU{DY1lDHr0F(ZU#mo0)V_Ty2X*oTtjx!6lv<)H1 zlBK56IsJ5Dmi1QY1C63O^xyC;Ux}rNei`#a%eUwOd3_ytgaVSKu$*+`8S>FdrhKR9 zbRhF}31A^_6M9UfH(+%Epr9I3fuG@dYvL@g##|E!`X_{j#ou=S|NREv zTe+0h5aU4Ra~BmsYnuYaLy`Zlqb#FnpC|{f7{fo$4sKI8ms^~%TuNh)$@Gub-sZJfmX!dyrAEE+f@jyIow#=2RXu}cTmJVa; zl780g7RY->fzw#PR&*=;CKpPvW(D~97u zYQi<+=JJd`2lYlo?s>kuzFfFvv=@7yxOOY^RGa&eU|MY5cjJzBZ+t?#lw~gKJBHv9 zNk%n7SEjp2Bp%2a8?tO^`$$9Y;)F|2#3}X88_!2VbBpNK8=weJBv_mju6C94$rp?* zCgHJvD^18$2BlvWU9$*4GftiAM|MJ1trg4lb&Gv7osmITgWVN$b=@b9hAr&d68wf} zuEKNrUCCdI+3-863H-m>)YchuJ=v-9ny^r=`K zVXapO&$)@=*tm@VvVzx<9Qs|Ozpaj^YfWY*YU=8eLzO&hv%EgZc3+y z(!=RBfv8+d>DN$TUB9Y?%g=eTNPZ~+C52*c0gEjMPSX^wp~{I`^Gdlr>YvXZ?bA3yNSF=Fp2jiiIK5)YTV}yh zJa~m>Zd{az{bC|4Ja)=54{Wr{)F%<5zj&21GZ9$)KIyT0!&%p(LEvEATC=Pcx=mdc zHTen7Ph+Js%^A<^IBT$$CYpy4f)DpPIS4ZjjGV0|2*It@jfkky;2Rw-(Pb*rtSAa&$ z7NZD1;8TAK4kz+x7KS1>4~i=KlrbcCwi?gI+Z>p4?#~{4%8JmagP$KAOTF%4tRNUC zyhD~Od~1;|x<5(X=4261i~jY*`dKz3ZZyJmsTb+aq+duQTJcts`UEDd_?0afmQJNP z>JqX0`+G|ib_L@aKMWTU0`mbHL^a;9CXlKv_h_S^Vmgzg%{wHJo@Yd|?FPYX&9D~< zg(wXxBe~~vG79RqgraCA0f_wEHd{Hf47W)=z^Wj_PXAQscF)pbZ8!Bq;Gtj@euW zon9D{lTqdk9*?@c201mB5O)|dBkOPp_{ibTtCl?=n_X< z2EE*F(pmflnNl;-%ClI;N^0eJ1ym8NL++!G)cI&N5VV3&d$iqY*6mhT>#NUo@Nqvn@ziH>ga5I8SLUpkx%o|BA8|$)u zmcJMWcJ&~jG=+hUjM@&|8hng=Q84MF;jfkVlPPzcST?f9R;Y@g0XL?hUw7U-D9%u{ zed2NKh$j%f#XQEE&QUbh-O}PhJ}kc+D8@fPlrHvSOg;D^(Fi5Xu%nRHigXU|>e;{O%q;r@G!?SEU#-lP^x6Nl$yQa;*x-}b*XEMY#;Db zUBeeW%OS&t*PdM^{gWY&9|mYxkzw7&l2vd|2H<{P^>04IKQcA zvt}O`#&60R9$Ln2!qH4lMM0k|5R62+c(T^*EqH&5ew}l{`>y(ttOpu|my{EcVQ)2A z(349y1KYz7O+fj5i_TNa>9So@pK);JVqjOuf`E#StJeljNjHPyaP*e-bLnF_)hrR5 ziJ&+I#!^zN5aWDI)jQ4YH{hCQMVWq`2b0dV{8*=HXb1>UMF5p=G~FIHUJDcutJkuJ zGO$m-mR2|jgeP!sU3}8@YRQokzD*WA@z5*8h2|a^-<>pR2}xdZYQ6bI=(dlCSZL`$ zrL6;NNN}?)9j|D$Y}un5Tt;InsmD$EjH-73_cg<**es@k=(xqRsv%% z3w&kr5dwY&B_dMXyD|8#m=1~)raIhhL+WuM%CZqL zYfFej+G*Y*C`FHkl;0cTnV+`%df2R2KjhQ=_wdWnOKU`{YpF5o*$Z8Zj_CA@{X$RG zggO4Ot~xDx%QSlu{GKLY(qAK=cI#@(X@$aSlLwdU7P*+16kBL{^2x5A*Cab18$DjD z<^qua0+d6a*}budf2o3$go&>`D_h(WQ`j4AeIspq1eSB&HvOP>G}+OHn6+6 z!%Go*v4+V`b@9%*ftDy4oM<0`p-8|<7+a-vtm`yz1 zkvOb;;iu=1WX%?|t-&5vS)B%=6_^oHkHYF>R@jto_nwb&ffD2znmQFA;D?+mWk7A8qSa z$S)D5lf>!k{HM&2Kmq>$!VCZw4jKj?3V`tc=`zrm|MZ&YMS-5dFf|^J%$*ORc!=eBKnc1C@0W%bjFnX^cxgWl}7+0Vz% zY*SL#keEnM#PbLpA!p+y?bUi>!bDLl;)H`l!YSdQKR9sCYF)xY9+{MqLT2zbdpAKp z9ao%2U*_P0ZjE!jAB{tQ=*sQU8aK=fCbpQ2OS<&?lZ;UIW6d5TjHSrQ zTcF%#F7qH?Y#z9p7C(*Xnt+$tZ|4zRMF;j|0naJTFu3+NP*q^XdaQ8ZwmS||Gj$q3 zcztG6)a^SrtJ#Rh8WZJ7n2rqv)zeEJL}~1@w(@`lW*} z!^<>+r2)EtS~oR=hy6->zO#5S)!nbPxvMhxV*?N6Cc^Q6;a!|+BeJ=xV7tEnk$lEx z_n(6{xUW6p4qMfbZ0iv;!BrB3RW9 zxpktS`cWwbhF2}rXZ7GmvywOSGz}UV9kmd;*r>VtxDpg@+p>ITdg=2%n0?mBTx2|F z8Ph#sK^$iyLin5lxZv#re!I$4`I%LO&U8fxQir;TXKDdh47j6mWx^Hb_nf6fK8&QG zag?~+`BSr9GTXq?aOqR9?}#DFb=kM(6m_o&={)3bSe)&2@7rW@_bw_yMM;nKhCN&PZ1bgY5IKs@(tvo2i*M+dGgM$|abHgL*nHj{3Bef}vg1^Xya7XwSE`1mEmtojz;ms92I^Bd;jiJFe$lV_?V8~hdu$paPU z#FjI{`2Avt5jcUD>s5B;WV3kZ0>G*aj_)%b$(2V8?C_t*$xzttzffVD9nkzPNw_`E zH=FGinLS_;klxYGn{B6|7bHX1xuiH##5pK#;ytFM~L z=sAmT%*@EFRL4#wPcUrDY_{O82FSVY)v~>M3Eu(2fW?Vb-|;=vrbwW*6~xIJ53c?MTu-=@;WJ?$JCG=if{(Hx}e z@Cs8(63($;_cW{Ilq=yT?SFcvoV?1iTXZq-t!ZO9m+sqyePB}I;#9>GQ8(?J&l&8b zQwlFsT_79-*KxYHLdVZPcli$yzdqh11gc!~V|@?2%e`wW{l*;U%0Y8_1|tY-)`7$~ z!PP%(5Gz;ENhJ5vkmOxqnQ}>$g)jfS1sjXZqu-vpOV>8c0SLlv{KyDuwZC&(*z3se z3g`s7VWe$pXpXRO2AI8~5%l-T7lsfuYGL7!nO-XA4fU!?!LuSPpnG%&vKBb)@THqo zbi-%!c==7^dA(x%cSRZwd>s!0ln#YE^6K~L}BUF~6_U*6d zY0+rW<|0WaPh6AO@jBUV@Wb>vmn~$DTI0@{HT8FPJa|HOQ`-Xv%>;t2QhV98JG<(< zw5&A0VQUMe|1Mh4x-eJVI3BzWWuqqjxu*entOne?nPF?~!X+9ZQEq-mw|EXs{Awspv{p^Vui=huhogy<-4NBe) zOjze?W1-|s$13IwtUC@YYL<)_->O=??jas8!bJl~;c61U^D;K}4?0+>wV77QQ3Ka1 zznctGRzoUQ73C7oE@)<)f}XxCe{B7PLTs?l9yzUvYE|CKMX=G%S3zgDiHyW5vrKzf zG2|Ke8q%Rz!^rXAm0VLCsTY9tqjXRIS_RhMf6JdVbX3m~k-9jkI6U_pqcP0rgQtgm z6X^zXn)-ACXrcrQq#=;HnsZs6eqM7ABQ@goFGaooK?GizG#oS@<9@s| zAmKpeLpk)@OC7VJ@cYXAQa7tfrPr?-r@^R#cxr7h#DeQQHRIupS}^6h14Aa=Th*1) zIf2t~`Mp_)KoK-$iM-6UCXE`k+UvcdB;lUEC2^7R7qC#{8)&h$fYYY$2ohm2?$u)h z5iurwApr`SFATICb5&P1%pCOH#jRh%M;o@Hj|)~EYP{sMRbfe3NGg9Q53mb4LAm&2 zc9+!RPe&)trdX|D+prGlo#;)va)70^Og6a?3sA5XIaWGM_Q9eBi>fG|>lA+1{bsg? z=R=&{f8zKK=M&u`EJTt06J6Xd39xA41!}t)CE@6?7|=j^y02*X{0a&O3a@jm?O{=RU2G9wLY(Ig-wh_PR#k z>v7G;1CFE+mihn!Yi!LU&V7w7n-2f**nZ9HeqqR+r6?!CZT7ZC@{LH23tOaDYxvU@ zowG^0P1s|+K|IKJqCAqTXPyp>`rYQOs}V1aMb)vbV>-fMwZ?|s)r?WU;0ifTPdpIu z6XEu|z`xw2a3pKo4MF_~hOJ&KB|R?-*hgxop%1BJ*YW(GP8w2Q<~$i+gen*f>~m^8 zDc>}LOYJ^M8PGXQ-pu^41r=sZ0c=3j=!R&jcT>pvhy1qj7So?_fBNXA1mXoz7VtOp zwfejv+YVS?lyfH`RfHz}2CpS>C&8m;Fm$w5#xc=3HE>- z54e!-NxGrW4x#yZF0Hxl-D0EtqioV~&+}U^l1p%^lsftN7W6|ZH-;E7|1rO!X^-9w#nIxP&u>(10+R8!?CHdL!KL zucs{2z4Z%b47zZiKT{xip5MFOP+7H$q6-`3^wsBoGE9}#o+|na!1VcY!R}Kgc$xB0 zdZy;y2T8mybjoRhpmJ{B^!WX;A)$y%70WhkBirp}{Us0&riZq(q7V4c$EXY4@e~zW7wCq#JCA8M zsspMYS6vU}vDQ639R&*vyi_lA>O}A^`!SB!x4m}AauzLqvci?huX*8pk1ccIKjrrR zqBa^=O=Vfp@WnByNwe3jr@qO^8aHc&erMjQP0y3?jn4%!=iMi;%1?k+6<;_=R_o4= zvN8ZHrAUaj;=d8JZ9+UKb~wC@)~psG zrzxEyS*wFiB}=H&T$N%ys0X?gTR)R;YDiU7BU|;mG|}Y=n3UIG%1X3`U$Zox{{r+Q z(r#tE6z=L@j45aI+XGI!>mVrddB+ScC@~qLS&CMl7-PyND-C9hZRbdS8=>1Dlsj-O zkl+;f9CPzGA;x_Ap|8h?ZEsGst(Or@sxC>74DYZ(Vr zSwKpetxC)uQ@Js#wLL}Z*l}|iBstQC^|~Po8+HXjfC3yGWjOE0*afCcxxsGqt_`^X z!8&JP4bCEXPho^+C)+IJMWJ$8yRb~I?f7g?!+bwxFsK=BITAdzQ3vnp=vRyE<9Sup zwHP$2{Fy&u3rix#_HE!-r|54GzMS(q-blrw-j27)Qlzv#5 z(xHF-eJ~4Nw*ly)&BDcjyLyDcK58weYMgf|zKUz`j^xsU!mAs*hdhD2nO^r7pt1e* z7f|67;~8ud>N9_<%>~olk-{{6?0eG*;~K~!LY;g$GEz#WW9z$acYJ@;*UJ^T$xjB5 z>!*MhwMyY91G4e8yh*M^MU?C$YuH>No2Yi}i6~Bwe64Cq(57yWloukL(-2;7n&^Nu zFX->VPv=8NW^bmE3%z)pa=@heN9O+&!LuvhFpWL-67t_jmPj_2mn?NF;UAPEL~aw$ z?q}8Jfr-vpv-6&I+VfPFyZ;NAErpnzMYQPSNu)Zhev?PBhkYJL!E&pTELl&B z85-$EJv#&`qkmj6&#*?pE38Th)TggDF}1G*du`ms_Y~yQw$9~u+rc>2nlx?8S=_U- zHL4@(gYeuC;G-LwR~3S@pJ#-RXe30@VX~+~YWzd}J$CBWTd!cDoNTqHIk3k!x}xk9 z^PNeN^N0~qvki+)@4C@~r&=hKI0y{7t|R_fzJL|vj6)I|{{psl5V>1h$mI$`yLg`G^jI})+*t_|V<4In z9)#4F+SeY&8}J~kjr@Am!+nB4Ffl^pE| zBw{Y}3nw4YQ$Tc(S0T`#Ea7j@rcRYRPY%S|Am>l#68!~xd4kZO05Q&?I9L0oZW*j1 zDC<<4Z9W->9AHi({Gwq^yKVMUEjdhJELi1-VM4|Pch-SF4h-|e>AiUcwoGHvnvc*w59MnSAquiT=R2j zN$Bwckd3XV6;Cj>=KRmF(_3|gUeyCik~U1ufo$WmpBe;o;VoTa(SmpxO4-L(zM=`vDYV&;~7l= z4Vz8Vu6I{*$Fi1Zt#>B{HVeA&Al05IbAMHh<+|~Jdc{0(Sa9Z7Z*hUd@@xvLuTH+q zG}YpYugFu7a^z{NtAn7?ZZxl%tof&jD_$}NBj3*~cPUzV2q?PiT5_~$5!CV+!W{(1 z8l7^S)?;o>-l`gU>PnbAiM#+;?O)8z>>E zZQ}#|K$9$knqOpm1nn+`e=@Cl&Qp{**ruJ%B*&<0CFk5oq|6+ z(OSoaCWV>SuwAFd=khw*dr&$FCf2~--&HVbM3D{X8c@l(2gB6?XK1c2Gn#_2czW!I zeMkCje%$18IRE~ZU5RCI&d+EX5_Z%Q*IDj3#47LxI8dHwLPiUw^TL1fh?BEN?ZW&Y zeO%u>V=!m^3;r2q=<`bl%o{@~jRHIa<=cD;mUUr5f=-0hJ0}_E7cgk+ja8fqGyU_7 z+24J6Ul(8$WNQMQQ4+nYYYk6x6I1ERRT!X;CEfdLXjjd_3%PspCR&g18M-;6v8<)qFBI(2-LrBhpdko5p z#@cH%I}Ru%dqBmI!4HzL&WNnkfd>xT#vRFvG5d4ciqc*5Ctz(2K*cA!)q&jXiC($~ zyx+fJd$K$LbU33^UKM35^OD;G4H61`gYL#|)Wupp0Y6`K z4*%-67LomUbE343(P~a9U3?cd6jBTc-}D!Pi9L`iw|aCoXssnRP6g|A=wb7RJblvp z3vlT83m9~j?&V2Xsh^P%eheq|T~8D4cV_ujyQpT*fO|#cu^Nm0&wx~n(X6}JXV9$> z_8ZB174tqnB0bVV*@rnZ;fJ!YAXhQeLr^(|c7GP%%7t@Uu7yw_+D z3g20}ACL7fpr{Luh8XEjWkVb1UMc1mEuVJ1l6>5Rn63+o6r*(DXi|x0#4^wVF$-;` zU=luPbTvsmpZRe$E_u9s*=1T4>8!iOx}Y-|R2lFI-Vs*9qGFV(ypLKEN&Hn<>Ub)`f8x23mdBy}U{M7qv2m({Q(Nzd_`0bF;qwkZt~0+Xu`b|LZ+1Wy+tSxsu91?2V#IYKCTgUCt{Dpu)h)KEp{OrPvj++`fs zCh;_~|K%@0u|%uq6nkabx@taI=8pfEp;2RzTTi(ijXTc1v>j%WeZIZJxRV z>7QL2fFvBncqUpPS^VRPwUK~>YYvjJ0rAi4Pojdlq=PxV3)qN^Z*sX3FWpIBBDr!G z9WVS4wLP}o`FG5gEwyO4T_dMwVtos<#*>Vh1`=1``O|j&_Y@v@8kIbvzOhQ*kb*sE9pnzNoH$IYnG8im90(vtJ!+`zPFyskA#`YJo=m7#*qLONupDEn;tt_HqK@mfx-) z6Wi}~x+oSQx+1lTbzNA!S%7~+9%HGrK-1JxIMZK1Dz7KVU3p!_)G>2>MVB`ATb>{Di|2P*n&e>d36PM=!7s)+AUY3U z^L++Y^Ii&D8+{#<#~;bT?>6zvMB0ju# zSi4psEZuK?!WU_c#g98|^88)%-tr1-YWNy_&(7JOif~00nyTq8dNAf~*gk>=+>8_k zk3KD=OpK*eSF&IU6v@sz-d7cDHlBco$%R`)jfkgvY(BZzf>Si%mIr3tPsbOyGRfE+ zcA7Uu@gfwrqGpU#6yho9)t4=1yr3u-zhX5soj$5=5C$x)7b3Q=T}m7!tbRPyxPNdd zN6F5Q^#JqN^=+~{8NxdqA*UZP1dm!2Z#sE86v_6gDfsuJgy7g%sL1=yz9-Na6PG;=E8x?BP^6R7@=gL>^zm zhz}-bL@T@gF#dJriIjQGOL&dsnF@weLn+n|olo-7nCTlJF;mY>aQbbBx{wph^|Vkw zp%bz^Jnz$t{MKz+@yJbEjFtd}+X7{c_dn5~>vVGFJwX{Uww=i)t@_&5L*=?5caw*T zpb!U}CYfWA7#{ri}dW2oZ_YcZV1We)X%+l^=J)zhM zTa8pfJ&>`AJOguG5?42TW5z^vJvvVx$c3wyC0-FM8s5_is~4qgrzIJXv+3l81Fr&Q znwMg0og|~{S5PrB8)9N}Iq=XxoVHVI2i_x|M1SmF5g;+4Kdh|r8IBfQHu1k0YFrJ> zVXIrAu|;(_12#GUzu&v+BK{1v3WV>KbQM;sbn#Va3>E&^Zk7)v_hu-si^X7S^+t(D zjaKH$po??7<0RfhSw#e5%mLds>E&7ppUcArcfd zpYO1jtGxsyOI0(7fkX*v>iov^=0>(%z!YCkRovQtDLxYk!P9gMQ_Y}u9zkk9v^bw> z1x*8HjqLo8g?VGJ5Q97fvzQSfLOrqq>Zf0=dmNCbxaRag_p|=2T?CQlOVDF3S(EdW zOg1AM@4mR|eG*yl7-UcH`q9WOr6Qa z>c9CeCSeGjIJN&hWf)s|T)MMqmw%5lAnR9>XkF{7>yDhflH{8@33^IUO#fHYf3}>R zz&_}!qrEv995<1ccWWK?j1r_-b1uiBcC#R%jg9Sv?;x3!O0m#;E2(H8CAG{})o?|V zN`v%4@{7|Q%Ov=?wXmklAQMkSINYRVA)nxEp$L$Y;+O8;yY#Lr){Nv4=8>=CcUZ8; zcDd0I6V|0yDY(*Gc~Z0GlsxqhTF6@`%q=3mb^acB%#p3lyW;x`IC8`V_LeBY&r1R} z#6S0~Z*ru0K1z25vRW7Nm}0XMs*usKOu7h!sw8qIqJZ zNP|^e`Qg!N473o;0(M}Kxe(+s(9vuYZ3nud!%5A@74vasGN2@AUjVOj3;t|W^O2cr ztkEbIpDBsYq-<+)e*r5d^EJTg76RAoCzp5BT_l5qy)M4gda4AL_@nq2 zz&btZCAJ^mGN4F8ipD;7JF#&rv}K^K7)JS57Pbg);%%N2+*HWC9UMU zzFnk?*ul~ew~^d5I{W1(oWP0t;BX$6{)$gn%kfC@qSn9561%+Nv$torr}faMak0VD zOJy~@sZ({GJ~rUGoAS=v(Df)rYNNYznkld^LceEU$w#on!P~sDu?_)G+Q#tmU$F5U ziW3c7k@dtZR2`bm$&RI`WuUWfycE-f;ndzt_zFurG}`9t1rl~{iU+T96f43|aj_dU z+~BR5eD)SfDS%`e%kF;~$d$0{Bnhc+_Y^g-K61pzb*&3@2TP4Y)e?%oqs~y^MaAUf&h;L z@3kO|^8n+~sSs)qZDv$q-NWGg%jqHtBH2 z<$En6!Fr+NbF()Y>U%1_o&u~PQK405Rteit+WyEMx$ft-ty6-O z2sa*r_l%)7zOzv?8<1_lwJ>!1d8}su!}wVMSnHaw;2ciF$<)U1qm6AW=X2tO6D;9- zQH3vR*K*9KJvK!}lk13vr~F^Oz2N?Vn2gO@^O2~9qE5ZwV#TCA60h@=J1R~feyLKd z*?=gXA|r+PGEf6FyQMy0&1wyT}@KUVw?8N zi)u1-aDGB|mF+QmA`)_2EoiMyN8*cNtgJ%~qPWj0grcwtWl&sPAmsH+8i(83=66?v z9Q%{K;to@`MYbdMQT{4bsp38_Uo zA9}-Lj}|u0vX`Ao6Bnpi1z;~x+eX0=RjJmR6_F@8PsKzT*$#|GezS;(VX!JyI~cO1oAG$~&u61HJIG0C#av@e55wM5q^qz6)y;i=G5X2%|FO z&XIUbhdA>FG(7e3D+sQ?@{dRy2;iQU^&LLBmBPvpOC+p89n%f?eC+WOu(02ZmnC^#Yr!>GL=xk`m zI;KIt=e_%r;>Yf2d-H4aMOq2vH~P;k!QNuls&MwgIx&(}`gOaOTa}OJ^P`#yBe@RC=PJB}tm&&xOA*JS_~|aW@^#~_yv+Pm z(eCBX(0MVeGa?sb(&0`I5K%ttVBZXT%T)8sGii~uhr^~X8~)(sTpB8JUz45tzaCT>2K8On#f|UMbrJs*Fob`mN=RtvRiJ*ZtB6byqBsr2 zB$b6Zs;Y27{g=8Etug(9=gv>(kITOIY#>qnQ@f9nVuxDQ`2}O!?7%br8yUQC`_O&~ zUxVc5lld$k58|T*pBKiwbs@6&sHF3*6cXqJyQTAU7HXX5FK^0E`Y*WlfJ79Ah zuwH+ThnvxuEhBg{%oN&_1gl<=Z#^dqBVN5Igc<1$bU?O0%{Th;cQ?O(_(mWRwFjM$ zge+os2=PZUB^q&6Ji2i6+!4P&o&u?=-TT^fLTDN={0Twz ze34#t9V?96Dk%`G1}(s5ilvl`lzSM$mS7@FRB0XQlgG-w#4&6uIcCsRbn#I#?6s=) z+?kR*6et?L5o+=K*~J%x!1Vx`I@xwoZ@|SmDGV& zL3we$Ss_(*zKpyrI4y@KCKDuY<`-<;N0gQ)+L3nF>k=%3A_6i4&`8vVJ>0Zox`g4H z2al!`9t%1>)zCJNe^E~rt>hkM%$-E9~Sh zKRXR%g8l#b6IOUx)y;k?V!ybz2$0nh8@-E02@&Hfo8VR_<);c+L}(i!8z&TXQHq52 zVUMM%YL|W42#}0$sMaP?iwpheM6zjJS*I6qq^7xJrKCxzim}o#o6d42orm@dM*7Jm zkUlR}xx__0p~|ingvHSvSIWg!IpwUu^X+HUa9Wd!*S@NBmZ`0ybZgEP`H0o2&FiOt zZ)Zgd8>m^CryKppmSxl1=@^6w$!t-70r5SNKY9(!7fJBQnEz}#D77ZnO5WC4;GUPRflAnfBEm50xwS55Oi;XeDF1Ehx&0K59lelKbtJ-l{ zqjeNZE8lB9^j~M-F_DK~U|-&fqZS9_YXX-jxx_0cJOoBK)Qa$xI3RptJ zBR$StQ4qJNrP77woivB}nD+U+D~fo*R2EzAid_wnF|;T%wZ^WucwALfeDRf}YvA$) z7Ogd6*hp;d-SRg^1I{E4-FfB7$g9MUEWZk7U6v zvsnrejw+I$GV(B?LFoeyb&07AY7G;~U1>8yat=VpWQPZ)x8_Q!*TciphVO}lI>qILdXfJnCEGe2a&)X;-#Am=&1XZ_q z(P4Fs4ccCov*3eVe(uZv?mCsLBK2OK;Ht28(&Hd+GbNyW=I%*jiP$(0QD*+kR>nN= zeT*R9-rD9pkPCvwZf?<~a;d?T88DqD6LSg=g$;IRukCywtn=)2s*`>}Vbg!}yT~>e z&&lXGsECe!7S`Ovq0g(UATT>goRG)p{Qa57iM8E|@?wiVG}GkMY1lH!WLx|Bt3C=P zsd6r^8yK|1R}`m4`(yBmYbp`xY|7ezU&@KO=NVirJH#ZSJ*hxNKVC+yr5ApG357XL z2R@_F>rF|3sLeP63=bqK-5&JIKs|FvB=0Oj)^*wYnxm%3Vq>^jQoir;p2F{+d^;*GJUC+Wh85K0UhRSlAV+ZrWRn)v1o44_v}# zYRHNx&4SRw63@8WL%w4!$LHfFPbbo3ySFoj;Lxlbw=L0E1UibLY{7H~AH`mR&r^p4 z>Q%jdlaA6kxTk3C`*LeOAwRm#n?}IFG_vglKSjn+3+c(Q^YdCjks#$%r#DNrz;1b+ zaQ^sGa`gBvg~LJnzW{zffxoaJ{?tFvrz@;if_v0cZN5X33b?<}W66t`hj`|yi15i8lf zqr;mILZ$)75nG3Id&Mpf#IV{Ulc#{hD!t1!R}f)&9imc;IK{(A;ke=%BD-p|oEpm7 ziD9mL#`;5nVq>YX5I+}8-h4%5VyGC0yhWTgs`0J7;nl-Xpk zh8GP5mpW6zOTA_)wa6|q$Tzko#gVctZzR|)nQ*JrtunlBQY<9PL{iC_8g&lUp1nG( z6{)K0{y3_G>=MnqwnMHrU;$W>#;ejEa>f;H!eDW0HlM1S@O4U1U<-=dnKx$6)nD4~ zAi=T?j?$5`Fht6tzB3-B7O$0wsJ;WjQc6BVP3$hMKA`~1M)g75oK|qnA~7cr|-%@>q}dZ;s4R{{ZQA zrg)8$34{=2?r0WnYh4F5{E$fz(@zx`DHym?pEu%3ivG~)98Ws7CkJNa*)zyiY zRBG%9-m1$iV{N&&3xgAnUm{po6-y$tp+1oW0}^a8Gv)%)(u@_w)T}Tp*_#i#s}|tB zOnRFyj^9;a?`FK;-NP2R`r>=f-f8?ep`hFVmrdELl1!^71JE!ZKsHgCSx zXXbQsu3tJAkj~@bSFArIOJ#ZA&+BXYiT?odJ9?|1%2HG&z73fcRUE!!$!XqpIhi`% zL-XN&JEZUWO=ucF+jc>Ud7rTsMb>BI`iWQ9EL7=%$T{hV6Cs^c1vEH|62WHrA{K42 z_CmJOz+e7Qijal#O}%8N8ndH!WD6D_FKy5+-8{XUkxD@z=QCl2M=?3fHO0 z9hUuxO`g45U~Rb*;p`5!fUx8=U5k&$iv*g9*RJH%>vihI=@?(GEbS$Mf)WaVT}#Wx zpto()xXG1sXIU+xYeJbLSkUv{#TQUqh(-FfALqz-7W#ITFs)9*WjKCU#=o# zxoDbG0){d5tBff*HxC|HRcYqCB%HvXN{cYP!dqQ6%vh#1^fAVnxSVwwwl|kLvZ=Yl zLeLVzCdNv{e8gC0^RJR95nqg3WjQ#08vRxFH^J(;Srap%inxAkt2*-k01V6Jb1%14 zzo&jd8V?xq(5L!*Pl@??(2RWfeJ11lPYG4^ISwMJ>e3Y1=b>cvvIeeJ%bK`%^&0Xn zI~&vz_RIk_9C--UXR4ACQ(>lZWWKe*s3WW=pD7*qC)q8|I*61qBQ`k(?qR_OgO6nQ zHmdz_Fk=c^)ST-zSSoW<(S+D3sbtC6&8Li>st*v83Mvu+hgIR$$zv%2eg zq>1lLR=mSf=~q-Zm!{m>BC-iaJ?;#~WSKLo7Uh#VDc@c@SYMD)v2T>Kc+H9AdjUQ^ z$RiXMIs`HS8ZiF=G3D7j&OQ?uzt=q3zsJU1kbX5Mlk%JhXK2ET;{6OzuT(Q2!33n1 z63>=nsY>Uw&Z(lSXmp~a34s*SQyV<24N;D_2h~P0qO}XXxeFV99jphiL^{ta3HR#L6O#ot@Y(? zX^)o;wbnT6_d%NzTyZ=JNJTX5WbE0ed4t_&stO9f8qQ=LI>fQQQ8sU&$!gYG?RKw% z@v)F>?5u(z0Ju``TbSZDsxsw&x9AX_Y}Fc3?9VLTIBrmh(y=T;!GUVlOl!?;S4E+& z))pyFv0cv?WZ8jCi49hUiy9W9C8tl)X~J-V-*2(jvMGzNCFLzzJVW26yv(PR{OM!b zVz9*H4zB`gjfu2L^I>4%;U(Zo;xbhf3lkHuGs%lhAJq9AA*;G`z-R(-O%9R7E&K7f1)@Kco(OKB} z135%?5@MnPNv-uQNf6rS1RX7F8X%IVj21WhmMmcU>zIn9brz+Jh=Y)kozgJ{pr*Au zv1NpdO*X*c-C5vg>d{!kj;qR}S=4u|wCArM*~y&yg%ye|o<^mTS_a6|u2SWrCg-)F z+RtG%fux@8{PuEUB`Q^1hofEESYLD1bS$*CO{LBDTdZ)^+2|-X`<0&gF}o?Tdxc@b z9lS{ft}ChRUXSt0meE0D3z7j|q})QSA@*=3YJ} zJ~Nbjltrvvs+^?bCx46t&ASYZj}UI{{6oqknb*4d?};Biu-iYRnchO30ScG-g=iYfD3oc#6%( zPRhm_wZ6*7kr-39lUwW#-;7ram?>qarJV{>By8HOxo0QcuG2P{-&AL=(q~wU2MLBv^(MgtY1lOi6=Kdrt9px7MBN%$ z1sgKnNH*BGro5%6-B1t$7hz*QR!LQ73ZQq^+QSnTWMG39PUkVixsRIJhsG`-j-f(^ zYC4J-JZ{`Tb{3rF*@cdyrXa#P34kIXvk?(LvcA(*P%4{R5&WXlv2vZ)0&AwVYQ`{| z6$$Y6!s?vy7QfpBm?#?Z5g00!pI8~DrnmqQ*&ysbH9bE@H*rdA!-g#~)u5Ovl39zl?mVznJWS7P|?Ao_l*C?lzvvpl8 zENe=~T=@1cANqd_wysj5Vihf9E?iiu0kx~kLg9hiN}W8K z{G`gtEIhl+m_)uBTFc{mHKgN=)W(T3`-rbnv2sx=yaJ!AY__rdEyvfgOR(OqvxppZ z<@+A|You(&PY=n+60vED1 zFM_vq%O_e#a&iX(Lr(JI!N~jt`QqJ1|*DY46LdZUO&M^bMafe5gxiBxv4 z65vo=q|BA3S}N@@l)(hj%OFNp>i{C}W?1^R_KNLgb7w;-nbWLFn+dMr+(y#XNma#Y z8zT@)#L%jZvJV_+lzOJ1lhfLPvRWFd>w;4hz&M=}J0puJL#o8}n^+DhoXtReHkcf3 zGaC5UjWduMidV7g9aGxuhgeZ0rXZNz!U?-C<{)?bke!%c0rH19rEq+rb+^v?@v5uz z$?f%&)<8|nAtmcH`LYcqkArv3mxF)(+%1a>VYd8iEA~a3#f2$xQ>?8H#wfNO?EewjCOC3ko}BD0CcC>rY+ zX5$`?wdL{jgPbpF3qrQVnt_uimzFV{iym;T_^To(Jr222s?K<(#8+IeL&=-EY&Ap1 zV4zl^Xt{jV79sE?saWKZD6-K108bg*+`*2L9g9JVN+4P+iJ2oKiP9LSPX{Wa z5B@JSN3CH?nJCvSV1D$XHS)jQa!s!*V*Kcs)5p1t-UHfzmRJcTFCZMrFRxt=Zx>^5 z3OV@5_}5LLvkbb5=~`u(H4n@bs9JZ~HC$2R3vew&%j*I?$PR9&SZMUTEb;JscmyD zeIUk6BExNjj4h6*1Z9p}FE`nnd3C5%n>gx}@)nuUUNgGdv&eYc>am?gjfB~&%Kre0 zF{$<_B*z(9T$!0G;V`6*@g~xS6TFn_d79vLFvSkj+Xwu7fj@a{OzRtxIt}DF?0{ZR zAQy_tbPVn&g0Q6*$PF);lJ(cT>+*rj50f;e) zk}I+Hz${~{^xSY&xr2q+_Fom?Z7K6xEfsBci}IhT-RoVtiv|%c0erX>d$%#;2D{cL zDPC6eVoTY9WsH5g?*j$XC5%DV^{W(A7I@&g->(98JbBDS;&YhNPp3srH^{Pe z?gV2j`9+&nl`>wFM63;@Hd*b~I`Ju%zQ#0LR0%7>S^AOwnn5Kfu*R+2PaBi2v_fQG zNzCefTdT+9#bHl0lUaKga;kb>(Zm{!M)v-y*|pLZZj`N|7;$i>Pgfpf`QYRAD+F4` z=IT>v0&+JpZzeoxUA0=ZK8(fMj-*S{@;2ju)Xn296C*U5zoxv{NGBX5?hm&*i?->^#b6@I)?j2)KC=oK z>Zq^+#MP@E++>~%xY{XNMLb?k$S*7&n(tOtI3g1k8e}GD_i|<_O0oQA7}6lHaB9co zEvKAWywSc#SijrROuduH1+C%~D1}y2LhUmO?Y6outju#|=&pd1S72Q;N;T73Vm^WU6 z7;;jYg-?Vw-bX9ZcFXFxDkc&uxgW+$bB~Wc5);Pm zyB_4QjWw*)ggD_gX5kK>M#78o^E%k@gvzZlLem(oHx0{+lVS`s>g@#qsn}*JklCmV zh-e|Gwm4+=YK6+zYZN;d-^UF@NbA@d^G1uwG*bJd$kkjLat&sO1!9`FTT^@#*-kSlVRlGP(qDGW zl5oe4Yi!Gsi_`4SE=9oz1hQpdkuF}ZPQk?;?YEw)cG}mn%qjYsf!RQ zV&3kJWU)3#f&(9mwN0MDjzxB8@Ym_4H44<=F?ieIWslFMhAd7~My))pF(vgKVjw(1 zG3P8RmzZ&AqQ9YXKf5zfI>#yRU@*pI+?DXD5?RcphPjxROq>+Mtnky$EY+rC13aP$ zvJ2aM%O5lH^%C}z4}yu-Ii1xOdMZw6xi%7wJYbh5($}zjDtump93gx z_6LqyF*boiW8-s(nB+Z-70gGQL2Zj7fU{W#)j{mJP1`WaCxsB17}u2)Fq)a~)xX8f|{2 zaMmsI750={<-VC@0HR_n12&i#1~N-W_~u7Y#RAJ#%bk`Khxpp4#d`wX2MLIkDei#V zUCn{oJlTX{h=QX`9gkBc{udaKGFp};kf?~GggVyP(#e$=Uk6vk6e4NwPGK*+iMwROOu*U1t#(W3^VpsfM#wb*bM>H5 z>f`?aL%x}A4YY1=V!xqGTP}>#E2tw>V)G_PL^(5?P7GCMyScW`s?)I7JC~OvS{(jM zixS*Dv{SV%wU+WlJ$(wr(Z}AWWv-_!$5|d%F4>fCO-1BX*fz?BNf}xL*=e>9CtI7& z^$YfI78l6+iO|MmT-wcw?p^WxL1lMpO2qNqcAUQZxNLinBT;y<7_$zgiL7NTdhm%u zPTjkVqCK9j9@tW>eO8|PWis2XGCm+F?2YDsFl{2y;{z436tr&lAddQZ_5@8iwPbph54MFk`NXe?^a0Sb@D9iL@!d$Z zRuNYokTJcUD&+^F$@@rL>-2Tm9@NsSiB>=k@3D=$N7t!)5;Ou>mS!>4=<$9h201$2 z^JB)60>WZ()M~7p4!ug~mX6WkdVjSsOlX`%J{k*!FkgZ zzPuKPp@X1N&03$6OLhOFI+Mc7MrdT#LnLK4nKZtUaY|8~wc>1VhjccrO`kst+@FDfGHmqZ$M)LFZ1i)X>QzK`w z6KbY3PASe!#cm3|QzLTIlQ}Ceub~ASiczi^Bj_KnOU~CgN{7gDH!?Q`1y>VR)-7st z{B9}zbd?agD+j1Qnz=HvmUFuXv{w}lFZ9a8wy*O}?N@xJejQ$90nsB3NQ68V8w5pg zIGt2lP(?2HXs`}jY2ec>y;U6jhZ?gln8fgA@DOSUm|IFF7(Q4=MN5^B9LUw9EGpDo z&T($uI3Y}3ZyU!*Rx-VQs#}pI*SG|xtU*+rW)4n;8sI&4#A~YkeO@IT#GPB{p1aTk=zi@llnoRcT1tj1q2hDvHJ(JAW;;Py)DX zKT;OT)+;{{7uPnBRSiv%NP}D=F>0|VZoEa?;w5Rk>t!)#`mS=bTh6U4C4875!Wg9r z0wWKP9B(@f&$)IH_!CN7)5(=7b`HwHKIy5&G;Pytu{RRB@~g6pg)ms^{oyi_(&Us~ z!uYf7_r0eD(>|uq*-9q!_TvUq3y95>w7PN0##N<3CYD z9ln?Zt%Fs%tWRFLq#4#;x=CXNG}Qz3@}M#FIo`+~Qm9B)@z+~NZO7saDYNwYh_W?S zw-=6ExaqU3jy}S89MiV;gs)Zw{3Z8m^RWit8-jbziKhV^^5fWlh%r`;UAneBxJ#H`@CWDVEI zC;_^V6^nnB_i<+uFN|-jr7|2pg1P$MINNqeskb+fN>CJ)?wlI|wm{>I>-2k7s)dVn z?e#$|455^;e3XwKJ7mJiWU-a2zk1YdO@2F7#nQiCYytS3t-G?}-$hncZGKBnKr0-Y z$J*)vjEAwKYSuQJK@YWG9PDz2yKxra;=5-Iba!=X_&IHPV=vYO(vL$2UbTv`FUjKU zLH5+iVJ}lktW|AG_QpH$)~nIR4OSb{H6#X}I`xH5V($WM8qU+?lgJ6*wr?2h|b2}3y?Pu-)dQps_^*dyTq5Tl)jWC@Y9^vq0Xn$?t!r-8(Y zfRf+%i90imk)jY$$?A@KFU#b&$MRK_R{XAZe)EHX;+tlECf%A9;B^^#)vNQ>uIm+y zm=k43_}YeFK(kcwYNw8BYu~`+ahAIod0VPjlqJos3$nAotq~yXHG9UZbZGePXmaeg z%J4QWJPyD6cIp$v>3I0hD)vhkN{BYQJYCqT>T;i$xreo9)=5^Ng@Hy=cN&PTBZopz zrrBuD%N^^&WOZ4<)v+ZNE>^G)1-6$lmO9NjY>e}vVCz<1PAtuS53cw_jrzyutJ(=o zTeR&y*iE?c;9zzy^uW1gmExDZhqOr?ffEu|%D+{`%3wTQBUDL#L5#VGknIYjBs>O^ z0a42jlv1~(>w=hc_tfjRUShRtSo;;Gl&CPc;pi_Vy@{)8yD@e$O}Hu){(D5=x<{e3T({B>J}>+qo&0q`wdGiw`&%$$6F;GC0(z8fb~yQQQB+SN zyGi)xtF&##Su8u)*~4bC7J>Aq80ORAcP|R0^H{~!#7GKT4FM9xV%U66*O9T5y8Q~@ z)ob;F5aRN4%33ma1F>U@D%Ch#I@_&5oT?P|B6z{jhQTvc?T8>Ly7o1q&C!l!uvZOM zV+6?URE0iU0_;2cM}zUqr!oa)c2?fGB)j0WD}x>fDC-U2SR%+{1%l(|5m+U^YpCjw zB-XA0!o$4c-Af~i8nyBkZ4nbcT!e(yLTr`h$8#p1tteXTI+e4Ewm(p~66OX-u2u|n z7vd?=LM7O|Vr1KIC37?4wHRtq`5rdiA6kuSzE576bu$^CdgMs4suGaNryELB161r4 zTBl!CcZPRDvlecd1Xv~a6He8NU#1KP>8braPLDwdk%wMm{e{JQKIdfm<$}pMg=MLA z>aJaOV(H^4BfOZ}thVV3pn!P_Z{c6eKu?el*iq~SR zl>HUj83G$R$siUodRJ>!Go!6nsCt5e`bniig@#D*9~{i~#Le6S+e!@W*aR(6r82dW zV=Pot+gXK9a(C#*f~v(7F65VH?n!tyZt6B@*f$3S`%H#b0i-rj zWE9carxS~zD~fJnsKML=R(_$a{T5kF17R6MQMqZ}(5Tn|)-Q`)TPUD14O3-!E=$T- zWr;O`r;1l8Mq>Rc{cg3AeW?~UTDDIo1>LvYAX@^>H9|kb*K}z!)UyT(;Lk*|I1QR!#ZW3)a~NdGCn4H3Z#FsA5XV^KKc~@*v{~G% zry-MZMVtN|B*S##d}&l1o2#Xg?pe*6-cwbsu(Qi&F&9c!B*no)YSz4#Cd8SxOw4sq z;=NWv5Uz0|@vSej*a74KhOrG*tG2>9v5xH(wa*og6~(&N5?Xqf?955zGAk$uMLRB7 zk+`K2T$@?VbTFNHRg{|QyKb;RPB`Z9FWGWUR^~{WnA@%>tE4%d%$eIbNFgQ9nqk0} z5ox)c*>1+(RfB+Xr*jU2V zUx<`wY{El<4Uk}RfxX?qab_L-ZRSvXE8qzo#)>O%DO1PX3K1{CU>EswtpB%h& z7TV=&J%o96ry$V?Re);>;m20*Q{WE3F0}S()@~cN)ojHDrb5*ZCi_a_+J_+dkaF$9 zp%LgkVJ1Uc^RI}u$7cqPIuhtgaya+*FMmqF8v9jF+iRCs7pBbdoK^JHeS%#UU$DN~ z{x?Z#B0KcckF>CxEc%aDE!8SIFImlI@YyQj2zu?YyrUgq_9u+qs#a|eG@^|)tdhoy zEd!Z0aFwhIvT4U*Z>kwq&Lz?8As$1Jxm812`ESloRC8^Kt1FhXGT90=6=WV&Z04#) zD;&1!ldf1PTak8Zv{$L(>66jZVOZ>IY^a25Ey=KZ=pNeo_WKeoDRTtkST0i89fe*# z2~3V9!SisI-GFb0KpL87d6TIS{pzy$5AAlXDBTQQ?wvhOe!l$u_cj(@!SuqCr!O7^sHL zYwF0_<(WIU*n^>s#Ddu}QdU`7>jbRlBd2-CX6fHqBFd{{V{_HKs;bzOiHZmFrHd;O zxclU&^=pMU?ahus!{9*cTvy!olOB3Ws7-uU^wxQl0XfWwAmGa*QFf`qZ9pBGUkR1g z`JPuK8i^L3V(V?H5|GShYwm!c+t%C7&&*{L5<^?buCmIK#`zXN4d~I6asg26T)SLb zGP^fwF$+prgQohcYUgXmTh z$@BGYzQvsOYHm6YDP`E<{yr&<44NNit&13p9aUuG5#`=JmcF^QluwW3t8}%sakcOC zHnkiFvlfKii?T2(%;ANK?yvc(nQ~?O6Sm(jnVI4eGqS%X{f4qBfgxL?{xUlpw_w*M z!M&os44t`8%tUr?%SSvubt@>`*W^<>)rrUW{ce-Bae!PH%DC+O12|+$B3D4t(Ms$v zVwM)AA?&G=&SP+DzL(H-@nuFVPQD|c5Y_mNR4xx$e;ZyoRb4=9DQ+*v8+O=AvO#+izP%+OVdTp+DlTe3zA@-jZllE8ky{dq z0Ky^2R}4trcsS8Hjiskxkqs9k8~p|ZD`4v~)~Q5(8K>a@EDF>=jY5dm`UTDhX$ zqc-O=v7hcM#IIh)Gcu~J?Q{I4c5-JU9<$R8uDoNIrr9=BsPqHI^_+^%Y63F31fJ7p zBxI0TC&_Kr4UUhJRQNM-%WV5~ZPKBi!+QwFW*!GM%Eg_zOA&gdFj-l(oSSNt4i77W zZI4ynPjR@!L&wN2uJw)ULpL4;l7kz%$9KPHe?%Teq9ODV{{s zNhP1H-a8ruoy%~-Wh+ip^()9~C?NxS`mWgx?lT-?9+=#>2%2kpTw6fY$M!pg+ZZI0 z+Xsd5besK?$y2JUFO@i?=1WtCtky)SI75S4WBsj=Ve~=xm70nj50Cqe1-}=I@myo! zC*&7t1$m`WIMsHw84o z4E;fes~%ZpxDV1;vrIujA}Yb|)c*ibay3x8)$Vz|b*hc<`OA-N_Nw|C1U770sC+?q z_jELiMA|R<2NE-c>eRq&IuZJPr23=^T`u3a1<-KMV912NNn!*CrSM61YHb5x_6V5$ z!>^Qpl3FLieUw*@C&UAuyqkaQhgkmrBFloNjZD4Utt9)?HWn&=lUfde*|f*g3t|YR zj9s)9k!m}H64iKxGN}N|-Ye?JOg|Tpq;N-E&RR;AO_hZvB_8{7nLD-!C3xK2 zV`S7yvy8MBiK)f1!~VHS9+UYRZ@#kP&afU^ZP(Xo+Ork2i?)Pc!{uw^ty;5)#b0BJ z@h)O{uIDM`{^D~aMHjLe3o$K>U60PVxq6B#G^}Z`*DIE)!wGB@f+p2zooA8N zb|MK<0m69dB6Tq%J4=pzP>I9u#PtRoB4%KJeRKZ+15lO@h%AE;mJd+~w|;z+E8AaL z*=#3S4Wb=nW?)IXkPi?`ERdvyFGFP}%47CuEM?3dvKWZlQtzoMC$&&QQ zB?Z4HBCRUnIkc;_&1hz>uW<_}lYSL8N%xf94`!UYLgC*309CqLS#k=bPp{Sl;(m&t zYiLxvj>U0avkO++FDa`8s*vAx+zcn#u0Xq>N|qn0GIv663Mc&>Xe;-1Mm^O++D|uc zCN2G6?O0Cf@`yT4$n3oGHMPoj#VIwqj^H*G^XsGrKQkQ<&$EH%B`xXH*YP1EEP3t zY4WOkA5t0tuuKjc%5Z%tpR&fZ>pff|jgVY3bw6CW_vrRO`d+iN{{Z^ReWrA$mOKj; zRnAhUzRE^a`5MUt%u8w$FU9z$Y-5!+0PNOgrWHxO#fSw%t z+rpxH4_m(IRLx!@bkq;HEjg7;GNm?Y(l$;`xF~eI0w5-ubVEChq^=*;sX@+b{3;%u zcRt<|gTUQX^xokt9#w61p5Zshj!ZXLB2dTJ5+zwv7CA$sSaDRLl*CGK5ZSuI83+XV z;m*jw3HIjYgzPJ{p=I2dLmh&Q6!J+oDYD?=S@@y@N3!CympM9R@(;4^`v>f7o2gR* z6usxT1lW#Q_@APy4;Kjf^y#($C__~}ZW|RuQ`X_qHnIxm2yUD(jgXs(RRC~q6Y%UN zX?w063XSgu6udrjw4i-$TA<@e@ToDH-6u$nP8xJFbouvAve!}CHWPI&0?lFBaOcbGG7Fs+YN*$+v8Ku3^j;H_J_N#PzQHhpfdN?*=}odZR|=pUNVO)Dn{2o+ zUq!-w;>(860Kygup0+uJfeMRSK8hOeV5*s86p%ds0HbTFZ71l0Fb5G*2jk2BPtgR= zi{%e>GMjWRR@Y6!nJVkqZh;hw#-!2d1_06R3Ik)Hj|!P1R4@Rk7C%Ud+#?1RU9O!q z8&JvROZ>pvGo3Zz(cHB+VOGHo*|lV$ED_R4meHE*RdY3nWoW(uI@%QVT!U9z2M zSw<7E7E@-)u?Q?G`f0wzC(Syrf0|(xH~f=YTU=jI!SWV)W~0ML;;Wa?1-h6#rV$rS z_@Ook!V(bV-hzxY6fhD005B@J4{a)p@iB}z~!lw{O-C+c~BALyA9tz<8FP!O-w4-F;T8Ka99NwIC*$mt!{`$=N1kUGPIn`@?NkoriESb{5 zGYV5BkenNP1$T1`v>|0W!M2>D;%SyUOaAZTpfRbS`=>#LfP~fc29-^)LtOLaF}g_E zQih{+Fo$9g22~MEF|lKmV633`)79DX%Hcy?24Q!am~YIQ=C#d;J_>ArER31upY%kO z&zcoIG_?mk=k!%!r+qh6o34{J2gS(=iyVEWjY|ryf;f= zEEIaBI!rGq+Dl#x_*FHM%aE?j-3S4CzenipZJ$YY+gd!J&EUP)1aRqTteL0D7GXuh;WZ}uy z<>eeOCIh4Y0J%a8*>JraH5-)^zPP``^Cv@_6P+k@o*-iLg;!Hc=NS~$$_cS#XNM%iRWMybuzX{*t{>CY+;vGp2ZeG_O(sR5Gq@vTva)^Q z1KhkXJf8%kJ{I8_)SJ^eWYJffX-kev#ol*!Z-=qkAsnCL7Axmupf z9#f^Qd1wB=>H}{)-5fuu1pB6k5wxcDzYR_XzP(Z4EcO0~{dr)XPUW{FH&}!(D()dh zQMd`+A#R6BU4@|o2yU4Y)!fZr)l$=>o1rBL3g};C*^rq5r7;TYV3IlODH7@-iWItQ2ybwg_};KtNQ%luz8n2bRbqlyA!j-Vw<|b>VD} zJSTE-f>mD|uL#@~=j?1yx;(tC`Oc(z$wFMM2rMIxMPd=OqiI;m9Z2;jVO^y<6-`7Y zk~{$`gmWq2ojkIcs-qpK1T<9eo6?Pumahs(ge+xFlD{?lf1(n$MGPlF2ye)vV2zPS zmHtV+ZpiYq^0^4tV0;DwLb&o4i0 z-&ZUg^A?nBw^&%_EQ)o4XtPU#BUkiKx^;ua{{R)#3vv*KpB}hdJFFBoR#+T@7hZW3R?E*b%-tSY=jB=g!ar-z+QRe9^7ANj-;m^1 z6TOp)Hb=ouyF_yT0CX*(JL}1x8%{i{Lx_ZIh2;cS@qUPM!tk=Zt&byg7FJ;k!Zsyg zI{yG@ttZ73nE6gto{l$cGjqDF0 z()l%8;PPhcBp|!k7*>Y{q*eK(c3LBwdw)-N>3AcpRZ?Jjg6^>N|yLBIOP}0X?2v~KO zg}qbDwnV1tYnU7iCK%^S;Z+)-3>5f{lhL{LXa1_lNx~2@qf3FUsY&WLnv4j2$WFj=k-`Z>cJbJhUhN~ zWI0q4EUH<&jg+FUgeNCiITds)u7b}XozyuzN*N_E;>m^(y2jxcMQ?@WG|Z!Qo>b*F z=-jy+wO|$4QJA_nEJu@3EtM@K${-nzZU73-kIexqe_Q)gZh|^oud2l|0`5(^XVEt0 zQ%rz`69qHappH?pb&4?MS`(~_f{pnJLM>%C=Z<43hErowvt@RWg@QIh>6k(g z-8ZEhayncSrf`5GQl}89nLve5BD}A*I8Ywun5`8%VG@w;7NcL28Qu0+fsMDr> zig2+O=E!uXLNfxd=%4r|^q^%PlZ1K!PW+KV$egSr2+0UzmiwpKIc$PAI7Z-M1uJ!) zL#QUovkG>pLr_%Ua-}6Q$U#n5D0O&DxJ(PQa>^aZVGfy1=~gJAGJ+-weSj+MW)ZyM zHqx4fkU6?(g1oN?!eG_qJO)PI>7l|EF)D*~!GOPJ%o|yL&xS}CLLBDHfkO(mf;U`A ze-!d$lYpW10O~(Q8NzII)t~yObtl6?RkgkcNLy^I!?FsMhj2}XMbn$C1*X|VLzAqT z(&b-BYN;+$^;?zOx@MCZO*UC03AR4TwpjU83X1C}SSoy*B&I|wEQAwbOqKl5{16#J z-pH-FWq4bZ+hG_xr$VW4nK#OI*a#b-u{T|<39_1-AyR_8{f88P@v5iMbK5PSb;Ab+ zfx7*k&&_ZRU$Y3Ler=GC@pSAyp%(!C)}VKCeCp3HyI1sH5U{$7;WMQ2v(s)yR$)5w zrhU`;f^SkcQ{`Y8UAwE@Hsv(yOtNC|hzRsI2;Cc}C&`PUCv|X`As8w0^kkirZMw-# z!goo)Rse3V)KgB9unHXA0XGT*RpmcP2Ih1)0~ zhcCFk^FOK*VZYH;__O=H*A?lPiPA`1Kkgg{s~KOz`X>B=H>4vT$m()zACEuvOi$Bf zWvwWJAy_maAe96sr+* zQ?`JcsFcSbeu?n;!lfFwDcXmH!{pg&8z)Je6&iwfrvCsFlmTy)1FX1c>Wn6IAVU=U zlrL}PO_);S4WW)fqlS6&cw%nwj2wd!hBgkDiYjbeE^M9(O zDpL&|C+Mp*a1Q?f@y-_hTH#8iY05r)q7kISn8u!3n}8 z8>=DJi9>a=W~Qd%@Tfw-L#exj!cM4PSWF(sbV$^gW-gd@5oAnjPTO=rtts(CWd*6Q z;BtU>lBq*SwpA{G5Dk9iUGS^|W}w$yG8_cLZJlJ@EXb7_h7q3uWCeG!>VY4Eu|H*imYbc_jClZDbW^r~g>Y*ThljVZ)Do2L>5krD-h zW|8zle67@zx^GGHx@8ioBLNZS=tNpp%CAF(N&Eb@af$)83>Z%aGkJ~R_z@?Cb&{?%5SLF9sHmJ z$dq|pl}!4CND7exVKN7&x}<#|XHuoAo1!t{Q2vRZ0PP_*cz?Sg4~b2{JNYlT5G2OP zolQ1srW(d4OPFiw{{X5Y4$HMN#`32L-O1B*EEYU2?+9T?wbjCKf>UQrfk82RrV6Gy zRX>V%PIk1*8>(kXvS;9gFbhg<5SFGyD|tty*hBz_;-pAGAF8L4szVLs9c~I(Rax99 zFo$kzq#xvmOIK*U)hN-tA`O>GXjMUQz*eYJ`?8NvO@?ersMpdBRK}JWO_KJ)Z%j-s zm$%A2yD89e)kV5MgaaT;lhNclSm0GLqA;1J428pJb6IzJLhf%W8nt&A0O3F$zf_Op zhxv=jbQ)(^M%2Ixo~OGd29GO<6BdQ@n);9asqjj9f#FhdTs@DJ$N>0M^s!7J=ph5{ zf}{gmWD^UTd?sDFS5PFvt2GIdx}M;SRAEpa+>}A4ESfEJ!LLerZ-`T(a)i`G>iesv zpib7&g zV<_j!1{H|HsSgdxtEs!Ha@Sy@{{S$K4ijLs-4a|=bOP1f+&LbWc~xUUJt@(s{{SRW zS8>?_*GMXKd#bH#Q|#8KSeR4=O&KeJYT&7^6_F~l8Y)r_&ycD#pJaAfCQ!N-KD>X? z7z_%eCm-QJ5K|BmN~jOAb(LeN-Ajj*!w4T+#9dR@sAIw~_nBB$he~&KipEti0cs_I zJF1~i)O;t$@Txs$bxzJUP3UeDVd|$-@_;S~Ko?$iO^0tO{W~K*Q%$m|ogi$4rdifi zG#_PD`jV@c6u?~Jc~rrq@8qglZ#yk6wUf8D$c-DKH77XI7!s)bKje%7u}lzTC>OI+ zPoZK}A=9)drXyo??MyyZUrmF-u}7;_$W^1J^0p*$%-T>MpaTlD9d=wvV}-m*X{guy z>bND_cvRf|XTss!KLp6rmGZ7Cu91PLs?Q_ADY~qI4)B;|PyT%W0P#;UPRKw;e5MfL zx;-$7{8db+-dEzfTPd3xezblEy8Gi2W5bbof>)l;A+ zX(K7x{goXyPU)N?HAYoZT+e%14!jikdxE}{A?FFw0<}V$A~Kr*`3MMCOfsxh&Qp6& zQe>22?)OhPM@vGe9Z|QZ3R;{mx~L$Wr!~#+j8>BkMtzk#v6x*C57P>yjXek#GgYBf zfg9NXYp~q?6TLp1r${8h1O`lxqOfF6G(9hp}sr*#dNl;Nq}p);gk>W88T(&O&9uT}AM z;XV~pZ*@{woze}}PN9@;jnjv&9Iw?jQx4e}F(7$IuvirF>YbGRP>O(gO#SBSsFQR{ zn0?pG7N90Vt*21(pQ*Flsy9=7+RA`<#DpcYg<=NjzR-yc8C4*rolcn`Dz{ptH>nZ~ zs+brjDwn{_fUEQ`iA~PJsivRZPXWg%y*^V(mcr;ruRZ}M@ml=WXpQBaU6J@5z`<2|k6d{F9sYl@# z4O8Jz4l!~SUZ1+U97WW;kus_G1j%&A;Gd$KO@gQcce;nUaNR&&oFy3^_#N+6k=CQpS+;hlnPUn;Q<#Hg!GjINN6Yo=9O zy8$~Gy6bP%QiwaDER6fB&DFv;P+CMLbfJJxKWI;^@u zJ;~mr>=*pj{{YoCBOstUMZ&NF_?A!WwFJlz9ibRP(pR0*ax zRSJE`LE2~VRK}2^#^O~HNW>Hz9qy(qZk?c=lbh=_DoriIqXWtjvZn0^a)ez~(%sc> zhTIfHIx@7F%BSCJ1x)BW!tB*_$kyFc2!luk7s-OZed?soI z#tL*3)TO|ARfta1?bYpXWyJK}8!6Y?Qvq~Dg;sTZt08Pq3HeSCc1_4AVDdXG`CfT8 z!)1hewpB&!oC(u-=IFYY0Gf4squy&<{{U3|P248+6i%!s>D7hAYiMp1eF9+BxlNfi zROC69TAC&TtX$$I>Zd_6n?FQ89B0DPUafS~sj?+8rL8wqbW&ja%4uyOx0TT9W*_c= z^$5CbX|i}|9#u_dEd>a=sW69axnLg^$=A(ZyA-1 zf_tgPR}rZ)YsB3*Zl~Y^h zYy|BfA*!AeM_I}yr>ZW}shf!&%9Qm)lw0e*UuYLqHpqPh0Pe3BmEG*Ri8xHs{nPbS9m&&tCqX$^DWJ7kJK}B3bT0HUx_#!!##E4< z5}z*LoTnaN^M7g$rs%>K_8C?5ZWDb*0}c8n5btE|5-x-p?oF34;ZqcjoUSa^sxt|M zPU*I4c-K$=02$p;V>p&rc}ymwU32W3J|}5ah#=W<0rdO?2&m^nrD=*{>Y)`#=?hcA zC>Jn0B<2 ziIsh6z(RJXl+gg;Fg3>tuCHKII^1)*gBxWG*;CX?9S*cC6=h;oQxSenhwM3dPSRmq zt0Q=_Xe&!p0yhcQB^g5rmQ7HP``_A`)es8PrS??ykGj=IPN^Uj!v|H|1lT}e zha>Q+_j@iQQ%&M!J&-YVTh)8^1vRcO@|_g{oLO+~Ep3Z{KsB`+ z(p6nsl}I{wQ`NFyGn5(}MpMi9omd_fHwV-mOSqe=a_2W-slCn?ErO`Izc6VFf+6iW zizbehcFqyMfT=m<_!m^pkSx-v4PFAKTGrl3MXe)ph-*>MhSC$1^PKO`#T zz3QPqc{{N66(y0}7jskZxRh?G%N#dWlAynMSWf9rAh!#(-Kp2vY*Et~ScN564nlc6 z1mx?&@`^nUPn6p&)<>=Vt4-S^A9ckFFC>=@R-a_YI-8@Y;XS5-7H;x{MmA2r)ijzR z(RAOUCn%eFPL==@w9yT)_f2tdb%|8Sd|Ancr9+1mm+Z9*<+xgnM8ZeWaOJFfx{7nO zMT$91$+GR)RBLr@Dcy6GTs}7ohtl>px-7K0h22e5ggLE0nxfrjB}b}zKw4OR)j&&a z2u{XAbj{&3YG-IvbpkvpX8sos)1>81I)EJj7*u^)PCr!?^*L758+3jsXHfS{aMEt& z9-iJ%Ep0)R!%>7ClTNCI2U9=QJTO{GTu^mSsvi-i&T4C&N-yqJK~I9ur@2)0(JG|5 z`A#LFQ!qDMWlRJn0upkWX+S_h2npBaDX61yaGaZzOI)9-eR=-?s^OXFw!&0us!4!w zpJrP7E+m)fv?m(19fdMag;c>Z6PQUL) zV-HaG2n_|X8e$g?Dv~~Bz_eA<;Z_X<;3tPQnDUriQ!rI8-L*(-4i(zo)oMjmHCp@+ z3L5>9FD2Ir#jju!_$sYybj{U9dbGFlm_~~jRiUG}CUw1*wo~%Do(#gE19aIO9@PP$ zLZlzcDlMi1c27it zd@e6jDjCvMIV}@p%@l?JQs3&ToXEPD>#}IpHd86Oe3eJksC=q=7~$BdPP>qsr&ol~ z9^9ez!k0bMRcMhcQ|NE->rb3O&+f`ApO|Rfv&#c~uAj z*9bY`-s{yMyHRc7G8hJv0ao;Nj<3aj)BROQeK3x^jO@VZ?x(yEePdZ9ZPU0|mjXAp~s19b2iF{M@b zWcH_!r5FV|#Y4T7G{phv!YR;K{IUF%V#@wJs+t+38I@mLl~{+fvP@4b>I%$`k0Qg2rZeeiX6Z5LS8&tS#SL~8n z-r_L00jQ0}7Y4%DUCM}KYBQ!66~Ad$@QKty)OD`eu_obhTDYb=8>SKny5Wgrss}+b zC*4zrJ75PM`_)DTAbaC2>&q+pEfF zsZh5eI0m-uX;h{xQ(>cI3_^T(P;R%tL#e5tdn*uZjh9nX2BMz;BZS_h*!ErOmZwh1 zx@tQBr~UytSNxyan{o206+gqdy(485=}o~Z?<*u>H%M@^laEWKaV&>evvj~J)*5xc zVpVNy*WA?i>-(yor`2~6cvUq$f&&KXtEm@v0cFHGv1iKrt|824fT}sb*pZ7xqQ2rq7gaxJ?X?m?~^FM$=`)Hia`iEh~s+KpnW*FjHekbV9AJoWkQv z9N&~VaUx0Xd0bD%j^l;KdaSH}C0=z4g#C3VCuO5g-YLQDnQoN!=%g&gB^@;HM*SfYcWat8bZ67gCWlg}c?o_XUVLPu+N(8s%DnatN z;u|tcu!CIC*mIRb*6ZC@aX%_~7Q%cmbuzW5mp{3(dRbNnTH-uDO zQg`5+P-ya;MF$&&G@lxt`A1z{FiNIc@KGVrAp#z9R}j%mVX~Ui->Df7H&oj3?wg{ML9(l%xBO7l$8J;yIP}6dMi9CJe5-`p zD!PQH=fZEO230*eJ$+w}l-(^kC|~G?kT+db9pzMswUv9bV5xyxdY*DscRz{#6Dj`y z4^4x&2$6oE0fk-Op=Oq_4c7)skC>If)&!X2rb~}77BdBB|9LH0S zWomPHK$R)W!hxyUvxQC1@2rjq1{qvQhk#lJ6Sa6vkQ1yC=_Nl^Hw&tR)6daHk0C#+bq*UW59F#F z45O*CyA$LUh(a29GXfAXxL-G^`YKUPwkoJDbabr^oTkHD{M8c8KQe6U%Zi}$QzhYR zBp;gT=9_8kFjLw=GO7cjA0FWbGCUoX7d4d4pXG2~qo=<}Pa%V4!Z_Qwgmkd%nJz7f z$|0g;5~(`34@tdHsJ}yhb|E>veWP#{Vj9a+{=$)jduHQFB-_6`L1o;kCE(g-i`X(yK#;8WGmPmMv7N zxOU3YLBK(brG|j2x}_JK1w@}xbpha~XrwSVOpzcdHtA~N+&G8!*;6kW(&QUXuif@d zZ7wFYDGq4KAs?AD;!+m(RrEQ|=>V$5{6kyQCR5v{bM!~r{F|nmaINFYHu+a08i8YYNU^H zZd)sk^hp}STVR|X#*30wN{pvA@D~_rH&`<(jumN^hddF2ngdueJSMc7mXsS(dT+Af zn)2<&5z|9V2JE;VktEhW6@4G7cSfmc3yEVLwbR&M4U5$#YsHtQ*;)pyxmMK1o2oa& zr%bxUs^RWUmd--Z505KLC%H{WFWuBCyb*wKoDD&^RXUH#GMZ`B1_b3^oaCw+uVNEw zQ-OTlQZO#KmwIB_7Y@U54qP;n-tCnhx;I=i+a1e-yMVkdAPT=TQ?*uh0{AjIj6&j^ zeu|jTipbacE-kEUcAKXiEj%n(~@cHj6sjl&|s6$2J!V&6l(`8yn*eB@vs+y{w1ys{c ze5W=sAw1NqaH;$|iT6&)z*O7yDeh|;EvLv$dz*qTnf_N`$EdV)qpg!JZl%*p)75_|lJSGGsMvUr0(*mgXx%qfk!3~x zkk~E>(BYH|Ktz&L6LWOLRoy5UPPAG)E2iN#Zl*UwyVbwsq|p}Ee)6eJuG13Z8j5>g zsZB{}U?jLkq{j_A3@5qFWwez;(|cPQkkrwaMS|j~>OIV1 z%&s1znHvSr;b?N8PlYXU@SiR(#NAGsgYZ?qi%Q|0PzQKiK9i!~RY)1Y3Y11;Wn1Ah zA#lAxxCg?cp_6q;>K(;6`fqjR5=%sMs-?5MT~>3O7Lf|LG}>+x#+KnTp>eHaoN!kb zakAq(nyTC`AW`zTx;40yyT~eoD^+;5pOsp!Ktrw90uc17QWghvAEm?POeN2Bl~thB z$SaF$bnI0Yzr%*>rNz^2ll@k@CCyvfe z;=~*x9!sDcLeTIAWjZH56-_s}Trt$_xp2b9B$U~K zU=>Y0WRJ~L*6A|)LLn`OZ6k5Dso zXHw37$xXr=uf<{8gyQ2Fh+ADu`B%z#(Kt=Y7)~8nT3#@l5tV2cPPkZ7k4{$Jtp5Nd z|HJ?%5CH)J0RaI40RaF40s{d60|5~LAu&NwVR36`}w9)k#y2!kUDzfnb1`%Qv@R-J%s-N z!!*zebMUzg1LPUcGH9}AUPc)dM5Z7US7P~Z@>rorXS7uxt^|_P=`uL56nciBB44I9 z2p(b#-Qy%T(&U8` zT_2pv(_jY7Q~+creU!;=hPBy-XXPF%=;T~bUzaAVHwVHPQzE&amzcox`K{evF+l6m z?z|w}r?)Rpz|tRIc;ZjOFC9OFFlQknJn}$Eg5)hbr;k1kiyUP+4tbeTxE1|C$XZ6u zFu5ro@$NyjqxFE{bu0Pt(kB_0{WxJU_J22HNto5%I>DNNshWVttf+j&W565_^RhCT zd68%#pPIREs0YsP{i^`V(44uH{{RkUksR>U0pp4r7tZee6EWsTQsA=vjtc9xf%xY} zKcbz8`Ivho$UicJVFS(MOJh>ggj-YVpjC8tVxDW~v$+MS?;{Q|UTd@hFzKzkFpxG6 zv$zvTEBwV3{{VHtRQ~|rxC2Rt)$&JsU~H9NaX&}%^GWs2!Y2(s<9}*my_5&$ zc`krrrnft^s9{ZY>q-8sOHtigL#{Z@L1&J9pXRCo{{UkJQaVWkL@Hz+=p*7h{{WSA zr713_ko<;n4~B^cfz?1zY$2C&ee`SQn!8B%tjbXh6y|?d95S?cVHl{&f`z}`0g*?3 zWhX6A@X7t|FQtz$n6KfOi9EWfN2&J%P>g!Ej^mi>jsa%md!#W72Jx?x*zai_2m0>T zt{+T*Xv>SII;p7Py%(;do{dbC0qTPKRR{hs=@~~5u8Cv^tVOw!gy$)hX)#da`Z{l~ zY9Cy;XWNx24ID$z*MLHqtWX$m7~KKl+m0Zz3;i!1>CfeJ%7xRRG0oWh7Nz{JV5;P7ye9K z5Q70;W5`)#IY1D1IrO(diQ`G4`kD25!1SFt^mk4k-969e%-rGoG{F!ft}aVfFVW6H z6@7&?GErAA<3!4-u#4O_Y)_9-Vx<6_J=~$>NWLSw zqiF7P!?p020~lPkN{8{(i_zx3W8&p1pi^IQ{pTwU!xuxaIe@=n>B53TB^>1)ToMK$ z`9~hWL`B3!L~z_lRZSd0gYq98MF5B9;r{@=A1+RC9;Qcea#NRjWD2v_Te$eg1j_Ub zF*Q#4`wNZ}>f`q;(O=V5B)M<(iC8uxJsJWZo=Jmk$o2K8$g;X>6ZroC<;07R{Kg+n z0VC)KB>mQ26?*SMKk{a&5;yeI-ycJDB^!B27-A-|r7-pS=ePbE>{$NO1;K}$10_%m z^`jUs&FE5KN-5}nfe*m1pK|mdgYH(alrSp@C^~P`he?k*M)*6Uf)6O`Me;Cz z-~j|b_>mF($<(^@_fcu}Lb)IiF|h*kh3bliAHdzmu-9v%A0{eowBarRaD!L5EZIP9E% z6Y+I0sy~iM5q<_Sz`smg0li1eK438YPkt-&JTd`F{{RWoUqvsYoMxB@6~#PRIQU}w z@Swk}a=?fW`HC_sFWjZFa$*`*cM((mtc-ug?Dl4>7ouP!!T^P?ugpJAKM3^vK<0o_ z*U`h9G2Q|g6zY9kSvfOd&lYt|G!FS5A(jI%b&9NpI|pLZiPFWuBj zw+2P$sq#Yb#}iAf;oFUL`u_j}F~X94LmW>;32d0Kfe%SaYlW znV1@`4rY$&IsBuxrN9C+Lq9e$gak9L1Sj_jsbT#dX0j*;FJp!O0KZ|55Sqv1D-dCh zPfz7BEw3_ZsT^cfxcd<){y3#Pb5R6!LG69g;?5&Q>c$G8#r}?T87k+FzaKNH$e)6o z;|VGGqm*_|u8xw4*??;L?>?XWaCf|7zlHU~RZ3STK=L1_x`eF?Gg=@fe!}P6F8seh zxa{MDyG8+|AV2=+c$D42=}k1L%CbL4tc2RC(28Pk0Kp%hhA&FU0W$CnYl@^C+>rtL zcPVeGIBwu#L-8{qa#WeKrh*>et&f1~mT(@9oc${?`KQ|bki==}P3ued6dlqRsCHyt zhKJltPbXz?pkg?plZNFwQq9K^M=_SnZb2l{jT_Rj_?uL{{Rl*M7RXJJj(CYmGe>$k;D%D(0XIkbl?U8kTjiMBd37H3RL7SHht^e zCqg^>i%}(S@U$s@29wR|XUR(A_e+6S{#g_C&(krAgZXiO!eIXZ+H@-LxIiA@-7J5) zH=P)9w+}S5{{SJ*agO*oMe7FmjTA;83VX@z%COzV$H8 zV^Tf1=mnP;OzvuSfP|DlKaYT zHiHDQl&Vp>`u?2~UwfaBFi70s-2@1Q%LnN&ODQReLgc|KnGKc)m<{SX$W_KG{TbR{ z_x9MbfPM_ErRea}(#dj}xN{)IpJspIp`mO3SpNX&vUrJ^a0l1bGfTNkH;@NMx8&6+ zkdPhB+U_A>{{Y@uFL|I%Z%%91)fVM2?l;i5s#nCbIV{0e0J;1fG)tKwOv+oLIgA@y9~pL4=r=0Kwq^F<_2O~;}1_U0-_8T_fsg@2Z6 z*=i2y-D%4BIUijTI@{_2z6H^a$^8^6fBU9vZv&sx{{Vz=t+v1Vh*R?zp&ZH_)9KY2 ze{nzuQF3Z}9#=ZrdHmv@Jr*W|4Kr+!f$vP`Fih3Dx5(e*XZU*^bu2eg-ep zV!wu>yU{$Hy7eNJT zW6PZ0ksE)DhgS#Y_XOo+=hjx{R4B6Maol!=!3|fbi0U36by*hRAjTyOl@sCC_%FM% z&*p=0tGT(l0I&PANxxQc=G2N)m{hK`3I70NgZ}`841e{Ow6gl(Vn_ZLDg}SN0^&Lu z=gQ^rhsOsg5sEGjm_wd`{{V~y4&s2I;h2RU=~L7>LTVHD2+TJnpPu&$gikR3N!J<~ zJi@x2K4xWaL-&39JTPpI5s(wRtP(N8h+iBNWbimMA#WqTKL{JBkPr~VjELd6(BBgR z_uMRUnurUHc{RhIvVX$iMqgo>yS!AO#yC4>dG- zVWW6V@chEmdYp4#oX_`$fAzdGAHd-NDl{7(5^v$!7#HVtpQlEZcc1q{&WIz>}M3HeAeGDeU^loB; z?nAQkEA@RhKp4Xb5qax(=h{C{XNNJ?9F0TdShGIJBVlv)o&9Kxm!0DGtsV)1Ln2L zoiKXBt_+~~56nUR0nD$m$mf8k2NKA+6WYW3o09y+J|C<`3KuA&tqgi|cNK4wo>{{VzYVF*kj{{Y9ABEkuNx=r||E1=5rRwzKut z5)UnlyNMWhwUu8c8r#=gyXnl#pg))}$um4eGXXoyatkr01LQoe0S`h4`H;c+{{Xzq zNQ?Ii--7Q+^63R7RC@0e`b@}6aitm!#11F?GAF<|sMwl>)AmZ4h$DF_auXn4v(x%U zaom|OJ6cV`1IfCIZ>3lB7CAy1m4`2gMqI7f16c%`%HF7*37}yCpV=6X`9ly>d>9@- zH<5CqK#Iwhs(LfG;v+Xl>(t6X#x-{*ezQou72I{+GokrRkW>Z{{{W8-=rL%;0rW$Y zJi1%kx4ui3{i>#(x#_2?{nUTaWdd1;@|OhD*1O>}OS9{npjw_avourtxjnAGsmRA3 zPuD3PKx5zCNyz#mo!KCF{{zmHRG(tAPGMQDVFd!t6nlDNoo4{H7fDacfb479-Jm2MR}F{kuZ0` z*H|SkIxuWws!XY`RL05vW|m3=7f2%i0K3H{jrpxdDv63AK<;QGiM;uL=o&Tc59hf< z^mP9KS30O6hse=js0QYnVi-Mm(bDpBaH`4Lp*ZZrH95rbx^`T6>WK!O^IplL|n~KGV zt;ykhjS|&ydiu@wK+N+O$%E@UXO+I0y}1PY-T5^Q$m&$b`7&n!RJSNj4h)^))CaWT zp!1nQ_qtcQMf;aIy>Gou%I;S+jWFxxSh)!OIgbD=xgpf=l`70D0Uu2-$mo!wUop=B ze7J=F0JdvsEYH8p%c#z-ECBS{i>vh$;Eo@m9T3J450Cc}@iDLu=6Z;h={AIaQN)rD ziNYRvwW)W^6(gu;#bpGJ?eUPxR`8OX{pXLs=8#9}&4GJiD^<^e{K4@o= z*M~f*roBAp?Jn-6P{M#ga-g-CFhs$@eJ-8Hy3CWfzXUi3(qhUMug@_VQ`v{a!Y`i_ zS{m5gm%N<0i@1YRtgMMwyYN&%2L?yU?k*zlzwTWC&kTpWNaMB&Kio?ccztFQ*#7|Y zJFt%;ovOv_!NFV}n%lf{w9D!QGQF(&q9U4)RieHz`{)Lmms`p)c4knpQpfdlhHH5tQp9s!VLZpYwk=)s%0m^Cx{d93!0FO0~E^ zJawFSOu60&=Zyj8f2?=zrTdA36w_g-j#(g>G=90Qj7RNrYJU6-0TKXU5dQ#$<yIcPE0##4`=0LCMlW?#hpF1u20fGlz{{YUi zDzmk(A{}rh(miMp0!RJI{EtoaM3~h{)q!6Fo@$i$PeApD`k1;;?;7SS0|vA?*XHxs z8n}g~L`~MVN1R>-zcsL;QNTr5+@;(7z>px{m_IOL{qsIbcZr)?f45n$5xE1uqT(01 z+!RI00{!z!KD3$sCB-AoH`tCSAFUo52%2gJ(|h;Ll^#V^JIP@sa?y#~=>)O<~F`5#nXRlixC! z6R>pHwWO)e?!rD?V7rh9hs@D|^yuYJpT{|6=(_5JamktI{Vois1;k)fbDZJ-0HM}V z!G;lI0l%;tv%;RaHzEozk#z>5lL!j|D(*uWypPQgDQ+zw_elHqfX_T-F;P3_X%en( zN81_vgffHIs~Op*eEe!vO(p}e<#SE+8Ha*2d6%XK%OjCh7cSq_!__;DMO`t2qxuI~ zxca~^raH(cH$X;Qpd^{!YGG3IATxoK@US%!8-(?k6L~YLITROKYM*x?{$fhw>xco( zd`4vMHO-2z_Z=;r$w%t$K|W=*skxPuLLyo|;3FL_xzoS8QN!wpx~)*bDxY)o;Qs*g zd)6FvD8IN95XG8hiril@;N(duKg@k`kYagj{vAhs?nM26(~utUZ#W^C^sxu`98(tp z!Y5C<&f?5W?~M9)r}ks$*WK;^05p=|eT_I{o$BbQizaV%4DaB4#W?=}7-i!{UJw_< zbNkE&MCtp%^oY!!9z8dje&&1A?CL}gXeJ^~+@^ZFwCOiHaxb9gfaS~!$I%mhLoRI- z4TJ2{wQhKVIypm-o{kHw>iV0=?uW-1^|^ch0GolVo{!&#rXO(3=P@(pk$l#!UO9-9 z^}I@TrHjz8kIYq}KI1wUJ(Q**itBKJ-^X@;g;U8)l!D!0z`4WdV2Fp?xR@A2;rxBD z3fC+1jKe%8g^f}F0C|+6VGp`4FdHQQhOnbkd;y37_+DeF|q9by33Yqx*5j zI0^MN@i~~op%)aveq=;RA8SP=Df>DxX|}d8X1_KmJ)8pDCrJ4;-F?W#E>$ z=}^HtxtmbTjzdp;8IL?%RAO&-pD|SOAtUBC)K(@W3TgF~{+MoY6{GQHCEO@w!pa15 zE#>eqAKVBeM9ibE6UV$i_;JIB)=H+1VYm5R)1qN$_hOOm7J3cD@Z^Ncrt2)kphe#Y zxc>lY^A8E!Rv9f+uQb?~TuzcizXFwf|2 zpgs2h3cc>(Ol1Ble8!vnGfe6i6^kC_HhWBbUa&x;u)?dkW>oULz^l{TsQAXh24gaP zo&K^f_W?1M4zYi8K$z4Y;fuxdFi#w4@M!01sgfqXOav!o#qE!#Ru+d3mnFx)69A>r z`^U`*X`RgaCUi?TjA@OYnfT)^T7u-|kGL3lUH)MJ`;4OofqG!36j+=@4mxH30OAep zdgKBijtww}8@)4g9#$YLxMK-n)IDR7sB@&WLkH0UxRvJ>b=bQc1Yme@%qxoO^l;>i z(waro_!D-ZYVU+PBK|pso?-`Ib)SLxkWXBKFS=NI7>T`FV7I{FCi&M07=4Y=n`R%P z!mFqF!r=XJmsP8Oqj6aBY2Xj{9JPDh1!G9}xMDoOjRA@vp#0L&AmuYYn6Q41Y?R=l zaeI$aKbY{G(;d=chiTkKULC{*jZpic2z)p&WPNn`58Px=%dW1t&_V85>>gw2mGfD0 z>K;BiDj3@B*9(CcmNggu0C;VLW;&nx6xJ7p{{ZVMNaSXiAo?4a69nrFl}N-r98#S4 zf8c{#*VC?$(Wslyir{V{v*uS;3Z!#%6hHLMI4BHtsO~@(G(U-8{zg>R??rBHnO^Q= zs#g#K3!K7Sdz;QLF0;xFd#RIH*OTt_SB%9Q%cZ|eV|v}{p*1`~`TWL!I<7JjuW^Sg zdf=UiMsD>v>Cj5yL5j?j>F~^263@*gIsnB@C3u(fNZf}(KR+{;E2sH{LXB|`+sxuc=glgh zFBq7(I7XJ1nj4Ti?u!{Hnp<7@$0h`CCv{Mrzv~;C90kw>%jSvPKgA*Yj&@EsVmx_+ zPz}JMa}~-tyF`!EMJf+5+*YhyXEA?hIAfsYbYlx3cXmF?WZZ2q6_bgYFAgotI_45P zlmxoU<&~>>b-HiP>N3VgC2VkGhxX#BT4r16H%+6=c!^Nu%!TtTiyWaDzjEKX;!Ic% zPPk!b2^CmK^5#(-9+%sLZ8x-^?gj|?i-8|p#9;;=)jl;KFVUl7>~R1S?gk)HGSHQA zAHiwP$R3&#!1ZGrkXLO~}*`)klX+Tmjo6|bw(l=ji!@+o-`>Hd~ zuesxWuw%4bal${jfGNj*GjD-@PUX;F!xd0IVyr8Yd_2n@qwWCD!tTNU0J(Ak3Qhk2 z9)}oZg@uGz8XvF{1*%WYi`F zlL#zepK-%NM-DqAy!x(0ryyWA@;GngLL$%cyOu~@&0r3X-0uGXBbuSb!OScUA)e*s zZ<1mchKTv8z=esWf{SMGqsy=f0dm|R=L)9nNt948K9_5(f(t~7h&rcwvE z6Wsc#@Ap?NpMBgieND{zDKrv?;fF517ZqFlnr`Uy(TO9?P*2%17RXi^b%BN0o~|?k zhmoK0bVJP2563PVKWA|*5^}oAQ^4e+*ZwdD@JBx$9VRZ*(^UD+lH4N4!JT&p{p1za zxAQ7F`(^GpZwu&a1e_zP9v^{jwkSLZWI_%DI&Yl$qQF1TG42k8?onEOq?z4&jRS@m z=4I2Of>yZbKtL-$nw0Q~#Kx-~P7SQF!=FtqE&9J9iCApA5Z&^x>KXTc|GMavRL5 z^0=u&(lgvdWv#>sfHOyRBb4_5`MyE~3^^F@nrr9^nDS8zgFy6iL_a?+Kh5u5MptL0 z^He26HO7S>GU)+k=}eoMi+b594$K)FpczT^D9L{cj;DfA=iFWv!tWMtkZ6ZLj%@(< zS+Ph=RF&k)8Enba;cn&-?8I?HGgcOMuf|}~zv+%DsXfsh(z&i74|7-${{WMyW;e-{ zta$5QMsVWftI(KseIJ=cjGyKp=y*xGe}n%3iANU+#}?m^UZZ|$$-J}QI5ai@Qy^%p z1kFaE759C|d-MMQ#t;7hM>mndHDjZS`GM-oOp)Xd`(b2zG3}ZjrbvjY>JojLxt=ET zpq12v#0jeCj){Wor#t570uvL`%Rjm=nAdUu-ybxXle>}s0H$%2*%$mVuLm5!tV1k! z__KAXWXI$glMo+n8Pedn)qb%`vO^MAIjP0Iy~+7mS11nO^5>wxoiQve1pUMbWM52W zk?{Wj_oqX7cjjFq+YF?(>hB_ES<6fyp}C4r5Y zHJ5@Fab@EG9oH?4V-^kqzx0L%W(`1RWzW>!>ymRl5#~4fv?gl49U9x%p#K2ZUy7@t z`~jSwz6*2sGyed_O7yyan0bI+k2h;fz8U&6Ju#P+FZ{dcf7Ir1YM`Iou)0-GW2+PD znK>!fP?4bxb^Znx#F`axD$p;+UR|8p^jJPQ*Wa_Xa+otS>_wdoqJ09CZtJC zK&%DkI!nS_G>%6oLBL*S7WH>p!$NMO5kSP9WAB>P#m9)>tSVw1-1VGbu%IC75MKZ0x}GqGf?%Q6T;(9Re#Zuck*Z~%5WL* zcWL((mH=*_5}iA}@FGS)uqr%md)Pn2{&Hvs!764&vRcAF824W5=C>lFlX8UCi!$;p z3C0*Mo&-K27T#+z1&mtRnT)laSZ5wfBWcv%Vih%bSE;b zGtmp@W{>{>=JOMM5%W`B;%KNgFn!NDDCfnM-~$2t(JzMO9+g-P2=V<{YLWi{D-%?tfOjFV z2%XD>5&r;NkFg=oGseuYos0hf4;sM)g9f)~+^m#E^qEG0c+)CL{<06-k>5Q&;MS?* ztZg`sQbNUjFu*s%?y@WK>8q22@o*_o!fWK<^wf0q#zRBHCsa-#+~c&1hZ?-ULA!%2 z0Y3K{7x-gOVuKK}J+N@eIut+mM&zjeuH-0F>4%M;3@AmDr*k-<{{YW)9Wp#}lm>hS z$O*#{nN#5kK)*;hC&Yvk|tR>@Y-%eixh+Y_a&|wL16i8BGRo@lnQ+ zKC^;n#$Hp<$Gc~^Gy*F0C6)6fBchY%IHqKf!sE1{-pV6Y?87PLzyBBzfC zx^{jq(Vxw&@VGe`pP}h2#R1A>8x z&Mo|4QRwo=n#Fog@aV#Z>luhB#b?dviC39jz9Bbc>zpnZL-ibGc|d%_o5v(KAssJS zq;2kJhFXMitfPuN(KamJZRL6(E>7O7!eQn}jgdXrY-;R%$JW6s?xo@mrE~=3JWQz|+nwF@Fd5F-%$31~U zo@Yl05pE&NHQ~Gbt(gy-zgcyC) zUSwnBWip>`7E0`C7ZS+4`eL`Btsl!0p5KV~@K>_45$jzIm*E?_4ILj=#y*7HnF z7`Bc&q!s&lx{`0g;G*_N4R~fIYlB|_m#0C1Ft|ppXJ6MIW(E0-2XVi-w5%TLqq)#u z@Z-|N@zeSec+zIRQVVm-LxceAKoh@J>Sy9i8@ccQ0H)8^&^w)E>V23|$;I#&2>f0KmOZxc>khXQ1AN61}`>s0IDNLb~ym;*A6* zd^Y;I_!a=n*U6XH5)?lfo@j24;5X(m%(Pi5Q>@~Ev7)1ewvXtU)8ov5K0v+E9PjnH zuuC7BpeggKK@g*_fB_?%n2e1Gcm5langcxxJ~=VsVerTOWiU+@2x}f5H+<>M``vA0P9iz~aHC%)WFPBdatVFBnakhlpnQxjcSgO1naze$C)?K*aYt z1cLoKL|6`q_eF&ze=;qGNq?riYX0U{A>4V*-n;(*S*TLnsDhw=uuAlW!Vy7y=i!Hd z-%zx45b1j4IPg(Zq>s_3=Ar)pL5+G9reNE}{Ad!%;!%-$PJfL_S$nWQO> zFGu@QYln;TMni|cbEjY+e9Dj9T!ltmA2CsII9_Yv^qwDcb)TjD&;%$i6~%~>SnTwH zp@EeAI1)rWRlso`9=brF{)RO^z26L^Ods$tiT+WqLdZY4TZ7}!`EUo_r7@{Kp?c(! zpAKN|vHBRl-c|XYK4DIiK&gm_*%K)zAY~kQNOW)<$^K`EI*stKYBZ_I?A#lAjFKN( z`gw)sax7@+C+7z=gXp@!a3A^7J$-=WG=0LMGhzTrncr1_2PWpgz0;&?HIw*<9K=qY zY$4a&B^X$Fhd56h!(Mc*elKlciyarnxq{YTE^7?O_YPDbH65Z?NP43G05u)Feo(?h zm;V5j#|)CMIgLreQT`@)3eV^rQuW~Zrf?O-0#H0Dt)b?bE|!5;opM}U9)lX2@Ot%r_`f?=sNBDv-(#`yM#fPoyj` z5^>?IAiVw4(t(a*XXO4J@$$~}#eE#C=Q%E9HmR+14 ziH181_qR^z-G%Q!b4l4dM4S<#U zJM9f`D1#jKE};28ns!@N?ll937Y2O(v7*$??&ef;?s3LaLfrmwsnUO#PNU`#82Z7+yzw6&O0lX} zh*xsaeHY-=&nXX?46QYXy%--yHT`+=W2ambyXa}m7{_8c`-NjELyRy8_GCj)57!p+ z;NZ6iGxIR~$C%Gzf&01jK)ft9*NF|}OdcNRu_zyK$T}aaac?^A9;+XjkL)g!+#Er9 zYsy*UB6`d_rO0plQY6QC+K=2<9j;jlf$cDa-We!p69ahuVjDnzx`>Vo=& z#unx7ZI$vv4TTT0%$VpRoVe0aK=qm5xtyOwaF`XK;a)Da#4q;T$X6aNTgAf$XnD~5 z54vog1M3F>4B6L=t!T2TFB?0K*ORjU0P76&#-1?@01#`FHN7VvbH5R~3s0$c@c#h( zJGtSgvlG2YgNEiMy$>0^i78KaSW3=AahE#CeOxo%2Q_9zOXhlNm?IEN&!$5_v;1?+ zR)-Ei#U8KB+6ujMe~y6Q`9T(}0W}|!HH0X#PdedzF^1sN0fD)M-L);sHbahhK)fDa zZmn8=FhvuKlXx7T%_v9D;$@x`f9_s20i4dmh9wNYM4%RaX4ENs27kMdSIq*ivYW4h z3gPIg3~geC!qp+ixKx3lg1RF@z(1}fbD-0}bRA5viu?xHISde>9F@!ZWPum6?g2S2 zWgi&VZmarFmb>@u>8;iH_beYk`eP!MVY#R~Wc~PLCsA{_0boDo22P+DjGDX*d?8sl zALf$hjJREynzM*so5BdB99+Ih(|yq3dNM9&TSy$gx-&I?kGb%O74W;&zhL?P;>WP1 z+l^I~AA^*U=@v}KPS8G@iK8#(Umz)dX%=IgG4P&@TU}vw%Z?hS^PO!$KT}`Mf^Psg zG(e-CWk^rj=2BOf{EC8y^BBO&`Q++zEmD4&g5Q&XwIb*W5j@Y}PS4yoD3475*79L> z@k{d{_fgX?G?m8DJx3Kd_%d1qoEREWCZyv@=j#(sy3kzu!$II2)Ts<)Q1$R~T}WpP zQ}PIJ6)C76xn;#fM+}>%&YHj0I%wU{k2HLLhINuO1cQ9d@}O((pg3-Q0oCNYj@N2N zpXVRfSTqJnT>_hF@kr(vJ{>Ec#)?oI(Z{uLCl8zM8cTYHX9z3;jhc*!OGUXi*`oY% zs~&;G#cJ^SakPFX=1K~knx3dCb%C8jQ_G)vD2|K2e%>E)3N;zgyhHJDr)qzd-qdQH zS*J2e^@)8}!!2KD=zLtHr0U1emaM?W>osjv{3 zx421O7|%){a~UIg@@t5nFYCVAiF5G^SQ=ohqujV3xg2FTJ9-oMnKdWZ`;akE!s2`w zH}LuIbGj*m_vUQn4LmWP*b7%&T@ZM=leL$8Y-c~9`T%fyWi2?(PlN_I9+W3NJG8=p zC94#h;{{i`3z1*hu2whmyIRudf70vIl<&DTjl7Nv+Z4aP9BP+1oxM+X@#we^ZWcR{ z8`b{+t+ru_Iu;> z#*h*(*8c$F;9jR9$v@ntn6Ugln<4O4zcP#Bb@xwC=)_RHezR$_`=%6j`U29qx8zDIAnk(oSEhkf# z84x{t?xPnfm_fCDe&Z6kT9@NHgz4!GZQ2JU%6=Z_oSu2SW?`JOe|NG1anWTSWBIQI z;JE()>tB)r_FvuP#GrCGkvsL0`G+u5p^g$(Q)8y%sk-IUY=Gd{Gkw4zYrmFQiQ^(* zQ^zCYiMa9f+%OAZ_bl-GaBJI7^)Q;~>Ly;NgQqjL>xWjLwb&q;>r^&H#pB2o!S7Q)Nc2y zrE;vGq4XR+q^(fQB%GgcVUiWAtzk+>^AAXmF{|~dmOhN<8`ac8RJjWDWp^1(KJ+z2 zJN_dB6MVh#lMs90aUdvh&+K!5mM$(Rvj8|uR6T3in(IAW9L9<9{G7s}(bSXU*@(mr z<^V*8dI9&+Hblac{WJ)vAc>md3&!GN-Y(bfx5`Kef4G~#0)jUNgh5`r#Mz=cP@yTF zUlUx?0%-ZX$EW5^$KFwMk5ucR-HLh+x71$64Jdd}@@~Ern;p|8iWZc8RUxgdbm4G} zo|c8q{;2}!_$EIFX0+12a{!$L^C}5k6vPk@9PHC=b}1M`dWJSa1;bml(HA0Gi)3O# z1U@01Pe-f^nW1~1G92hJX!5L+*{FEpzdZ<_%p^H02RHZ^2WXq5yw#O}ly5FGT8rjU zZ$kKBuG61}SO=r^G{Jg@3A&|Qf6v{_Do+E47^Y&o5k_@KKY3S5l*2i}B{E=hrL)pG;=x;Y}ixe$z0| zXQ}Cz*g9gmea;_|{K!h@mg^7uBdq(a3S<8OzWXDBCKu-l{{R5xVM6Gi++%{3#~*LU zh1C(B#T-B^C&Rq*l?MUAJB80Mp7`E51pb-kkbOAKhX6VTIT}`x)_~_?4i+iGHwj zYI_40Rz48xHBfx(j<%Xo6u=E|rP9a+IVaN+SivtfMKPDC#`pyX+;Z?2qT+xqf&23f zG79+N7)l%a_Y5LJIT+iM*uVm( zljPDjfnMW#YW^5f!|Obm{{Xiu${j$)dyDQV*e@nRz155<%(AKV&TASe4(?YxeBNM3 zpWNcIIs5+r(#@em>n#HvryVQ#$_u*KqlPOv-|~ zDTc}GyoCWka^OS}W(x?oc+QB2__$UP>N;b1826{)kpO^J9T%96Og5MpC2sqlY99s-N)JiplWz5?wYhjm%!T>b;SBpdl0w z0gaK-&Oq4uwy!gWJTy{!o*cC=%v+N+V>5@40ryr7$poJ_dn?<%*(Y6Me-VFofR4lc-!{vzFza3LujO~G zy!e>_6bq@3r>q|=xXMVLE(uynM0X}r&@iT91F9JDLXdNO{D66cwB|<> zfjONYjuKVS`u_lNDGsmR;PR+c8e|~6ZZTg0d~(`VHx~DGsCENBY-}HKuXC^${{VVf zm99o0^ZYxM8(^5wPDVM;nJ5Wq@`8%TK zF;ho4pbH>>nqAh#h`srVpST?1qJdtk{C6{3^B)cd(w2#ZOeCWcb$CMI^i%^~$mUFZ z-wq3`h_BOrwOL^cI zaWamd_8#FP;TCu9D2WpYz|%tUW6Z9C6W9LDeXazVMe=ZJ?(`lr$?q6BF!`6r_nV(@ zBojxUH7RhTzPdl~y1t?TO_+v&^ud0@H5#3|1Cs~Im$Ev4oWjF1$Ra5n#wOHy`M(Xb?8EVd6QP|jis;V`(hqSy-)}~_ zaCRSVb|_Ezv2gzYy(y`z(C`*!{{Z53F=?CxO(UzDeZvEsOj=6-q zCI0}IyTdE?87{6B*DC6Ob%ikkz+?N7qxLUyLTzEYr=Gr|;(84LTp$9+_n1|zk7>V( zV?EON=jgdFPfnt0Gxj?33+y}&Ki*J;!`5Q{({+vxx3LQU05PkFN~lnMy2n+-G_hUqEr z8g4h3d?ehl-SN9c%G5s0%Ho0>i+u;+H2zQHCqD1t8_dMRd;FP^gnmO9usAz{NKwl@ zf}}m!x|p2ZOkxJQ)hN5+oMrn5Onb)p@t~diO#YPP^E$l(RTE5f0}Vj09SqC3FSubu zHsR2DpQ}Y!JaS*m_t)A-2cUQ2sFPRH|nd6!*Cr93eh)<=#@O^c-9 zGF$f%Q}gY`f#N5tDY3)rBnY_Y%t#{b7?_rU{l%>Bfym?@Pu+MtJKQg${${zue{9O1 z{{V+e$}c0K5TCKbP689IL5Gy)G6b3W$*?G9TN+b)h&POAxHEWmKk$^yetQSf_cBgt z;^J!o!UF^T7Ss$~$|sHd#(O4bxYVhh4R@l6T`?#H{>CiwPhp77XUrMo<=$Zj9s~B{ z#-&5Q^v(%opRcnIzkP6Y@CYjd=5(IW2N5}4YyRbwEN>#=r!s#}yMt&_&bgIerBKvK zaI$J0XV6>|jYE20E(OhQ2Oa+a@Xjt?h*Na=KnJ)ev%6X}Ed0zQ;t+0YRdcQ=v3M{1 z<=igRF>=ZwF(wyX_5Nj$#H^pm)z}|FCKJwB0S7tbM=)0_P&fqSsOl+0=x3Ka;`q+o8<+- z!}2HG^^fcBO7L}4K>6ljGvi38V9#(L4U;|RIfbLhba$ig$o}OfFvazPpG+aztD-oV z`Gpplr41o23KRh}8Rqz#P%Hju-3lP?seme-T}yWy9xsl($YHwQoJ8X@I|`9}QAOZQiIBWc6!4`8EBU`KN&9F8D%#b1Y-!p~Bc4sjd5a;Vq>-03WzAVrs>fm5* z%Z!!h)x~meT%^)XH)I`jp+8S66P^@bbb~>_($8_!SD>>5l|-bz=bR8eFnV#p_rOj=F6ks_rSTYO0XB=f*L4y z+(m`cf`820Ciyn7OrCDwAM{bfr!g>RJ}Q=(x{6x4P=%&VLb;vJtDsZu)hDIc>J3Rd z!$SQ9`7RoCEys3sHF`c=o+zLk;>Qt$&$7rX{+vW9kx^pPpuN8qy5H|1SYq;_l9$Cj zl0{QVls8Y&m}|ia6Z)AjFsrZW7kpn(U;4)#!$9Mm0wVta?7{PD!1$pD58sS{Q4i>!l!@ix4Iq;=^Bl9D$BJxs&_+Q64w?q}!!Jy7oqsq4W zZiQbu&MkiG{{S}w$FVIPvB z^M767?9M@{;_K`j2ygx7msD4O-)};zZcu&xV9q9ye|Kszjs*e^9Z!AGQgB0d1tj+1?~({X zh|y6kL%Hh78&m{lym~m+st0J}zJ~_PfAalIXUL4KW+P`vew}I1DCpxwRbNwz?EN@( zEI{$~d8v1r*FC{QxOYr8j<@4VCoSfxCboLcGeO0{??A(9$G=7U?qT8~bpjX}-P^}J zv!l;#$E8 z=sA;)wFqa@#I+NcX6eF!aNCt{T-oDLecpiax^o-cd@F*01iv$Vo(eH{Syj)S>W9Y^z9^%fh7{h0AzDH4aIbRfmlqm@E;}2~VSI8l<=P zK&u<#+@N{D#s=W%iw0E133NM13!OqiY*=-Gu0dy<1v@=1oY2V zNgQNL*x|)87cw5`x}WA_icx&aI?jHKfRTzgQXfwZ5tpgmyIQc145oCy2>0B(}{B`q{z zBn@i#Rt2nLc?4cy7DcBU7k#CqkO9o(sDR%L-MoP_nZX3%t+`G6)y?Ckfx-U(bg1Xj zxTaOXmXUFO8HGR*OS}rJ#=p3(5O2GY;`M#avZDaz{CqICH~o1md3Dc&hy`fA+u~5; ziXo}fjZCb@baAGtO>BypotN_6&O?e((Rp04fmx0|5X60{{X70RsgA000010s{mE5&#hs1{5J86(A!q2P7pjGc^C& z00;pB0RadAHkg!x!P)6ApCo~>7G=)yyiI_B3t_0 zOTIjL^hTZShHDPVlUJ=7CfZlFcL|$ZGd%GPeBLfQoJelvH{qp(4D{wq=h9glgVv48 zCY>`(@|PJ?{hc5&JxhK)__b*1t`P1cF+f57N1FC$#UpJTpCemhg03~|s@7gMPMUHv zwhei5CdcC*?25iog>m~qi5>fnGsb9_e z9kZi2;$g1qTyGlB;QmS~C-iwrGt#Ix?HIJTUJFO;MAOAU+|?9vaL&4sPaMi=sU?-= zY=V^08Id25zagE64yDYa=1DGWT&o8}Jm;q3O;H#Un?D;Cd=xniMm zy@VqnS*vTRLy2Y^v`f#(tD&~4M=uOH9FJMzo;$1K3A1{@cHkh{j#%&{uD0;2fhi_Q+eVMnqae*V<}S!K z(oMwY64J~$;-hW4aGbTVx=LHQJzQY@y!pR3-J3hMo-3WVEi-Ldxf}-+`5kBTcwyP6 z&&ayVtRk&x)8tW>1g)XSn5YMwbrXt@^s{7!2BUJdrgwI%M24Ub*PN6l1+8KIN$eU{fPFJH;=+S|Uf z;o6ea-t^*~bZF!O%MBAxryjs?vVg}K!E*tt@XUI5zU0Nl|9}y<73UQ`^T`m-3L=_yI-X`8>Dc{{V5eWIJOTYMFwe%UHS1S-W$!VanuV zDkBJ}D1T~hIh3`w?^fgZwV20o6wHg!_~gu@r#I!+yK-xko$EB1jz+GCnc1*bVf*Cd zz7o%!%j>@xPt)L+PZx)$F_M$47Lp#n(vGxH$bVz!b<3l4F~P5;xy<|F$7P1OA7~*pj6=R^8BKI zv-sXf)?9{8F9BO+S{P4N@1?WbW<~cVjz6ThhMBkKoVr%CbpHUU z8n9zr>G-$gaoMAh<+h$rBDK}yoY$%F@xecqm0mC=Qt+WDZ5YR zylthYJ$Noke4=ss#jXZ*TI_8VtIdBA&3yj=PTb_c-`$b2=+%{%@;L_2o}Oo&5*G=k z1U%>G8wmj=ZUS@9#Ohh{->#e-X-y-K(#J)y#!tp_5?niTaq#pfrZ&j5UQ^KHxca0d z5;*Ds{{W9KcYa55F9_<{3Jt}RH<_EVdmoT~mHeafAI15Fw%pt$ zE2BuGf3_R3zayJ2%4Vh+t0=?U)s9cn-ZnbyvdoVu4Y`+1u!5n zQn~a1WSxm9^ObS?!|3tb3Vui&{y&WdUM>|LtCxvHo#r+1l!V*mz>~j@9&%C-!{&&P zU(vZ;I*?p-UspglZRim<@)i^ zW%(-JTaJi+9cI&-G-}RCnqcUN2hwp_D_Z{mx4u>SW@|!Ozjkh%n*qmDu+oVaL)bf& zM^q4jUL_Q9PJHI)*M^s@iF&W}h@yRc<@#MJTO5C09+Q*nIiw#BsJ1MS3I6~dXv;_0 zrh|rHmZFW|2p@U9{1AD}W=%PfXtP;m0BK74pGoNm&o>%Vx=Tf~%x7xO*~e1q{9{== zoY^HtL|f>!%Dc_F;mqrhmX50!T;-%G=P&r|^-^1~K2R^QPbG>~{c4}b~p!0Gq#`H{RxtPp^OkL|4-e7A-nij_AmuxLng)7aODX#L{h; zKxRN=Y~hKOkus9}2e;!7Ze$~I=pB-O(85#GQVCwM&hGc*NF?LY2X37RTn;; zNx3M$RFx>p^-|6LH3|O6e5?gis^ZB@xX!5sOidA*Y%G&$6Z-0fi1t^^x4s}m_}!7y zKS{|<(-XMkH8b+@mzp^vWGL=roh+wSY!lQcOEDzn=*Ww@BUW{QY&um;ycxT6R<<=t z0W9R@D6_R2OlXX-(NmX)4bY~{_Qq-5l@lJ7@X)7go##h7UQtn`}|m15qy?N%IObV{&gEhd(VDmNUG zT}rcT?eZNKMQj?I_pW`fm1CP;$+bLs>%|IYJYDy3&ceHSYFahS+$w5LVq)!{eVWyd zV7|Wdq>&2vXq^Kqx+nMYwRQHwx*kK@{nbhk_Y*AV?fp$BeYcPR%Vj_Nc)Qt?)hVWl z)`>0ES*thsI(jq|L~l7DoR75+>JEy8Ge^2n^6=t6nOFM3XB@?`X)bF!gl(v3o@5&CMJCS{jq`Z|{Tguvje36#hTg_fzRJ@gvwL=|)wj@JONt9fOIt zbTUN#zC`~3bLiDnB0jgPRcA8?u|+BsD8O3Ns523OnzI`YiYQ{J=}U-D+xn$dXyp-x zg(|*!>kpt!I$8QmSFFM+*ZzAg1#A`ejc6FbGgzQu?7D~)lIJQ>hr!XKM|Sk>2dg_P zaaey(B$f8+8FSYDWaqXUmAv^XKPhj02G{Q7#%(Fb@=2ke$!C1^ADjKuWlK!XottNykx6YI-Jb4(YOME9Z^){o0_SCag zLNt%}1#6TFhVt3e(Ggw%s;7xLqK`x15y|x>dAewAFMiVO%HYrKcUG0AmClAfdZhqK zZz6KF9(tOIol8!xjJ_a_T{wwWB}bo-@+}ukzF2lo>6TwAeTe2TvdQGUyy{wW(E)bI z@6NzeZ6D48PcBjlesy1|X#KjSOGmS-md`=DL`B&%cx2= zxVw~Z3bM)CaQ?7%A{r>nQFB;$smiZbI+?7HX~Xu?Pxm!lO&S3<~%>Pnq~>O{w1 zetjo7{Tue~l`LVrfL`3j33BsMT-;PM(rjx(P>BzO7@+eNK9HMv`7TpmgQ`NLOf*EL zH_YoPBuY<-KzTT)gsJ{~<>-&P%Df~x@>!_%yqYNUEgxQ{wpuYnogmU?HPsU)YEMzO zQ#AJ?tSXZ$?Pc)<0(ScLw9?YG)7|vu`Sgsbrt>jXtwRafDBLT2*B;;u+n3 zvRB=oLe-`f!DN#puh`6Ho`q@CVzMeubkAc`B+*G~Y3Z_1v~*5tm8Ws@&Qs>d*B*EDTa((cUpY6Q| zzd)ZgK6>cCz8*+7Nzj*p3ewP%W(=D98U>eUNf|WdC+xTMvr@02vsFfhyvgBX% zc2nvY)ql0PuuW(q$i9P-jQ&24@xp?;W(#au(vZ<;{{ZsqG3u|RanY`lt|`YSX_npl zM#v{@1eLps0k0)LeC8PbpFh5Agq`0wy-k<66vPf~!UZ)TU&4Bl;H7w{UdJMU`nh(FgwkwE`@S*FMR-*^OxqZ5#K|KHCYBMPbs^%Y%TO(6QHN95YK|D!B!d5+P;GF z(4HL8_}GTmgD_Gzj%G+g9WYs*!> zlD>XPvg@2%_ISsgme6HsQdozmdRA0qlaCWcOhjnvwMUIJr^m7e*vg#`Yk~EOa*sAj zQ(LBy?h!YlPPP-RSW30S1U-&=*C&3zUArjABMsU!g=xOPWIWP8WoJ7i2|p>k25WVB znJasx5^!|tjqZfetQb;>G;|ZG6kskj+28r~Xum zhGQ2qUy09k&J322vXcHK5|To|N=@xv(hcsJ?k+l}uR#vbLS zXIbo9+m@zSWk79Z(`1*GF;7a8%(|jq#~M~*c5DjZGsZd`{{Xo(aHG{Mfb}e5KZUEb z$}!@i)Z}vMX`kAa);Nx%CkG~}+IoJcR)u*)KlvEhpDt~@x+QeIE_Y>DdZ=V2lFOkU zaRt6Kc2VD|(IfWx{Ur%Avr6MrD*>Il%nuhlkg-`hq}W~RtuUlnXbh4%^1rrtYssOQ zCY5KAWyLy}8Ecf*WtYOmnYza&_s;(StCJlqy%D+WmDL#htd%kfQzz|2>1h2h-1}-8 zO=mS_D!)0s8Gn%M2ScZg`-h~Vnug0t=*Oz7)p#bN?u<=d29Ao@wB_!eGRrKe$!jj0 zcwQ3S4C>2syB3htlfx%ZtY?&(Ekk1CS~VBTi!+9>4>&U~gOQnEQG1nn_IHyoh-e88 zwOm8jDD|1$2>6Ob)(dA)eSUmKRsK#XuUd5&6C?} z(J!A9PopAQovMp0YKh!iF1M7ozI_G?Ra=l^tiB}oyj&(bJV&iB_0(CRo$xH}BD0Y^1E4+q*_!Jr-*I zmW-#jzdml|L(zZ$uo5gwr}9Yhuv-u*$A4WYhyTO?HW2^<0s;a71OWpB0t5yF00033 z01*Qr5-~vpB2i%zGI4Maif035j4mc`uJgQ8%zFKykz6p9pO5Tm)1 zM9=~i$3ugLTMRzF;p|YTivF$LgBNNz!0Si5iup0^!hv99Y8Nmn`liZQwdf%h03R?G zb3h60V*dcA%+0_c1$8|`WWaGyK=u{Sdhs!<>XuOm#Qs9)Ulhtv;GolZ<{{5Rhj zjd8aTYNo*Yhx=t!1ILs535)#KiM2`&Li^(U`*p z$QiMfzDg!+r#?)jvU(zvxYmyoJtkp|tOhCVAV>C{2+HYT^BWtLz_y6o?78j{$QwVj z6=`Q$xXh#RlHV5+g0Wj8kMkDT^{MIhElIw-U)-%UVtHwXYc#s zR$=-{7gN)-@&5p74F_{cR?@*;Y~&T^F=qZjH`=&J!0vK%KUOI{8_k|_T3`fg87 zi;dhIoM@5;myB|T_7yo6HPl?5pYkUbC@6fxs-h_<;d?Y0lkIJ%4H%bdMvFXuif%6lAMY3gB74`Ys@QZ2_SX3ja^rH zEY}r2VuxGVR`DzGOc1%n=&ijqXc?&NW|Jy5c6OD^yCb$hJqvHIs^kfpl4TIZic`=@Z)EGBif?Qg!JSboIx#L?Kd&;dt)-z6h9qE9Nbw zU1DR`?gYRwf8nv7M0Di&uN_BJK=)0eH^dm6{{W{7OdKd11MamkBd4wnjnTH)^?eD5 z5Yov8k1@9Js(GUbzo~Jj}te4kReDSQE4LQz7J(6=qPGi|I-3;9|rm*@Gnv z%3FelU_6iybNYre>itGV+p7Lw^;d{G>aKrM&-sovA(WS}$8j>g9Jy>eFc#o!u=5lO zV^nU({4bGN^^<=e6A}Q74FDhkK{N+IMCgDB?l{Ki54*`GWf?N>a9J4`@|igjmlPZQ zMEp#Jc@l1VPfhBwarTxZ6$~nj357o(q{O0vj+9hNVuSerObd|yUwJ?6TRRi_dwQHX zag`cJ9 z$yU&pj4jMPiP&n1WVzq(G>0FP_bsUfkElm4 zA0m2_?jxBe&%h88;q921wZxPrX9&31GZEx`3F%yV{9(WFhp>&;BOS6jxnN6Ox$TUt zjTpcu;6R_ma51(eBT$3v6RreIr|~Be0r4{uxthOts>Aa#n6$jNQ#csedLBP`74j zcPNmGwoic^mJ7d+?7FIp{{StI7=K@?Hc#1|UCdk?mk|E|HxrmZNaN9o!3z#Q#K1-* z&}f?@-CiOF;p!JCWRsbi5SJR06L`|Olgk>T17e+Uu@Ij0VzBdE}303qVzYz#PdyGhO3{upBgu%hFiZBWmCO$%EBb^qm zg#M&`#~Q9O4P@YK9Y)AjwZ%+)p%qDDQ^1Xz7xfHE3O@=NK8$b3UB?i^sMbb8n)#lY z+advGzGgAO*_<9VK#r!uFg%1H({~v{04ikH8JlLDz^ zc!SxHS8yw;o$`zL{{R}gk;l4%zaJO=E*%9}JWNF7vwyT2gI|;%?S!KKP=6sElQ57; z6w?u6tZcwkR=%OSUd(aLoWeLR!~~%>Y()yEVAN#vxO-N~`s?Xus%+EUf%7=C2i(wZMh;v^GM9cKZ3D9xmOG-1rcl9^zN$a1h3*M5uIIsIUMA;|6?WFkci5JE{wMf{L@L26hmL zJ+F)g5fL)-4B!gpARN4(*uV=eFd)%lVo|Z&$G%#NM#SGHqXn=%L2Lk=Rfy=!l%G=Q zo4MA+U<7b^`~@?&XXt&5Gwp5qZ$qxA^|nvS82 z#M#x&UQS1F5DifsMzR)xor&E_*J1%j;&vgbFzgxXT(RjFsrrV@enB}iC2QVKpO^^t z-}s;IDK|?up>^d<8fDl&1~nQPObr@&>BZZ@~V_#bl zC@}2CT?(dzStn5J8K#L&SM^3bOv=Sh_{=)+d`3V}^rOr>B6bOuDrRM>G(4Hi{zs)_ z_Z#10WdVYXTsq<|H4pknJBMVz4&nfNBpQO3$+!m$22$n!M`~6Hs6XXe%)j-w8C(YlPlY^=GjcI*}lZt`a zjt`S@^foK*WdPSv(2>=59+!@zfB@)Bq~yQc)a1;3OdyihZ^R00?~jQ5R0)ww28B&| z`w8k10ay?!VI%;)6>;dNHLM($u0&IaaDv7AUmAk(Yd0&r_F%U;Un1kKBxH=Jyljp=x8>=16O~`g) zR{X#M)KEzgxGNeYd6}74hj6LhnD-HQpC(rVNpCxz#WW*bYvy`Z??jD82n`TsaM($! zgWL0PmNW<-F^N2E59|aN$J3!x@M93~jYwU8=>*5&>G-weAa*(x2R6@%%OACBidAk| z99pmTn3S9D3gRnWTm#*$L80vxKS&!t6Wq2dcPW(jak%o-@O+;T&|+vAvsPk`Hx)yM zn8{|VAZSGo@@63tiI}Zwu0OZ7ST)_5$bdCaGyd}j2D|;?zB~S9oMc^n#|3^S5b4I6 z;?ng?2_W8yl$=4_Z78p!{f^@Xf5e9W0CFe%x5tdVey?hZqcOM$nHPuu0GCy2I;Hb9 zX_o&0{mfe(EMRh;XI?ebHcwKx)y4Cn4ShzUMSl@)fvCM$XujUbi~wf;0PDo1E7|#u z3aB;#bSR70-*Ls&gA}t}kWX<)`Vby}ap3xe^fU>x|`NKjcjVR0L8J&)mp&3m=w;+vLJRfz)ED zUB~9NuMj9$0Dp?}2f4KfAZ-5tF%@ZnV=|^TqA@YmpNWxIDbcScDdK9#gJssopX@0Z zS&mhS_=4(~Z^F!&&=g%=%*b2l4HFoa!b21CVH;7Q9C0etJfCTpTXt8)OcptBeHAkLM6F*QVPdFWiRyF2X0 z3Vd8NN5`3q+Q-^SgOT_XUI>l>UWCXLd{>!VuECpsljbSl!fh|Wg_t7-KpKF6wl$(J zuw!ttBi>a#n0yF6RLq4z1TYbK1_TL=4`l-^)iF2m{gJEyEKB82L;nDYhi9;SODkf) z`GA53r_qFl^D57G@*v=$s9(g)rxJoq8J3r;_?2MxM|0bg z=07DH6$1g?#pXO3g#h@NPcPnL@-gFl@Q?B)N)9v#_J{#sZ@%g|8Jt*FI+#D(YJ6O- zM7K02RpKWKCy?>Ej*B+f2-ZjYssPMtm_%+>Wc6ZtrhaxG8?Mxe>5sGZF#CZ9LSgrR zh{F=7(Yq!dd-JjcDwYkf8xtoiYT70hWWIpv0?s>GhP8GW5xLPfQw?EZLx~*$7gb$u zH7NRq!OL+o-Ge6`$G%AeFF|axZq9Fr&2Vy8ve@OG@EVy;1k?7)K<0AE)#DROmMGZ;KgmM2vcmNI&2N)>t$fXmo5F@Vee0HhOt zy!6Et>;fxfMrCeYL<$qOK48!t*O|nE{@N634{$U?PQwdXN_i0&s9o>OH=|L3vTiDU z0|?|tj)8`NUog=GU3U{_WSDymui{}~;e|Sj>syDA;>9(feox#|cx##)1jpI7*^CQc zx+x2e$ZrBNyAnAI>`ra%05r(?0<}6BiB2#90{St>6QU20ARGWD!HijNW9&e@Z(+1S zimi_$02%xliw4_?2-bgjz%QE?A}Ih3fvNbA_pLyu9RO|uxfGhSqcfs86rk>Ug&-w@ zKnzT9#(=Th$Q6O|V}@F~O~okWSGZJnX9qxRv4xe31t;odwd|AgGLUYM*U*6R8c@BG zVB^bm@iGADKyVjrIV-Ju2BsSPg6Qg_lInV2xQxYz zBcn0oIKBv@V9Y|pb}l)5Rnd%zk4jVk2EL|l3~1=p&tAC{t+(wfsU%TGMymQGy0yN7w9Qeoka=xBSK*jaVp*qZzJL zEQtRAkBxpZM`k$W8j770F+MzOXGuv7;SFD&ohvh_Uim&tdQYzF*IYzybP+1%Uqm5!{N{RXk=xi!EQ8KTx(p zrmuuKF${v&+xmfzmn9B~fU1xx{{ZYvV~nD+av<5S7xyuhP{fuFIsX7Kr7vKDV-TUb z#yre{RlKnG5e|yjJ=_4}Vc78rJdT0$2L-Jd`d|HC0g5&?7nP2{hywor-eNDWYJ=IA ziQH{25Phw`JwhGUkAqVXe2wG|gvvh)fJ(6Fn3Nrj1;6t>BV=F&vd1``2;wpV49>YS zD54YtbJD4BXa@jlVMYYo2J!rfgxis1f3%p)GKQhL++e?UW@LPoYzUr|6c<+A8DKJy zYGY;%`kYK`Lv~bMU(7=I>u0L|A}9muenlPeHU#w~wP-E>0NP^r14Oa^0Q(SjEx8(p z2U;*3jLKVr8H4cZ=j}su~XaRFCOGHeJNDeCWElkM3sjzkpdAOkPVmYz# z3eq|Fd=7bt2ZA8mb@zo$oq0AL@DY%p#l#a8?#g#f1hY(*UcEc>be3N5!b0Z0&x zc{maf>=WQmN)sWigUbHqirJ}2ulF9$)b#45f8Ba$;avWuhTL)^dj4Pm$h%nUN44@| zs&Nn@3Ej*K$ZTkMiB3*}jgta^Kf(TEaZolCS%LV0*ONT~$gJF)^Y@?GOb?GDzb3}k zrXTrWlN=*!Kx}_ABJdv3t(uKU{Ss)$<;N9MPg3bSfcsKITwqwdQli%Ey>;IT?p0+g{ByDvDa2 zTvt(p*xIQy56w;E$fOWN88L;}H8V4%P@Inwqu8hx$6#%sqKT>XbP~zyf4x{5s7JgFld{xg=`1UQB-Ooxhs<2}D6pf#Yo|h&DBeY}3{6~Db zhJbURCLxd>$RdPJEMoxGa9uC)5O}}Zd;_>e2nCRQZ>j1Fb0G1$Ivt7X@;4u451y(! zs33TV`CFJLk5nOAHUMdX7;+B2+oC1_j*a@UfVK~VQ#$cnc73!^pC56D5IaL$p(#V& zIg+$z9Eg5J%s7Td8x43#kUmp`oTNCPxCjLAKA~6&r1`skcK@}DtBtlVP|Jncu z0RsU62mmYwxQFaI&~};NZ5~a52Tr7RBV+&w*C5hf=Ck%e0*g9KxiMV1+nIJt-HHU? zzACU`!anCy1SsPVvjJ(cmF$SDq_ZFj3Z)0tSdq~4#(<$}1Yx=`P@oRr1EAnzw>xaE z*`LG^#-)6V$O@9Ssr;Yiu2$(ShWRrwc-gV82|S4VDdMil;%z2{6izDxTGoTA1_RtbkW?Q;JO0+RwBCSULMCfdDI~U50RBC)(@nW~e zCmy~QhRZ1=uv5Sk4|8HvakNR)2@b=V1MYb_tJCa(P||#w001xe(4uT0;<*p>5_d&T zbozz|V@HJncdKu zqpy*0k-IzN$zrSr^CMu6V#5~-PPD5+^@}W~0|FQo(B+5Rb;PR@(0~1r$S#|FzQs=^ zMOM*E$R$~Y(12Met{8Jg`jg&ax^a8!yu!crmu+k@JJI5=uUZ|(t~Jh%A45#r9L5B1 znJn7U_;whDV7CpLnpQ5c5x|2iSh%#_%I|8C5KU4#;x)25~{8f zH;zHseGSq8*SN$`S*lyz8>|Q1lVCiAkgL6kc+iqsu1V{U=}#-MCFB?ad($GbC1Y%Y z<&ibqf?V}=96#WI50G;DJ%J6nDJ!8OIgZ4J=29_mME5oajG?(s6~ zBbQW`i03qLR_}0ZT;(;k@vrf2Awt2N2l^S43_lLfRj@=!Ah&_(4{ah!ilg`Xr z&>$fr8H3D`V0S{EO< z+>2?zIXmsK`9e9=1HLPsnmD9*n@>YHqYmn$ipuG}eole=Xa^pUg&>i^t?|9l*OLp> z4iwUo5qk3>Np12|*86n&vEbP~ORmqTu|yke#41;j&6((wAUcJZ1o|1@E|}fXKinA% zpe{3$iz?LRZThZ3o$+XbejyYJ7p)tBjSkK1o?|H+<=6K(q;^eZ098*AA7E2p@dD{} zi~`B^V85qbu|jrK13GGM5qJvp2bBJa@GgRoI9T}~l0Q+$*h9gitz=u+P?cf%dhJS( z!053%I#7Vcj|}vuO)@rl@iqdjCO4W=s44M~$=w_MzluJ>zaADb)Pe1;emKm5%JN6i z9C8YU8!f$KoihV{9T19puCfN_qMnt=(C4M;$!yKFm9dPYiy(g~Edm2is5*!4Cn%uB z7l3^eia5Xqav)E#Xg-?ak6Y!FXME~N^kPWgJY54}$b_8q=E zPl{0FxRf83e21QvAZxcel?00oQJh2E_rsvPAaW>4JfP7{s9x*K>%@2n=y-qpZFOEm zYG;=pusERp0)js(y~7?N^&YF}DB?cRSBuXD8OsAElK|BIo+tE<33*RY7h4>wXebnU z6-7nUHkqOb`iYUP_!NzZNljF8m~E}|4y}r4t(9|K4j1SFFFuYaf+*4qD2m63p-}Mp z9RC13fcAM`eK_m(8o&0qG!$?S70?`pjQL^vTH=5Tag+Z5kxioxEWI80JFd8?)RDo( z`zk2Bl&N*sZH@tSE$35>_rJZiKGYZ>(nqTpAZq*aRl0FlY!8Z60wk3fzBs)}`S?C4>x z>gZf`qbQIe2FOA0itIlJ?at7a} zDRt^FWWC+8;<0^*ELy-eYE1(fs3)%E@K~T{%EsyFj`n6=!5ki9WGwQ_pYG-oac0jp zLhL$)X{4O5CV>2wfka@WWk8A+4V;+J+ji{C<_+yvfCAlss;3;zlzyzGPL3Uf>(J(8 zclwQ;j(B<~F4K0`M8dNY!7%b@33mAFmE_@=qsFa?PO3TjJb?O>d|ip`CGq((e%!=r zOI}uGCuo#^J0Q-R(@ZChs)B-|orqPHNC%Q^iI2Wd$V!v(%-#q-rlo-73iWEzE8T9Gp3YP^N`g zw^EJM!@&xd1C__gN;ydfsu|L3lTeeqa?5Qo@G2COYzn?PnaKDP5Uh#uFE$L{q#z#@ zW3s74#)J&dAL%+dKQ4*_qFO!^XPRKV{HoMdr;QF&*qN`*MV2V%@^}k!{e}zbbAg23 z-}OU@!}xa08sm$O90sq+PH6^EMAbXKA(ZCEiewp|nC<3g?y=DA%HpLul|}Hj*ku-0 zQ9)`10aIa>s8j^fPkUy|LaDW$cv?wYK%ii&sunBkP5A1+RUOz(!#mE4G`YohWE^}d zZF*M@K5jlD z&Uo!yX)*;W=@$MR1IuHhq(W_sHSLy$ez=#R_OA8cqEM&q8iP`+ z4!#oQDKrJRLpM>#rBt2e8?^@^i6rwo6maDxTC*p~458!kxu;35MIoFo1mE=m8k1@^ z!B$lW9>XE)5q#@SQbOt)YdYEsku{evv6&E~0;{!ZuTn})>Nk7*mQl^jt+@$zq|=#y zAMxoFE1L;^m8TO-tU|FA+JTD!6?z|jbL` zK4zHeMT(Vf9KB5SHKQc@pQighGO5TLI@oSZNh*@>bUt z>GvG{YL*(%aokp@uqt$eqMLYw_DAl%Ca0HZ%RCmGfJg48ofyPUpd-;y3h-V)m!+~H zH3y7FfrQ#@?ITQfa19z|89+_c3P?OH;-w32)OT zg9^rOj2bXSkA+Ne4$GC|s_28ANtlEI0Q})Xf2E6$lbl2ur{M1Dcw<1% zrpL*l2$aRK3NE+`^6IVe?2H1>6{n^&(CVB?H9VY@toNevk@1R`sqZqv1=@?v%U?@L zWhO{tQ{)ZWS$Vm+5wb!8Uuo4VZyvT7DR*XjB^RcvQ2jiK_5;J_>gI~;*g9p5gk@HL zKqgJ|Wl}_LEhKI>XaTvCY2HNfSgGtGLefTRR!Ey*%dkdSLY}q0V%Do9L73pd$qcLk z<#rqdhl3&61>j@|qzg?#AKooZcErB;O9CeJY@#wz+y!RIJc z(dwlUkvvpF%hA28oYEO7b=MB;^E1S%qB<~QScZ$V)x4`rdD4)RQsf5$t+HCK!5A(+ zMOka-zEiN!o>^10mDJ9?(t}RmAHm`RQA*xVQVPqmC%c+g>@m8E4*p_7`0`t_9!j%j zY0Dy*nD1NK6jc8Jw9iST!qjCTKS{cyYmW02Ju8qZrfww6otYI$Q$kTKr==XLiN;f= zs`?90S(#FbJ97)<(z!E5YYnu-V1)M>7FmT+Op*l9b!#~l$TL*xxODPK=VIfd%_tXO zs!5r;a+uEHsT1)x=w6hEg^6*BUrZ~ER)0lGr?;1Ws5wH?UqF^XhVBnl1-dX{3kL^z z8WKE{W3Xci;`gdOX*Zna@e`#$+uIH`7}SPB*(}a7bj5h)Pd4p~Ba8q`+PM2g$Grs} z`7MyB5DA{Gu4L6^F7FFRE6YNwDC|ugLD{vJzNOnF6c2O~8bf=}sb|R{dk-_pYU(U} z!5A-=sD~#?#(48JnzqAqa$7qpIX;3~UR`q^DIZ~|7O@ECa)jE3w!69lbw%XyW)EaA z+vr9P0oBy_o3qJ`q>( zQh&Q4Lt#oNn2fw3cE+Fil&Ow_Z0*{!MX#1=^ks-dW~Aza#)_dqT8yk(AyqEL4_T@e z>1yPr5{`#9c^1^$p$wopz&rCLG4j)BWXnp$D=4iSbH=>nBBO-=nEv+uy%O3XfMR`V+zl(a4cWm8Xk?@}nTn1TU2Fh9#ijx?3d*6K(;n(JIXs^;x{Qjb zDqSc{)j`K=DMywHVxIg;t&MEi4^pU@9x|&ot0C&NL@7ERdD1|tB;qJcYmF)quhK7I zL#{V%hWZJ5- z?!X`yAF!e*Z2<<_VR}E9dO~ts1lGAT0nS%~@#Ead^(&sfh-7s)X9r8+U+Mu={|%YW*2# zc^(EK?igVBfZ-tSD#`#+D8Zj_fbigF__ZkPPg7Gqy!(O=kBhAH3m|4xuL1xih9x&4 zHk<8XKtUPEZgX$wPInys)nWcyau@`dhR4o4+S{#Ur!@>rLhaY`gT|2 zCIW3>Q6-~4me?hYcq8&5Cg%=hxo)7GK<(~QyHNwjxH5QQ&-{9Ovy6cV*xOwA;dtSA z7*OZNF%yF?#&W0j<*YIac#6iyvDB{6Fr)rQ_RyX5uzUwJtwJ-prxW>U*woHpp5`)P z>-Kd7<-*ngg+51)6gVT#5a7YzgW*4L&K$*#CQoJ;`})3CWg#bxYBXmgP zGa%ft0;nw*1ufC`CZ=Cw5Q2oOfPkT8AR);l%!dTTuzm;fl|$Szr5RXp4hOR^1f!?8 z*Kq!2#J)hx(FUn*niZ~j25SRKhmjA&43jOqzNIaJY2ksAqy8EXx)MA^Q_W-orBleQwVr5-oCjfmp ziz@NjmL$B1MP3PkR9!PYuzHoM2XmGpqq(YE)Ko(bV)}pwfAun_Iv_AqKND$auRT4Wg?yH}(1xVtgh%{A;#!H-ox?Byx ze{$^_lH#vGLAlP)6Hf#{N-}^ImoH$y;w{y|QowFZO1-!+3jhFsiUGld5NGy%!U_E5 z3=$N&tH_Gg>MzNNmdKiCCy|)HxkfBl$^oHijK5W<8F zzsdI%Kr-a41u6hI2 z#!TIee5$4p=2hwS`GIO6#K{uE9EeeugAfM%90A1sSpNXH0j;eoO67o4QsC+?jwPMJ zD0MWCANw+hER)nM+zpErXQw|-?V63!gH4yrrsf9_+m1v#_S+>#T7!pyKT=Vm002?< zEo)zQ$PgT*jx#?_ZL|^vM?z_8X^Qn>EDn*Si%5f54T*!{!H$)+&1={)r*#hNI^#ca z8ay~jY7dt{z^o6(bl8j$VE%!GJ=TNq;IN{WCIvuHKIhBo1Ox}DS*b*36aH&e0CK?S zN?w)JJ1bq759~^UP|D1y+ti>}43cs9lqHZ?K#5Vfkar6trmc^y#fh!*d`lmX0C50* zls_R|m_HaYI|T(PZS=1dFlvErNjysw1{A%4GNp0E(?gSQQr0xK&_HM`xm(zZx75-# zy=|UR1^_NCjr>ZmkwOC7W>m6DVN1JPDt;faH5F`5(e4$T%Hg1($Q??#fZ2oMI06F1 zqA+@t1RNW%u^FvH2)8UYU?nIS_GQ+sh5(y%>rw?6JDBCmu&@x76}g(vM7!H|OP?Fw z&7YLHHNXJZxKX}F3xpPU5~R|$sT(*br=|B!epG0MDsGVmMZSP>0Xl(`KH8rxs$SR;M`=PMbzS6E;eq zKmbgMsH!5ZI207z6H8ZGY6fm<_BrlqXY8*&=D9V8NC) zT!}-et5JpV%%Z0oPDUj+TB9`qmLS>)9}|-0kK>3%jv^FOUnW?s< zEaA;lw0-hr2tjTfiK=bewp=6*L0=GNpcXE)$_w!|YASMtrm!DiOPU9txnunqnxFZ^ zu|9#6wFCH>TaBOYMhWyGI7$mv!n=W7!b!F=5BB8{=LTvLWh-JZ;H7^kMPSkN47~vm zo#h?2C`wQom55JYMpDZ4<@%bp01#~%)y~lHW;?JD5PNd z$8C0rqr8R88xc6f&g?i@>2xNm;Z%?th1Q@!&+&MaL^dEeJxgvovIkR8TPT6lu9>JP zn$ezQ+gPN1KGZI7wV@FuNqYX1Ns zyWiC0@VF3>TVUbNcmVegXsAp_sBO6P@hM?mK1IfB(GZ++#4f`iON!VRWH}zDm;f?> zpY4%ZYL*w`Bj|`NQa|9rMXIKiNc_z(?VfP}6-OPoNj`)&e_4P9E**+w;loQ0LR5_8 zK!MAe*0MRQu&L}JEEwbfrnpudrx7jg5E|H6h)suH#Qi_&?Mm2f8esx{*Hixh&Csrb zG|{cVz90KJq54e|YL{y1)FsnWht)I-%0IkMQF%H38Q=Vp~PDO`eUiPWSDr#5?f=z5&nj}!) z1RR;Cune1$8Jg!%%WcL4RjD~5EzEKonBH@eVJ@a>sZi_R+#7-QD2LHAPPWk_`It%l=xoaiCB*cEq TKPNd-&@+etg#Ka3gVg`oQm{Lf literal 0 HcmV?d00001 From 5066d6024aaa0441630ee824990e8979267ac545 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Fri, 7 Mar 2025 11:35:28 -0500 Subject: [PATCH 13/74] Created function prototypes for roll_pitch_yaw to rotation conversion, and relative transforamtion calculation --- .../relative_meas_gen/include/apriltag_meas_gen.h | 4 +++- .../relative_meas_gen/src/apriltag_meas_gen.cpp | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 8bdb1b8..aeda93b 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -21,7 +21,9 @@ class ApriltagMeasurer { YAML::Node config; void imageCallback(const sensor_msgs::CompressedImage msg); - void publishRelativeMeasurement(std::string, Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q); + void publishRelativeMeasurement(std::string, Eigen::Matrix4f transformation); + Eigen::Matrix4f calculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag); + Eigen::Matrix4f ApriltagMeasurer::RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw) ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 49a8093..438590d 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -73,8 +73,10 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { bot_tag_T << x, y, z; Eigen::Quaternionf bot_tag_Q(qw, qx, qy, qz); + Eigen::Matrix4f transformation = calculateRelativeTransformation(/*The matrices*/); + // Calculate bot -> bot transformation and publish - publishRelativeMeasurement(bot_id, bot_cam_RT, cam_tag_RT, bot_tag_T, bot_tag_Q); + publishRelativeMeasurement(bot_id, transformation); } } @@ -122,7 +124,15 @@ int main(int argc, char** argv) { return 0; } -void ApriltagMeasurer::publishRelativeMeasurement(std::string bot_id, Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Vector3f bot_to_tag_T, Eigen::Quaternionf bot_to_tag_Q) { +Eigen::Matrix4f ApriltagMeasurer::RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw) { + +} + +Eigen::Matrix4f calculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag) { + +} + +void ApriltagMeasurer::publishRelativeMeasurement(std::string bot_id, Eigen::Matrix4f transformation) { geometry_msgs::Pose pose_msg; From 744398336b695941ecd4e6763884ff14205e0334 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Sun, 9 Mar 2025 15:05:18 -0400 Subject: [PATCH 14/74] Generalized for any dataset, needs calculation implementation --- .../bash_scripts/tmux_relative_meas.sh | 2 +- .../include/apriltag_meas_gen.h | 8 +- .../launch/CoPeD_relative_meas_gen.launch | 2 +- .../params/{relative_meas.yaml => CoPeD.yaml} | 21 +-- .../src/apriltag_meas_gen.cpp | 146 ++++++++++++------ 5 files changed, 110 insertions(+), 69 deletions(-) rename frontend/relative_meas_gen/params/{relative_meas.yaml => CoPeD.yaml} (90%) diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh index 28bc497..b91c464 100755 --- a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -43,7 +43,7 @@ tmux split-window -h -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:1.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r 4.0 -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r 6.0 -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index aeda93b..2bec886 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -21,9 +22,10 @@ class ApriltagMeasurer { YAML::Node config; void imageCallback(const sensor_msgs::CompressedImage msg); - void publishRelativeMeasurement(std::string, Eigen::Matrix4f transformation); - Eigen::Matrix4f calculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag); - Eigen::Matrix4f ApriltagMeasurer::RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw) + void PublishRelativeMeasurement(int8_t, Eigen::Matrix4f transformation); + Eigen::Matrix4f CalculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag); + Eigen::Matrix4f RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw); + std::tuple> LoadTransformations(slidetag tag); ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch index 9477eec..f54f2b2 100644 --- a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -7,6 +7,6 @@ - + \ No newline at end of file diff --git a/frontend/relative_meas_gen/params/relative_meas.yaml b/frontend/relative_meas_gen/params/CoPeD.yaml similarity index 90% rename from frontend/relative_meas_gen/params/relative_meas.yaml rename to frontend/relative_meas_gen/params/CoPeD.yaml index 00702f6..38a829f 100644 --- a/frontend/relative_meas_gen/params/relative_meas.yaml +++ b/frontend/relative_meas_gen/params/CoPeD.yaml @@ -1,4 +1,6 @@ +dataset: "CoPeD" robot0: + id: 0 name: "wilbur" camera0: image_width: 1440 @@ -13,10 +15,6 @@ robot0: pitch: -0.005559586035104693 roll: 0.0004926383242970126 yaw: 0.04223758430352057 - R: - - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] - - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] - - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] distortion_model: plumb_bob distortion_coefficients: [-0.3798306402200569, 0.1959307510959587, 0.0004038542688319737, 0.000190820656519979, 0] projection_matrix: @@ -36,10 +34,6 @@ robot0: pitch: -0.005559586035104693 roll: 0.0004926383242970126 yaw: 0.04223758430352057 - R: - - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] - - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] - - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] distortion_model: plumb_bob distortion_coefficients: [-0.3831901211400326, 0.2233720519081885, 0.0008931300229381834, 0.001068644642920918, 0] projection_matrix: @@ -59,6 +53,7 @@ robot0: ] robot1: + id: 1 name: "wanda" camera0: image_width: 1440 @@ -73,10 +68,6 @@ robot1: pitch: -0.005559586035104693 roll: 0.0004926383242970126 yaw: 0.04223758430352057 - R: - - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] - - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] - - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] distortion_model: plumb_bob distortion_coefficients: [-0.423121, 0.173144, -0.003773, -0.003173, 0.000000] projection_matrix: @@ -93,10 +84,6 @@ robot1: x: 0.01869175488738828 y: -0.5607127105911416 z: 0.28672623687109766 - R: - - [0.9990926853610706, -0.04222775802962085, -0.005533796630073261] - - [0.04222437413156798, 0.9991078891268013, -0.0007269513638700898] - - [0.005559557391636409, 0.0004926306909445937, 0.9999844240888706] pitch: -0.005559586035104693 roll: 0.0004926383242970126 yaw: 0.04223758430352057 @@ -119,6 +106,7 @@ robot1: ] robot2: + id: 2 name: "race1" camera0: image_width: 640 @@ -185,6 +173,7 @@ robot2: - [0.0, 0.0, 0.0, 1.0] robot3: + id: 3 name: "race5" camera0: image_width: 640 diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 438590d..e596feb 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -11,38 +11,69 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { std::vector tags = ExtractAprilTags(img, this->intrinsics, this->tagsize);\ - // Load RT matrix from host bot -> camera from config - Eigen::Matrix4f bot_cam_RT; - YAML::Node R = config[robot_ID][camera_ID]["R"]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - bot_cam_RT(i, j) = R[i][j].as(); - } - } - bot_cam_RT(0, 3) = config[robot_ID][camera_ID]["x"].as(); - bot_cam_RT(1, 3) = config[robot_ID][camera_ID]["y"].as(); - bot_cam_RT(2, 3) = config[robot_ID][camera_ID]["z"].as(); - // Publish transformation for each detected apriltag for (slidetag t : tags) { + int8_t bot_id; + Eigen::Matrix4f bot_to_cam; + Eigen::Matrix4f cam_to_tag; + Eigen::Matrix4f tag_to_bot; + // Obtain RT from camera -> detected apriltag - Eigen::Matrix4f cam_tag_RT; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - cam_tag_RT(i, j) = t.rotation.data[i + j]; + cam_to_tag(i, j) = t.rotation.data[i + j]; } - cam_tag_RT(3, i) = t.translation.data[i]; + cam_to_tag(3, i) = t.translation.data[i]; } std::cout << "Tag ID: " << t.id << std::endl; - float x; - float y; - float z; - float qw; - float qx; - float qy; - float qz; - std::string bot_id; + std::tuple> loaded_transformations = LoadTransformations(t); + std::array transformations = std::get<1>(loaded_transformations); + bot_id = std::get<0>(loaded_transformations); + + if (bot_id == -1) { + ROS_ERROR("Transformations not loaded for apriltag id: " + t.id); + continue; + } + bot_to_cam = transformations[0]; + tag_to_bot = transformations[1]; + + Eigen::Matrix4f total_transformation = CalculateRelativeTransformation(bot_to_cam, cam_to_tag, tag_to_bot); + PublishRelativeMeasurement(bot_id, total_transformation); + } + +} + +std::tuple> ApriltagMeasurer::LoadTransformations(slidetag t) { + std::tuple> return_tuple; + int8_t bot_id = -1; + std::array transformations; + + std::string dataset = config["dataset"].as(); + if (dataset == "CoPeD") { + // Load RT matrix from host bot -> camera from config + Eigen::Matrix4f bot_to_cam = Eigen::Matrix4f::Identity(); + if (config[robot_ID][camera_ID]["R"].IsDefined()) { + YAML::Node R = config[robot_ID][camera_ID]["R"]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + bot_to_cam(i, j) = R[i][j].as(); + } + } + bot_to_cam(0, 3) = config[robot_ID][camera_ID]["x"].as(); + bot_to_cam(1, 3) = config[robot_ID][camera_ID]["y"].as(); + bot_to_cam(2, 3) = config[robot_ID][camera_ID]["z"].as(); + } else { + float x = config[robot_ID][camera_ID]["x"].as(); + float y = config[robot_ID][camera_ID]["y"].as(); + float z = config[robot_ID][camera_ID]["z"].as(); + float roll = config[robot_ID][camera_ID]["roll"].as(); + float pitch = config[robot_ID][camera_ID]["pitch"].as(); + float yaw = config[robot_ID][camera_ID]["yaw"].as(); + bot_to_cam = RollPitchYaw_to_RT(x, y, z, roll, pitch, yaw); + } + + Eigen::Matrix4f tag_to_bot; bool tag_on_bot = false; // Obtain translation vector and quaternion from detected apriltag -> detected robot @@ -52,35 +83,37 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { YAML::Node tags = config[key]["tags"]; for (auto tag = tags.begin(); tag != tags.end(); ++tag) { if (t.id == (*tag)["id"].as()) { - bot_id = key; - x = (*tag)["x"].as(); - y = (*tag)["y"].as(); - z = (*tag)["z"].as(); - qw = (*tag)["qw"].as(); - qx = (*tag)["qx"].as(); - qy = (*tag)["qy"].as(); - qz = (*tag)["qz"].as(); + bot_id = config[key]["id"].as(); + float x = (*tag)["x"].as(); + float y = (*tag)["y"].as(); + float z = (*tag)["z"].as(); + Eigen::Vector3f translation = Eigen::Vector3f(x, y, z); + float qw = (*tag)["qw"].as(); + float qx = (*tag)["qx"].as(); + float qy = (*tag)["qy"].as(); + float qz = (*tag)["qz"].as(); + Eigen::Quaternionf q = Eigen::Quaternionf(qw, qx, qy, qz); tag_on_bot = true; + tag_to_bot = Eigen::Matrix4f::Identity(); + tag_to_bot.block<3,3>(0,0) = q.toRotationMatrix(); + tag_to_bot.block<3,1>(0,3) = translation; break; } } } } if (!tag_on_bot) { - std::cout << "Detected tag does not belong to any robot" << std::endl; - } else { - Eigen::Vector3f bot_tag_T; - bot_tag_T << x, y, z; - Eigen::Quaternionf bot_tag_Q(qw, qx, qy, qz); - - Eigen::Matrix4f transformation = calculateRelativeTransformation(/*The matrices*/); - - // Calculate bot -> bot transformation and publish - publishRelativeMeasurement(bot_id, transformation); - + ROS_WARN("Detected tag does not belong to any robot in dataset"); } - } + transformations[0] = bot_to_cam; + transformations[1] = tag_to_bot; + return_tuple = std::make_tuple(bot_id, transformations); + return return_tuple; + + } else { + ROS_INFO("Loading from config for requested dataset not supported."); + } } ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { @@ -125,21 +158,38 @@ int main(int argc, char** argv) { } Eigen::Matrix4f ApriltagMeasurer::RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw) { - + /* + TODO: Write calculations + */ } -Eigen::Matrix4f calculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag) { - +Eigen::Matrix4f ApriltagMeasurer::CalculateRelativeTransformation(Eigen::Matrix4f bot_to_cam, Eigen::Matrix4f cam_to_tag, Eigen::Matrix4f bot_to_tag) { + /* + TODO: Write calculations + */ } -void ApriltagMeasurer::publishRelativeMeasurement(std::string bot_id, Eigen::Matrix4f transformation) { +void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4f transformation) { geometry_msgs::Pose pose_msg; + sloam_msgs::RelativeInterRobotMeasurement msg; + + geometry_msgs::Point position; + geometry_msgs::Quaternion orientation; + + /* + TODO: some math to get these two fields + */ + msg.header.stamp = ros::Time::now(); + msg.relativePose = pose_msg; + /* - Calculate pose + TODO: Get odometry data */ + msg.robotId = bot_id; + std::cout << "Publishing pose from host to " << bot_id << std::endl; relative_meas.publish(pose_msg); From edc8c2c529e08eac46123106c7ef7ab72179130a Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Tue, 11 Mar 2025 15:08:17 +0000 Subject: [PATCH 15/74] Relative Meas multiUGV sim to SlideSLAM publisher, first prototype inter-robot measurements --- .../script/CoPeD/tmux_CoPeD.sh | 14 +- backend/sloam/CMakeLists.txt | 2 + backend/sloam/include/core/robot.h | 3 +- backend/sloam/include/core/sloamNode.h | 12 +- backend/sloam/include/factorgraph/graph.h | 2 +- backend/sloam/include/helpers/definitions.h | 24 +- .../sloam/launch/decentralized_sloam.launch | 4 +- .../launch/rviz/decentralized_sloam.rviz | 48 +- backend/sloam/params/sloam_multiUGV.yaml | 57 ++ backend/sloam/results/.gitignore | 2 + backend/sloam/src/core/inputNode.cpp | 22 +- backend/sloam/src/core/robot.cpp | 33 +- backend/sloam/src/core/sloam.cpp | 6 +- backend/sloam/src/core/sloamNode.cpp | 121 +++- backend/sloam/src/factorgraph/graph.cpp | 32 +- .../src/tests/inter_robot_factor_test.cpp | 4 +- frontend/relative_meas_gen/CMakeLists.txt | 12 +- frontend/relative_meas_gen/__init__.py | 1 + .../bash_scripts/multi_UGV_run.sh | 84 +++ .../relative_meas_gen/config/multiUGV.rviz | 178 ++++++ .../launch/multiUGVToSlideSLAM.launch | 12 + .../launch/odomRelativeMeasSync.launch | 10 + .../relative_meas_gen/launch/testCases.launch | 3 + frontend/relative_meas_gen/package.xml | 3 + frontend/relative_meas_gen/requirements.txt | 2 - frontend/relative_meas_gen/results/.gitignore | 2 + .../scripts/multiUGVToSlideSLAM.py | 217 +++++++ .../scripts/odomRelativeMeasSync.py | 56 ++ frontend/relative_meas_gen/setup.py | 11 + .../src/relative_meas_gen/__init__.py | 0 .../src/relative_meas_gen/transforms.py | 30 + .../relative_meas_gen/tests/testMultiUGV.py | 41 ++ frontend/relative_meas_gen/worlds/stuff.world | 584 ++++++++++++++++++ 33 files changed, 1529 insertions(+), 103 deletions(-) create mode 100755 backend/sloam/params/sloam_multiUGV.yaml create mode 100644 backend/sloam/results/.gitignore create mode 100644 frontend/relative_meas_gen/__init__.py create mode 100755 frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh create mode 100644 frontend/relative_meas_gen/config/multiUGV.rviz create mode 100644 frontend/relative_meas_gen/launch/multiUGVToSlideSLAM.launch create mode 100644 frontend/relative_meas_gen/launch/odomRelativeMeasSync.launch create mode 100644 frontend/relative_meas_gen/launch/testCases.launch delete mode 100644 frontend/relative_meas_gen/requirements.txt create mode 100644 frontend/relative_meas_gen/results/.gitignore create mode 100755 frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py create mode 100755 frontend/relative_meas_gen/scripts/odomRelativeMeasSync.py create mode 100644 frontend/relative_meas_gen/setup.py create mode 100644 frontend/relative_meas_gen/src/relative_meas_gen/__init__.py create mode 100644 frontend/relative_meas_gen/src/relative_meas_gen/transforms.py create mode 100755 frontend/relative_meas_gen/tests/testMultiUGV.py create mode 100644 frontend/relative_meas_gen/worlds/stuff.world diff --git a/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh b/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh index e81048e..8199ab3 100755 --- a/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh +++ b/backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh @@ -41,17 +41,17 @@ tmux split-window -h -t $SESSION_NAME # Setup commands for main window tmux select-pane -t $SESSION_NAME:1.0 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch infer_node.launch param_file:=config/CoPeD/infer_node_params_wilbur.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch scan2shape_launch infer_node.launch param_file:=config/CoPeD/infer_node_params_wilbur.yaml" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_modeller sync_semantic_measurements.launch" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch object_modeller sync_semantic_measurements.launch" Enter tmux select-pane -t $SESSION_NAME:1.2 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test_LiDAR.launch enable_rviz:=true" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch sloam single_robot_sloam_test_LiDAR.launch enable_rviz:=true" Enter tmux select-pane -t $SESSION_NAME:1.3 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch process_cloud_node_outdoor_with_ns.launch param_file:=config/CoPeD/process_cloud_node_params_wilbur.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch scan2shape_launch process_cloud_node_outdoor_with_ns.launch param_file:=config/CoPeD/process_cloud_node_params_wilbur.yaml" Enter tmux select-pane -t $SESSION_NAME:1.4 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wilburOusterOS1-64.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wilburOusterOS1-64.yaml" Enter tmux select-pane -t $SESSION_NAME:1.5 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 18; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore @@ -61,7 +61,7 @@ tmux split-window -v -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:2.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter tmux select-pane -t $SESSION_NAME:2.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; rosparam set /use_sim_time true" Enter tmux select-pane -t $SESSION_NAME:2.2 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; rosrun image_transport republish compressed in:=/wilbur/stereo_left/image_rect_color raw out:=/wilbur/stereo_left/image_rect_color/uncompressed _image_transport:=compressed " Enter diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 55752c4..6c798d5 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS tf2 tf roscpp + roslib actionlib actionlib_msgs ) @@ -107,6 +108,7 @@ cc_library( if (USE_CLIPPER) target_link_libraries(${CC_TARGET_PREFIX}_core PRIVATE semantic_clipper) endif() +target_link_libraries(${CC_TARGET_PREFIX}_core PRIVATE m) cc_binary( NAME node diff --git a/backend/sloam/include/core/robot.h b/backend/sloam/include/core/robot.h index 3254d5c..102daaa 100755 --- a/backend/sloam/include/core/robot.h +++ b/backend/sloam/include/core/robot.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -100,7 +101,7 @@ class Robot { // Callback functions for Subscribers void RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg); void RobotObservationCb(const sloam_msgs::SemanticMeasSyncOdom &observation_msg); - void RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurement &relativeMeas_msg); + void RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurementOdom &relativeMeas_msg); // Helper methods for converting messages to objects std::vector rosEllipsoid2EllipObj(const std::vector& msgs); diff --git a/backend/sloam/include/core/sloamNode.h b/backend/sloam/include/core/sloamNode.h index 87894ef..fd6bbbb 100755 --- a/backend/sloam/include/core/sloamNode.h +++ b/backend/sloam/include/core/sloamNode.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -92,9 +93,9 @@ class SLOAMNode : public sloam { private: // TODO(xu): load the following four params from rosparam bool save_inter_robot_closure_results_ = true; - string save_results_dir_ = "/home/sam"; - bool save_robot_trajectory_as_csv_ = false; - string save_runtime_analysis_dir_ = "/home/sam"; + string save_results_dir_; + bool save_robot_trajectory_as_csv_ = true; + string save_runtime_analysis_dir_; double inter_robot_place_recognition_frequency_; @@ -109,6 +110,7 @@ class SLOAMNode : public sloam { const std::vector> &landmarks); void publishMap_(const ros::Time stamp); void publishCubeMaps_(const ros::Time stamp); + void publishInterRobotFactors(const int &robotID); bool prepareInputs_(const SE3 relativeMotion, const SE3 prevKeyPose, CloudT::Ptr tree_cloud, CloudT::Ptr ground_cloud, @@ -158,6 +160,8 @@ class SLOAMNode : public sloam { ros::Publisher pubMapCubeModel_; ros::Publisher pubSubmapCubeModel_; + ros::Publisher pubRelInterRobotFactors_; + // Transform tf2_ros::Buffer tf_buffer_; std::unique_ptr tf_listener_; @@ -185,7 +189,9 @@ class SLOAMNode : public sloam { ros::Time last_rel_inter_robot_factor_stamp_; std::thread relInterRobotFactorthread_; std::vector feasible_relative_meas_for_factors; // All measurements that could still be used to generate a factor + std::vector relative_inter_robot_factors; // All measurements that have been used to generate a factor std::mutex feasRelMeasVectorMtx_; + std::mutex relInterRobotFactorsMtx_; // For cuboid semantic landmarks CubeMapManager cube_semantic_map_; diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index 2ef38b9..3b2b758 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -92,7 +92,7 @@ class SemanticFactorGraph { void addLoopClosureFactor(const Pose3 poseRelative, const size_t fromIdx, const size_t fromRobotID, const size_t toIdx, const size_t toRobotID); - void addRelativeMeasurementFactor(const Pose3 poseRelative, const size_t fromIdx, + void addRelativeMeasFactor(const Pose3 poseRelative, const size_t fromIdx, const size_t fromRobotID, const size_t toIdx, const size_t toRobotID); void addPointLandmarkKey(const size_t ugvIdx, const Point3& ugv_position); diff --git a/backend/sloam/include/helpers/definitions.h b/backend/sloam/include/helpers/definitions.h index 41b8705..14146bb 100755 --- a/backend/sloam/include/helpers/definitions.h +++ b/backend/sloam/include/helpers/definitions.h @@ -111,11 +111,33 @@ struct FeatureModelParams { * between the current robot and robot assigned to * "robotIndex". * @param robotIndex - the index assigned to the robot - * whose relative pose was measured. + * that isn't the host. If onlyUseOdom is true, then + * this is the robot that observed us. If onlyUseOdom + * is false, this is the robot we observed. + * @param onlyUseOdom - if true, only use the odometry. + * This is for when we are observed by another robot, + * so we add a factor to the graph, but don't add the + * relative measurement to the database. */ struct RelativeMeas { ros::Time stamp; SE3 odomPose; SE3 relativePose; int robotIndex; + bool onlyUseOdom; +}; + +/** + * @brief This struct holds information on all + * created relative inter-robot factors so that + * they can be easily published and visualized + * with RViz. + */ +struct RelativeInterRobotFactor { + int hostRobotID; + int observedRobotID; + int hostPoseIndex; + int observedPoseIndex; + SE3 relativePose; + ros::Time stamp; }; \ No newline at end of file diff --git a/backend/sloam/launch/decentralized_sloam.launch b/backend/sloam/launch/decentralized_sloam.launch index a1bff06..4898ac2 100755 --- a/backend/sloam/launch/decentralized_sloam.launch +++ b/backend/sloam/launch/decentralized_sloam.launch @@ -5,7 +5,8 @@ - + + @@ -32,6 +33,7 @@ + diff --git a/backend/sloam/launch/rviz/decentralized_sloam.rviz b/backend/sloam/launch/rviz/decentralized_sloam.rviz index 0961a2e..6ffa46d 100755 --- a/backend/sloam/launch/rviz/decentralized_sloam.rviz +++ b/backend/sloam/launch/rviz/decentralized_sloam.rviz @@ -9,7 +9,6 @@ Panels: - /robot01 - /robot01/centroid map1 - /robot01/Odometry1 - - /robot01/Odometry1/Status1 - /robot01/MarkerArray1 - /robot01/cylinder map1 - /robot01/MarkerArray2 @@ -18,6 +17,7 @@ Panels: - /robot01/MarkerArray5 - /robot01/CubeMap1 - /robot01/raw_detections1 + - /robot01/MarkerArray6 - /robot11/Odometry1 - /robot11/Cylinders Map1 - /robot11/MarkerArray1 @@ -25,7 +25,6 @@ Panels: - /robot11/MarkerArray3 - /robot11/MarkerArray4 - /robot11/CubeMap1 - - /robot21 - /robot21/MarkerArray1 - /robot21/MarkerArray2 - /robot21/CylinderMap1 @@ -53,7 +52,6 @@ Panels: - /robot51/MarkerArray7 - /robot51/MarkerArray8 - /robot51/MarkerArray9 - - /robot61 - /robot61/Odometry1 - /robot61/MarkerArray2 - /robot61/Semantic Map1 @@ -64,11 +62,8 @@ Panels: - /robot61/MarkerArray7 - /robot61/MarkerArray8 - /robot61/MarkerArray9 - - /Axes1 - - /PointCloud22 - - /PointCloud23 Splitter Ratio: 0.39753738045692444 - Tree Height: 865 + Tree Height: 1786 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -88,14 +83,14 @@ Panels: SyncMode: 0 SyncSource: "" - Class: rviz/Displays - Help Height: 70 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Grid1 - /PointCloud22/Autocompute Value Bounds1 Splitter Ratio: 0.5 - Tree Height: 371 + Tree Height: 275 Preferences: PromptSaveOnExit: true Toolbars: @@ -128,8 +123,7 @@ Visualization Manager: Marker Topic: /robot_0/sloam/optimized_point_landmarks Name: centroid map Namespaces: - "": true - text: true + {} Queue Size: 100 Value: true - Angle Tolerance: 1 @@ -196,7 +190,7 @@ Visualization Manager: Marker Topic: /sloam0/robot1/trajectory Name: MarkerArray Namespaces: - {} + points_and_lines: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -220,7 +214,7 @@ Visualization Manager: Marker Topic: /robot_0/sloam/cubes_map Name: CubeMap Namespaces: - {} + "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -231,6 +225,14 @@ Visualization Manager: {} Queue Size: 100 Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /robot_0/sloam/rel_inter_robot_factors + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true Enabled: true Name: robot0 - Class: rviz/Group @@ -1155,7 +1157,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 28.3713321685791 + Distance: 18.432209014892578 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1163,27 +1165,27 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 + X: 1.7884167432785034 + Y: -3.7740259170532227 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.3852035403251648 + Pitch: 0.6052038669586182 Target Frame: body - Yaw: 3.3053972721099854 + Yaw: 4.750397205352783 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 2032 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001cb0000039efc020000001cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000024c0000018f0000001600fffffffb000000100044006900730070006c00610079007300000002e0000000fb000000c900fffffffb0000000a0049006d00610067006500000002010000010a0000001600fffffffb0000000a0049006d0061006700650000000201000000ad0000001600fffffffb0000000a0049006d00610067006500000002a10000013a0000001600fffffffb0000000a0049006d00610067006500000001a3000000600000000000000000fb0000000a0049006d0061006700650200000000000001e300000280000001e0fb0000000a0049006d00610067006500000002f90000007e0000000000000000fb0000000a0049006d006100670065020000000000000323000002c00000010ffb0000000a0049006d00610067006500000001560000009d0000000000000000fb0000000a0049006d006100670065000000019a000001dd0000000000000000fb0000000a0049006d006100670065000000003d0000019f0000000000000000fb0000000a0049006d00610067006500000002b2000000c50000000000000000fb0000000a0049006d0061006700650000000181000000ba0000000000000000fb0000000a0049006d006100670065000000003d0000033a0000000000000000fb0000000a0049006d0061006700650100000244000001330000000000000000fb0000000a0049006d0061006700650100000232000001a90000000000000000fb0000000a0049006d00610067006501000002600000017b0000000000000000fb0000000a0049006d0061006700650000000310000000cb0000000000000000fb0000000a0049006d0061006700650100000216000001c50000000000000000000000010000015f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d00650000000000000007380000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000005670000039e00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -1192,6 +1194,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 - Y: 27 + Width: 3700 + X: 140 + Y: 54 diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml new file mode 100755 index 0000000..250101a --- /dev/null +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -0,0 +1,57 @@ +enable_rviz: true +sloam: + # ------- MOST IMPORTANT PARAMS TO TUNE: ------- + use_slidematch: false # VERY IMPORANT: (recommended: false) place recognition algorithm choice, if true, use slidematch, if false, use slidegraph + communication_wait_time: 1.0 # when running multi-robot bags, intermittent communication is replicated by allowing them to share information every communication_wait_time seconds + inter_robot_place_recognition_frequency: 0.05 # frequency at which place recognition is run between robots + cylinder_match_thresh: 3.0 # Deafult-1.0 cylinder data association threshold + cuboid_match_thresh: 2.0 # Default-2.0 # cuboid data association threshold + ellipsoid_match_thresh: 0.75 # Default-0.75 ellipsoid data association threshold + min_robot_altitude: -100 # minimum height for adding factors, should be set to negative if you want factors to be added always + min_odom_distance: 0.5 # minimum distance between two odometry messages to be considered for processing + main_node_rate: 5.0 # rate at which the main node runs, should not be less than the desired keyframe rate (e.g. 1-5 Hz is usually good) + place_recognition_attempt_time_offset: 7.0 # (keep default unless you know what you are doing) time offset for place recognition attempts for different robot's nodes to avoid extra compute + #----------------------------------------------- + intra_robot_place_recognition_frequency: 0.05 # [intra-robot place recog is default turned off] frequency at which place recognition is run within a robot + number_of_robots: 2 + robot_ns_prefix: /robot # robot_namespace will be robot_prefix + ROBOT_ID (e.g. /quadrotor1, /quadrotor2) + debug_mode: true + odom_freq_filter: 10 # Add every odom_freq_filter odom messages to the queue. Target same freq. as lidar. odom_freq = raw_odom_freq / odom_freq_filter, if you want to use all odometry messages, set this to 1 + max_queue_size: 200 # maximum queue size for point clouds buffer (odom buffer will be 10x this) + map_frame_id: quadrotor/map + odom_frame_id: quadrotor/odom + robot_frame_id: body + + # parameters for place recognition + place_recognition: + # ------- MOST IMPORTANT PARAMS TO TUNE: ------- + search_xy_step_size: 0.1 # forest / parking lot: 0.5 # indoor: 0.1 # default: 0.5 # data association search step size for XY position + search_yaw_step_size_degrees: 15.0 # 5.0 for indoor # default 2.0 # data association search step size for yaw angle + match_threshold_position: 0.75 # forest / parking lot: 0.5 # indoor: 0.75 or 1.0 # default 0.5 # threshold for position matching (XY distance) + compute_budget_sec: 10.0 # default 5.0 # compute budget in seconds for place recognition algorithm, it will return the best estimate within this time + min_num_inliers: 8 # forest / parking lot: 15 # 5 for indoor # default 10 # minimum number of inliers to consider a valid place recognition + min_num_map_objects_to_start: 5 # 10 for indoor # 30 for outdoor parking lot # 100 for forests # minimum number of objects in a map to start SlideMatch place recognition attempt + #----------------------------------------------- + dilation_factor: 1.2 # place recognition search region along XY will be dilation_factor_ * max(map_1, map_2) + match_yaw_half_range: 180.0 # search range for yaw angle (half range, total range is 2 * half range) + disable_yaw_search: false # disable yaw search or not + match_threshold_dimension: 5.0 # threshold for dimension matching (dimension difference) + ignore_dimension: true # ignore the dimension difference in object matching or not + #min_loop_closure_overlap_percentage: 0.3 # [replaced by min_num_inliers] number of inliners need to be larger than this percentage * min(# map_1_objects, # map_2_objects) + use_nonlinear_least_squares: true # use nonlinear least squares to solve the transformation + visualize_matching_results: true # visualize the matching results + # only for intra-robot place recognition + match_x_half_range_intra: 5.0 # only for intra-robot place recognition + match_y_half_range_intra: 5.0 # only for intra-robot place recognition + match_yaw_half_range_intra: 10 # only for intra-robot place recognition + + # parameters for place recognition (slide graph version) + place_recognition_slidegraph: + # ------- MOST IMPORTANT PARAMS TO TUNE: ------- + num_inliners_threshold: 2 + descriptor_matching_threshold: 0.1 + min_num_map_objects_to_start: 4 # 30 for outdoor parking lot # 100 for forests # minimum number of objects in a map to start SlideGraph place recognition attempt + #----------------------------------------------- + sigma: 0.1 # CLIPPER params, default 0.01, spread / "variance" of exponential kernel + epsilon: 0.3 # CLIPPER params, default 0.06, < bound on consistency score, determines if inlier/outlier, consistency matching threshold + diff --git a/backend/sloam/results/.gitignore b/backend/sloam/results/.gitignore new file mode 100644 index 0000000..c96a04f --- /dev/null +++ b/backend/sloam/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 511497e..2f95409 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -181,6 +181,7 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { if(!turn_off_rel_inter_robot_factor) { // For each received measurement for (int i = 0; i < robot.robotRelativeMeasQueue_.size(); i++) { + // If measurement is at least robot.semantic_meas_delay_tolerance_ seconds old if ((robot.robotRelativeMeasQueue_.back().stamp - robot.robotRelativeMeasQueue_[i].stamp).toSec() > robot.semantic_meas_delay_tolerance_) { @@ -219,18 +220,21 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { updateLastPose(raw_vio_odom_used_for_sloam, hostRobotID_); } - // Extract Host Robot Data class - sloam_->dbMutex.lock(); - robotData data = sloam_->dbManager.getHostRobotData(); - - // Pass the relative inter-robot measurement to the database manager - // and factor generation thread - data.relativeMeasPacket.push_back(relativeMeas); - sloam_->addRelativeMeasurement(relativeMeas); + // If this is a relative measurement we observed + if (!relativeMeas.onlyUseOdom) { + // Extract Host Robot Data class + sloam_->dbMutex.lock(); + robotData data = sloam_->dbManager.getHostRobotData(); + + // Pass the relative inter-robot measurement to the database manager + // and factor generation thread + data.relativeMeasPacket.push_back(relativeMeas); + sloam_->addRelativeMeasurement(relativeMeas); + sloam_->dbMutex.unlock(); + } // Remove this value from the queue, as it's been used robot.robotRelativeMeasQueue_.erase(robot.robotRelativeMeasQueue_.begin() + i); - sloam_->dbMutex.unlock(); i--; } diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 854dcbe..8bdfde0 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -46,11 +46,10 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { bool turn_off_rel_inter_robot_factor = nh_.param(node_name+"/turn_off_rel_inter_robot_factor", true); if (!turn_off_rel_inter_robot_factor) { - std::string relativeMeasSub_topic = robot_ns_prefix_ + std::to_string(robotId_) + "/relative_inter_robot_meas"; + std::string relativeMeasSub_topic = "/relative_inter_robot_meas_sync"; RobotRelativeMeasSub_ = nh_.subscribe(relativeMeasSub_topic, 10, &Robot::RobotRelativeMeasCb, this); } - ROS_INFO_STREAM("Robot Initialized! Robot # " << robotId_); } @@ -160,16 +159,30 @@ void Robot::RobotObservationCb( * inputNode will pass this onto the databaseManager * in sloamNode. * - * @param sloam_msgs::RelativeInterRobotMeasurement + * @param sloam_msgs::RelativeInterRobotMeasurementOdom * &relativeMeas_msg */ -void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurement &relativeMeas_msg) { - RelativeMeas cur_measurement; - cur_measurement.stamp = relativeMeas_msg.header.stamp; - cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometry.pose.pose); - cur_measurement.relativePose = databaseManager::toSE3Pose(relativeMeas_msg.relativePose); - cur_measurement.robotIndex = relativeMeas_msg.robotId; - robotRelativeMeasQueue_.push_back(cur_measurement); +void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurementOdom &relativeMeas_msg) { + // If this relative measurement involves our robot in some way, add it + if(relativeMeas_msg.robotIdObserver == robotId_ || relativeMeas_msg.robotIdObserved == robotId_) { + RelativeMeas cur_measurement; + cur_measurement.stamp = relativeMeas_msg.header.stamp; + cur_measurement.relativePose = databaseManager::toSE3Pose(relativeMeas_msg.relativePose); + + // If we are observed, we only use the odometry to add a factor, + // but don't consider it for a relative inter-robot factor + if(relativeMeas_msg.robotIdObserver == robotId_) { + cur_measurement.onlyUseOdom = false; + cur_measurement.robotIndex = relativeMeas_msg.robotIdObserved; + cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserver.pose.pose); + } else { + cur_measurement.onlyUseOdom = true; + cur_measurement.robotIndex = relativeMeas_msg.robotIdObserver; + cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserved.pose.pose); + } + + robotRelativeMeasQueue_.push_back(cur_measurement); + } } std::vector Robot::rosCylinder2CylinderObj( diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index 1bb0285..ef5fa6f 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -76,13 +76,13 @@ void sloam::matchModels(const std::vector &currObjects, std::vector &matchIndices) { size_t obj_counter = 0; if (currObjects.size() == 0) { - ROS_WARN("No cylinder detected in current scan!"); + ROS_WARN_THROTTLE(30, "No cylinder detected in current scan!"); if (mapObjects.size() == 0) { - ROS_WARN("No cylinder in submap!"); + ROS_WARN_THROTTLE(30, "No cylinder in submap!"); } return; } else if (mapObjects.size() == 0) { - ROS_WARN("No cylinder in submap!"); + ROS_WARN_THROTTLE(30, "No cylinder in submap!"); return; } for (const auto &co : currObjects) { diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index c05d356..53eecbc 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -117,10 +117,14 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) ROS_WARN("Relative Inter-Robot Loop Closure is turned off"); } else { relInterRobotFactorthread_ = std::thread(&SLOAMNode::relInterRobotFactorThread_, this); + pubRelInterRobotFactors_ = nh_.advertise( + "rel_inter_robot_factors", 1, true); ROS_WARN("Relative Inter-Robot Loop Closure is turned on"); } - // initialize the runtime analysis variables + // initialize save variables + save_results_dir_ = ros::package::getPath("sloam") + "/results"; + save_runtime_analysis_dir_ = save_results_dir_ + "/runtime_analysis"; runtime_analysis_file = save_runtime_analysis_dir_ + "/robot"+std::to_string(hostRobotID)+"_runtime_analysis.txt"; ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file); } @@ -211,11 +215,72 @@ void SLOAMNode::publishCubeMaps_(const ros::Time stamp) { pubSubmapCubeModel_.publish(cubeSubMapTMarkerArray); } +void SLOAMNode::publishInterRobotFactors(const int &robotID) { + + relInterRobotFactorsMtx_.lock(); + + // Create a marker array of all inter-robot factors + visualization_msgs::MarkerArray inter_robot_factors_; + for(int i = 0; i < relative_inter_robot_factors.size(); i++) { + + // Get this factor's information + RelativeInterRobotFactor factor = relative_inter_robot_factors[i]; + + // Represent it as a line between the two poses + visualization_msgs::Marker line; + line.header.frame_id = map_frame_id_; + line.header.stamp = factor.stamp; + line.id = i; + line.type = visualization_msgs::Marker::LINE_LIST; // Use LINE_LIST to create independent line segments + line.action = visualization_msgs::Marker::ADD; + + // Initialize default quaternion + line.pose.orientation.w = 1.0; + + // Define the line width + line.scale.x = 0.05; // Adjust as needed + + // Set color (e.g., red) + line.color.r = 1.0; + line.color.g = 0.0; + line.color.b = 0.0; + line.color.a = 1.0; + + // Get the poses associated with this factor + gtsam::Pose3 hostRobotPose; + gtsam::Pose3 otherRobotPose; + factorGraph_.getPose(factor.hostPoseIndex, factor.hostRobotID, hostRobotPose); + factorGraph_.getPose(factor.observedPoseIndex, factor.observedRobotID, otherRobotPose); + + // Add the two pose positions as points + geometry_msgs::Point p1, p2; + gtsam::Point3 positionHost = hostRobotPose.translation(); + p1.x = positionHost.x(); + p1.y = positionHost.y(); + p1.z = positionHost.z(); + gtsam::Point3 positionObserved = otherRobotPose.translation(); + p2.x = positionObserved.x(); + p2.y = positionObserved.y(); + p2.z = positionObserved.z(); + + line.points.push_back(p1); + line.points.push_back(p2); + + // Add to the marker array + inter_robot_factors_.markers.push_back(line); + } + + // Publish for RViz + pubRelInterRobotFactors_.publish(inter_robot_factors_); + relInterRobotFactorsMtx_.unlock(); +} + void SLOAMNode::publishResults_(const SloamInput &sloamIn, const SloamOutput &sloamOut, ros::Time stamp, const int &robotID) { publishMap_(stamp); publishCubeMaps_(stamp); + publishInterRobotFactors(robotID); std::vector allLandmarks; std::vector allLabels; @@ -250,18 +315,20 @@ void SLOAMNode::publishResults_(const SloamInput &sloamIn, // Save the trajectory of the robot and corresponding timestamps for // each pose as a csv file save rotation as quaternion std::ofstream myfile; - std::string fname = save_results_dir_ + "/trajectory.csv"; + std::string fname = save_results_dir_ + "/robot" + std::to_string(robotID) + "trajectory.txt"; myfile.open(fname); - myfile << "x,y,z,qx,qy,qz,qw,timestamp\n"; for (size_t i = 0; i < trajectory.size(); i++) { - myfile << trajectory[i].translation()[0] << "," - << trajectory[i].translation()[1] << "," - << trajectory[i].translation()[2] << "," - << trajectory[i].so3().unit_quaternion().x() << "," - << trajectory[i].so3().unit_quaternion().y() << "," - << trajectory[i].so3().unit_quaternion().z() << "," - << trajectory[i].so3().unit_quaternion().w() << "," - << KeyPoseTimeStamps[i] << "\n"; + myfile << KeyPoseTimeStamps[i] << " " + << trajectory[i].translation()[0] << " " + << trajectory[i].translation()[1] << " " + << trajectory[i].translation()[2] << " " + << trajectory[i].so3().unit_quaternion().x() << " " + << trajectory[i].so3().unit_quaternion().y() << " " + << trajectory[i].so3().unit_quaternion().z() << " " + << trajectory[i].so3().unit_quaternion().w(); + if (i != trajectory.size() - 1) { + myfile << "\n"; + } } } @@ -702,31 +769,47 @@ void SLOAMNode::relInterRobotFactorThread_() { GetIndexClosestPoseMstPair(dbManager.getRobotDataByID(relativeMeas.robotIndex).poseMstPacket, relativeMeas.stamp, indexClosestOtherRobot, timeDiffClosest); if (indexClosestOtherRobot == -1 || timeDiffClosest > maxTimeDiff || - indexClosestOtherRobot >= pose_counter_other) { continue; } + indexClosestOtherRobot >= pose_counter_other) { + continue; + } // Iterate through our own poses, and make sure one exists in the factor graph thats close // enough in time to our measurement - size_t pose_counter_host = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); + size_t pose_counter_host = factorGraph_.getPoseCounterById(hostRobotID); GetIndexClosestPoseMstPair(poseMstPacket, relativeMeas.stamp, indexClosestHostRobot, timeDiffClosest); if (indexClosestHostRobot == -1 || timeDiffClosest > maxTimeDiff || - indexClosestHostRobot >= pose_counter_host) { continue; } + indexClosestHostRobot >= pose_counter_host) { + continue; + } // We found a match, so add it to the factor graph matches_found += 1; factorGraphMtx_.lock(); gtsam::Pose3 relativeMeasPose = SE3ToGTSAMPose3(relativeMeas.relativePose); - factorGraph_.addLoopClosureFactor(relativeMeasPose, indexClosestHostRobot, + factorGraph_.addRelativeMeasFactor(relativeMeasPose, indexClosestHostRobot, hostRobotID, indexClosestOtherRobot, relativeMeas.robotIndex); factorGraphMtx_.unlock(); - ROS_DEBUG_STREAM("A Relative Inter-Robot Measurement Factor is added between Robot #" << hostRobotID << " Pose " << indexClosestHostRobot - << "and Robot #" << relativeMeas.robotIndex << " Pose " << indexClosestOtherRobot); + //ROS_DEBUG_STREAM("Rel-Inter-Robot Factor added between Robot #" << hostRobotID << " Pose " << indexClosestHostRobot + // << "and Robot #" << relativeMeas.robotIndex << " Pose " << indexClosestOtherRobot); + + // Save this factor separately so that we can visualize it + RelativeInterRobotFactor factor; + factor.hostPoseIndex = indexClosestHostRobot; + factor.hostRobotID = hostRobotID; + factor.observedPoseIndex = indexClosestOtherRobot; + factor.observedRobotID = relativeMeas.robotIndex; + factor.relativePose = relativeMeas.relativePose; + factor.stamp = relativeMeas.stamp; + relInterRobotFactorsMtx_.lock(); + relative_inter_robot_factors.push_back(factor); + relInterRobotFactorsMtx_.unlock(); // Remove this measurement from vector, so we don't use it again feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); i--; } - // Check which factors are no longer feasible for removal + // Check which factors are no longer feasible so we can remove them for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { // Get pose_counter (tracks number of poses added to factor graph) @@ -758,7 +841,7 @@ void SLOAMNode::relInterRobotFactorThread_() { } } else { - ROS_DEBUG("No Available Relative Inter Robot Measurements"); + ROS_DEBUG_THROTTLE(5.0, "No Available Relative Inter Robot Measurements"); } // Unlock the mutexes and sleep diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index bbe2d21..6d86c76 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -46,11 +46,12 @@ SemanticFactorGraph::SemanticFactorGraph() { noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec); noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01); - - // XYZ 5mm standard deviation, Rotation 0.0175 rad standard deviation (~1 degree) + + // For Relative Inter-Robot Measurements + // XYZ 0.1m standard deviation, Rotation 0.1 rad standard deviation (~5.7 degree) // TODO: These are currently just estimates, update! Vector6 noise_model_rel_meas_vec; - noise_model_rel_meas_vec << 0.005, 0.005, 0.005, 0.0175, 0.0175, 0.0175; + noise_model_rel_meas_vec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; noise_model_rel_meas = noiseModel::Diagonal::Sigmas(noise_model_rel_meas_vec); start_time_ = ros::Time::now(); @@ -303,21 +304,18 @@ void SemanticFactorGraph::addLoopClosureFactor(const Pose3 poseRelative, noise_model_closure)); } -/** - * @brief Adds a Relative Inter-Robot Measurement Factor to the factor graph. - * - * @param poseRelative - Transformation from Robot #fromRobotID to Robot #toRobotID - * @param fromIdx - - */ -void SemanticFactorGraph::addRelativeMeasurementFactor(const Pose3 poseRelative, - const size_t fromIdx, const size_t fromRobotID, - const size_t toIdx, const size_t toRobotID) { - - // TODO: Scale the noise model based on the relative distance - fgraph.add(BetweenFactor(getSymbol(fromRobotID, fromIdx), - getSymbol(toRobotID, toIdx), poseRelative, +void SemanticFactorGraph::addRelativeMeasFactor(const Pose3 poseRelative, + const size_t prevIdx, + const size_t robotID1, + const size_t curIdx, + const size_t robotID2) { + // Correct interface for BetweenFactor(pose1, pose2, H_pose2^pose1, + // noise) poseRelative is H_query^candidate, i.e., from query pose to + // candidate (history) pose candidate is robotID1, prevIdx query is robotID2, + // curIdx + fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), + getSymbol(robotID2, curIdx), poseRelative, noise_model_rel_meas)); - } void SemanticFactorGraph::solve() { diff --git a/backend/sloam/src/tests/inter_robot_factor_test.cpp b/backend/sloam/src/tests/inter_robot_factor_test.cpp index 93d00c2..be2811b 100644 --- a/backend/sloam/src/tests/inter_robot_factor_test.cpp +++ b/backend/sloam/src/tests/inter_robot_factor_test.cpp @@ -1,9 +1,7 @@ #include #include -TEST(InterRobotFactorTests, TestMeasurementCallback) { - Robot robot; -} +// No Tests defined currently int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 50d5de0..e5c5d6f 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -8,6 +8,7 @@ project(relative_meas_gen) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + geometry_msgs message_generation message_runtime message_filters @@ -27,7 +28,7 @@ find_package(yaml-cpp) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -168,8 +169,8 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# scripts/multiUGVToSlideSLAM.py +# DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} # ) ## Mark executables for installation @@ -212,3 +213,8 @@ include_directories( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(launch/testCases.launch) +endif() \ No newline at end of file diff --git a/frontend/relative_meas_gen/__init__.py b/frontend/relative_meas_gen/__init__.py new file mode 100644 index 0000000..8194a68 --- /dev/null +++ b/frontend/relative_meas_gen/__init__.py @@ -0,0 +1 @@ +from scripts.multiUGVToSlideSLAM import MultiUGVPub \ No newline at end of file diff --git a/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh new file mode 100755 index 0000000..a415380 --- /dev/null +++ b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh @@ -0,0 +1,84 @@ +#!/bin/bash + +# This script runs the backend of SlideSLAM (sloam) on simulated jackal +# robots from the multi-UGV-gazebo-sim. In order to bypass SlideSLAM's +# frontend, we spawn a publisher to convert GT information into fake +# frontend measurements. + +# Set parameters +SESSION_NAME=multi_UGV_run +BAG_PLAY_RATE=1 +BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/multi-UGV-gazebo-sim-data/' +SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" + +# Set Display Settings +CURRENT_DISPLAY=${DISPLAY} +if [ -z ${DISPLAY} ]; +then + echo "DISPLAY is not set" + CURRENT_DISPLAY=:0 +fi + +# Start Tmux Session if not already in one +if [ -z ${TMUX} ]; +then + TMUX= tmux new-session -s $SESSION_NAME -d + echo "Starting new session." +else + echo "Already in tmux, leave it first." + exit +fi + +# Make mouse useful in copy mode +tmux setw -g mouse on + +# Setup the panes for the main window +tmux new-window -t $SESSION_NAME -n "Main" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.0 +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:1.3 +tmux split-window -h -t $SESSION_NAME + +# Setup commands for main window +tmux select-pane -t $SESSION_NAME:1.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch relative_meas_gen odomRelativeMeasSync.launch" Enter +tmux select-pane -t $SESSION_NAME:1.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play stuff_world-two_jackal_amcl.bag --clock -r $BAG_PLAY_RATE -s 0 --topic /jackal0/odometry/local_filtered /jackal1/odometry/local_filtered /gazebo/model_states /jackal0/odometry/local_filtered:=/robot0/odometry/local_filtered /jackal1/odometry/local_filtered:=/robot1/odometry/local_filtered" Enter +tmux select-pane -t $SESSION_NAME:1.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch relative_meas_gen multiUGVToSlideSLAM.launch publish_for_rviz:=true" Enter +tmux select-pane -t $SESSION_NAME:1.3 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch object_modeller sync_semantic_measurements.launch robot_name:=robot0 odom_topic:=/robot0/odometry/local_filtered" Enter +tmux select-pane -t $SESSION_NAME:1.4 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch object_modeller sync_semantic_measurements.launch robot_name:=robot1 odom_topic:=/robot1/odometry/local_filtered" Enter +tmux select-layout -t $SESSION_NAME tiled + +# Add window for roscore +tmux new-window -t $SESSION_NAME -n "roscore" +tmux split-window -h -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:2.0 +tmux split-window -v -t $SESSION_NAME +tmux select-pane -t $SESSION_NAME:2.2 +tmux split-window -v -t $SESSION_NAME + +# Set commands for roscore window +tmux select-pane -t $SESSION_NAME:2.0 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter +tmux select-pane -t $SESSION_NAME:2.1 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=0 odom_topic:=/robot0/odometry/local_filtered enable_rviz:=true turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter +tmux select-pane -t $SESSION_NAME:2.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter +tmux select-pane -t $SESSION_NAME:2.3 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=1 odom_topic:=/robot1/odometry/local_filtered turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter + +# Add window to easily kill all processes +tmux new-window -t $SESSION_NAME -n "Kill" +tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}" + +# Go back to the first window +tmux select-window -t $SESSION_NAME:1 +tmux -2 attach-session -t $SESSION_NAME diff --git a/frontend/relative_meas_gen/config/multiUGV.rviz b/frontend/relative_meas_gen/config/multiUGV.rviz new file mode 100644 index 0000000..4d8ae96 --- /dev/null +++ b/frontend/relative_meas_gen/config/multiUGV.rviz @@ -0,0 +1,178 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Pose2 + - /Pose3 + Splitter Ratio: 0.5 + Tree Height: 1550 + - 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: "" +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 + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /relative_inter_robot_meas/rviz/jackal0_pose + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 11; 47; 255 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /relative_inter_robot_meas/rviz/jackal1_pose + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 255; 15 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /relative_inter_robot_meas/rviz/relative_pose + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 22.374713897705078 + 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.6437821388244629 + Y: 0.0025311261415481567 + Z: -1.0751668214797974 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9903979301452637 + Target Frame: + Yaw: 3.2835824489593506 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000514000006fcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000abfb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000006fc0000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000006fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000006fc0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7400000056fc0100000002fb0000000800540069006d0065010000000000000e740000058200fffffffb0000000800540069006d00650100000000000004500000000000000000000007f9000006fc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3700 + X: 140 + Y: 54 diff --git a/frontend/relative_meas_gen/launch/multiUGVToSlideSLAM.launch b/frontend/relative_meas_gen/launch/multiUGVToSlideSLAM.launch new file mode 100644 index 0000000..c361d83 --- /dev/null +++ b/frontend/relative_meas_gen/launch/multiUGVToSlideSLAM.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/odomRelativeMeasSync.launch b/frontend/relative_meas_gen/launch/odomRelativeMeasSync.launch new file mode 100644 index 0000000..4c48027 --- /dev/null +++ b/frontend/relative_meas_gen/launch/odomRelativeMeasSync.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/testCases.launch b/frontend/relative_meas_gen/launch/testCases.launch new file mode 100644 index 0000000..9634106 --- /dev/null +++ b/frontend/relative_meas_gen/launch/testCases.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 96ccc6d..1bdc3ce 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -54,6 +54,7 @@ rospy sloam_msgs std_msgs + gazebo_msgs yaml-cpp sensor_msgs apriltag @@ -62,6 +63,7 @@ roscpp rospy sloam_msgs + gazebo_msgs std_msgs yaml-cpp sensor_msgs @@ -72,6 +74,7 @@ roscpp rospy sloam_msgs + gazebo_msgs std_msgs yaml-cpp sensor_msgs diff --git a/frontend/relative_meas_gen/requirements.txt b/frontend/relative_meas_gen/requirements.txt deleted file mode 100644 index 7f7efa0..0000000 --- a/frontend/relative_meas_gen/requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -pyproj=3.5.0 -numpy=1.22.3 \ No newline at end of file diff --git a/frontend/relative_meas_gen/results/.gitignore b/frontend/relative_meas_gen/results/.gitignore new file mode 100644 index 0000000..c96a04f --- /dev/null +++ b/frontend/relative_meas_gen/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore \ No newline at end of file diff --git a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py new file mode 100755 index 0000000..780d752 --- /dev/null +++ b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python + +import rospy +import rospkg +from geometry_msgs.msg import Pose, PoseStamped +from gazebo_msgs.msg import ModelStates +from sloam_msgs.msg import RelativeInterRobotMeasurement, StampedRvizMarkerArray +from relative_meas_gen import transforms +from std_msgs.msg import Header +from visualization_msgs.msg import MarkerArray, Marker +import xml.etree.ElementTree as ET + +# This class takes GT robot poses and world structure from the multi-UGV-gazebo-sim and +# publishes corresponding relative inter-robot measurements and object detections +# for the SlideSLAM backend (sloam) +class MultiUGVPub: + def __init__(self): + # Get parameters + self.publish_for_rviz = rospy.get_param("publish_for_rviz", default=False) + + # Set up rospack + self.rospack = rospkg.RosPack() + + # Subscribers + self.sub_gazebo = rospy.Subscriber('/gazebo/model_states', ModelStates, self.gazebo_callback) + + # Publishers + self.pub_relative = rospy.Publisher("/relative_inter_robot_meas", RelativeInterRobotMeasurement, queue_size=10) + self.pub_jackal0_cuboids = rospy.Publisher("/robot0/cuboid_measurements", StampedRvizMarkerArray, queue_size=10) + self.pub_jackal1_cuboids = rospy.Publisher("/robot1/cuboid_measurements", StampedRvizMarkerArray, queue_size=10) + + # Publishers for RVIZ + if (self.publish_for_rviz): + self.pub_jackal0_rviz = rospy.Publisher("/relative_inter_robot_meas/rviz/jackal0_pose", PoseStamped, queue_size=10) + self.pub_jackal1_rviz = rospy.Publisher("/relative_inter_robot_meas/rviz/jackal1_pose", PoseStamped, queue_size=10) + self.pub_relative_rviz = rospy.Publisher("/relative_inter_robot_meas/rviz/relative_pose", PoseStamped, queue_size=10) + + # Keep track of messages sent + self.sequence_number = 0 + + # Frequency with which to publish relative inter-robot measurements + self.publish_rate = 1 # Hz + self.last_publish_time = rospy.Time.now() + + # Write the GT trajectory files + with open(self.rospack.get_path('relative_meas_gen') + '/results/robot0trajectoryGT.txt', 'w') as f: + pass + with open(self.rospack.get_path('relative_meas_gen') + '/results/robot1trajectoryGT.txt', 'w') as f: + pass + + # Helper method that publishes a PoseStamped message for RVIZ + def publish_rviz(self, pose: Pose, frame_id: str, publisher: rospy.Publisher): + rviz_msg = PoseStamped() + rviz_msg.header.stamp = rospy.Time.now() + rviz_msg.header.seq = self.sequence_number + rviz_msg.header.frame_id = frame_id + rviz_msg.pose = pose + publisher.publish(rviz_msg) + + # Helper method that creates a Marker message for a cuboid + def create_marker_for_cuboid(self, model: ET.Element, robot_name: str, robot_pose: Pose) -> Marker: + cuboid = Marker() + cuboid.header.stamp = rospy.Time.now() + cuboid.header.seq = self.sequence_number + cuboid.header.frame_id = robot_name + "/base_link" + cuboid.ns = robot_name + "/cuboid" + cuboid.id = int(model.get('name')[9]) + cuboid.type = Marker.CUBE + cuboid.action = Marker.ADD + + # Extract the pose of the cuboid + pose_vals = model.find('pose').text.strip().split(' ') + pose = Pose() + pose.position.x = float(pose_vals[0]) + pose.position.y = float(pose_vals[1]) + pose.position.z = float(pose_vals[2]) + pose.orientation.x = 0 + pose.orientation.y = 0 + pose.orientation.z = 0 + pose.orientation.w = 1 + if float(pose_vals[3]) != 0 or float(pose_vals[4]) != 0 or float(pose_vals[5]) != 0: + rospy.logerr_throttle(10, "Cuboid rotation matrix is not Identity! Program functionality for this!") + return cuboid + + # Transform pose into the relative frame of the robots + cuboid.pose = transforms.calculate_relative_pose(robot_pose, pose) + + # Extract the scale of the cuboid + scale_vals = model.find('link').find('collision').find('geometry').find('box').find('size').text.strip().split(' ') + cuboid.scale.x = float(scale_vals[0]) + cuboid.scale.y = float(scale_vals[1]) + cuboid.scale.z = float(scale_vals[2]) + + # Set other values + cuboid.lifetime = rospy.Duration(0) # 0 means forever + cuboid.frame_locked = False # Should stay in relative position for this frame + return cuboid + + # This method calculates a relative inter-robot measurement + # from the ground truth poses of two robots. It also publishes + # the relative positions of the cuboids relative to the robots. + def gazebo_callback(self, msg: ModelStates): + # Make sure the clock is publishing before we start + while rospy.Time.now() == 0.0: + rospy.loginfo_throttle(10, "Waiting for clock to start... ModelStates msg dropped...") + return + + # Get the index of jackal0 and jackal1 robots + try: + jackal0_idx = msg.name.index('jackal0') + jackal1_idx = msg.name.index('jackal1') + except ValueError: + rospy.logerr_throttle(10, "Could not find jackal0 or jackal1 in the ModelStates message") + return + + # Get the pose of both robots + jackal0_pose: Pose = msg.pose[jackal0_idx] + jackal1_pose: Pose = msg.pose[jackal1_idx] + + # Calculate the relative measurement from jackal0 to jackal1 + relative_meas: Pose = transforms.calculate_relative_pose(jackal0_pose, jackal1_pose) + + # Create the header for the messages + header = Header() + header.stamp = rospy.Time.now() + header.seq = self.sequence_number + header.frame_id = "jackal0/base_link" + + # Publish a relative inter-robot measurement + if (self.last_publish_time + rospy.Duration(1.0/self.publish_rate) < rospy.Time.now()): + msg = RelativeInterRobotMeasurement() + msg.header = header + msg.relativePose = relative_meas + msg.robotIdObserver = 0 + msg.robotIdObserved = 1 + self.pub_relative.publish(msg) + self.last_publish_time = rospy.Time.now() + + # Publish for RVIZ + if(self.publish_for_rviz): + self.publish_rviz(jackal0_pose, "map", self.pub_jackal0_rviz) + self.publish_rviz(jackal1_pose, "map", self.pub_jackal1_rviz) + self.publish_rviz(relative_meas, "jackal0/base_link", self.pub_relative_rviz) + + # Increase the sequence number + self.sequence_number += 1 + + # Get path to .world file + world_file = self.rospack.get_path('relative_meas_gen') + '/worlds/stuff.world' + + # Parse the XML file + tree = ET.parse(world_file) + root = tree.getroot() + world = root.find('world') + + # Create the MarkerArrays for jackal0 and jackal1 + jackal0_cuboids = MarkerArray() + jackal1_cuboids = MarkerArray() + for model in world.findall('model'): + name = model.get('name') + if "_box_" in name: + jackal0_cuboids.markers.append(self.create_marker_for_cuboid(model, "jackal0", jackal0_pose)) + jackal1_cuboids.markers.append(self.create_marker_for_cuboid(model, "jackal1", jackal1_pose)) + + # Create the StampedRvizMarkerArray messages + jackal0_array = StampedRvizMarkerArray() + jackal0_array.header = header + jackal0_array.cuboid_rviz_markers = jackal0_cuboids + jackal1_array = StampedRvizMarkerArray() + jackal1_array.header = header + jackal1_array.cuboid_rviz_markers = jackal1_cuboids + + # Publish the cuboid measurements + self.pub_jackal0_cuboids.publish(jackal0_array) + self.pub_jackal1_cuboids.publish(jackal1_array) + + # Check the first GT trajectory file to see if its empty + # Assume the second file is in the same state + empty = False + with open(self.rospack.get_path('relative_meas_gen') + '/results/robot0trajectoryGT.txt') as f: + if not f.read(1): + empty = True + + # Create the GT trajectory file for jackal0 and jackal1 + with open(self.rospack.get_path('relative_meas_gen') + '/results/robot0trajectoryGT.txt', 'a') as f: + if not empty: + f.write('\n') + f.write(str(header.stamp.to_sec()) + ' ' + + str(jackal0_pose.position.x) + ' ' + + str(jackal0_pose.position.y) + ' ' + + str(jackal0_pose.position.z) + ' ' + + str(jackal0_pose.orientation.x) + ' ' + + str(jackal0_pose.orientation.y) + ' ' + + str(jackal0_pose.orientation.z) + ' ' + + str(jackal0_pose.orientation.w)) + with open(self.rospack.get_path('relative_meas_gen') + '/results/robot1trajectoryGT.txt', 'a') as f: + if not empty: + f.write('\n') + f.write(str(header.stamp.to_sec()) + ' ' + + str(jackal1_pose.position.x) + ' ' + + str(jackal1_pose.position.y) + ' ' + + str(jackal1_pose.position.z) + ' ' + + str(jackal1_pose.orientation.x) + ' ' + + str(jackal1_pose.orientation.y) + ' ' + + str(jackal1_pose.orientation.z) + ' ' + + str(jackal1_pose.orientation.w)) + +if __name__ == '__main__': + # Initialize the node with given name + node_name = rospy.get_param("multi_UGV_to_SlideSLAM_node_name", default="multi_UGV_pub") + rospy.init_node(node_name) + meas_gen = MultiUGVPub() + + # Run until shutdown + while not rospy.is_shutdown(): + rospy.spin() + rospy.loginfo("Shutting down {}...".format(node_name)) \ No newline at end of file diff --git a/frontend/relative_meas_gen/scripts/odomRelativeMeasSync.py b/frontend/relative_meas_gen/scripts/odomRelativeMeasSync.py new file mode 100755 index 0000000..f6c9f69 --- /dev/null +++ b/frontend/relative_meas_gen/scripts/odomRelativeMeasSync.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +import rospy +import sensor_msgs.msg +import nav_msgs.msg +from sloam_msgs.msg import RelativeInterRobotMeasurement, RelativeInterRobotMeasurementOdom, StampedRvizMarkerArray +import message_filters + +class OdomRelativeMeasSync: + def __init__(self): + + # Synchronize the odometry and RelativeInterRobotMeasurement + sub_relative_meas = message_filters.Subscriber("/relative_inter_robot_meas", RelativeInterRobotMeasurement) + sub_odom_jackal0 = message_filters.Subscriber("/robot0/odometry/local_filtered", nav_msgs.msg.Odometry) + sub_odom_jackal1 = message_filters.Subscriber("/robot1/odometry/local_filtered", nav_msgs.msg.Odometry) + self.ts = message_filters.ApproximateTimeSynchronizer([sub_relative_meas, sub_odom_jackal0, sub_odom_jackal1], queue_size=100, slop=0.0125) + self.ts.registerCallback(self.publish_sync) + + # Publisher + self.pub = rospy.Publisher('/relative_inter_robot_meas_sync', RelativeInterRobotMeasurementOdom, queue_size=10) + + # Keep track of messages sent + self.sequence_number = 0 + + def publish_sync(self, msg_relative: RelativeInterRobotMeasurement, + msg_jackal0: nav_msgs.msg.Odometry, msg_jackal1: nav_msgs.msg.Odometry): + + # Create a new RelativeInterRobotMeasurementOdom message + msg = RelativeInterRobotMeasurementOdom() + msg.header.seq = self.sequence_number + msg.header.stamp = msg_relative.header.stamp # Assume odometry at same timestamp as relative measurement + msg.header.frame_id = "jackal0/base_link" + msg.relativePose = msg_relative.relativePose + msg.robotIdObserver = 0 + msg.robotIdObserved = 1 + msg.odometryObserver = msg_jackal0 + msg.odometryObserved = msg_jackal1 + self.pub.publish(msg) + + # Output the difference in timestamps + rospy.loginfo_throttle(10, "Time difference from jackal0 Odom to Relative Measurement: {}".format(msg_jackal0.header.stamp.to_sec() - msg_relative.header.stamp.to_sec())) + rospy.loginfo_throttle(10, "Time difference from jackal1 Odom to Relative Measurement: {}".format(msg_jackal1.header.stamp.to_sec() - msg_relative.header.stamp.to_sec())) + + # Increment the sequence number + self.sequence_number += 1 + +if __name__ == '__main__': + # Initialize the node with given name + node_name = rospy.get_param("/odom_gps_sync_node_name", default="odom_gps_sync_node_name") + rospy.init_node(node_name) + sync = OdomRelativeMeasSync() + + # Run until shutdown + while not rospy.is_shutdown(): + rospy.spin() + rospy.loginfo("Shutting down {}...".format(node_name)) \ No newline at end of file diff --git a/frontend/relative_meas_gen/setup.py b/frontend/relative_meas_gen/setup.py new file mode 100644 index 0000000..9d09582 --- /dev/null +++ b/frontend/relative_meas_gen/setup.py @@ -0,0 +1,11 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['relative_meas_gen'], + package_dir={'': 'src'}) + +setup(**setup_args) \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/relative_meas_gen/__init__.py b/frontend/relative_meas_gen/src/relative_meas_gen/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py new file mode 100644 index 0000000..acb2291 --- /dev/null +++ b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py @@ -0,0 +1,30 @@ +import numpy as np +from scipy.spatial.transform import Rotation +from geometry_msgs.msg import Pose + +# This method calculates the pose of "pose2" in the frame of "pose1" +def calculate_relative_pose(pose1: Pose, pose2: Pose) -> Pose: + p1 = np.array([pose1.position.x, pose1.position.y, pose1.position.z]) + p2 = np.array([pose2.position.x, pose2.position.y, pose2.position.z]) + + # Extract orientations (quaternions) + q1 = np.array([pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w]) + q2 = np.array([pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w]) + + # Convert quaternions to rotation matrices + R1 = Rotation.from_quat(q1).as_matrix() + R2 = Rotation.from_quat(q2).as_matrix() + + # Compute the inverse of R1 and apply it to p2-p1 to get relative translation + p_rel = R1.T @ (p2 - p1) + + # Compute relative rotation + R_rel = R1.T @ R2 + q_rel = Rotation.from_matrix(R_rel).as_quat() # Convert back to quaternion + + # Create new Pose message + rel_pose = Pose() + rel_pose.position.x, rel_pose.position.y, rel_pose.position.z = p_rel + rel_pose.orientation.x, rel_pose.orientation.y, rel_pose.orientation.z, rel_pose.orientation.w = q_rel + + return rel_pose \ No newline at end of file diff --git a/frontend/relative_meas_gen/tests/testMultiUGV.py b/frontend/relative_meas_gen/tests/testMultiUGV.py new file mode 100755 index 0000000..7d900ab --- /dev/null +++ b/frontend/relative_meas_gen/tests/testMultiUGV.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +import unittest +import numpy as np +import rospy +from geometry_msgs.msg import Pose, PoseStamped +from relative_meas_gen.transforms import calculate_relative_pose + +class TestMultiUGV(unittest.TestCase): + """ + Test multiUGVToSlideSLAM.py + """ + + def setUp(self): + rospy.init_node('multi_ugv_test_node', anonymous=True) + + def test_calculate_relative_pose(self): + """ + Test that the calculate_relative_pose method properly calculates + the relevant transform between two poses. + """ + + # Setup two test poses + pose1 = Pose(position=Pose().position.__class__(3.4, -5.2, 1.1), orientation=Pose().orientation.__class__(0.4029115, 0.1611646, 0.805823, 0.4029115)) + pose2 = Pose(position=Pose().position.__class__(1, 2, 3), orientation=Pose().orientation.__class__(0, 0, 0.7071068, 0.7071068)) + + # Call the method + rel_pose = calculate_relative_pose(pose1, pose2) + + # Make sure it's correct + np.testing.assert_almost_equal(rel_pose.position.x, 7.43896085266152, 6) + np.testing.assert_almost_equal(rel_pose.position.y, -2.13116887703829, 6) + np.testing.assert_almost_equal(rel_pose.position.z, -1.15324631249453, 6) + np.testing.assert_almost_equal(rel_pose.orientation.x, -0.398862) + np.testing.assert_almost_equal(rel_pose.orientation.y, 0.1709409) + np.testing.assert_almost_equal(rel_pose.orientation.z, -0.2849014) + np.testing.assert_almost_equal(rel_pose.orientation.w, 0.8547043) + +if __name__ == '__main__': + import rostest + rostest.rosrun('relative_meas_gen', 'test_multi_ugv', TestMultiUGV) \ No newline at end of file diff --git a/frontend/relative_meas_gen/worlds/stuff.world b/frontend/relative_meas_gen/worlds/stuff.world new file mode 100644 index 0000000..798d751 --- /dev/null +++ b/frontend/relative_meas_gen/worlds/stuff.world @@ -0,0 +1,584 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.5 -1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 1 + + + + 0.01 + 1 + 100 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 2.81763 -1.21896 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + -3.03468 -2.57831 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + -2.87758 5.71413 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 8.58956 0.355426 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 4.52527 2.44264 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 1 + + + + 5.09022 -6.14052 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 1 + + + + -5.93603 2.22882 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 1 + + + + 59 980000000 + 61 209324015 + 1517798482 659137209 + 5998 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.000575 -0.019283 0.063512 1e-06 -0.00041 0.105134 + 1 1 1 + + 0.000575 -0.019283 0.063512 1e-06 -0.00041 0.105134 + -1e-06 0.000996 0.000632 -0.000957 0.00481 -0.002507 + 0.299849 0.227732 -0.400401 -2.98511 0.110231 2.33181 + 5.28454 4.01354 -7.05667 0 -0 0 + + + 0.11115 0.18116 0.098038 -0.002502 0.10051 0.104791 + 0.000383 0.000481 0.000605 -0.003962 0.003993 -0.000452 + 0.186142 0.077355 0.085022 -0.561616 1.29688 2.36612 + 0.08879 0.036898 0.040555 0 -0 0 + + + 0.150544 -0.192223 0.098044 0.002815 0.225969 0.105804 + -9.4e-05 0.000623 0.000749 -0.007298 -0.001021 -0.001007 + 0.004335 0.106121 0.144738 -1.04582 -0.687296 -3.082 + 0.002068 0.05062 0.06904 0 -0 0 + + + -0.149307 0.153744 0.098008 0.000992 -1.17374 0.103593 + -0.001408 0.001028 0.000124 -0.011007 -0.010058 0.01965 + 0.173612 0.285241 -1.00917 0.458202 1.49833 0.199793 + 0.082813 0.13606 -0.481375 0 -0 0 + + + -0.109898 -0.219769 0.098011 0.000873 -0.817357 0.104165 + -0.001882 0.00098 -2.5e-05 -0.010527 -0.015712 0.014983 + -0.01136 0.26693 -1.04157 -2.67571 -0.278088 2.75121 + -0.005419 0.127325 -0.496827 0 -0 0 + + + + 2.81765 -1.21927 0.49967 0.00066 -0 0 + 1 1 1 + + 2.81765 -1.21927 0.49967 0.00066 -0 0 + 0 0 0 0 -0 0 + 0.001271 -3.30003 -4.29997 0.316814 0.001898 0.000471 + 0.001271 -3.30003 -4.29997 0 -0 0 + + + + -3.03466 -2.57824 0.49907 -0.0001 0 0 + 1 1 1 + + -3.03466 -2.57824 0.49907 -0.0001 0 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + -2.87756 5.71381 0.49967 0.00066 -0 0 + 1 1 1 + + -2.87756 5.71381 0.49967 0.00066 -0 0 + 0 0 0 0 -0 0 + 0.001272 -3.30003 -4.29997 0.316814 0.001899 0.000471 + 0.001272 -3.30003 -4.29997 0 -0 0 + + + + 8.58957 0.355483 0.49907 -0.0001 0 0 + 1 1 1 + + 8.58957 0.355483 0.49907 -0.0001 0 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 4.52547 2.44244 0.499789 0.000422 0.000422 -3e-06 + 1 1 1 + + 4.52547 2.44244 0.499789 0.000422 0.000422 -3e-06 + 0 0 0 0 -0 0 + 2.11189 -2.11212 -3.11199 1.08259 -1.08223 3.1415 + 2.11189 -2.11212 -3.11199 0 -0 0 + + + + 5.09009 -6.14039 0.499824 -0.000249 -0.000249 -2e-06 + 1 1 1 + + 5.09009 -6.14039 0.499824 -0.000249 -0.000249 -2e-06 + 0 0 0 0 -0 0 + -4.34984 4.35027 -0.051026 0.726793 -0.72756 -3.14154 + -4.34984 4.35027 -0.051026 0 -0 0 + + + + -5.93601 2.2288 0.499096 4.8e-05 4.8e-05 -1e-06 + 1 1 1 + + -5.93601 2.2288 0.499096 4.8e-05 4.8e-05 -1e-06 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 17.6556 -10.443 5.76936 0 0.275643 2.35619 + orbit + perspective + + + + From daa3a95056797c75b0714daf201e6d907685eeb2 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Tue, 11 Mar 2025 21:21:23 +0000 Subject: [PATCH 16/74] Prototype relative transformation function, no test case yet --- .../include/apriltag_meas_gen.h | 4 +-- .../src/apriltag_meas_gen.cpp | 36 +++++++++++++------ 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 2bec886..30ad4cc 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -23,8 +23,8 @@ class ApriltagMeasurer { void imageCallback(const sensor_msgs::CompressedImage msg); void PublishRelativeMeasurement(int8_t, Eigen::Matrix4f transformation); - Eigen::Matrix4f CalculateRelativeTransformation(Eigen::Matrix4f bot_to_cam_RT, Eigen::Matrix4f cam_to_tag_RT, Eigen::Matrix4f bot_to_tag); - Eigen::Matrix4f RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw); + Eigen::Matrix4d CalculateRelativeTransformation(Eigen::Matrix4d H_hostBot_to_cam, + Eigen::Matrix4d H_cam_to_tag, Eigen::Matrix4d H_observedBot_to_tag); std::tuple> LoadTransformations(slidetag tag); ros::NodeHandle nh_; diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index e596feb..1e4bfae 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -157,16 +157,32 @@ int main(int argc, char** argv) { return 0; } -Eigen::Matrix4f ApriltagMeasurer::RollPitchYaw_to_RT(float x, float y, float z, float roll, float pitch, float yaw) { - /* - TODO: Write calculations - */ -} - -Eigen::Matrix4f ApriltagMeasurer::CalculateRelativeTransformation(Eigen::Matrix4f bot_to_cam, Eigen::Matrix4f cam_to_tag, Eigen::Matrix4f bot_to_tag) { - /* - TODO: Write calculations - */ +/* + * This function calculates the relative transformation between + * two robots. + * + * Parameters: + * H_hostBot_to_cam - Transformation from the host robot base frame + * to the camera frame + * H_cam_to_tag - Transformation from the camera to the observed tag + * H_observedBot_to_tag - Transformation from the observed robot base + * frame to the observed tag + * + * Returns: + * Eigen::Matrix4d - Transformation from the host robot base frame to + * the observed robot base frame. + */ +Eigen::Matrix4d ApriltagMeasurer::CalculateRelativeTransformation(Eigen::Matrix4d H_hostBot_to_cam, + Eigen::Matrix4d H_cam_to_tag, Eigen::Matrix4d H_observedBot_to_tag) { + // Calculate transformation from bot_to_tag + Eigen::Matrix4d H_bot_to_tag = H_hostBot_to_cam * H_cam_to_tag; + + // Invert to get tag to observedBot + Eigen::Matrix4d T_tag_to_observedBot = H_observedBot_to_tag.inverse(); + + // Chain to get bot -> observedBot + Eigen::Matrix4d T_bot_to_observedBot = H_bot_to_tag * T_tag_to_observedBot; + return T_bot_to_observedBot; } void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4f transformation) { From c3601efc500f5a2a440a566fd3f3c9c303ca2957 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Sat, 15 Mar 2025 16:53:45 +0000 Subject: [PATCH 17/74] Fix vunerability in run_container.sh, fix bug in Dockerfile --- docker/Dockerfile | 2 +- docker/build_image.sh | 0 docker/run_container.sh | 4 ++-- 3 files changed, 3 insertions(+), 3 deletions(-) mode change 100644 => 100755 docker/build_image.sh diff --git a/docker/Dockerfile b/docker/Dockerfile index b8f6bf9..48204ec 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -17,7 +17,7 @@ RUN git clone https://github.com/AprilRobotics/apriltag.git && \ ldconfig # Install pyproj into the python environment -RUN pip install pyproj=3.5.0 +RUN pip install pyproj==3.5.0 # Set working directory back to default WORKDIR /app diff --git a/docker/build_image.sh b/docker/build_image.sh old mode 100644 new mode 100755 diff --git a/docker/run_container.sh b/docker/run_container.sh index 00401bb..10be7ca 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -2,7 +2,6 @@ SlideSlamWs="/home/dbutterfield3/slideslam_docker_ws" # point to your workspace SlideSlamCodeDir="/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR='/home/dbutterfield3/slideslam_docker_ws/src/SLIDE_SLAM/bags' # point to your bags / data directory -xhost +local:root # for the lazy and reckless docker run -it \ --name="slideslam_ros" \ --net="host" \ @@ -11,13 +10,14 @@ docker run -it \ --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ --env="QT_X11_NO_MITSHM=1" \ - --env="XAUTHORITY=$XAUTH" \ + --env="XAUTHORITY=/root/.Xauthority" \ --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ --volume="$BAGS_DIR:/opt/bags" \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ + --volume="$XAUTHORITY:/root/.Xauthority:ro" slideslam_ros_image \ bash From 130bd44f29eb65245e40be4289d518a6cc77ee9d Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Sat, 15 Mar 2025 22:44:39 +0000 Subject: [PATCH 18/74] Untested code for ensuring all measurements added to factor graph in timestep order --- backend/sloam/CMakeLists.txt | 2 +- backend/sloam/include/core/input.h | 14 + backend/sloam/include/core/inputNode.h | 90 +++++ backend/sloam/include/core/robot.h | 10 +- backend/sloam/src/core/cylinderMapManager.cpp | 5 + backend/sloam/src/core/input.cpp | 95 +++++ backend/sloam/src/core/inputNode.cpp | 353 +++++------------- backend/sloam/src/core/robot.cpp | 91 +++-- 8 files changed, 340 insertions(+), 320 deletions(-) create mode 100644 backend/sloam/include/core/input.h create mode 100644 backend/sloam/include/core/inputNode.h create mode 100644 backend/sloam/src/core/input.cpp diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 6c798d5..ecf60c6 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -100,7 +100,7 @@ cc_library( cc_library( NAME core INCS include/core ${CLIPPER_DIR}/include - SRCS "src/core/sloam.cpp" "src/core/robot.cpp" "src/core/databaseManager.cpp" "src/core/place_recognition.cpp" + SRCS "src/core/sloam.cpp" "src/core/robot.cpp" "src/core/databaseManager.cpp" "src/core/place_recognition.cpp" "src/core/input.cpp" DEPS sloam_log sloam_base sloam_objects sloam_viz sloam_map sloam_factorgraph Eigen3::Eigen gtsam ${catkin_LIBRARIES} -ldw DEFINES USE_CLIPPER=${CLIPPER_VALUE} ) diff --git a/backend/sloam/include/core/input.h b/backend/sloam/include/core/input.h new file mode 100644 index 0000000..4ce7434 --- /dev/null +++ b/backend/sloam/include/core/input.h @@ -0,0 +1,14 @@ +#include + +class Input { + +public: + void PickNextMeasurementToAdd(std::deque &robotOdomQueue_, + std::queue &robotObservationQueue_, + std::deque &robotRelativeMeasQueue_, + StampedSE3 &robotLatestOdom_, + ros::Time current_time, + double msg_delay_tolerance, + float minOdomDistance_, + int &meas_to_add); +}; \ No newline at end of file diff --git a/backend/sloam/include/core/inputNode.h b/backend/sloam/include/core/inputNode.h new file mode 100644 index 0000000..4ce55fe --- /dev/null +++ b/backend/sloam/include/core/inputNode.h @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "sloam_msgs/ROSRangeBearingSyncOdom.h" +#include "sloam_msgs/ROSSyncOdom.h" + +class InputManager : public Input { + public: + explicit InputManager(ros::NodeHandle nh); + void RunInputNode(const ros::TimerEvent &e); + void saveRuntimeCommUsage(); + Robot robot; + + private: + void resetAllFlags(); + float runInputNodeRate_; + ros::Timer timer_; + + void updateLastPose(const StampedSE3 &odom, const int &robotID); + + double max_timestamp_offset_ = 0.01; + + bool callSLOAM(SE3 relativeRawOdomMotion, ros::Time stamp, + std::deque &odomQueue, const int &robotID); + void PublishAccumOdom_(const SE3 &relativeRawOdomMotion); + void Odom2SlamTf(); + void PublishOdomAsTf(const nav_msgs::Odometry &odom_msg, + const std::string &parent_frame_id, + const std::string &child_frame_id); + + SE3 computeSloamToVioOdomTransform(const SE3 &sloam_odom, + const SE3 &vio_odom); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + tf2_ros::TransformBroadcaster broadcaster_; + + // params + std::string map_frame_id_; + std::string odom_frame_id_; + std::string robot_ns_prefix_; + int number_of_robots_; + std::string odom_topic_; + std::string robot_frame_id_; + float minOdomDistance_; + float minSLOAMAltitude_; + + // vars + boost::shared_ptr sloam_ = nullptr; + + bool publishTf_; + ros::NodeHandle nh_; + + // robotID + int hostRobotID_; + + bool turn_off_intra_loop_closure_; + bool turn_off_rel_inter_robot_factor; +}; \ No newline at end of file diff --git a/backend/sloam/include/core/robot.h b/backend/sloam/include/core/robot.h index 102daaa..296af7b 100755 --- a/backend/sloam/include/core/robot.h +++ b/backend/sloam/include/core/robot.h @@ -59,11 +59,8 @@ class Robot { ros::NodeHandle nh_; public: - bool robotOdomUpdated_ = false; - bool robotObservationUpdated_ = false; StampedSE3 robotLatestOdom_; SE3 robotLastSLOAMKeyPose_ = SE3(); - bool robotFirstOdom_; size_t robotOdomCounter_; bool turn_off_rel_inter_robot_loop_closure; @@ -81,8 +78,8 @@ class Robot { // Used by inputNode.cpp to track if sloam has already updated a previous odometry bool robotOdomReceived_ = false; - // How many seconds max to wait for semantic measurements to arrive - double semantic_meas_delay_tolerance_ = 3.0; + // Time to wait for all incoming messages to be recieved before processing + double msg_delay_tolerance = 3.0; // seconds // Publishers ros::Publisher pubRobotHighFreqSLOAMPose_; @@ -106,4 +103,7 @@ class Robot { // Helper methods for converting messages to objects std::vector rosEllipsoid2EllipObj(const std::vector& msgs); std::vector rosCylinder2CylinderObj(const std::vector& rosCylinders); + + // Logging methods + void PrintLateMsgWarning(const std::string &msg_type); }; diff --git a/backend/sloam/src/core/cylinderMapManager.cpp b/backend/sloam/src/core/cylinderMapManager.cpp index eb9f9b7..0cb0cb9 100755 --- a/backend/sloam/src/core/cylinderMapManager.cpp +++ b/backend/sloam/src/core/cylinderMapManager.cpp @@ -106,6 +106,11 @@ SE3 CylinderMapManager::getPose(const size_t idx, const int robotID) { return robotKeyFrames_[robotID].poses[idx]; } +/* + * @param inputPose: Input pose for checking the candidate loop closure region + * @param at_least_num_of_poses_old: At least how many poses away should be + * regarded as "revisiting" instead of "consecutive" poses + */ bool CylinderMapManager::InLoopClosureRegion( const double &max_dist_xy, const double &max_dist_z, const SE3 &inputPose, const int robotID, const size_t &at_least_num_of_poses_old) { diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp new file mode 100644 index 0000000..60bbd9c --- /dev/null +++ b/backend/sloam/src/core/input.cpp @@ -0,0 +1,95 @@ +#include + +/* + * @brief This measurement handles logic for picking which measurement to add + * to the factor graph next. It ensures they are added in order of timestamp + * (assuming that each queue is ordered by timestamp). This prevents jumping + * back and forth through time in the factor graph, which breaks covariances. + * + * @param robotOdomQueue_: The deque of odometry measurements. + * @param robotObservationQueue_: The queue of observation measurements. + * @param robotRelativeMeasQueue_: The deque of relative measurements. + * @param current_time: The current time of the robot. + * @param msg_delay_tolerance: The amount of time to wait before adding + * a measurement. + * @param minOdomDistance_: The minimum distance of estimated odometry + * movement to add a new odometry measurement. + * @param meas_to_add: Value will represent which measurement to add. + * 0: None + * 1: Odometry + * 2: Observation + * 3: Relative Measurement + */ +void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, + std::queue &robotObservationQueue_, + std::deque &robotRelativeMeasQueue_, + StampedSE3 &robotLatestOdom_, + ros::Time current_time, + double msg_delay_tolerance, + float minOdomDistance_, + int &meas_to_add) { + + // Create variables to hold statuses and values of valid measurements + bool validObs = false; + bool validRelMeas = false; + Observation obs; + RelativeMeas relMeas; + + // Get the oldest observation if it exists + if (!robotObservationQueue_.empty()) { + obs = robotObservationQueue_.front(); + validObs = (current_time - obs.stampedPose.stamp).toSec() > msg_delay_tolerance; + } else { + validObs = false; + } + + // Get the oldest relative measurement if it exists + if (!robotRelativeMeasQueue_.empty()) { + relMeas = robotRelativeMeasQueue_.front(); + validRelMeas = (current_time - relMeas.stamp).toSec() > msg_delay_tolerance; + } else { + validRelMeas = false; + } + + // If they are both valid, add the oldest one + if (validObs && validRelMeas) { + if (obs.stampedPose.stamp < relMeas.stamp) { meas_to_add = 2; } + else { meas_to_add = 3; } + return; + } + + // If only one is valid, add that + else if (validObs || validRelMeas) { + if (validObs) { meas_to_add = 2; } + else if (validRelMeas) { meas_to_add = 3; } + return; + } + + // If neither are valid, check if we have a valid odometry measurement + for (int i = robotOdomQueue_.size() - 1; i >= 0; i--) { + + // Check if this odometry measurement is valid yet. Otherwise, move to next one + if ((current_time - robotOdomQueue_[i].stamp).toSec() > msg_delay_tolerance) { + + // If it is, check if we have moved enough to add a new odometry measurement + SE3 currRelativeMotion = robotLatestOdom_.pose.inverse() * robotOdomQueue_[i].pose; + double accumMovement = currRelativeMotion.translation().norm(); + if (accumMovement > minOdomDistance_) { + + // Add the odometry measurement + meas_to_add = 1; + + // Pop all previous odometry measurements, as we won't add them + for (int j = 0; j < i; j++) { + robotOdomQueue_.pop_front(); + } + return; + } + break; + } + } + + // If we got this far, then no odometry measurements are valid & moved enough + meas_to_add = 0; + return; +} \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 2f95409..71d0db7 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -7,96 +7,7 @@ * */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "sloam_msgs/ROSRangeBearingSyncOdom.h" -#include "sloam_msgs/ROSSyncOdom.h" - -class InputManager { - public: - explicit InputManager(ros::NodeHandle nh); - void RunInputNode(const ros::TimerEvent &e); - void saveRuntimeCommUsage(); - Robot robot; - - private: - void resetAllFlags(); - float runInputNodeRate_; - ros::Timer timer_; - - void updateLastPose(const StampedSE3 &odom, const int &robotID); - - double max_timestamp_offset_ = 0.01; - - bool callSLOAM(SE3 relativeRawOdomMotion, ros::Time stamp, - std::deque &odomQueue, const int &robotID); - void PublishAccumOdom_(const SE3 &relativeRawOdomMotion); - void Odom2SlamTf(); - void PublishOdomAsTf(const nav_msgs::Odometry &odom_msg, - const std::string &parent_frame_id, - const std::string &child_frame_id); - - SE3 computeSloamToVioOdomTransform(const SE3 &sloam_odom, - const SE3 &vio_odom); - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - tf2_ros::TransformBroadcaster broadcaster_; - - // params - std::string map_frame_id_; - std::string odom_frame_id_; - std::string robot_ns_prefix_; - int number_of_robots_; - std::string odom_topic_; - std::string robot_frame_id_; - float minOdomDistance_; - float minSLOAMAltitude_; - - // vars - boost::shared_ptr sloam_ = nullptr; - - bool publishTf_; - ros::NodeHandle nh_; - - // robotID - int hostRobotID_; - - bool turn_off_intra_loop_closure_; - bool turn_off_rel_inter_robot_factor; -}; +#include InputManager::InputManager(ros::NodeHandle nh) : nh_(nh), tf_listener_{tf_buffer_}, robot(nh) { @@ -123,13 +34,17 @@ InputManager::InputManager(ros::NodeHandle nh) auto sloam_ptr = boost::make_shared(nh_); sloam_ = std::move(sloam_ptr); + + // Print information about parameters + if (turn_off_intra_loop_closure_) { + ROS_INFO_ONCE("Intra Loop closure is turned off, the default of the variable is false"); + } } void InputManager::RunInputNode(const ros::TimerEvent &e) { if (robot.robotOdomQueue_.size() == 0) { ROS_INFO_STREAM_THROTTLE(1, "Odom queue is not filled yet for robot " - << robot.robotId_ - << ", waiting for odometry..."); + << robot.robotId_ << ", waiting for odometry..."); return; } @@ -158,199 +73,101 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { // navigation stack sloam_msgs::ROSSyncOdom syncOdom; syncOdom.header.stamp = odom_stamp; - auto odom_msg = - sloam::toRosOdom_(highFreqSLOAMPose, map_frame_id_, odom_stamp); - syncOdom.vio_odom = - sloam::toRosOdom_(cur_vio_odom.pose, map_frame_id_, odom_stamp); + auto odom_msg = sloam::toRosOdom_(highFreqSLOAMPose, map_frame_id_, odom_stamp); + syncOdom.vio_odom = sloam::toRosOdom_(cur_vio_odom.pose, map_frame_id_, odom_stamp); syncOdom.sloam_odom = odom_msg; - // calculate this for drift compensation - SE3 sloam_to_vio_tf = - computeSloamToVioOdomTransform(highFreqSLOAMPose, cur_vio_odom.pose); + + // Calculate this for drift compensation + SE3 sloam_to_vio_tf = computeSloamToVioOdomTransform(highFreqSLOAMPose, cur_vio_odom.pose); + // publish sloam_to_vio_tf as a odometry message - auto sloam_to_vio_msg = - sloam::toRosOdom_(sloam_to_vio_tf, map_frame_id_, odom_stamp); + auto sloam_to_vio_msg = sloam::toRosOdom_(sloam_to_vio_tf, map_frame_id_, odom_stamp); robot.pubSloamToVioOdom_.publish(sloam_to_vio_msg); - - robot.pubRobotHighFreqSLOAMPose_.publish( - sloam::makeROSPose(highFreqSLOAMPose, map_frame_id_, odom_stamp)); - + robot.pubRobotHighFreqSLOAMPose_.publish(sloam::makeROSPose(highFreqSLOAMPose, map_frame_id_, odom_stamp)); robot.pubRobotHighFreqSLOAMOdom_.publish(odom_msg); robot.pubRobotHighFreqSyncOdom_.publish(syncOdom); - // Adding Factors for Relative Inter-Robot Measurements - if(!turn_off_rel_inter_robot_factor) { - // For each received measurement - for (int i = 0; i < robot.robotRelativeMeasQueue_.size(); i++) { + // Find out if we have any measurements to add + int meas_to_add; + PickNextMeasurementToAdd(robot.robotOdomQueue_, robot.robotObservationQueue_, robot.robotRelativeMeasQueue_, + robot.robotLatestOdom_, ros::Time::now(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); + + // Keep adding measurements as long as we have valid ones to add + while(meas_to_add != 0) { + // Holds measurement information to add to factor graph + Observation latestObservation; + + // Extract measurement information (depends on measurement type) + switch(meas_to_add) { + case 0: { // No measurements to add + break; + } + case 1: { // Odometry Measurement + latestObservation = Observation(); + latestObservation.stampedPose = robot.robotOdomQueue_.front(); + robot.robotOdomQueue_.pop_front(); + break; + } + case 2: { // Object Observation Measurement + + // Set variables for InLoopClosureRegion + double max_dist_xy = 10; + double max_dist_z = 2; // 0.01; // approximately = 0.5 * floor height + size_t at_least_num_of_poses_old = 30; + SE3 inputPose = robot.robotObservationQueue_.front().stampedPose.pose; + + // Check if we have the potential to establish a loop closure + // If yes, skip adding this factor, otherwise, add it + if (!turn_off_intra_loop_closure_ && + sloam_->semanticMap_.InLoopClosureRegion(max_dist_xy, max_dist_z, + inputPose, hostRobotID_, at_least_num_of_poses_old)) { + sloam_->isInLoopClosureRegion_ = true; + } else { + sloam_->isInLoopClosureRegion_ = false; + } - // If measurement is at least robot.semantic_meas_delay_tolerance_ seconds old - if ((robot.robotRelativeMeasQueue_.back().stamp - robot.robotRelativeMeasQueue_[i].stamp).toSec() - > robot.semantic_meas_delay_tolerance_) { + // Pass our observation (with synched odometry) to be added + latestObservation = robot.robotObservationQueue_.front(); + robot.robotObservationQueue_.pop(); + break; + } + case 3: { // Relative Inter-Robot Measurement - // Create the stamped pose - RelativeMeas relativeMeas = robot.robotRelativeMeasQueue_[i]; + // Extract synced odometry from relative measurement + RelativeMeas relativeMeas = robot.robotRelativeMeasQueue_.front(); StampedSE3 validStampedPose; validStampedPose.pose = relativeMeas.odomPose; validStampedPose.stamp = relativeMeas.stamp; - - // Create the observation (with only odometry, no objects) - Observation latestObservation = Observation(); latestObservation.stampedPose = validStampedPose; - // Use odom to estimate motion since last key frame - StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; - SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; - - // Get the Previous Key Pose - SE3 prevKeyPose; - if (robot.robotKeyPoses_.size() > 0) { - prevKeyPose = robot.robotKeyPoses_[robot.robotKeyPoses_.size() - 1]; - } else { - ROS_WARN("No previous key pose. Use identity as the previous key pose."); - prevKeyPose = SE3(); - } - - // Send the relative measurement synched odometry to SLOAM - SE3 keyPose; - bool success = sloam_->runSLOAMNode( - relativeRawOdomMotion, prevKeyPose, latestObservation.cylinders, - latestObservation.cubes, latestObservation.ellipsoids, - latestObservation.stampedPose.stamp, keyPose, hostRobotID_); - if (success) { - robot.robotKeyPoses_.push_back(keyPose); - updateLastPose(raw_vio_odom_used_for_sloam, hostRobotID_); - } - - // If this is a relative measurement we observed + // If we observed another robot (rather than being observed) if (!relativeMeas.onlyUseOdom) { - // Extract Host Robot Data class - sloam_->dbMutex.lock(); - robotData data = sloam_->dbManager.getHostRobotData(); - - // Pass the relative inter-robot measurement to the database manager - // and factor generation thread - data.relativeMeasPacket.push_back(relativeMeas); - sloam_->addRelativeMeasurement(relativeMeas); - sloam_->dbMutex.unlock(); - } + sloam_->dbMutex.lock(); - // Remove this value from the queue, as it's been used - robot.robotRelativeMeasQueue_.erase(robot.robotRelativeMeasQueue_.begin() + i); - i--; + // Pass the relative meas to database manager & factor generation thread + robotData data = sloam_->dbManager.getHostRobotData(); + data.relativeMeasPacket.push_back(relativeMeas); + sloam_->addRelativeMeasurement(relativeMeas); - } - // Remaining measurements are not old enough to add, so finish - else { break; } - } - } + sloam_->dbMutex.unlock(); + } - // ADDING OTHER FACTORS - // add odom factor only if the robot has moved enough - bool add_odom_factor = false; - bool valid_pose_found = false; - StampedSE3 validStampedPose; - if (robot.robotOdomUpdated_) { - // search in the odom queue, find the first pose that is atleast - // robot.semantic_meas_delay_tolerance_ seconds old because we want to avoid - // adding pose nodes back and forth due to semantic measurements delay - // iterate from the back of the queue to the front - for (int i = robot.robotOdomQueue_.size() - 1; i >= 0; i--) { - if ((robot.robotOdomQueue_.back().stamp - robot.robotOdomQueue_[i].stamp) - .toSec() > robot.semantic_meas_delay_tolerance_) { - // found the first pose that is atleast - // robot.semantic_meas_delay_tolerance_ seconds old add odom factor - // record this stamped pose - validStampedPose = robot.robotOdomQueue_[i]; - // ROS_INFO_STREAM( - // "Found the first pose that is atleast " - // "robot.semantic_meas_delay_tolerance_ seconds old"); - valid_pose_found = true; + // Pop the relative measurement from the queue + robot.robotRelativeMeasQueue_.pop_front(); break; } - } - - if (valid_pose_found) { - SE3 currRelativeMotion; - // Use odom to estimate motion since last key frame - currRelativeMotion = - robot.robotLatestOdom_.pose.inverse() * validStampedPose.pose; - double accumMovement = currRelativeMotion.translation().norm(); - // ROS_INFO_STREAM("current odom distance is " << accumMovement); - bool moved_enough = accumMovement > minOdomDistance_; - if (moved_enough) { - add_odom_factor = true; - // ROS_INFO_STREAM("Moved enough. Accumulated movement is " - // << accumMovement); - } else { - // ROS_WARN_STREAM("Not moved enough. Accumulated movement is " - // << accumMovement); - } - } - } - - if (add_odom_factor || robot.robotObservationUpdated_) { - // add both odometry and object factor - Observation latestObservation; - if (robot.robotObservationUpdated_) { - // max distance to check for loop closure - // TODO(xu): make this ROS param - double max_dist_xy = 10; - double max_dist_z = 2; // 0.01; // approximately = 0.5 * floor height - // at least how many poses away should be regarded as "revisiting" instead - // of "consecutive" poses - size_t at_least_num_of_poses_old = 30; - // input pose for checking the candidate loop closure region - SE3 inputPose = robot.robotObservationQueue_.back().stampedPose.pose; - if (turn_off_intra_loop_closure_) { - ROS_INFO_THROTTLE(5.0, - "Intra Loop closure is turned off, the default of the variable is " - "false"); - } - - // check if we have the potential to establish a loop closure, if yes, - // skip adding this factor, otherwise, add it - if (sloam_->semanticMap_.InLoopClosureRegion(max_dist_xy, max_dist_z, - inputPose, hostRobotID_, - at_least_num_of_poses_old) && - !turn_off_intra_loop_closure_) { - // still takes the latest observation as the input, because loop closure - // node needs this however set the flag isInLoopClosureRegion_ to true - // to avoid adding landmarks to the map! - latestObservation = robot.robotObservationQueue_.back(); - // reset the flags - robot.robotObservationUpdated_ = false; - robot.robotOdomUpdated_ = false; - sloam_->isInLoopClosureRegion_ = true; - // ROS_WARN_STREAM( - // "Robot inside candidate loop closure region, skipping the semantic " - // "observation addition..."); - } else { - // ROS_INFO_STREAM_THROTTLE(3.0, - // "Robot outside candidate loop closure region, adding the semantic " - // "observation..."); - sloam_->isInLoopClosureRegion_ = false; - // add factors - latestObservation = robot.robotObservationQueue_.back(); - // reset the flags - robot.robotObservationUpdated_ = false; - robot.robotOdomUpdated_ = false; + default: { + ROS_FATAL_STREAM("Invalid measurement # returned from PickNextOb: " << meas_to_add); + ros::shutdown(); + break; } - } else { - // reset the flag - robot.robotOdomUpdated_ = false; - latestObservation = Observation(); - // assemble observation with the valid pose - latestObservation.stampedPose = validStampedPose; } - SE3 keyPose; - SE3 relativeRawOdomMotion; - // Use odom to estimate motion since last key frame + // Use odometry (individual measurement or synced) to estimate motion since last key frame StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; - // VERY IMPORTANT: use latestObservation pose instead of odomQueue.back() - // pose since we want the odom that is synced with the observation - relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * - raw_vio_odom_used_for_sloam.pose; + SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; + // Get the Previous Key Pose SE3 prevKeyPose; if (robot.robotKeyPoses_.size() > 0) { prevKeyPose = robot.robotKeyPoses_[robot.robotKeyPoses_.size() - 1]; @@ -359,7 +176,8 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { prevKeyPose = SE3(); } - // timestamp is used for visualization + // Send the measurement to SLOAM + SE3 keyPose; bool success = sloam_->runSLOAMNode( relativeRawOdomMotion, prevKeyPose, latestObservation.cylinders, latestObservation.cubes, latestObservation.ellipsoids, @@ -368,10 +186,13 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { robot.robotKeyPoses_.push_back(keyPose); updateLastPose(raw_vio_odom_used_for_sloam, hostRobotID_); } - } else { - ROS_INFO_STREAM_THROTTLE(3.0, "Neither the odometry nor the observation is updated for robot " << hostRobotID_); + + // Check to see if we have another measurement to add + PickNextMeasurementToAdd(robot.robotOdomQueue_, robot.robotObservationQueue_, robot.robotRelativeMeasQueue_, + robot.robotLatestOdom_, ros::Time::now(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); } + // If desired, save runtime & communication usage info if (sloam_->save_runtime_analysis) { saveRuntimeCommUsage(); } @@ -412,8 +233,10 @@ void InputManager::PublishOdomAsTf(const nav_msgs::Odometry &odom_msg, broadcaster_.sendTransform(tf); } +/* + * @brief Save the runtime & communication usage info to a txt file + */ void InputManager::saveRuntimeCommUsage() { - // save the runtime communication usage variables to a txt file ROS_DEBUG("Saving runtime communication usage to a txt file 1..."); std::ofstream file(sloam_->runtime_analysis_file, std::ios::out | std::ios::trunc); diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 8bdfde0..1a4e3be 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -22,7 +22,6 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { nh_.param(idName, robotId_, 0); // Initialize variables - robotFirstOdom_ = true; robotOdomCounter_ = 0; // Initialize publishers @@ -53,7 +52,16 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { ROS_INFO_STREAM("Robot Initialized! Robot # " << robotId_); } +/* + * @brief This method is called when a new odometry message is received. + * It extracts the odometry and adds it to a queue. It filters out + * messages at the rate of odomFreqFilter_, and skips a message if the + * robot's z position is below minSLOAMAltitude_. + * + * @param odom_msg: The odometry message received. + */ void Robot::RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg) { + // Only take 1 out of every odomFreqFilter_ odometry messages if (odomFreqFilter_ > 1) { robotOdomCounter_++; if (robotOdomCounter_ % odomFreqFilter_ != 0) @@ -61,64 +69,43 @@ void Robot::RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg) { robotOdomCounter_ = 0; } + // Check to make sure that this message is not too old + if ((ros::Time::now() - odom_msg->header.stamp).toSec() > msg_delay_tolerance) { + PrintLateMsgWarning("Odometry"); + return; + } + + // Extract info from the message auto pose = odom_msg->pose.pose; ros::Time odomStamp = odom_msg->header.stamp; Quat rot(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); Vector3 pos(pose.position.x, pose.position.y, pose.position.z); + // Construct odom as SE3 object SE3 odom = SE3(); odom.setQuaternion(rot); odom.translation() = pos; - - if (robotFirstOdom_ && pose.position.z < minSLOAMAltitude_) { - ROS_INFO_STREAM_THROTTLE( - 5, "Robot is too low, will not call sloam, height threshold is " - << minSLOAMAltitude_); + // Skip adding odometry if the robot is too low + if (pose.position.z < minSLOAMAltitude_) { + ROS_WARN_STREAM_THROTTLE(5, "Robot is too low, will not call sloam, height threshold is " << minSLOAMAltitude_); return; } - // if either robotObservationQueue_.empty() or robotOdomQueue_.empty(), we need to add odom factor - bool is_first_run = false; - if (robotObservationQueue_.empty() || robotOdomQueue_.empty()) { - ROS_INFO_STREAM_THROTTLE(1.0, "Either robotObservationQueue_ or robotOdomQueue_ is empty, will add odom factor"); - is_first_run = true; - } - + // Add odometry to the queue, and pop the oldest one if the queue is too long robotOdomQueue_.emplace_back(odom, odomStamp); - if (robotOdomQueue_.size() > 10 * maxQueueSize_) - robotOdomQueue_.pop_front(); - - if(is_first_run){ - robotOdomUpdated_ = true; - return; - } else { - // we set robotOdomUpdated_ to true only if the latest semantic measurements can be discard (i.e. its timestamp is at least semantic_meas_delay_tolerance_ seconds earlier than the latest odometry stamp) - auto latest_observation_stamp = robotObservationQueue_.back().stampedPose.stamp; - if ((odomStamp - latest_observation_stamp).toSec() > semantic_meas_delay_tolerance_) { - ROS_INFO_STREAM_THROTTLE(5.0, "Odometry delay is enabled, and semantic observation is old enough, setting robotOdomUpdated_ to true"); - robotOdomUpdated_ = true; - } else { - robotOdomUpdated_ = false; - } - } + if (robotOdomQueue_.size() > 10 * maxQueueSize_) robotOdomQueue_.pop_front(); } -void Robot::RobotObservationCb( - const sloam_msgs::SemanticMeasSyncOdom &observation_msg) { - // ROS_INFO_STREAM("RobotObservationCb!"); - // compared the stamp of the observation with the latest odometry stamp, if too old, discard this observation msg - if (robotOdomQueue_.empty()) { - ROS_WARN_STREAM("RobotOdomQueue is empty, cannot compare stamp"); - return; - } - auto latest_odom_stamp = robotOdomQueue_.back().stamp; - if ((latest_odom_stamp - observation_msg.header.stamp).toSec() > semantic_meas_delay_tolerance_) { - ROS_WARN_STREAM("Semantic observation arrived too late, discard this observation"); - ROS_WARN_STREAM("Semantic observation arrived " << (latest_odom_stamp - observation_msg.header.stamp).toSec() << " seconds late, tolerance is " << semantic_meas_delay_tolerance_); +void Robot::RobotObservationCb(const sloam_msgs::SemanticMeasSyncOdom &observation_msg) { + // Check to make sure that this message is not too old + if ((ros::Time::now() - observation_msg.header.stamp).toSec() > msg_delay_tolerance) { + PrintLateMsgWarning("Observation"); return; } + + // Save the observation Observation cur_observation; cur_observation.stampedPose.pose = databaseManager::toSE3Pose(observation_msg.odometry.pose.pose); @@ -149,20 +136,21 @@ void Robot::RobotObservationCb( cur_observation.ellipsoids = rosEllipsoid2EllipObj(observation_msg.ellipsoid_factors); robotObservationQueue_.push(cur_observation); - robotObservationUpdated_ = true; } /** - * @brief This method converts a - * sloam_msgs::RelativeInterRobotMeasurement into - * a RelativeMeas struct and adds it to the queue. - * inputNode will pass this onto the databaseManager - * in sloamNode. + * @brief This method converts a sloam_msgs::RelativeInterRobotMeasurement + * into a RelativeMeas struct and adds it to the queue. * - * @param sloam_msgs::RelativeInterRobotMeasurementOdom - * &relativeMeas_msg + * @param relativeMeas_msg: The relative measurement message received. */ void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurementOdom &relativeMeas_msg) { + // Check to make sure that this message is not too old + if ((ros::Time::now() - relativeMeas_msg.header.stamp).toSec() > msg_delay_tolerance) { + PrintLateMsgWarning("Relative Measurement"); + return; + } + // If this relative measurement involves our robot in some way, add it if(relativeMeas_msg.robotIdObserver == robotId_ || relativeMeas_msg.robotIdObserved == robotId_) { RelativeMeas cur_measurement; @@ -218,4 +206,9 @@ std::vector Robot::rosEllipsoid2EllipObj( } return ellipsoids; +} + +void Robot::PrintLateMsgWarning(const std::string &msg_type) { + ROS_WARN_STREAM_THROTTLE(10, msg_type << "message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); + ROS_WARN_STREAM_THROTTLE(10, "Please increase `msg_delay_tolerance` to account for lag in your system"); } \ No newline at end of file From b43db5b00a72fff2881570d731f225b108d936b1 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Mon, 17 Mar 2025 18:17:40 +0000 Subject: [PATCH 19/74] Test cases for PickNextMeasurementToAdd --- README.md | 12 ++ backend/sloam/CMakeLists.txt | 32 +---- backend/sloam/include/core/input.h | 2 +- backend/sloam/src/core/input.cpp | 10 +- backend/sloam/src/core/inputNode.cpp | 4 +- .../{tests => test}/deprecated/core_test.cpp | 0 .../deprecated/cube_factor_test.cpp | 0 .../deprecated/cylinder_factor_test.cpp | 0 .../deprecated/cylinder_test.cpp | 0 .../deprecated/loop_closure_test.cpp | 0 .../{tests => test}/deprecated/plane_test.cpp | 0 backend/sloam/src/test/input_test.cpp | 133 ++++++++++++++++++ .../place_recognition_test.cpp | 0 .../src/tests/inter_robot_factor_test.cpp | 9 -- 14 files changed, 158 insertions(+), 44 deletions(-) rename backend/sloam/src/{tests => test}/deprecated/core_test.cpp (100%) rename backend/sloam/src/{tests => test}/deprecated/cube_factor_test.cpp (100%) rename backend/sloam/src/{tests => test}/deprecated/cylinder_factor_test.cpp (100%) rename backend/sloam/src/{tests => test}/deprecated/cylinder_test.cpp (100%) rename backend/sloam/src/{tests => test}/deprecated/loop_closure_test.cpp (100%) rename backend/sloam/src/{tests => test}/deprecated/plane_test.cpp (100%) create mode 100644 backend/sloam/src/test/input_test.cpp rename backend/sloam/src/{tests => test}/place_recognition_test.cpp (100%) delete mode 100644 backend/sloam/src/tests/inter_robot_factor_test.cpp diff --git a/README.md b/README.md index cd1cc53..adc9503 100755 --- a/README.md +++ b/README.md @@ -101,6 +101,7 @@ docker rm slideslam_ros ``` before you run the docker image again. + **Troubleshoot**: - If you do not see your code or bags inside docker, double check `run_slide_slam_docker.sh` file to make sure you have your workspace and BAG folders mapped properly. @@ -372,6 +373,17 @@ Finally, execute this script If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions. +# Run Test Cases + +Use the following commands to run the test cases for this repository: + +``` +catkin build -DCMAKE_BUILD_TYPE=Debug --catkin-make-args tests +catkin run_tests +``` + +Make sure to rebuild with `-DCMAKE_BUILD_TYPE=Release` when you want to run the algorithm as normal. + # Troubleshoot **Rate of segmentation:** - When running on your own data, we recommend to throttle the segmentation topic (segmented point cloud or images) rate to 2-4 Hz to avoid computation delay in the front end, especially if you’re experiencing performance issues at higher rates. Please also update the `expected_segmentation_frequency` parameter in the corresponding `process_cloud_node_*_params.yaml` file as well as the `desired_frequency` in the `infer_node_params.yaml` to the actual rate of the topic. diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index ecf60c6..2a14765 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -117,34 +117,12 @@ cc_binary( # DEPS sloam_log sloam_base sloam_objects sloam_core sloam_viz sloam_segmentation sloam_factorgraph gtsam ${catkin_LIBRARIES} -ldw #sloam_loop ) +# Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter) cc_binary( NAME place_recognition_test - SRCS "src/tests/place_recognition_test.cpp" + SRCS "src/test/place_recognition_test.cpp" DEPS sloam_base sloam_core) - - -if(BUILD_TESTING) - enable_testing() - - cc_test( - NAME inter_robot_factor_test - SRCS "src/tests/inter_robot_factor_test.cpp" - DEPS sloam_base sloam_objects sloam_core GTest::GTest) - -# These test cases are not up to date, and thus won't work on current code. -# -# Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter) -# cc_test( - # NAME all_sloam_test - # SRCS "src/tests/run_all_test.cpp" "src/tests/plane_test.cpp" "src/tests/cylinder_test.cpp" "src/tests/core_test.cpp" - # DEPS sloam_base sloam_objects sloam_core GTest::GTest) -# cc_test( - # NAME fg_test - # SRCS "src/tests/cylinder_factor_test.cpp" - # DEPS sloam_base sloam_factorgraph GTest::GTest) -# cc_test( - # NAME cube_fg_test - # SRCS "src/tests/cube_factor_test.cpp" - # DEPS sloam_base sloam_factorgraph GTest::GTest) -endif() \ No newline at end of file +# Add Unit Tests +catkin_add_gtest(input_test "src/test/input_test.cpp") +target_link_libraries(input_test sloam_core ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/backend/sloam/include/core/input.h b/backend/sloam/include/core/input.h index 4ce7434..0f4f2c4 100644 --- a/backend/sloam/include/core/input.h +++ b/backend/sloam/include/core/input.h @@ -7,7 +7,7 @@ class Input { std::queue &robotObservationQueue_, std::deque &robotRelativeMeasQueue_, StampedSE3 &robotLatestOdom_, - ros::Time current_time, + double current_time, double msg_delay_tolerance, float minOdomDistance_, int &meas_to_add); diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp index 60bbd9c..94a1560 100644 --- a/backend/sloam/src/core/input.cpp +++ b/backend/sloam/src/core/input.cpp @@ -9,7 +9,7 @@ * @param robotOdomQueue_: The deque of odometry measurements. * @param robotObservationQueue_: The queue of observation measurements. * @param robotRelativeMeasQueue_: The deque of relative measurements. - * @param current_time: The current time of the robot. + * @param current_time: The current time of the robot (in seconds) * @param msg_delay_tolerance: The amount of time to wait before adding * a measurement. * @param minOdomDistance_: The minimum distance of estimated odometry @@ -24,7 +24,7 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, std::queue &robotObservationQueue_, std::deque &robotRelativeMeasQueue_, StampedSE3 &robotLatestOdom_, - ros::Time current_time, + double current_time, double msg_delay_tolerance, float minOdomDistance_, int &meas_to_add) { @@ -38,7 +38,7 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, // Get the oldest observation if it exists if (!robotObservationQueue_.empty()) { obs = robotObservationQueue_.front(); - validObs = (current_time - obs.stampedPose.stamp).toSec() > msg_delay_tolerance; + validObs = (current_time - obs.stampedPose.stamp.toSec()) >= msg_delay_tolerance; } else { validObs = false; } @@ -46,7 +46,7 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, // Get the oldest relative measurement if it exists if (!robotRelativeMeasQueue_.empty()) { relMeas = robotRelativeMeasQueue_.front(); - validRelMeas = (current_time - relMeas.stamp).toSec() > msg_delay_tolerance; + validRelMeas = (current_time - relMeas.stamp.toSec()) >= msg_delay_tolerance; } else { validRelMeas = false; } @@ -69,7 +69,7 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, for (int i = robotOdomQueue_.size() - 1; i >= 0; i--) { // Check if this odometry measurement is valid yet. Otherwise, move to next one - if ((current_time - robotOdomQueue_[i].stamp).toSec() > msg_delay_tolerance) { + if ((current_time - robotOdomQueue_[i].stamp.toSec()) >= msg_delay_tolerance) { // If it is, check if we have moved enough to add a new odometry measurement SE3 currRelativeMotion = robotLatestOdom_.pose.inverse() * robotOdomQueue_[i].pose; diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 71d0db7..7f8451a 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -90,7 +90,7 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { // Find out if we have any measurements to add int meas_to_add; PickNextMeasurementToAdd(robot.robotOdomQueue_, robot.robotObservationQueue_, robot.robotRelativeMeasQueue_, - robot.robotLatestOdom_, ros::Time::now(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); + robot.robotLatestOdom_, ros::Time::now().toSec(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); // Keep adding measurements as long as we have valid ones to add while(meas_to_add != 0) { @@ -189,7 +189,7 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { // Check to see if we have another measurement to add PickNextMeasurementToAdd(robot.robotOdomQueue_, robot.robotObservationQueue_, robot.robotRelativeMeasQueue_, - robot.robotLatestOdom_, ros::Time::now(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); + robot.robotLatestOdom_, ros::Time::now().toSec(), robot.msg_delay_tolerance, minOdomDistance_, meas_to_add); } // If desired, save runtime & communication usage info diff --git a/backend/sloam/src/tests/deprecated/core_test.cpp b/backend/sloam/src/test/deprecated/core_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/core_test.cpp rename to backend/sloam/src/test/deprecated/core_test.cpp diff --git a/backend/sloam/src/tests/deprecated/cube_factor_test.cpp b/backend/sloam/src/test/deprecated/cube_factor_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/cube_factor_test.cpp rename to backend/sloam/src/test/deprecated/cube_factor_test.cpp diff --git a/backend/sloam/src/tests/deprecated/cylinder_factor_test.cpp b/backend/sloam/src/test/deprecated/cylinder_factor_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/cylinder_factor_test.cpp rename to backend/sloam/src/test/deprecated/cylinder_factor_test.cpp diff --git a/backend/sloam/src/tests/deprecated/cylinder_test.cpp b/backend/sloam/src/test/deprecated/cylinder_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/cylinder_test.cpp rename to backend/sloam/src/test/deprecated/cylinder_test.cpp diff --git a/backend/sloam/src/tests/deprecated/loop_closure_test.cpp b/backend/sloam/src/test/deprecated/loop_closure_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/loop_closure_test.cpp rename to backend/sloam/src/test/deprecated/loop_closure_test.cpp diff --git a/backend/sloam/src/tests/deprecated/plane_test.cpp b/backend/sloam/src/test/deprecated/plane_test.cpp similarity index 100% rename from backend/sloam/src/tests/deprecated/plane_test.cpp rename to backend/sloam/src/test/deprecated/plane_test.cpp diff --git a/backend/sloam/src/test/input_test.cpp b/backend/sloam/src/test/input_test.cpp new file mode 100644 index 0000000..bffda58 --- /dev/null +++ b/backend/sloam/src/test/input_test.cpp @@ -0,0 +1,133 @@ +#include +#include + +class InputTest : public ::testing::Test { +protected: + Input input; + + // Queues + std::deque robotOdomQueue_; + std::queue robotObservationQueue_; + std::deque robotRelativeMeasQueue_; + std::deque robotOdomQueue_1; + std::queue robotObservationQueue_1; + std::deque robotRelativeMeasQueue_1; + std::deque robotOdomQueue_10; + std::queue robotObservationQueue_10; + std::deque robotRelativeMeasQueue_10; + std::deque robotOdomQueue_Large; + + // StampedSE3s + StampedSE3 robotLatestOdom; + StampedSE3 identity_1; + StampedSE3 identity_10; + StampedSE3 oneMeter_1; + StampedSE3 oneMeter_10; + + InputTest() { + // Create various ROS times + ros::Time time_1(1.0); + ros::Time time_10(10.0); + ros::Time time_100(100.0); + + // Create StampedSE3s + identity_1 = StampedSE3(SE3(), time_1); + identity_10 = StampedSE3(SE3(), time_10); + SE3 oneMeter = SE3(); + oneMeter.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); + oneMeter_1 = StampedSE3(oneMeter, time_1); + oneMeter_10 = StampedSE3(oneMeter, time_10); + + // 1 Queues all have stamps of timestep 1 + robotOdomQueue_1.push_back(oneMeter_1); + + Observation obs = Observation(); + obs.stampedPose = identity_1; + robotObservationQueue_1.push(obs); + + RelativeMeas relMeas = RelativeMeas(); + relMeas.stamp = time_1; + robotRelativeMeasQueue_1.push_back(relMeas); + + // 10 Queues all have stamps of timestep 10 + robotOdomQueue_10.push_back(oneMeter_10); + + Observation obs_10 = Observation(); + obs_10.stampedPose = identity_10; + robotObservationQueue_10.push(obs_10); + + RelativeMeas relMeas_10 = RelativeMeas(); + relMeas_10.stamp = time_10; + robotRelativeMeasQueue_10.push_back(relMeas_10); + + // Add a large number of odometry measurements to the queue + for (int i = 0; i < 100; i++) { + robotOdomQueue_Large.push_back(StampedSE3(oneMeter, ros::Time(i))); + } + } +}; + +TEST_F(InputTest, PickNextMeasurementToAdd) { + // Should return 0 when all queues are empty + int meas_to_add = 4; + input.PickNextMeasurementToAdd(robotOdomQueue_, robotObservationQueue_, robotRelativeMeasQueue_, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 0); + + // Should return 1 when only odom queue has a valid measurement + input.PickNextMeasurementToAdd(robotOdomQueue_1, robotObservationQueue_, robotRelativeMeasQueue_, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 1); + + // Should return 2 when only observation queue has a valid measurement + input.PickNextMeasurementToAdd(robotOdomQueue_, robotObservationQueue_1, robotRelativeMeasQueue_, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 2); + + // Should return 3 when only relative measurement queue has a valid measurement + input.PickNextMeasurementToAdd(robotOdomQueue_, robotObservationQueue_, robotRelativeMeasQueue_1, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 3); + + // Should return queue with the earliest timestamp between Observation and RelativeMeas + input.PickNextMeasurementToAdd(robotOdomQueue_, robotObservationQueue_1, robotRelativeMeasQueue_10, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 2); + input.PickNextMeasurementToAdd(robotOdomQueue_, robotObservationQueue_10, robotRelativeMeasQueue_1, + robotLatestOdom, 1000.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 3); + + // Make sure no-longer-useful odometry measurements are popped from the queue + input.PickNextMeasurementToAdd(robotOdomQueue_Large, robotObservationQueue_, robotRelativeMeasQueue_, + robotLatestOdom, 76.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 1); + EXPECT_EQ(robotOdomQueue_Large.size(), 27); + EXPECT_EQ(robotOdomQueue_Large.front().stamp.toSec(), 73.0); + + // No measurement should be recommended if the odometry shows the robot hasn't moved enough + input.PickNextMeasurementToAdd(robotOdomQueue_Large, robotObservationQueue_, robotRelativeMeasQueue_, + robotLatestOdom, 76.0, 3.0, 1.5, meas_to_add); + EXPECT_EQ(meas_to_add, 0); + EXPECT_EQ(robotOdomQueue_Large.size(), 27); + EXPECT_EQ(robotOdomQueue_Large.front().stamp.toSec(), 73.0); + + // Test the msg_delay_tolerance argument + input.PickNextMeasurementToAdd(robotOdomQueue_1, robotObservationQueue_10, robotRelativeMeasQueue_10, + robotLatestOdom, 10.0, 8.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 1); + input.PickNextMeasurementToAdd(robotOdomQueue_1, robotObservationQueue_1, robotRelativeMeasQueue_10, + robotLatestOdom, 10.0, 8.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 2); + input.PickNextMeasurementToAdd(robotOdomQueue_1, robotObservationQueue_10, robotRelativeMeasQueue_1, + robotLatestOdom, 10.0, 8.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 3); +} + +int main(int argc, char **argv) { + // Run this to allow use of Ros::Time in tests + ros::Time::init(); + + // Initialize GoogleTest and run all tests + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/backend/sloam/src/tests/place_recognition_test.cpp b/backend/sloam/src/test/place_recognition_test.cpp similarity index 100% rename from backend/sloam/src/tests/place_recognition_test.cpp rename to backend/sloam/src/test/place_recognition_test.cpp diff --git a/backend/sloam/src/tests/inter_robot_factor_test.cpp b/backend/sloam/src/tests/inter_robot_factor_test.cpp deleted file mode 100644 index be2811b..0000000 --- a/backend/sloam/src/tests/inter_robot_factor_test.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -// No Tests defined currently - -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file From 8c5876ba0bb0bbb057c6f9b979ec36d86f9245ed Mon Sep 17 00:00:00 2001 From: Cam Jackson Date: Mon, 17 Mar 2025 14:44:58 -0400 Subject: [PATCH 20/74] Organizational changes for apriltag branch pull request 3-17-25 --- ..._run_container.sh => run_container_cam.sh} | 0 ..._container.sh => run_container_dbutter.sh} | 0 frontend/relative_meas_gen/CMakeLists.txt | 4 ++-- .../bash_scripts/tmux_relative_meas.sh | 4 +++- .../include/apriltag_meas_gen.h | 8 ++++---- .../{slidetag.h => apriltag_wrapper.h} | 10 +++++----- .../launch/CoPeD_relative_meas_gen.launch | 10 +++++----- .../relative_measurement_generator.launch | 20 +++++++++---------- .../src/apriltag_meas_gen.cpp | 10 +++++----- .../{slidetag.cpp => apriltag_wrapper.cpp} | 14 ++++++------- 10 files changed, 41 insertions(+), 39 deletions(-) rename docker/{cam_desktop_run_container.sh => run_container_cam.sh} (100%) rename docker/{run_container.sh => run_container_dbutter.sh} (100%) rename frontend/relative_meas_gen/include/{slidetag.h => apriltag_wrapper.h} (62%) rename frontend/relative_meas_gen/src/{slidetag.cpp => apriltag_wrapper.cpp} (82%) diff --git a/docker/cam_desktop_run_container.sh b/docker/run_container_cam.sh similarity index 100% rename from docker/cam_desktop_run_container.sh rename to docker/run_container_cam.sh diff --git a/docker/run_container.sh b/docker/run_container_dbutter.sh similarity index 100% rename from docker/run_container.sh rename to docker/run_container_dbutter.sh diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index b060ea3..52dfedb 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -144,7 +144,7 @@ include_directories( include ) -add_executable(ApriltagMeasurer src/apriltag_meas_gen.cpp src/slidetag.cpp) +add_executable(ApriltagMeasurer src/apriltag_meas_gen.cpp src/apriltag_wrapper.cpp) set_target_properties(ApriltagMeasurer PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES} yaml-cpp) @@ -224,7 +224,7 @@ target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apri if (CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roscpp rostest) - catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/slidetag.cpp) + catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/apriltag_wwapper.cpp) target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES}) endif() diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh index b91c464..d87540d 100755 --- a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -43,7 +43,9 @@ tmux split-window -h -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:1.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r 6.0 -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux select-pane -t $SESSION_NAME:1.2 +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch relative_meas_gen CoPeD_relative_meas_gen.launch" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 30ad4cc..6751714 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include #include @@ -13,8 +13,8 @@ class ApriltagMeasurer { public: explicit ApriltagMeasurer(ros::NodeHandle nh); private: - ros::Subscriber robot_images; - ros::Publisher relative_meas; + ros::Subscriber robot_images_sub; + ros::Publisher relative_meas_pub; std::string robot_ID; std::string camera_ID; float intrinsics[4]; @@ -25,7 +25,7 @@ class ApriltagMeasurer { void PublishRelativeMeasurement(int8_t, Eigen::Matrix4f transformation); Eigen::Matrix4d CalculateRelativeTransformation(Eigen::Matrix4d H_hostBot_to_cam, Eigen::Matrix4d H_cam_to_tag, Eigen::Matrix4d H_observedBot_to_tag); - std::tuple> LoadTransformations(slidetag tag); + std::tuple> LoadTransformations(apriltag_wrapper tag); ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/slidetag.h b/frontend/relative_meas_gen/include/apriltag_wrapper.h similarity index 62% rename from frontend/relative_meas_gen/include/slidetag.h rename to frontend/relative_meas_gen/include/apriltag_wrapper.h index 82b8ecf..b8e42af 100644 --- a/frontend/relative_meas_gen/include/slidetag.h +++ b/frontend/relative_meas_gen/include/apriltag_wrapper.h @@ -10,7 +10,7 @@ #include #include -class slidetag { +class apriltag_wrapper { public: int id; double center[2]; @@ -18,10 +18,10 @@ class slidetag { matd_t rotation; matd_t translation; - slidetag(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation); - slidetag(int id); - slidetag(); + apriltag_wrapper(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation); + apriltag_wrapper(int id); + apriltag_wrapper(); }; -std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4], float tagsize); +std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4], float tagsize); cv::Mat MatFromImage(const sensor_msgs::CompressedImage msg); diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch index f54f2b2..6a321e0 100644 --- a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -2,11 +2,11 @@ - - + + - - - + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch index 5a2957c..9ac20ec 100644 --- a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch @@ -1,22 +1,22 @@ - - + + - - - + + + - - - - + + + + - + \ No newline at end of file diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 1e4bfae..4a2808a 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -9,10 +9,10 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { return; } - std::vector tags = ExtractAprilTags(img, this->intrinsics, this->tagsize);\ + std::vector tags = ExtractAprilTags(img, this->intrinsics, this->tagsize);\ // Publish transformation for each detected apriltag - for (slidetag t : tags) { + for (apriltag_wrapper t : tags) { int8_t bot_id; Eigen::Matrix4f bot_to_cam; Eigen::Matrix4f cam_to_tag; @@ -44,7 +44,7 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { } -std::tuple> ApriltagMeasurer::LoadTransformations(slidetag t) { +std::tuple> ApriltagMeasurer::LoadTransformations(apriltag_wrapper t) { std::tuple> return_tuple; int8_t bot_id = -1; std::array transformations; @@ -145,8 +145,8 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { intrinsics[3] = cy; // Instantiate sub and pub - robot_images = nh_.subscribe(image_topic, 1, &ApriltagMeasurer::imageCallback, this); - relative_meas = nh_.advertise(return_topic, 10); + robot_images_sub = nh_.subscribe(image_topic, 1, &ApriltagMeasurer::imageCallback, this); + relative_meas_pub = nh_.advertise(return_topic, 10); } int main(int argc, char** argv) { diff --git a/frontend/relative_meas_gen/src/slidetag.cpp b/frontend/relative_meas_gen/src/apriltag_wrapper.cpp similarity index 82% rename from frontend/relative_meas_gen/src/slidetag.cpp rename to frontend/relative_meas_gen/src/apriltag_wrapper.cpp index a43949e..d698c46 100644 --- a/frontend/relative_meas_gen/src/slidetag.cpp +++ b/frontend/relative_meas_gen/src/apriltag_wrapper.cpp @@ -1,10 +1,10 @@ -#include "slidetag.h" +#include "apriltag_wrapper.h" -slidetag::slidetag(int id) : +apriltag_wrapper::apriltag_wrapper(int id) : id (id) {} -slidetag::slidetag(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation) : +apriltag_wrapper::apriltag_wrapper(int id, double center[2], double corners[4][2], matd_t *rotation, matd_t *translation) : id (id) { this->center[0] = center[0]; @@ -20,9 +20,9 @@ slidetag::slidetag(int id, double center[2], double corners[4][2], matd_t *rotat } } -slidetag::slidetag() {} +apriltag_wrapper::apriltag_wrapper() {} -std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float tagsize) { +std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float tagsize) { // Create usable image object from Mat image_u8_t img_header = { .width = img.cols, @@ -41,7 +41,7 @@ std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float t zarray_t *detections = apriltag_detector_detect(td, image); // Create tag object vector - std::vector tags; + std::vector tags; // Iterate through detected apriltags for (int i = 0; i < zarray_size(detections); i++) { @@ -72,7 +72,7 @@ std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float t } } - slidetag new_tag(id, center, corners, pose.R, pose.t); + apriltag_wrapper new_tag(id, center, corners, pose.R, pose.t); tags.push_back(new_tag); } From 532f7c134d2abaadb19e6de6270097ed86cb6a49 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Mon, 17 Mar 2025 22:08:10 +0000 Subject: [PATCH 21/74] Untested changes for custom covariances scaled by dist --- backend/sloam/include/factorgraph/graph.h | 24 ++-- .../sloam/include/factorgraph/graphWrapper.h | 5 +- backend/sloam/params/sloam_multiUGV.yaml | 7 ++ backend/sloam/src/core/sloamNode.cpp | 9 ++ backend/sloam/src/factorgraph/graph.cpp | 107 +++++++----------- .../sloam/src/factorgraph/graphWrapper.cpp | 26 ++++- frontend/relative_meas_gen/package.xml | 1 + .../scripts/multiUGVToSlideSLAM.py | 19 +++- .../src/relative_meas_gen/transforms.py | 41 ++++++- 9 files changed, 154 insertions(+), 85 deletions(-) diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index 3b2b758..8591da5 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -9,6 +9,7 @@ #pragma once #define MAX_NUM_ROBOTS 13 +#include #include #include #include @@ -112,26 +113,31 @@ class SemanticFactorGraph { std::vector point_landmark_labels_; void logEntropy(); - // these two noise models will be passed as parameters from graphWarpper - boost::shared_ptr noise_model_pose; Vector6 noise_model_pose_vec_per_m; - SharedNoiseModel noise_model_bearing; double noise_model_pose_inflation = 0.0; double start_timestamp = 0.0; - boost::shared_ptr noise_model_closure; - // TODO(xu:) unify the following two functions Symbol getSymbol(const int& robotID, const int idx); Symbol getSymbol(const int& robotID, const size_t idx); protected: - // Noise models + // Noise models (per unit of relative distance) + double noise_floor = 0.1; // Clipping noises prevents numerical issues boost::shared_ptr noise_model_prior_first_pose; - boost::shared_ptr noise_model_gps; + Vector6 noise_model_prior_first_pose_vec; + + Vector6 noise_model_odom_vec; + + boost::shared_ptr noise_model_closure; + + Vector9 noise_model_cube_vec; + boost::shared_ptr noise_model_cylinder; - boost::shared_ptr noise_model_cube; - boost::shared_ptr noise_model_rel_meas; + + SharedNoiseModel noise_model_bearing; + + Vector6 noise_model_rel_meas_vec; // Aux attributes Pose3 odomToGPS_; diff --git a/backend/sloam/include/factorgraph/graphWrapper.h b/backend/sloam/include/factorgraph/graphWrapper.h index bb9fa47..c2fdb99 100755 --- a/backend/sloam/include/factorgraph/graphWrapper.h +++ b/backend/sloam/include/factorgraph/graphWrapper.h @@ -23,7 +23,7 @@ class SemanticFactorGraphWrapper : public SemanticFactorGraph { public: // constructor - SemanticFactorGraphWrapper(int numRobots = 1); + SemanticFactorGraphWrapper(const ros::NodeHandle &nh, int numRobots = 1); /** brief: addLoopClosureObservation (only for active SLAM, currently not used * in slideSLAM) @@ -130,4 +130,7 @@ class SemanticFactorGraphWrapper : public SemanticFactorGraph { size_t point_landmark_counter_; std::vector pose_counter_robot_; std::vector robot_prev_pose_; + + // ROS NodeHandle for getting rosparams + ros::NodeHandle nh_; }; diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index 250101a..6d54855 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -55,3 +55,10 @@ sloam: sigma: 0.1 # CLIPPER params, default 0.01, spread / "variance" of exponential kernel epsilon: 0.3 # CLIPPER params, default 0.06, < bound on consistency score, determines if inlier/outlier, consistency matching threshold + # Parameters for Factor graph + factor_graph: + #VERY IMPORTANT: First three (Rotation) and last three (Translation) + noise_model_prior_first_pose_vec: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + noise_model_odom_vec: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + noise_model_cube_vec: [0.017, 0.017, 0.017, 0.5, 0.5, 0.5, 0.05, 0.05, 0.05] + noise_model_rel_meas_vec: [0.0017, 0.0017, 0.0017, 0.06, 0.06, 0.06] diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index 53eecbc..226910b 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -862,10 +862,19 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, cubeSemanticMapMtx_.lock(); ellipsoidSemanticMapMtx_.lock(); dbMutex.lock(); + + // Make sure that factors are created in timestamp order + ros::Time last_stamp = dbManager.getHostRobotData().poseMstPacket.back().stamp; + if (stamp < last_stamp) { + ROS_ERROR_STREAM("Adding a factor with a timestamp earlier than the last " + "one in the factor graph! This should never happen."); + } + SE3 poseEstimate = prevKeyPose * relativeRawOdomMotion; // compute translation and add to the trajectory length double translation = relativeRawOdomMotion.translation().norm(); trajectory_length += translation; + // if isInLoopClosureRegion_ clear all measurements to avoid adding them to // pollute the map, however, keep track of them in PoseMstPair so that loop // closure can use it diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index 6d86c76..4ee3649 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -17,44 +17,33 @@ SemanticFactorGraph::SemanticFactorGraph() { isam_params.relinearizeThreshold = 0.1; isam = new ISAM2(isam_params); + // Get the start time + start_time_ = ros::Time::now(); + + // ================ First Pose Noise models ================ // VERY IMPORTANT: the first three corresponds to Rotation and the last three // corresponds to Translation - Vector6 noise_vec_prior_first_pose; - noise_vec_prior_first_pose << 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, - 0.00001; - noise_model_prior_first_pose = - noiseModel::Diagonal::Sigmas(noise_vec_prior_first_pose); + noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); - // Landmark Noise model - // TODO(xu:) update the cylinder measurement noise - noise_model_cylinder = - noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); + // ================= Loop Closure models ================= + // It is assumed to have 0.01 of the noise of the odometry + noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); + + // ================ Landmark Noise models ================ // TODO(xu:) update the cuboid measurement noise - double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); - double stddev_pos = 1.0; // 3.0; - Vector9 noise_vec; - noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, - stddev_pos, stddev_pos, stddev_pos, stddev_pos; - noise_model_cube = noiseModel::Diagonal::Sigmas(noise_vec); + // double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); + // double stddev_pos = 1.0; // 3.0; + // Vector9 noise_vec; + // noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, + // stddev_pos, stddev_pos, stddev_pos, stddev_pos; + + // TODO(xu:) update the cylinder measurement noise + noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); // For range and bearing (ellipsoid objects) measurements double bearing_noise_std_temp = 1; noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); - - Vector6 noise_model_pose_vec = Vector6::Ones() * 0.00001; - noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec); - noise_model_closure = - noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01); - - // For Relative Inter-Robot Measurements - // XYZ 0.1m standard deviation, Rotation 0.1 rad standard deviation (~5.7 degree) - // TODO: These are currently just estimates, update! - Vector6 noise_model_rel_meas_vec; - noise_model_rel_meas_vec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; - noise_model_rel_meas = noiseModel::Diagonal::Sigmas(noise_model_rel_meas_vec); - - start_time_ = ros::Time::now(); } void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, @@ -77,50 +66,29 @@ void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, // -------------------------------- } -// No longer used - GPS factor -// void SemanticFactorGraph::addGPSFactor(const Point3 &gps_measurement) { -// size_t pose_counter_GPS = 0; // TODO: make this counter correct -// auto current_pose_key = X(pose_counter_GPS); -// GPSFactor gps_factor(current_pose_key, -// gps_measurement, // N,E,D -// noise_model_gps); -// fgraph.add(gps_factor); -// } - void SemanticFactorGraph::addKeyPoseAndBetween( const size_t prevIdx, const size_t curIdx, const Pose3 &relativeMotion, const Pose3 &poseEstimate, const int &robotID, boost::optional> cov, const bool &loopClosureFound, const SE3 &loop_closure_relative_pose, const size_t &closure_matched_pose_idx) { - // bool use_msckf_cov = false; + // Pose3 poseRelative = poseFrom.between(poseEstimate); Vector6 cur_noise_vec; // disable the inflation of covariance - // cur_noise_vec = noise_model_pose->sigmas() * + // cur_noise_vec = noise_model_odom->sigmas() * // (1.0 + noise_model_pose_inflation * mins_elapsed); - // ROS_INFO_STREAM_THROTTLE( - // 1, - // "Using predefined between-factor covariance instead of " - // "VIO estimated covariance (due to its inaccuracy)"); - - // scale covariance by travel distance so that motion uncertainty is properly - // accounted for - cur_noise_vec = noise_model_pose->sigmas(); - double relative_dist = relativeMotion.translation().norm(); - // clip the relative dist to avoid numerical issues when optimizing - if (relative_dist < 0.1) { - relative_dist = 0.1; - } - Vector6 noise_vec_scaled_by_travel_dist = cur_noise_vec * relative_dist; + // Scale covariance by travel distance + double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); + Vector6 noise_vec_scaled = noise_model_odom_vec * relative_dist; // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance fgraph.add(BetweenFactor( getSymbol(robotID, prevIdx), getSymbol(robotID, curIdx), relativeMotion, - noiseModel::Diagonal::Sigmas(noise_vec_scaled_by_travel_dist))); + noiseModel::Diagonal::Sigmas(noise_vec_scaled))); // Only for active SLAM // fgraph_loop.add(BetweenFactor( @@ -274,8 +242,14 @@ void SemanticFactorGraph::addCubeFactor( // Update: removed this since our process node can handle this properly CubeMeasurement cube_local_meas = cube_global_meas.project(pose.inverse()); + + // Calculate noise based on distance from the robot to the cube + double relative_dist = std::max(cube_local_meas.pose.translation().norm(), 0.1); + Vector9 noise_vec_scaled = noise_model_cube_vec * relative_dist; + + // Add the factor fgraph.add(CubeFactor(getSymbol(robotID, poseIdx), C(cubeIdx), - cube_local_meas, noise_model_cube)); + cube_local_meas, noiseModel::Diagonal::Sigmas(noise_vec_scaled))); if (cube_local_meas.pose.translation().norm() > 50) { ROS_WARN_THROTTLE(1, "cube_local_meas.pose.translation().norm() is larger " @@ -304,18 +278,17 @@ void SemanticFactorGraph::addLoopClosureFactor(const Pose3 poseRelative, noise_model_closure)); } -void SemanticFactorGraph::addRelativeMeasFactor(const Pose3 poseRelative, - const size_t prevIdx, - const size_t robotID1, - const size_t curIdx, - const size_t robotID2) { - // Correct interface for BetweenFactor(pose1, pose2, H_pose2^pose1, - // noise) poseRelative is H_query^candidate, i.e., from query pose to - // candidate (history) pose candidate is robotID1, prevIdx query is robotID2, - // curIdx +void SemanticFactorGraph::addRelativeMeasFactor(const Pose3 poseRelative, const size_t prevIdx, + const size_t robotID1, const size_t curIdx, const size_t robotID2) { + + // Scale noise by distance of the relative measurement + double relative_dist = std::max(poseRelative.translation().norm(), noise_floor); + Vector6 noise_vec_scaled = noise_model_rel_meas_vec * relative_dist; + + // Add the factor fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), getSymbol(robotID2, curIdx), poseRelative, - noise_model_rel_meas)); + noiseModel::Diagonal::Sigmas(noise_vec_scaled))); } void SemanticFactorGraph::solve() { diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index e44f698..2d6f7b5 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -9,8 +9,8 @@ #include -SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(int num_of_robots) - : numRobots(num_of_robots) { +SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(const ros::NodeHandle &nh, int num_of_robots) + : nh_(nh), numRobots(num_of_robots) { cyl_counter_ = 0; cube_counter_ = 0; point_landmark_counter_ = 0; @@ -21,6 +21,26 @@ SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(int num_of_robots) // keep this because we need it for old indoor activeSlamInputNode // should remove this after we finish the new activeSlamInputNode pose_counter_robot1_ = 0; + + // Pull covariance parameters and convert to Eigen vectors + std::vector noise_model_prior_first_pose_temp; + std::vector noise_model_odom_temp; + std::vector noise_model_cube_temp; + std::vector noise_model_rel_meas_temp; + + nh_.param("noise_model_prior_first_pose_vec", noise_model_prior_first_pose_temp, std::vector(6, 0.000001)); + nh_.param("noise_model_odom_vec", noise_model_odom_temp, std::vector(6, 0.1)); + nh_.param("noise_model_cube_vec", noise_model_cube_temp, std::vector(9, 0.1)); + nh_.param("noise_model_rel_meas_vec", noise_model_rel_meas_temp, std::vector(6, 0.1)); + + for(int i = 0; i < 6; i++) { + noise_model_prior_first_pose_vec(i) = noise_model_prior_first_pose_temp[i]; + noise_model_odom_vec(i) = noise_model_odom_temp[i]; + noise_model_rel_meas_vec(i) = noise_model_rel_meas_temp[i]; + } + for(int i = 0; i < 9; i++) { + noise_model_cube_vec(i) = noise_model_cube_temp[i]; + } } // for active team localization @@ -81,7 +101,7 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( "pose prior, this may cause problems!!!"); } else { // if cov is not specified, deafult covariance will be used, see - // noise_model_pose param in graph.cpp + // noise_model_odom param in graph.cpp addKeyPoseAndBetween(pose_counter - 1, pose_counter, relativeMotion, curr_pose, robotID); } diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 1bdc3ce..2e051e4 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -80,6 +80,7 @@ sensor_msgs python-pyproj message_filters + python-numpy diff --git a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py index 780d752..7da6f41 100755 --- a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py +++ b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py @@ -2,6 +2,7 @@ import rospy import rospkg +import numpy as np from geometry_msgs.msg import Pose, PoseStamped from gazebo_msgs.msg import ModelStates from sloam_msgs.msg import RelativeInterRobotMeasurement, StampedRvizMarkerArray @@ -85,11 +86,16 @@ def create_marker_for_cuboid(self, model: ET.Element, robot_name: str, robot_pos # Transform pose into the relative frame of the robots cuboid.pose = transforms.calculate_relative_pose(robot_pose, pose) - # Extract the scale of the cuboid + # Add 0.5 m and 1 degree/0.017 rad (per unit distance) Gaussian noise to the cuboid pose + relative_dist = np.sqrt(cuboid.pose.position.x**2 + cuboid.pose.position.y**2 + cuboid.pose.position.z**2) + cuboid.pose = transforms.add_zero_mean_gaussian_noise_to_pose(cuboid.pose, 0.5 * relative_dist, 0.017 * relative_dist) + + # Extract the scale of the cuboid and add 0.05 m Gaussian noise (per unit distance) scale_vals = model.find('link').find('collision').find('geometry').find('box').find('size').text.strip().split(' ') - cuboid.scale.x = float(scale_vals[0]) - cuboid.scale.y = float(scale_vals[1]) - cuboid.scale.z = float(scale_vals[2]) + noise = np.random.normal(0, 0.05 * relative_dist, 3) + cuboid.scale.x = float(scale_vals[0] + noise[0]) + cuboid.scale.y = float(scale_vals[1] + noise[1]) + cuboid.scale.z = float(scale_vals[2] + noise[2]) # Set other values cuboid.lifetime = rospy.Duration(0) # 0 means forever @@ -120,6 +126,11 @@ def gazebo_callback(self, msg: ModelStates): # Calculate the relative measurement from jackal0 to jackal1 relative_meas: Pose = transforms.calculate_relative_pose(jackal0_pose, jackal1_pose) + # Add 0.06 m and 0.1 degree/0.0017 rad (per unit distance) Gaussian noise to the relative measurement + # Current inspiration for this is Fig. 5 & 6 of AprilTag 2 paper. + relative_dist = np.sqrt(relative_meas.position.x**2 + relative_meas.position.y**2 + relative_meas.position.z**2) + relative_meas = transforms.add_zero_mean_gaussian_noise_to_pose(relative_meas, 0.06 * relative_dist, 0.0017 * relative_dist) + # Create the header for the messages header = Header() header.stamp = rospy.Time.now() diff --git a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py index acb2291..e939339 100644 --- a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py +++ b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py @@ -27,4 +27,43 @@ def calculate_relative_pose(pose1: Pose, pose2: Pose) -> Pose: rel_pose.position.x, rel_pose.position.y, rel_pose.position.z = p_rel rel_pose.orientation.x, rel_pose.orientation.y, rel_pose.orientation.z, rel_pose.orientation.w = q_rel - return rel_pose \ No newline at end of file + return rel_pose + +def add_zero_mean_gaussian_noise_to_pose(pose: Pose, stddev_translation: float, stddev_rotation: float) -> Pose: + """ + Adds Gaussian noise to a Pose message. + + Args: + pose (Pose): The original pose. + stddev_translation (float): Standard deviation for translation noise, in meters. + stddev_rotation (float): Standard deviation for rotation noise, specifically on + the euler angles, in radians. + + Returns: + Pose: The noisy pose. + """ + + # Add translation noise + noisy_pose = Pose() + noise_T = np.random.normal(0, stddev_translation, 3) # 3 for position, 3 for orientation + noisy_pose.position.x = pose.position.x + noise_T[0] + noisy_pose.position.y = pose.position.y + noise_T[1] + noisy_pose.position.z = pose.position.z + noise_T[2] + + # Extract quaternion and convert to euler angles + q = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) + r = Rotation.from_quat(q, scalar_first=False) + euler_angles = r.as_euler('xyz', degrees=False) + + # Add rotation noise + noise_R = np.random.normal(0, stddev_rotation, 3) + noisy_euler_angles = euler_angles + noise_R + + # Convert back to quaternion + noisy_rotation = Rotation.from_euler('xyz', noisy_euler_angles).as_quat() + noisy_pose.orientation.x = noisy_rotation[0] + noisy_pose.orientation.y = noisy_rotation[1] + noisy_pose.orientation.z = noisy_rotation[2] + noisy_pose.orientation.w = noisy_rotation[3] + + return noisy_pose \ No newline at end of file From 276d44abedf1ce4de4c1c1e32e5f3cb8a22c38f8 Mon Sep 17 00:00:00 2001 From: Cam Jackson Date: Thu, 20 Mar 2025 17:24:28 -0400 Subject: [PATCH 22/74] tf tree ping for bot->cam transformation, using doubles now instead of floats, made rosparams for camera frames, annotated CoPeD config, removed bot->cam loading from config --- .../include/apriltag_meas_gen.h | 12 +- .../include/apriltag_wrapper.h | 2 +- .../launch/CoPeD_relative_meas_gen.launch | 6 +- .../relative_measurement_generator.launch | 5 + frontend/relative_meas_gen/params/CoPeD.yaml | 17 ++- .../src/apriltag_meas_gen.cpp | 105 +++++++++--------- .../src/apriltag_wrapper.cpp | 2 +- 7 files changed, 85 insertions(+), 64 deletions(-) diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 6751714..5c27aa2 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -7,6 +7,9 @@ #include #include #include +#include +#include +#include class ApriltagMeasurer { @@ -17,15 +20,16 @@ class ApriltagMeasurer { ros::Publisher relative_meas_pub; std::string robot_ID; std::string camera_ID; - float intrinsics[4]; - float tagsize; + double intrinsics[4]; + double tagsize; YAML::Node config; + Eigen::Matrix4d bot_to_cam; void imageCallback(const sensor_msgs::CompressedImage msg); - void PublishRelativeMeasurement(int8_t, Eigen::Matrix4f transformation); + void PublishRelativeMeasurement(int8_t, Eigen::Matrix4d transformation); Eigen::Matrix4d CalculateRelativeTransformation(Eigen::Matrix4d H_hostBot_to_cam, Eigen::Matrix4d H_cam_to_tag, Eigen::Matrix4d H_observedBot_to_tag); - std::tuple> LoadTransformations(apriltag_wrapper tag); + std::tuple LoadTransformations(apriltag_wrapper tag); ros::NodeHandle nh_; }; \ No newline at end of file diff --git a/frontend/relative_meas_gen/include/apriltag_wrapper.h b/frontend/relative_meas_gen/include/apriltag_wrapper.h index b8e42af..7173b7d 100644 --- a/frontend/relative_meas_gen/include/apriltag_wrapper.h +++ b/frontend/relative_meas_gen/include/apriltag_wrapper.h @@ -23,5 +23,5 @@ class apriltag_wrapper { apriltag_wrapper(); }; -std::vector ExtractAprilTags(cv::Mat image, float intrinsics[4], float tagsize); +std::vector ExtractAprilTags(cv::Mat image, double intrinsics[4], double tagsize); cv::Mat MatFromImage(const sensor_msgs::CompressedImage msg); diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch index 6a321e0..591b34d 100644 --- a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -6,7 +6,9 @@ - - + + + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch index 9ac20ec..cfbb0e1 100644 --- a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch @@ -4,6 +4,8 @@ + + @@ -16,6 +18,9 @@ + + + diff --git a/frontend/relative_meas_gen/params/CoPeD.yaml b/frontend/relative_meas_gen/params/CoPeD.yaml index 38a829f..5c1a4bf 100644 --- a/frontend/relative_meas_gen/params/CoPeD.yaml +++ b/frontend/relative_meas_gen/params/CoPeD.yaml @@ -1,8 +1,11 @@ dataset: "CoPeD" + +# Configuration for wilbur robot robot0: id: 0 name: "wilbur" - camera0: + # Camera pose information taken from extrinsics config file + camera0: # Left optical link -> info taken from 19504539.yaml in calibration/wilbur image_width: 1440 image_height: 1080 fx: 1903.520006386324 @@ -21,7 +24,7 @@ robot0: rows: 3 cols: 4 data: [1794.791137695312, 0, 676.5970053006567, 0, 0, 1833.858642578125, 515.3424035623175, 0, 0, 0, 1, 0] - camera1: + camera1: # Right optical link -> info taken from 19509631.yaml in calibration/wilbur image_width: 1440 image_height: 1080 fx: 1836.368323539261 @@ -40,6 +43,7 @@ robot0: rows: 3 cols: 4 data: [1725.12744140625, 0, 723.5165065089095, 0, 0, 1763.703735351562, 522.8930017468156, 0, 0, 0, 1, 0] + # Tag information from tag_bundle_wilbur.yaml in calibration/wilbur tags: [ {id: 5, size: 0.17, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}, @@ -52,10 +56,12 @@ robot0: {id: 6, size: 0.17, x: 0.4940, y: 0.2477, z: -0.0743, qw: 0.9572, qx: -0.2750, qy: -0.0895, qz: -0.0070} ] +# Configuration for wanda robot robot1: id: 1 name: "wanda" - camera0: + # Camera pose information taken from extrinsics config file + camera0: # left optical link -> info taken from 19509635.yaml in calibration/wanda image_width: 1440 image_height: 1080 fx: 1192.45276 @@ -74,7 +80,7 @@ robot1: rows: 3 cols: 4 data: [ 980.67499, 0, 738.69215, 0, 0, 1076.08105, 566.97831, 0, 0, 0, 1, 0] - camera1: + camera1: # right optical link -> info taken from 19509600.yaml in calibration/wanda image_width: 1440 image_height: 1080 fx: 1190.130507756544 @@ -93,6 +99,7 @@ robot1: rows: 3 cols: 4 data: [975.4818115234375, 0, 734.7269645256092, 0, 0, 1071.288208007812, 571.0164652120948, 0, 0, 0, 1, 0] + # Tag information from tag_bundle_wanda.yaml in calibration/wanda tags: [ {id: 8, size: 0.17, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}, @@ -105,6 +112,7 @@ robot1: {id: 13, size: 0.17, x: 0.3355, y: -0.5865, z: 0.0061, qw: 0.9447, qx: 0.3228, qy: 0.0416, qz: -0.0409} ] +# Configuration for race1 drone from camchain-imucam.yaml in calibration/race1 robot2: id: 2 name: "race1" @@ -172,6 +180,7 @@ robot2: - [0.010993599480977004, 0.9993874128976246, 0.03322558821523644, -0.0593976812355273] - [0.0, 0.0, 0.0, 1.0] +# Configuration for race5 drone from camchain-imucam.yaml in calibration/race5 robot3: id: 3 name: "race5" diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 4a2808a..8408b34 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -14,9 +14,8 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { // Publish transformation for each detected apriltag for (apriltag_wrapper t : tags) { int8_t bot_id; - Eigen::Matrix4f bot_to_cam; - Eigen::Matrix4f cam_to_tag; - Eigen::Matrix4f tag_to_bot; + Eigen::Matrix4d cam_to_tag; + Eigen::Matrix4d tag_to_bot; // Obtain RT from camera -> detected apriltag for (int i = 0; i < 3; i++) { @@ -27,53 +26,29 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { } std::cout << "Tag ID: " << t.id << std::endl; - std::tuple> loaded_transformations = LoadTransformations(t); - std::array transformations = std::get<1>(loaded_transformations); + std::tuple loaded_transformations = LoadTransformations(t); + Eigen::Matrix4d tag_to_bot = std::get<1>(loaded_transformations); bot_id = std::get<0>(loaded_transformations); if (bot_id == -1) { ROS_ERROR("Transformations not loaded for apriltag id: " + t.id); continue; } - bot_to_cam = transformations[0]; - tag_to_bot = transformations[1]; - Eigen::Matrix4f total_transformation = CalculateRelativeTransformation(bot_to_cam, cam_to_tag, tag_to_bot); + Eigen::Matrix4d total_transformation = CalculateRelativeTransformation(bot_to_cam, cam_to_tag, tag_to_bot); PublishRelativeMeasurement(bot_id, total_transformation); } } -std::tuple> ApriltagMeasurer::LoadTransformations(apriltag_wrapper t) { - std::tuple> return_tuple; +std::tuple ApriltagMeasurer::LoadTransformations(apriltag_wrapper t) { + std::tuple return_tuple; int8_t bot_id = -1; - std::array transformations; + Eigen::Matrix4d tag_to_bot; std::string dataset = config["dataset"].as(); if (dataset == "CoPeD") { - // Load RT matrix from host bot -> camera from config - Eigen::Matrix4f bot_to_cam = Eigen::Matrix4f::Identity(); - if (config[robot_ID][camera_ID]["R"].IsDefined()) { - YAML::Node R = config[robot_ID][camera_ID]["R"]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - bot_to_cam(i, j) = R[i][j].as(); - } - } - bot_to_cam(0, 3) = config[robot_ID][camera_ID]["x"].as(); - bot_to_cam(1, 3) = config[robot_ID][camera_ID]["y"].as(); - bot_to_cam(2, 3) = config[robot_ID][camera_ID]["z"].as(); - } else { - float x = config[robot_ID][camera_ID]["x"].as(); - float y = config[robot_ID][camera_ID]["y"].as(); - float z = config[robot_ID][camera_ID]["z"].as(); - float roll = config[robot_ID][camera_ID]["roll"].as(); - float pitch = config[robot_ID][camera_ID]["pitch"].as(); - float yaw = config[robot_ID][camera_ID]["yaw"].as(); - bot_to_cam = RollPitchYaw_to_RT(x, y, z, roll, pitch, yaw); - } - Eigen::Matrix4f tag_to_bot; bool tag_on_bot = false; // Obtain translation vector and quaternion from detected apriltag -> detected robot @@ -84,17 +59,17 @@ std::tuple> ApriltagMeasurer::LoadTransfo for (auto tag = tags.begin(); tag != tags.end(); ++tag) { if (t.id == (*tag)["id"].as()) { bot_id = config[key]["id"].as(); - float x = (*tag)["x"].as(); - float y = (*tag)["y"].as(); - float z = (*tag)["z"].as(); - Eigen::Vector3f translation = Eigen::Vector3f(x, y, z); - float qw = (*tag)["qw"].as(); - float qx = (*tag)["qx"].as(); - float qy = (*tag)["qy"].as(); - float qz = (*tag)["qz"].as(); - Eigen::Quaternionf q = Eigen::Quaternionf(qw, qx, qy, qz); + double x = (*tag)["x"].as(); + double y = (*tag)["y"].as(); + double z = (*tag)["z"].as(); + Eigen::Vector3d translation = Eigen::Vector3d(x, y, z); + double qw = (*tag)["qw"].as(); + double qx = (*tag)["qx"].as(); + double qy = (*tag)["qy"].as(); + double qz = (*tag)["qz"].as(); + Eigen::Quaterniond q = Eigen::Quaterniond(qw, qx, qy, qz); tag_on_bot = true; - tag_to_bot = Eigen::Matrix4f::Identity(); + tag_to_bot = Eigen::Matrix4d::Identity(); tag_to_bot.block<3,3>(0,0) = q.toRotationMatrix(); tag_to_bot.block<3,1>(0,3) = translation; break; @@ -106,9 +81,7 @@ std::tuple> ApriltagMeasurer::LoadTransfo ROS_WARN("Detected tag does not belong to any robot in dataset"); } - transformations[0] = bot_to_cam; - transformations[1] = tag_to_bot; - return_tuple = std::make_tuple(bot_id, transformations); + return_tuple = std::make_tuple(bot_id, tag_to_bot); return return_tuple; } else { @@ -122,7 +95,7 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { std::string image_topic; nh_.param("apriltag_node/image_topic", image_topic, "/default_topic"); nh_.param("apriltag_node/host_robot", robot_ID, "robot0"); - nh_.param("apriltag_node", tagsize, .17); + nh_.param("apriltag_node", tagsize, .17); nh_.param("apriltag_node/camera_ID", camera_ID, "camera0"); std::cout << "Subscribing to topic: " << image_topic << std::endl; std::string config_file; @@ -133,11 +106,39 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { // Load config file config = YAML::LoadFile(config_file); + std::string base_link; + std::string camera_link; + nh_.param("apriltag_node/base_link", base_link, "/default_base_link"); + nh_.param("apriltag_node/camera_link", camera_link, "/default_cam_link"); + + // Get bot to cam transformation from tf tree + tf::TransformListener listener; + geometry_msgs::TransformStampted transformStamped; + + listener.waitForTransform(camera_link, base_link, ros::Time(0), ros::Duration(5.0)); + listener.lookupTransform(camera_link, base_link, ros::Time(0), transformStamped); + + double x, y, z, qw, qx, qy, qz; + x = transformStamped.transform.translation.x; + y = transformStamped.transform.translation.y; + z = transformStamped.transform.translation.z; + qw = transformStamped.transform.rotation.w; + qx = transformStamped.transform.rotation.x; + qy = transformStamped.transform.rotation.y; + qz = transformStamped.transform.rotation.z; + + Eigen::Vector3d translation = Eigen::Vector3d(x, y, z); + Eigen::Quaterniond q = Eigen::Quaterniond(qw, qx, qy, qz); + + bot_to_cam = Eigen::Matrix4d::Identity(); + bot_to_cam.block<3,1>(0,3) = translation; + bot_to_cam.block<3,3>(0,0) = q.toRotationMatrix(); + // Load host_robot properties - float fx = config[robot_ID][camera_ID]["fx"].as(); - float cx = config[robot_ID][camera_ID]["cx"].as(); - float fy = config[robot_ID][camera_ID]["fy"].as(); - float cy = config[robot_ID][camera_ID]["cy"].as(); + double fx = config[robot_ID][camera_ID]["fx"].as(); + double cx = config[robot_ID][camera_ID]["cx"].as(); + double fy = config[robot_ID][camera_ID]["fy"].as(); + double cy = config[robot_ID][camera_ID]["cy"].as(); intrinsics[0] = fx; intrinsics[1] = cx; @@ -185,7 +186,7 @@ Eigen::Matrix4d ApriltagMeasurer::CalculateRelativeTransformation(Eigen::Matrix4 return T_bot_to_observedBot; } -void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4f transformation) { +void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4d transformation) { geometry_msgs::Pose pose_msg; sloam_msgs::RelativeInterRobotMeasurement msg; diff --git a/frontend/relative_meas_gen/src/apriltag_wrapper.cpp b/frontend/relative_meas_gen/src/apriltag_wrapper.cpp index d698c46..12d1977 100644 --- a/frontend/relative_meas_gen/src/apriltag_wrapper.cpp +++ b/frontend/relative_meas_gen/src/apriltag_wrapper.cpp @@ -22,7 +22,7 @@ apriltag_wrapper::apriltag_wrapper(int id, double center[2], double corners[4][2 apriltag_wrapper::apriltag_wrapper() {} -std::vector ExtractAprilTags(cv::Mat img, float intrinsics[4], float tagsize) { +std::vector ExtractAprilTags(cv::Mat img, double intrinsics[4], double tagsize) { // Create usable image object from Mat image_u8_t img_header = { .width = img.cols, From 065311ca4997a205bc8e17b1c5b9ab7eee3e30c0 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Sun, 23 Mar 2025 20:23:51 -0400 Subject: [PATCH 23/74] Finished incorporating changes into relative_meas_gen from pull request 03-10-25 --- frontend/relative_meas_gen/CMakeLists.txt | 3 +- .../include/apriltag_meas_gen.h | 4 +- .../launch/CoPeD_relative_meas_gen.launch | 2 +- .../relative_measurement_generator.launch | 2 +- frontend/relative_meas_gen/package.xml | 3 +- .../src/apriltag_meas_gen.cpp | 46 ++++++++++--------- .../relative_meas_gen/test/apriltag_test.cpp | 10 ++-- .../relative_meas_gen/tests/testMultiUGV.py | 3 +- 8 files changed, 40 insertions(+), 33 deletions(-) diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index d09939e..ef9a7f8 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS sloam_msgs std_msgs sensor_msgs + tf ) catkin_package( @@ -226,7 +227,7 @@ target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apri if (CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roscpp rostest) - catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/apriltag_wwapper.cpp) + catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/apriltag_wrapper.cpp) target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES}) endif() diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index 5c27aa2..f393ce6 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include @@ -18,7 +18,7 @@ class ApriltagMeasurer { private: ros::Subscriber robot_images_sub; ros::Publisher relative_meas_pub; - std::string robot_ID; + int robot_ID; std::string camera_ID; double intrinsics[4]; double tagsize; diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch index 591b34d..fe18347 100644 --- a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -2,7 +2,7 @@ - + diff --git a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch index cfbb0e1..2917895 100644 --- a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch @@ -1,7 +1,7 @@ - + diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 4ac5b22..0b230b5 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -54,6 +54,7 @@ rospy sloam_msgs std_msgs + tf gazebo_msgs yaml-cpp sensor_msgs @@ -81,12 +82,12 @@ python-pyproj message_filters python-numpy + tf rostest gtest - diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 8408b34..f37a462 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -27,7 +27,7 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { std::cout << "Tag ID: " << t.id << std::endl; std::tuple loaded_transformations = LoadTransformations(t); - Eigen::Matrix4d tag_to_bot = std::get<1>(loaded_transformations); + tag_to_bot = std::get<1>(loaded_transformations); bot_id = std::get<0>(loaded_transformations); if (bot_id == -1) { @@ -94,7 +94,7 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { // Load params std::string image_topic; nh_.param("apriltag_node/image_topic", image_topic, "/default_topic"); - nh_.param("apriltag_node/host_robot", robot_ID, "robot0"); + nh_.param("apriltag_node/host_robot", robot_ID, 0); nh_.param("apriltag_node", tagsize, .17); nh_.param("apriltag_node/camera_ID", camera_ID, "camera0"); std::cout << "Subscribing to topic: " << image_topic << std::endl; @@ -113,19 +113,19 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { // Get bot to cam transformation from tf tree tf::TransformListener listener; - geometry_msgs::TransformStampted transformStamped; + tf::StampedTransform transformStamped; listener.waitForTransform(camera_link, base_link, ros::Time(0), ros::Duration(5.0)); listener.lookupTransform(camera_link, base_link, ros::Time(0), transformStamped); double x, y, z, qw, qx, qy, qz; - x = transformStamped.transform.translation.x; - y = transformStamped.transform.translation.y; - z = transformStamped.transform.translation.z; - qw = transformStamped.transform.rotation.w; - qx = transformStamped.transform.rotation.x; - qy = transformStamped.transform.rotation.y; - qz = transformStamped.transform.rotation.z; + x = transformStamped.getOrigin().x(); + y = transformStamped.getOrigin().y(); + z = transformStamped.getOrigin().z(); + qw = transformStamped.getRotation().w(); + qx = transformStamped.getRotation().x(); + qy = transformStamped.getRotation().y(); + qz = transformStamped.getRotation().z(); Eigen::Vector3d translation = Eigen::Vector3d(x, y, z); Eigen::Quaterniond q = Eigen::Quaterniond(qw, qx, qy, qz); @@ -194,20 +194,24 @@ void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4d geometry_msgs::Point position; geometry_msgs::Quaternion orientation; - /* - TODO: some math to get these two fields - */ + position.x = (float) transformation(0, 3); + position.y = (float) transformation(1, 3); + position.z = (float) transformation(2, 3); + + Eigen::Matrix3d rotation = bot_to_cam.block<3,3>(0,0); + Eigen::Quaterniond quat(rotation); + + orientation.x = (float) quat.x(); + orientation.y = (float) quat.y(); + orientation.z = (float) quat.z(); + orientation.w = (float) quat.w(); msg.header.stamp = ros::Time::now(); msg.relativePose = pose_msg; - - /* - TODO: Get odometry data - */ - - msg.robotId = bot_id; + msg.robotIdObserved = bot_id; + msg.robotIdObserver = robot_ID; - std::cout << "Publishing pose from host to " << bot_id << std::endl; + std::cout << "Publishing pose from robot " << robot_ID << " to robot " << bot_id << std::endl; - relative_meas.publish(pose_msg); + relative_meas_pub.publish(pose_msg); } \ No newline at end of file diff --git a/frontend/relative_meas_gen/test/apriltag_test.cpp b/frontend/relative_meas_gen/test/apriltag_test.cpp index 19fcdd9..720301f 100644 --- a/frontend/relative_meas_gen/test/apriltag_test.cpp +++ b/frontend/relative_meas_gen/test/apriltag_test.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include TEST(apriltag_test, Detect) { @@ -17,16 +17,16 @@ TEST(apriltag_test, Detect) { cv::Mat image; cv::cvtColor(matrix, image, cv::COLOR_BGR2GRAY); - float intrinsics[4]; + double intrinsics[4]; intrinsics[0] = 1903.520006386324; intrinsics[1] = 681.5060629940748; intrinsics[2] = 1895.619818759923; intrinsics[3] = 517.1128313302568; - float tagsize = 0.17; + double tagsize = 0.17; - std::vector tags = ExtractAprilTags(image, intrinsics, tagsize); - slidetag test_tag = tags[0]; + std::vector tags = ExtractAprilTags(image, intrinsics, tagsize); + apriltag_wrapper test_tag = tags[0]; std::cout << "Detected tag: " << test_tag.id << std::endl; std::cout << "Expected tag: 6" << std::endl; diff --git a/frontend/relative_meas_gen/tests/testMultiUGV.py b/frontend/relative_meas_gen/tests/testMultiUGV.py index 7d900ab..279c5d3 100755 --- a/frontend/relative_meas_gen/tests/testMultiUGV.py +++ b/frontend/relative_meas_gen/tests/testMultiUGV.py @@ -38,4 +38,5 @@ def test_calculate_relative_pose(self): if __name__ == '__main__': import rostest - rostest.rosrun('relative_meas_gen', 'test_multi_ugv', TestMultiUGV) \ No newline at end of file + rostest.rosrun('relative_meas_gen', 'test_multi_ugv', TestMultiUGV) + rostest.rosrun('relative_meas_gen', 'apriltag_test', 'Detect') \ No newline at end of file From 85572e9878788572cb3db11758a3df3ed413a4e9 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 31 Mar 2025 13:53:51 -0400 Subject: [PATCH 24/74] Completed apriltag transformation implementation, created github actions --- .github/workflows/SLIDE_SLAM_test.yml | 48 ++++++++++++++++++ docker/run_container.sh | 23 +++++++++ frontend/relative_meas_gen/CMakeLists.txt | 17 ++----- .../relative_meas_gen/bash_scripts/frames.gv | 47 +++++++++++++++++ .../bash_scripts/tmux_relative_meas.sh | 10 ++-- .../include/apriltag_meas_gen.h | 1 + .../launch/CoPeD_relative_meas_gen.launch | 7 +-- .../relative_measurement_generator.launch | 12 +++-- .../src/apriltag_meas_gen.cpp | 30 ++++++----- .../{test => tests}/apriltag_test.cpp | 2 +- .../{test => tests}/apriltag_test.test | 0 .../{test => tests}/apriltag_test_pic.jpg | Bin 12 files changed, 156 insertions(+), 41 deletions(-) create mode 100644 .github/workflows/SLIDE_SLAM_test.yml create mode 100755 docker/run_container.sh create mode 100644 frontend/relative_meas_gen/bash_scripts/frames.gv rename frontend/relative_meas_gen/{test => tests}/apriltag_test.cpp (94%) rename frontend/relative_meas_gen/{test => tests}/apriltag_test.test (100%) rename frontend/relative_meas_gen/{test => tests}/apriltag_test_pic.jpg (100%) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml new file mode 100644 index 0000000..5cff356 --- /dev/null +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -0,0 +1,48 @@ +name: SLIDE SLAM Tests + +on: + push: + branches: [ "**" ] + pull_request: + branches: [ "**" ] + +jobs: + build-and-test: + name: "SLIDE_SLAM build-and-test" + runs-on: ubuntu-latest + timeout-minutes: 180 + + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + + - name: Pull Prebuilt Docker Image + run: | + docker pull xurobotics/slide-slam:latest + + - name: Set Up Workspace and Clone Repo + run: | + mkdir -p ~/slideslam_docker_ws/src && + cd ~/slideslam_docker_ws/src && + git clone https://github.com/lunarlab-gatech/SLIDE_SLAM.git + + - name: Build Docker Image + run: | + cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker && + ./build_image.sh + + - name: Run Docker Container + run: | + cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker && + export WS_PATH="$(pwd)" && + ./run_container.sh && + ./enter_container.sh && + + - name: Build SLIDE_SLAM inside Container + run: | + catkin_make && + source devel/setup.bash + + - name: Run SLIDE_SLAM Tests + run: | + catkin_make run_tests \ No newline at end of file diff --git a/docker/run_container.sh b/docker/run_container.sh new file mode 100755 index 0000000..7fa8737 --- /dev/null +++ b/docker/run_container.sh @@ -0,0 +1,23 @@ +SlideSlamWs="$WS_PATH" # point to your workspace directory +SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM +BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory + +xhost +local:root # for the lazy and reckless +docker run -it \ + --name="slideslam_ros" \ + --net="host" \ + --privileged \ + --gpus="all" \ + --workdir="/opt/slideslam_docker_ws" \ + --env="DISPLAY=$DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --env="XAUTHORITY=$XAUTH" \ + --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ + --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ + --volume="$BAGS_DIR:/opt/bags" \ + --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + --volume="/home/$USER/repos:/home/$USER/repos" \ + slideslam_ros_image \ + bash + diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index ef9a7f8..d4020bf 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -60,17 +60,6 @@ catkin_python_setup() ## * add "message_generation" and every package in MSG_DEP_SET to ## find_package(catkin REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg # ) ## Generate services in the 'srv' folder @@ -147,9 +136,9 @@ include_directories( include ) -add_executable(ApriltagMeasurer src/apriltag_meas_gen.cpp src/apriltag_wrapper.cpp) -set_target_properties(ApriltagMeasurer PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) -target_link_libraries(ApriltagMeasurer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES} yaml-cpp) +add_executable(apriltag_meas_gen src/apriltag_meas_gen.cpp src/apriltag_wrapper.cpp) +set_target_properties(apriltag_meas_gen PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) +target_link_libraries(apriltag_meas_gen ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES} yaml-cpp) ## Declare a C++ library # add_library(${PROJECT_NAME} diff --git a/frontend/relative_meas_gen/bash_scripts/frames.gv b/frontend/relative_meas_gen/bash_scripts/frames.gv new file mode 100644 index 0000000..d1c381a --- /dev/null +++ b/frontend/relative_meas_gen/bash_scripts/frames.gv @@ -0,0 +1,47 @@ +digraph G { +"wilbur/base" -> "wilbur/base_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/odom" -> "wilbur/base"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 125.356 Hz\nMost recent transform: 1684528100.338 ( -0.958 sec old)\nBuffer length: 2.457 sec\n"]; +"wilbur/base_link" -> "wilbur/chassis_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/chassis_link" -> "wilbur/diff_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/chassis_link" -> "wilbur/embedded_imu_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_bottom_screw_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/forward_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_bottom_screw_frame" -> "wilbur/forward_camera_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_color_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_depth_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_aligned_depth_to_color_frame" -> "wilbur/forward_color_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_depth_frame" -> "wilbur/forward_depth_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/base_link" -> "wilbur/sensor_plate_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_depth_frame" -> "wilbur/forward_left_ir_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_left_ir_frame" -> "wilbur/forward_left_ir_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_depth_frame" -> "wilbur/forward_right_ir_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_right_ir_frame" -> "wilbur/forward_right_ir_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/imu_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/ouster_center_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/ouster_front_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/stereo_left_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/stereo_left_link" -> "wilbur/stereo_left_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/stereo_right_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/stereo_right_link" -> "wilbur/stereo_right_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/chassis_link" -> "wilbur/top_chassis_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/sensor_plate_link" -> "wilbur/ublox_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/imu_link" -> "wilbur/imu_link_base"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/ouster_center_link" -> "wilbur/os_imu"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/ouster_center_link" -> "wilbur/os_lidar"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_aligned_depth_to_color_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_infra1_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_infra1_frame" -> "wilbur/forward_infra1_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_link" -> "wilbur/forward_infra2_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/forward_infra2_frame" -> "wilbur/forward_infra2_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; +"wilbur/chassis_link" -> "wilbur/left_diff_unit_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 32.220 Hz\nMost recent transform: 1684528099.344 ( 0.036 sec old)\nBuffer length: 2.421 sec\n"]; +"wilbur/chassis_link" -> "wilbur/right_diff_unit_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 32.220 Hz\nMost recent transform: 1684528099.344 ( 0.036 sec old)\nBuffer length: 2.421 sec\n"]; +"wilbur/map" -> "wilbur/odom"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10.435 Hz\nMost recent transform: 1684528099.301 ( 0.078 sec old)\nBuffer length: 2.300 sec\n"]; +"wilbur/left_diff_unit_link" -> "wilbur/front_left_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; +"wilbur/right_diff_unit_link" -> "wilbur/front_right_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; +"wilbur/left_diff_unit_link" -> "wilbur/rear_left_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; +"wilbur/right_diff_unit_link" -> "wilbur/rear_right_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1684528099.380"[ shape=plaintext ] ; + }->"wilbur/map"; +} \ No newline at end of file diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh index d87540d..29af786 100755 --- a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -2,7 +2,7 @@ # Set parameters SESSION_NAME=slide_slam_nodes -BAG_PLAY_RATE=0.5 +BAG_PLAY_RATE=4.0 BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/' SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" @@ -40,12 +40,12 @@ tmux select-pane -t $SESSION_NAME:1.3 tmux split-window -h -t $SESSION_NAME # Setup commands for main window -tmux select-pane -t $SESSION_NAME:1.0 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter +#tmux select-pane -t $SESSION_NAME:1.0 +#tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /tf /tf_static /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter tmux select-pane -t $SESSION_NAME:1.2 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch relative_meas_gen CoPeD_relative_meas_gen.launch" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 4; roslaunch relative_meas_gen CoPeD_relative_meas_gen.launch" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore diff --git a/frontend/relative_meas_gen/include/apriltag_meas_gen.h b/frontend/relative_meas_gen/include/apriltag_meas_gen.h index f393ce6..244402d 100644 --- a/frontend/relative_meas_gen/include/apriltag_meas_gen.h +++ b/frontend/relative_meas_gen/include/apriltag_meas_gen.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch index fe18347..7e25481 100644 --- a/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch +++ b/frontend/relative_meas_gen/launch/CoPeD_relative_meas_gen.launch @@ -2,13 +2,14 @@ - + + - - + + \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch index 2917895..9e88f06 100644 --- a/frontend/relative_meas_gen/launch/relative_measurement_generator.launch +++ b/frontend/relative_meas_gen/launch/relative_measurement_generator.launch @@ -1,25 +1,27 @@ - + + - - + + - + + - + diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index f37a462..1ffbd05 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -22,7 +22,7 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { for (int j = 0; j < 3; j++) { cam_to_tag(i, j) = t.rotation.data[i + j]; } - cam_to_tag(3, i) = t.translation.data[i]; + cam_to_tag(i, 3) = t.translation.data[i]; } std::cout << "Tag ID: " << t.id << std::endl; @@ -43,7 +43,7 @@ void ApriltagMeasurer::imageCallback(const sensor_msgs::CompressedImage msg) { std::tuple ApriltagMeasurer::LoadTransformations(apriltag_wrapper t) { std::tuple return_tuple; - int8_t bot_id = -1; + int8_t bot_id; Eigen::Matrix4d tag_to_bot; std::string dataset = config["dataset"].as(); @@ -52,13 +52,13 @@ std::tuple ApriltagMeasurer::LoadTransformations(aprilt bool tag_on_bot = false; // Obtain translation vector and quaternion from detected apriltag -> detected robot - for (auto it = config.begin(); it != config.end(); ++it) { + for (auto it = ++config.begin(); it != config.end(); ++it) { std::string key = it->first.as(); if (config[key]["tags"].IsDefined()) { YAML::Node tags = config[key]["tags"]; for (auto tag = tags.begin(); tag != tags.end(); ++tag) { if (t.id == (*tag)["id"].as()) { - bot_id = config[key]["id"].as(); + bot_id = config[key]["id"].as(); double x = (*tag)["x"].as(); double y = (*tag)["y"].as(); double z = (*tag)["z"].as(); @@ -80,7 +80,6 @@ std::tuple ApriltagMeasurer::LoadTransformations(aprilt if (!tag_on_bot) { ROS_WARN("Detected tag does not belong to any robot in dataset"); } - return_tuple = std::make_tuple(bot_id, tag_to_bot); return return_tuple; @@ -94,10 +93,11 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { // Load params std::string image_topic; nh_.param("apriltag_node/image_topic", image_topic, "/default_topic"); - nh_.param("apriltag_node/host_robot", robot_ID, 0); + std::string host_robot; + nh_.param("apriltag_node/host_robot", host_robot, "robot0"); + nh_.param("apriltag_node/robot_ID", robot_ID, 0); nh_.param("apriltag_node", tagsize, .17); nh_.param("apriltag_node/camera_ID", camera_ID, "camera0"); - std::cout << "Subscribing to topic: " << image_topic << std::endl; std::string config_file; nh_.param("apriltag_node/config_file", config_file, "default.yaml"); std::string return_topic; @@ -115,8 +115,8 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { tf::TransformListener listener; tf::StampedTransform transformStamped; - listener.waitForTransform(camera_link, base_link, ros::Time(0), ros::Duration(5.0)); - listener.lookupTransform(camera_link, base_link, ros::Time(0), transformStamped); + listener.waitForTransform(camera_link, base_link, ros::Time::now(), ros::Duration(5.0)); + listener.lookupTransform(camera_link, base_link, ros::Time::now(), transformStamped); double x, y, z, qw, qx, qy, qz; x = transformStamped.getOrigin().x(); @@ -135,10 +135,10 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { bot_to_cam.block<3,3>(0,0) = q.toRotationMatrix(); // Load host_robot properties - double fx = config[robot_ID][camera_ID]["fx"].as(); - double cx = config[robot_ID][camera_ID]["cx"].as(); - double fy = config[robot_ID][camera_ID]["fy"].as(); - double cy = config[robot_ID][camera_ID]["cy"].as(); + double fx = config[host_robot][camera_ID]["fx"].as(); + double cx = config[host_robot][camera_ID]["cx"].as(); + double fy = config[host_robot][camera_ID]["fy"].as(); + double cy = config[host_robot][camera_ID]["cy"].as(); intrinsics[0] = fx; intrinsics[1] = cx; @@ -148,6 +148,7 @@ ApriltagMeasurer::ApriltagMeasurer(ros::NodeHandle nh): nh_(nh) { // Instantiate sub and pub robot_images_sub = nh_.subscribe(image_topic, 1, &ApriltagMeasurer::imageCallback, this); relative_meas_pub = nh_.advertise(return_topic, 10); + std::cout << "Subscribed to topic: " << image_topic << std::endl; } int main(int argc, char** argv) { @@ -211,6 +212,9 @@ void ApriltagMeasurer::PublishRelativeMeasurement(int8_t bot_id, Eigen::Matrix4d msg.robotIdObserved = bot_id; msg.robotIdObserver = robot_ID; + double mag = pow(pow(position.x, 2) + pow(position.y, 2) + pow(position.z, 2), .5); + + std::cout << "Transformation magnitude: " << mag << std::endl; std::cout << "Publishing pose from robot " << robot_ID << " to robot " << bot_id << std::endl; relative_meas_pub.publish(pose_msg); diff --git a/frontend/relative_meas_gen/test/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp similarity index 94% rename from frontend/relative_meas_gen/test/apriltag_test.cpp rename to frontend/relative_meas_gen/tests/apriltag_test.cpp index 720301f..d71420c 100644 --- a/frontend/relative_meas_gen/test/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -5,7 +5,7 @@ TEST(apriltag_test, Detect) { - std::string path = "/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/frontend/relative_meas_gen/test/apriltag_test_pic.jpg"; + std::string path = "/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/frontend/relative_meas_gen/tests/apriltag_test_pic.jpg"; //std::string package_path = ros::package::getPath("relative_meas_gen"); //std::string path = package_path + "/test/apriltag_test_pic.jpg"; diff --git a/frontend/relative_meas_gen/test/apriltag_test.test b/frontend/relative_meas_gen/tests/apriltag_test.test similarity index 100% rename from frontend/relative_meas_gen/test/apriltag_test.test rename to frontend/relative_meas_gen/tests/apriltag_test.test diff --git a/frontend/relative_meas_gen/test/apriltag_test_pic.jpg b/frontend/relative_meas_gen/tests/apriltag_test_pic.jpg similarity index 100% rename from frontend/relative_meas_gen/test/apriltag_test_pic.jpg rename to frontend/relative_meas_gen/tests/apriltag_test_pic.jpg From 23b757a0ed38b1021e99d6e897299d42340e1595 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 31 Mar 2025 15:27:17 -0400 Subject: [PATCH 25/74] Added branch checkout to github actions --- .github/workflows/SLIDE_SLAM_test.yml | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 5cff356..cc71c44 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -22,25 +22,26 @@ jobs: - name: Set Up Workspace and Clone Repo run: | - mkdir -p ~/slideslam_docker_ws/src && - cd ~/slideslam_docker_ws/src && + mkdir -p ~/slideslam_docker_ws/src + cd ~/slideslam_docker_ws/src git clone https://github.com/lunarlab-gatech/SLIDE_SLAM.git - name: Build Docker Image run: | - cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker && + cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker + git checkout apriltag ./build_image.sh - name: Run Docker Container run: | - cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker && - export WS_PATH="$(pwd)" && - ./run_container.sh && - ./enter_container.sh && + cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker + export WS_PATH="$(pwd)" + ./run_container.sh + ./enter_container.sh - name: Build SLIDE_SLAM inside Container run: | - catkin_make && + catkin_make source devel/setup.bash - name: Run SLIDE_SLAM Tests From 7241d63f06181ebc073680d36e19cd17508746f1 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 31 Mar 2025 16:42:57 -0400 Subject: [PATCH 26/74] added executable permissions to bash scripts in github actions --- .github/workflows/SLIDE_SLAM_test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index cc71c44..d26aa36 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -36,6 +36,8 @@ jobs: run: | cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" + chmod +x run_container.sh + chmod +x enter_container.sh ./run_container.sh ./enter_container.sh From a9217cbdf89c93f911c8736a5a220fe7009c50b0 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 2 Apr 2025 17:37:10 -0400 Subject: [PATCH 27/74] github actions test 4/2 5:36pm --- .github/workflows/SLIDE_SLAM_test.yml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index d26aa36..b22cdd9 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -34,18 +34,17 @@ jobs: - name: Run Docker Container run: | + xhost _local:root cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" chmod +x run_container.sh - chmod +x enter_container.sh ./run_container.sh - ./enter_container.sh - name: Build SLIDE_SLAM inside Container run: | - catkin_make - source devel/setup.bash + docker exec -it slideslam_ros sh -c "catkin_make" + docker exec -it slideslam_ros sh -c "source devel/setup.bash" - name: Run SLIDE_SLAM Tests run: | - catkin_make run_tests \ No newline at end of file + docker exec -it slideslam_ros sh -c "catkin_make run_tests" \ No newline at end of file From 78cbf23e909294022792b1f3aa338302f6d28754 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 2 Apr 2025 17:45:50 -0400 Subject: [PATCH 28/74] github actions test 4/2 5:45 --- .github/workflows/SLIDE_SLAM_test.yml | 4 +--- docker/run_container.sh | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index b22cdd9..754974c 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -34,7 +34,6 @@ jobs: - name: Run Docker Container run: | - xhost _local:root cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" chmod +x run_container.sh @@ -42,8 +41,7 @@ jobs: - name: Build SLIDE_SLAM inside Container run: | - docker exec -it slideslam_ros sh -c "catkin_make" - docker exec -it slideslam_ros sh -c "source devel/setup.bash" + docker exec slideslam_ros sh -c "catkin_make && source devel/setup.bash" docker exec slideslam_ros sh -c "source devel/setup.bash" - name: Run SLIDE_SLAM Tests run: | diff --git a/docker/run_container.sh b/docker/run_container.sh index 7fa8737..4dba125 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -2,7 +2,7 @@ SlideSlamWs="$WS_PATH" # point to your workspace directory SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory -xhost +local:root # for the lazy and reckless +# xhost +local:root # for the lazy and reckless docker run -it \ --name="slideslam_ros" \ --net="host" \ From 46c1ec0119a51f758b7584cf451b48dad05ae577 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 2 Apr 2025 17:56:07 -0400 Subject: [PATCH 29/74] github actions test 4/2 5:55 --- docker/run_container.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/run_container.sh b/docker/run_container.sh index 4dba125..0d80f29 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -3,7 +3,7 @@ SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory # xhost +local:root # for the lazy and reckless -docker run -it \ +docker run \ --name="slideslam_ros" \ --net="host" \ --privileged \ From 69ac512c659f73ee4657d348401b58794eb1f962 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 2 Apr 2025 18:03:11 -0400 Subject: [PATCH 30/74] github actions test 4/2 6:03 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 754974c..4c22eee 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -37,7 +37,7 @@ jobs: cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" chmod +x run_container.sh - ./run_container.sh + ./run_container.sh --no-gpu - name: Build SLIDE_SLAM inside Container run: | From 23254dc2de18cbaa6f9fc72cbf0306a851c27377 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 3 Apr 2025 15:11:22 +0000 Subject: [PATCH 31/74] Not fully tested implementation of inter-robot measurements with covariances --- backend/sloam/include/core/databaseManager.h | 1 + backend/sloam/include/core/input.h | 3 + backend/sloam/include/core/robot.h | 3 + backend/sloam/include/core/sloamNode.h | 4 +- backend/sloam/include/factorgraph/graph.h | 11 +- .../sloam/include/factorgraph/graphWrapper.h | 4 +- backend/sloam/include/helpers/definitions.h | 2 + backend/sloam/params/sloam_multiUGV.yaml | 13 +- backend/sloam/src/core/databaseManager.cpp | 10 +- backend/sloam/src/core/input.cpp | 52 +- backend/sloam/src/core/inputNode.cpp | 15 +- backend/sloam/src/core/robot.cpp | 25 +- backend/sloam/src/core/sloamNode.cpp | 28 +- backend/sloam/src/factorgraph/graph.cpp | 66 +- .../sloam/src/factorgraph/graphWrapper.cpp | 78 +-- .../sloam/src/factorgraph/graph_important.cpp | 663 ++++++++++++++++++ backend/sloam_msgs/msg/PoseMst.msg | 1 + .../bash_scripts/multi_UGV_run.sh | 6 +- .../scripts/multiUGVToSlideSLAM.py | 28 +- .../src/relative_meas_gen/transforms.py | 2 +- 20 files changed, 903 insertions(+), 112 deletions(-) create mode 100755 backend/sloam/src/factorgraph/graph_important.cpp diff --git a/backend/sloam/include/core/databaseManager.h b/backend/sloam/include/core/databaseManager.h index aaa1a70..29fab35 100755 --- a/backend/sloam/include/core/databaseManager.h +++ b/backend/sloam/include/core/databaseManager.h @@ -46,6 +46,7 @@ struct PoseMstPair { SE3 keyPose; // the current pose of the robot SE3 relativeRawOdomMotion; // relative motion from last keyPose to current as // measured using odometry + std::array relativeRawOdomMotionCov; // Covariance for relativeRawOdomMotion ros::Time stamp; // the timestamp associated with the keyPose // measurement in robot frame diff --git a/backend/sloam/include/core/input.h b/backend/sloam/include/core/input.h index 0f4f2c4..77f8a44 100644 --- a/backend/sloam/include/core/input.h +++ b/backend/sloam/include/core/input.h @@ -11,4 +11,7 @@ class Input { double msg_delay_tolerance, float minOdomDistance_, int &meas_to_add); + std::array computeRelativeMotionCovariance( + const SE3 &T1, const std::array &cov1, + const SE3 &T2, const std::array &cov2); }; \ No newline at end of file diff --git a/backend/sloam/include/core/robot.h b/backend/sloam/include/core/robot.h index 296af7b..bb9bbca 100755 --- a/backend/sloam/include/core/robot.h +++ b/backend/sloam/include/core/robot.h @@ -37,10 +37,12 @@ #include struct StampedSE3 { + StampedSE3(SE3 p, ros::Time s, std::array c) : pose(p), stamp(s), covariance(c) {}; StampedSE3(SE3 p, ros::Time s) : pose(p), stamp(s){}; StampedSE3() : pose(SE3()), stamp(ros::Time::now()){}; SE3 pose; ros::Time stamp; + std::array covariance; // Ordering XYZRPY }; struct Observation { @@ -103,6 +105,7 @@ class Robot { // Helper methods for converting messages to objects std::vector rosEllipsoid2EllipObj(const std::vector& msgs); std::vector rosCylinder2CylinderObj(const std::vector& rosCylinders); + std::array ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg); // Logging methods void PrintLateMsgWarning(const std::string &msg_type); diff --git a/backend/sloam/include/core/sloamNode.h b/backend/sloam/include/core/sloamNode.h index fd6bbbb..2430766 100755 --- a/backend/sloam/include/core/sloamNode.h +++ b/backend/sloam/include/core/sloamNode.h @@ -62,7 +62,9 @@ class SLOAMNode : public sloam { using ConstPtr = boost::shared_ptr; // timestamp is used for visualization - bool runSLOAMNode(const SE3 &relativeRawOdomMotion, const SE3 &prevKeyPose, + bool runSLOAMNode(const SE3 &relativeRawOdomMotion, + std::array relativeRawOdomMotionCov, + const SE3 &prevKeyPose, const std::vector &cylindersBody, const std::vector &cubesBody, const std::vector &ellipsoidBody, diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index 8591da5..2044781 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -85,8 +85,8 @@ class SemanticFactorGraph { bool alreadyExists, const int& robotID); void addKeyPoseAndBetween( const size_t fromIdx, const size_t toIdx, const Pose3& relativeMotion, + const std::array relativeMotionCov, const Pose3& poseEstimate, const int& robotID, - boost::optional> cov = boost::none, const bool& loopClosureFound = false, const SE3& loop_closure_pose = SE3(), const size_t& closure_matched_pose_idx = 0); @@ -126,7 +126,9 @@ class SemanticFactorGraph { double noise_floor = 0.1; // Clipping noises prevents numerical issues boost::shared_ptr noise_model_prior_first_pose; Vector6 noise_model_prior_first_pose_vec; - + + // If true, ignore noise_model_odom_vec and use the cov from the nav message instead + bool use_odom_cov_in_nav_msg_; Vector6 noise_model_odom_vec; boost::shared_ptr noise_model_closure; @@ -139,6 +141,11 @@ class SemanticFactorGraph { Vector6 noise_model_rel_meas_vec; + + /// TO REMOVE + boost::shared_ptr noise_model_pose; + boost::shared_ptr noise_model_cube; + // Aux attributes Pose3 odomToGPS_; diff --git a/backend/sloam/include/factorgraph/graphWrapper.h b/backend/sloam/include/factorgraph/graphWrapper.h index c2fdb99..392bcc6 100755 --- a/backend/sloam/include/factorgraph/graphWrapper.h +++ b/backend/sloam/include/factorgraph/graphWrapper.h @@ -88,7 +88,9 @@ class SemanticFactorGraphWrapper : public SemanticFactorGraph { const std::vector &cubes, const std::vector &ellipsoid_matches, const std::vector &ellipsoids, - const SE3 &relativeMotion, const SE3 &poseEstimates, + const SE3 &relativeMotion, + const std::array relativeMotionCov, + const SE3 &poseEstimates, const int &robotID, bool opt = true); /** diff --git a/backend/sloam/include/helpers/definitions.h b/backend/sloam/include/helpers/definitions.h index 14146bb..3edd89f 100755 --- a/backend/sloam/include/helpers/definitions.h +++ b/backend/sloam/include/helpers/definitions.h @@ -107,6 +107,7 @@ struct FeatureModelParams { * * @param stamp - the timestamp. * @param odomPose - the synced odometry. + * @param covariance - the covariance of the odometry. * @param relativePose - the relative measurement * between the current robot and robot assigned to * "robotIndex". @@ -122,6 +123,7 @@ struct FeatureModelParams { struct RelativeMeas { ros::Time stamp; SE3 odomPose; + std::array covariance; SE3 relativePose; int robotIndex; bool onlyUseOdom; diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index 6d54855..5b4dd88 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -48,7 +48,7 @@ sloam: # parameters for place recognition (slide graph version) place_recognition_slidegraph: # ------- MOST IMPORTANT PARAMS TO TUNE: ------- - num_inliners_threshold: 2 + num_inliners_threshold: 3 descriptor_matching_threshold: 0.1 min_num_map_objects_to_start: 4 # 30 for outdoor parking lot # 100 for forests # minimum number of objects in a map to start SlideGraph place recognition attempt #----------------------------------------------- @@ -57,8 +57,11 @@ sloam: # Parameters for Factor graph factor_graph: - #VERY IMPORTANT: First three (Rotation) and last three (Translation) - noise_model_prior_first_pose_vec: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + # For the noise models below: + # VERY IMPORTANT: For Pose3 Noise models, ordering is RPYXYZ. + # Values are standard deviation of the noise in radians and meters respectively + noise_model_prior_first_pose_vec: [0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001] + use_odom_cov_in_nav_msg: false # If true, ignore noise_model_odom_vec and use the covariance included in the nav_msgs/Odometry message noise_model_odom_vec: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - noise_model_cube_vec: [0.017, 0.017, 0.017, 0.5, 0.5, 0.5, 0.05, 0.05, 0.05] - noise_model_rel_meas_vec: [0.0017, 0.0017, 0.0017, 0.06, 0.06, 0.06] + noise_model_cube_vec: [0.017, 0.017, 0.017, 0.05, 0.05, 0.05, 0.01, 0.01, 0.01] + noise_model_rel_meas_vec: [0.001, 0.001, 0.001, 0.03, 0.03, 0.03] diff --git a/backend/sloam/src/core/databaseManager.cpp b/backend/sloam/src/core/databaseManager.cpp index 319f679..b80dbb5 100755 --- a/backend/sloam/src/core/databaseManager.cpp +++ b/backend/sloam/src/core/databaseManager.cpp @@ -113,6 +113,9 @@ void databaseManager::poseMstCb_(const sloam_msgs::PoseMstBundle &msgs) { // construct a PoseMstPair struct poseMst.keyPose = pose; poseMst.relativeRawOdomMotion = relativeOdom; + for (int i = 0; i < 6; i++) { + poseMst.relativeRawOdomMotionCov[i] = singleMsg.relativeRawOdomCov[i]; + } poseMst.stamp = singleMsg.stamp; // convert msg to object for (int j = 0; j < singleMsg.cylinders.size(); j++) { @@ -197,7 +200,8 @@ void databaseManager::measureReceivedCommMsgSize(const sloam_msgs::PoseMstBundle for (int i = 0; i < msgs.poseMstPair.size(); i++) { cur_received_msg_size_bytes += 56; cur_received_msg_size_bytes += 56; - cur_received_msg_size_bytes += 8; // For the time stamp (theorized, not prove) + cur_received_msg_size_bytes += 48; // From covariance + cur_received_msg_size_bytes += 8; // For the time stamp (theorized, not proven) cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].ellipsoids.size(); cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].cubes.size(); cur_received_msg_size_bytes += 37*msgs.poseMstPair[i].cylinders.size(); @@ -237,6 +241,9 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { pmMsg.pose = ToRosPoseMsg(pmp.keyPose); pmMsg.stamp = pmp.stamp; pmMsg.relativeRawOdom = ToRosPoseMsg(pmp.relativeRawOdomMotion); + for (int i = 0; i < 6; i++) { + pmMsg.relativeRawOdomCov[i] = pmp.relativeRawOdomMotionCov[i]; + } pmMsg.cylinders = obj2RosObjMsg(pmp.cylinderMsts); pmMsg.cubes = obj2RosObjMsg(pmp.cubeMsts); pmMsg.ellipsoids = obj2RosObjMsg(pmp.ellipsoidMsts); @@ -247,6 +254,7 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { cur_publish_msg_size_bytes += 56; cur_publish_msg_size_bytes += 56; cur_publish_msg_size_bytes += 8; // From time stamp (not proven) + cur_publish_msg_size_bytes += 48; // From covariance } for (int i = 0; i < robotMapDict_[curRobotID].size(); i++) { diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp index 94a1560..17c7830 100644 --- a/backend/sloam/src/core/input.cpp +++ b/backend/sloam/src/core/input.cpp @@ -1,6 +1,6 @@ #include -/* +/** * @brief This measurement handles logic for picking which measurement to add * to the factor graph next. It ensures they are added in order of timestamp * (assuming that each queue is ordered by timestamp). This prevents jumping @@ -92,4 +92,54 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, // If we got this far, then no odometry measurements are valid & moved enough meas_to_add = 0; return; +} + +/** + * @brief Computes the relative motion covariance from odometry pose T1 to odometry + * pose T2 given their respective covariance matrices. Note that cov2 includes + * the covariance of T1 plus any additional covariance from the relative motion. + * + * @param T1: The first odometry pose + * @param cov1: The associated covariance for T1 + * @param T2: The second odometry pose + * @param cov2: The associated covariance for T2 + * @return The covariance for the relative motion + */ +std::array Input::computeRelativeMotionCovariance(const SE3 &T1, + const std::array &cov1, const SE3 &T2, const std::array &cov2) { + + std::array cov_rel; + + // Compute relative transformation + SE3 T_rel = T1.inverse() * T2; + + // Extract rotation and translation from T_rel + Eigen::Matrix3d R_rel = T_rel.rotationMatrix(); + Eigen::Vector3d t_rel = T_rel.translation(); + + // Compute Adjoint matrix + Eigen::Matrix Adj_T_rel = Eigen::Matrix::Zero(); + Adj_T_rel.block<3, 3>(0, 0) = R_rel; + Adj_T_rel.block<3, 3>(0, 3) = skewSymmetric(t_rel) * R_rel; + Adj_T_rel.block<3, 3>(3, 3) = R_rel; + + // Convert std::array to Eigen::Matrix + Eigen::Matrix Sigma1 = Eigen::Matrix::Zero(); + Eigen::Matrix Sigma2 = Eigen::Matrix::Zero(); + + for (int i = 0; i < 6; ++i) { + Sigma1(i, i) = cov1[i]; + Sigma2(i, i) = cov2[i]; + } + + // Compute the relative motion covariance + Eigen::Matrix Sigma1_transformed = Adj_T_rel * Sigma1 * Adj_T_rel.transpose(); + Eigen::Matrix Sigma_rel = Sigma2 - Sigma1_transformed; + + // Store result back in std::array + for (int i = 0; i < 6; ++i) { + cov_rel[i] = Sigma_rel(i, i); // Extract diagonal elements + } + + return cov_rel; } \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 7f8451a..1669b4b 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -11,6 +11,7 @@ InputManager::InputManager(ros::NodeHandle nh) : nh_(nh), tf_listener_{tf_buffer_}, robot(nh) { + nh_.param("main_node_rate", runInputNodeRate_, 5.0); timer_ = nh.createTimer(ros::Duration(1.0 / runInputNodeRate_), &InputManager::RunInputNode, this); @@ -138,6 +139,7 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { StampedSE3 validStampedPose; validStampedPose.pose = relativeMeas.odomPose; validStampedPose.stamp = relativeMeas.stamp; + validStampedPose.covariance = relativeMeas.covariance; latestObservation.stampedPose = validStampedPose; // If we observed another robot (rather than being observed) @@ -167,6 +169,11 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; + // Estimate the covariance of the relative motion + std::array relativeRawOdomCovariance = computeRelativeMotionCovariance( + robot.robotLatestOdom_.pose, robot.robotLatestOdom_.covariance, + raw_vio_odom_used_for_sloam.pose, raw_vio_odom_used_for_sloam.covariance); + // Get the Previous Key Pose SE3 prevKeyPose; if (robot.robotKeyPoses_.size() > 0) { @@ -179,9 +186,10 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { // Send the measurement to SLOAM SE3 keyPose; bool success = sloam_->runSLOAMNode( - relativeRawOdomMotion, prevKeyPose, latestObservation.cylinders, - latestObservation.cubes, latestObservation.ellipsoids, - latestObservation.stampedPose.stamp, keyPose, hostRobotID_); + relativeRawOdomMotion, relativeRawOdomCovariance, prevKeyPose, + latestObservation.cylinders, latestObservation.cubes, + latestObservation.ellipsoids, latestObservation.stampedPose.stamp, + keyPose, hostRobotID_); if (success) { robot.robotKeyPoses_.push_back(keyPose); updateLastPose(raw_vio_odom_used_for_sloam, hostRobotID_); @@ -202,6 +210,7 @@ void InputManager::updateLastPose(const StampedSE3 &odom, const int &robotID) { robot.robotOdomReceived_ = true; robot.robotLatestOdom_.pose = odom.pose; robot.robotLatestOdom_.stamp = odom.stamp; + robot.robotLatestOdom_.covariance = odom.covariance; if (robot.robotKeyPoses_.size() == 0) { robot.robotLastSLOAMKeyPose_ = robot.robotLatestOdom_.pose; } else { diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 1a4e3be..3cd574a 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -93,8 +93,11 @@ void Robot::RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg) { return; } + // Extract covariance + std::array covariance = ExtractCovarianceFromOdometry(*odom_msg); + // Add odometry to the queue, and pop the oldest one if the queue is too long - robotOdomQueue_.emplace_back(odom, odomStamp); + robotOdomQueue_.emplace_back(odom, odomStamp, covariance); if (robotOdomQueue_.size() > 10 * maxQueueSize_) robotOdomQueue_.pop_front(); } @@ -110,6 +113,8 @@ void Robot::RobotObservationCb(const sloam_msgs::SemanticMeasSyncOdom &observati cur_observation.stampedPose.pose = databaseManager::toSE3Pose(observation_msg.odometry.pose.pose); cur_observation.stampedPose.stamp = observation_msg.header.stamp; + cur_observation.stampedPose.covariance = ExtractCovarianceFromOdometry(observation_msg.odometry); + std::vector scan_cubes_body; int total_cuboids = observation_msg.cuboid_factors.size(); for (const sloam_msgs::ROSCube &cur_cuboid_marker : @@ -163,10 +168,12 @@ void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurementO cur_measurement.onlyUseOdom = false; cur_measurement.robotIndex = relativeMeas_msg.robotIdObserved; cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserver.pose.pose); + cur_measurement.covariance = ExtractCovarianceFromOdometry(relativeMeas_msg.odometryObserver); } else { cur_measurement.onlyUseOdom = true; cur_measurement.robotIndex = relativeMeas_msg.robotIdObserver; cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserved.pose.pose); + cur_measurement.covariance = ExtractCovarianceFromOdometry(relativeMeas_msg.odometryObserved); } robotRelativeMeasQueue_.push_back(cur_measurement); @@ -211,4 +218,20 @@ std::vector Robot::rosEllipsoid2EllipObj( void Robot::PrintLateMsgWarning(const std::string &msg_type) { ROS_WARN_STREAM_THROTTLE(10, msg_type << "message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); ROS_WARN_STREAM_THROTTLE(10, "Please increase `msg_delay_tolerance` to account for lag in your system"); +} + +/** + * @brief This method extracts the covariance values from a + * nav_msgs::Odometry message. + * + * @param odom_msg: The odometry message from which to extract the covariance. + * @return A std::array containing the covariance diagonal values (in XYZRPY order) + */ +std::array Robot::ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg) { + std::array covariance; + for (size_t i = 0; i < 6; i++) { + // Absolute value used in case some very small covariance values come in negative + covariance[i] = std::abs(odom_msg.pose.covariance[i * 6 + i]); + } + return covariance; } \ No newline at end of file diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index 226910b..960bb0c 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -16,7 +16,7 @@ namespace sloam { SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) - : nh_(nh), dbManager(nh), inter_loopCloser_(nh), intra_loopCloser_(nh) { + : nh_(nh), dbManager(nh), inter_loopCloser_(nh), intra_loopCloser_(nh), factorGraph_(nh) { // initialize intra loop closure and inter loop closure nodes intra_loopCloser_.inter_loop_closure = false; @@ -147,7 +147,7 @@ void SLOAMNode::initParams_() { semanticMap_ = CylinderMapManager(numRobots); cube_semantic_map_ = CubeMapManager(); ellipsoid_semantic_map_ = EllipsoidMapManager(); - factorGraph_ = SemanticFactorGraphWrapper(numRobots); + factorGraph_ = SemanticFactorGraphWrapper(nh_, numRobots); fmParams_.cylinderMatchThresh = nh_.param("cylinder_match_thresh", 2.0); // Cylinder data association threshold fmParams_.cuboidMatchThresh = nh_.param("cuboid_match_thresh", 2.0); // Cuboid data association threshold fmParams_.ellipsoidMatchThresh = nh_.param("ellipsoid_match_thresh", 0.75); // Ellipsoid data association threshold @@ -852,6 +852,7 @@ void SLOAMNode::relInterRobotFactorThread_() { } bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, + std::array relativeRawOdomMotionCov, const SE3 &prevKeyPose, const std::vector &cylindersBodyIn, const std::vector &cubesBodyIn, @@ -864,11 +865,14 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, dbMutex.lock(); // Make sure that factors are created in timestamp order - ros::Time last_stamp = dbManager.getHostRobotData().poseMstPacket.back().stamp; - if (stamp < last_stamp) { - ROS_ERROR_STREAM("Adding a factor with a timestamp earlier than the last " - "one in the factor graph! This should never happen."); - } + std::deque packets = dbManager.getHostRobotData().poseMstPacket; + if(packets.size() >= 1) { + ros::Time last_stamp = dbManager.getHostRobotData().poseMstPacket.back().stamp; + if (stamp < last_stamp) { + ROS_ERROR_STREAM("Adding a factor with a timestamp earlier than the last " + "one in the factor graph! This should never happen."); + } + } SE3 poseEstimate = prevKeyPose * relativeRawOdomMotion; // compute translation and add to the trajectory length @@ -883,6 +887,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, pmp.cylinderMsts = cylindersBodyIn; pmp.stamp = stamp; pmp.relativeRawOdomMotion = relativeRawOdomMotion; + pmp.relativeRawOdomMotionCov = relativeRawOdomMotionCov; pmp.cubeMsts = cubesBodyIn; pmp.ellipsoidMsts = ellipsoidBodyIn; dbManager.getHostRobotData().poseMstPacket.push_back(pmp); @@ -979,8 +984,8 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, semanticMap_, cube_semantic_map_, ellipsoid_semantic_map_, sloamOut.cylinderMatches, sloamOut.scanCylindersWorld, sloamOut.cubeMatches, sloamOut.scanCubesWorld, sloamOut.ellipsoidMatches, - sloamOut.scanEllipsoidsWorld, relativeRawOdomMotion, sloamOut.T_Map_Curr, - robotID); + sloamOut.scanEllipsoidsWorld, relativeRawOdomMotion, relativeRawOdomMotionCov, + sloamOut.T_Map_Curr, robotID); double fg_optimization_end = ros::Time::now().toSec(); double time_diff = fg_optimization_end - fg_optimization_start; fg_optimization_time.push_back(time_diff); @@ -1029,6 +1034,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, iter->second.poseMstPacket[i].keyPose; SE3 relativeRawOdomMotionInRefFrame = iter->second.poseMstPacket[i].relativeRawOdomMotion; + std::array relativeRawOdomMotionCovInRefFrame = iter->second.poseMstPacket[i].relativeRawOdomMotionCov; // transform the pose and landmark into host robot map frame std::vector CylinderMeasurement = iter->second.poseMstPacket[i].cylinderMsts; @@ -1082,8 +1088,8 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, semanticMap_, cube_semantic_map_, ellipsoid_semantic_map_, cylinderMatchIndices, cylinderInRefFrame, cubeMatchIndices, cubeInRefFrame, ellipsoidMatchIndices, ellipsoidInRefFrame, - relativeRawOdomMotionInRefFrame, poseEstimateInRefFrame, curRobotID, - false); + relativeRawOdomMotionInRefFrame, relativeRawOdomMotionCovInRefFrame, + poseEstimateInRefFrame, curRobotID, false); } factorGraph_.solve(); dbManager.updateFGBookmark(curSize, curRobotID); diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index 4ee3649..b7b04f9 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -20,30 +20,36 @@ SemanticFactorGraph::SemanticFactorGraph() { // Get the start time start_time_ = ros::Time::now(); - // ================ First Pose Noise models ================ // VERY IMPORTANT: the first three corresponds to Rotation and the last three // corresponds to Translation - noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); + Vector6 noise_vec_prior_first_pose; + noise_vec_prior_first_pose << 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, + 0.00001; + noise_model_prior_first_pose = + noiseModel::Diagonal::Sigmas(noise_vec_prior_first_pose); - // ================= Loop Closure models ================= - // It is assumed to have 0.01 of the noise of the odometry - noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); - - // ================ Landmark Noise models ================ + // Landmark Noise model + // TODO(xu:) update the cylinder measurement noise + noise_model_cylinder = + noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); // TODO(xu:) update the cuboid measurement noise - // double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); - // double stddev_pos = 1.0; // 3.0; - // Vector9 noise_vec; - // noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, - // stddev_pos, stddev_pos, stddev_pos, stddev_pos; - - // TODO(xu:) update the cylinder measurement noise - noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); + double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); + double stddev_pos = 1.0; // 3.0; + Vector9 noise_vec; + noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, + stddev_pos, stddev_pos, stddev_pos, stddev_pos; + noise_model_cube = noiseModel::Diagonal::Sigmas(noise_vec); // For range and bearing (ellipsoid objects) measurements double bearing_noise_std_temp = 1; noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); + + Vector6 noise_model_pose_vec; + noise_model_pose_vec << 0.01, 0.01, 0.01, 0.02, 0.02, 0.02; // 2% drift per dist traveled & 0.5 degree + noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec); + noise_model_closure = + noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01); } void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, @@ -68,27 +74,26 @@ void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, void SemanticFactorGraph::addKeyPoseAndBetween( const size_t prevIdx, const size_t curIdx, const Pose3 &relativeMotion, - const Pose3 &poseEstimate, const int &robotID, - boost::optional> cov, const bool &loopClosureFound, + const std::array relativeMotionCov, + const Pose3 &poseEstimate, const int &robotID, const bool &loopClosureFound, const SE3 &loop_closure_relative_pose, const size_t &closure_matched_pose_idx) { - - // Pose3 poseRelative = poseFrom.between(poseEstimate); - + Vector6 cur_noise_vec; - - // disable the inflation of covariance - // cur_noise_vec = noise_model_odom->sigmas() * - // (1.0 + noise_model_pose_inflation * mins_elapsed); - - // Scale covariance by travel distance - double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); - Vector6 noise_vec_scaled = noise_model_odom_vec * relative_dist; + // scale covariance by travel distance so that motion uncertainty is properly + // accounted for + cur_noise_vec = noise_model_pose->sigmas(); + double relative_dist = relativeMotion.translation().norm(); + // clip the relative dist to avoid numerical issues when optimizing + if (relative_dist < 0.1) { + relative_dist = 0.1; + } + Vector6 noise_vec_scaled_by_travel_dist = cur_noise_vec * relative_dist; // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance fgraph.add(BetweenFactor( getSymbol(robotID, prevIdx), getSymbol(robotID, curIdx), relativeMotion, - noiseModel::Diagonal::Sigmas(noise_vec_scaled))); + noiseModel::Diagonal::Sigmas(noise_vec_scaled_by_travel_dist))); // Only for active SLAM // fgraph_loop.add(BetweenFactor( @@ -118,7 +123,6 @@ void SemanticFactorGraph::addKeyPoseAndBetween( "ERROR: get closure_matched_pose_idx fail to fetch pose for pose_idx " ": " << closure_matched_pose_idx); - auto odom_pose_prior_factor_noise = cur_noise_vec * 1.0; // here the insert also set the initial estimate for the optimization fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); // Only for active SLAM @@ -174,7 +178,6 @@ void SemanticFactorGraph::addKeyPoseAndBetween( ROS_INFO_STREAM("loop_closure_relative_pose being added"); // current_pose_global_ = est_loop_closure_pose; } else { - auto odom_pose_prior_factor_noise = cur_noise_vec * 1.0; // here the insert also set the initial estimate for the optimization fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); // Only for active SLAM @@ -247,7 +250,6 @@ void SemanticFactorGraph::addCubeFactor( double relative_dist = std::max(cube_local_meas.pose.translation().norm(), 0.1); Vector9 noise_vec_scaled = noise_model_cube_vec * relative_dist; - // Add the factor fgraph.add(CubeFactor(getSymbol(robotID, poseIdx), C(cubeIdx), cube_local_meas, noiseModel::Diagonal::Sigmas(noise_vec_scaled))); diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index 2d6f7b5..2155e48 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -28,10 +28,11 @@ SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(const ros::NodeHandle &nh std::vector noise_model_cube_temp; std::vector noise_model_rel_meas_temp; - nh_.param("noise_model_prior_first_pose_vec", noise_model_prior_first_pose_temp, std::vector(6, 0.000001)); - nh_.param("noise_model_odom_vec", noise_model_odom_temp, std::vector(6, 0.1)); - nh_.param("noise_model_cube_vec", noise_model_cube_temp, std::vector(9, 0.1)); - nh_.param("noise_model_rel_meas_vec", noise_model_rel_meas_temp, std::vector(6, 0.1)); + nh_.param("factor_graph/noise_model_prior_first_pose_vec", noise_model_prior_first_pose_temp, std::vector(6, 0.000001)); + nh_.param("factor_graph/use_odom_cov_in_nav_msg", use_odom_cov_in_nav_msg_, true); + nh_.param("factor_graph/noise_model_odom_vec", noise_model_odom_temp, std::vector(6, 0.1)); + nh_.param("factor_graph/noise_model_cube_vec", noise_model_cube_temp, std::vector(9, 0.1)); + nh_.param("factor_graph/noise_model_rel_meas_vec", noise_model_rel_meas_temp, std::vector(6, 0.1)); for(int i = 0; i < 6; i++) { noise_model_prior_first_pose_vec(i) = noise_model_prior_first_pose_temp[i]; @@ -44,36 +45,36 @@ SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(const ros::NodeHandle &nh } // for active team localization -bool SemanticFactorGraphWrapper::addLoopClosureObservation( - const SE3 &relativeMotionSE3, const SE3 &poseEstimateSE3, - const boost::array &cov, const SE3 &loop_closure_relative_pose, - const size_t &closure_matched_pose_idx) { - // default only have 1 aerial robot for active team localization - gtsam::Pose3 relativeMotion(relativeMotionSE3.matrix()); - gtsam::Pose3 poseEstimate(poseEstimateSE3.matrix()); - int robotID = 0; - bool optimize = false; - - if (pose_counter_robot1_ == 0) { - ROS_ERROR_STREAM( - "ERROR: First pose should not be the loop closure pose!!!!!!!!!"); - return false; - } else { - // add BetweenFactor for two consecutive poses (initialize the new pose node - // with the curr_pose too) - bool loopClosureFound = true; - - addKeyPoseAndBetween(pose_counter_robot1_ - 1, pose_counter_robot1_, - relativeMotion, poseEstimate, robotID, cov, - loopClosureFound, loop_closure_relative_pose, - closure_matched_pose_idx); - pose_counter_robot1_++; - ROS_INFO_STREAM("Running factor-graph optimization, pose counter is: " - << pose_counter_robot1_); - solve(); - return true; - } -} +// bool SemanticFactorGraphWrapper::addLoopClosureObservation( +// const SE3 &relativeMotionSE3, const SE3 &poseEstimateSE3, +// const boost::array &cov, const SE3 &loop_closure_relative_pose, +// const size_t &closure_matched_pose_idx) { +// // default only have 1 aerial robot for active team localization +// gtsam::Pose3 relativeMotion(relativeMotionSE3.matrix()); +// gtsam::Pose3 poseEstimate(poseEstimateSE3.matrix()); +// int robotID = 0; +// bool optimize = false; + +// if (pose_counter_robot1_ == 0) { +// ROS_ERROR_STREAM( +// "ERROR: First pose should not be the loop closure pose!!!!!!!!!"); +// return false; +// } else { +// // add BetweenFactor for two consecutive poses (initialize the new pose node +// // with the curr_pose too) +// bool loopClosureFound = true; + +// addKeyPoseAndBetween(pose_counter_robot1_ - 1, pose_counter_robot1_, +// relativeMotion, poseEstimate, robotID, cov, +// loopClosureFound, loop_closure_relative_pose, +// closure_matched_pose_idx); +// pose_counter_robot1_++; +// ROS_INFO_STREAM("Running factor-graph optimization, pose counter is: " +// << pose_counter_robot1_); +// solve(); +// return true; +// } +// } bool SemanticFactorGraphWrapper::addSLOAMObservation( const CylinderMapManager &semanticMap, const CubeMapManager &cube_semantic_map, @@ -83,6 +84,7 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( const std::vector &scan_cubes_world, const std::vector &ellipse_matches, const std::vector &ellipses, const SE3 &relativeMotionSE3, + const std::array relativeMotionCov, const SE3 &poseEstimateSE3, const int &robotID, bool opt) { size_t pose_counter; @@ -92,7 +94,7 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( pose_counter = pose_counter_robot_[robotID]; // ROS_INFO_STREAM("################ ROBOT " << robotID << " pose counter: " // << "################"); - + if (pose_counter == 0) { // set priors for the first pose setPriors(curr_pose, robotID); @@ -100,10 +102,8 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( "ERROR: Factor graph optimization is done when adding the first " "pose prior, this may cause problems!!!"); } else { - // if cov is not specified, deafult covariance will be used, see - // noise_model_odom param in graph.cpp addKeyPoseAndBetween(pose_counter - 1, pose_counter, relativeMotion, - curr_pose, robotID); + relativeMotionCov, curr_pose, robotID); } const auto matchesMap = semanticMap.getMatchesMap(); const auto cubeMatchesMap = cube_semantic_map.getMatchesMap(); @@ -192,8 +192,8 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( // ROS_INFO_STREAM("pose counter for robotID " << robotID // << " is: " << pose_counter); - bool optimize = true; // updated logic: always optimize unless opt is set to false + bool optimize = true; if (optimize && opt) { // ROS_INFO_STREAM("Running factor graph optimization"); size_t num_factors = fgraph.size(); diff --git a/backend/sloam/src/factorgraph/graph_important.cpp b/backend/sloam/src/factorgraph/graph_important.cpp new file mode 100755 index 0000000..06023f7 --- /dev/null +++ b/backend/sloam/src/factorgraph/graph_important.cpp @@ -0,0 +1,663 @@ +/** +* This file is part of SlideSLAM +* +* Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari +* +* TODO: License information +* +*/ + +#include +#include +#include + +SemanticFactorGraph::SemanticFactorGraph() { + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 1; + isam_params.relinearizeThreshold = 0.1; + isam = new ISAM2(isam_params); + + // Get the start time + start_time_ = ros::Time::now(); + + // For all Pose3 Noise models note that the first three values + // correspond to RPY and the last three corresponds to XYZ. + // Proof from Frank Daellart here: + // https://github.com/borglab/gtsam/issues/205?utm_source=chatgpt.com + + // ================ First Pose Noise models ================ + noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); + + // ================= Loop Closure models ================= + // It is assumed to have 0.01 of the noise of the odometry + noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); + + // ================ Landmark Noise models ================ + + // TODO(xu:) update the cuboid measurement noise + // double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); + // double stddev_pos = 1.0; // 3.0; + // Vector9 noise_vec; + // noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, + // stddev_pos, stddev_pos, stddev_pos, stddev_pos; + + // TODO(xu:) update the cylinder measurement noise + noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); + + // For range and bearing (ellipsoid objects) measurements + double bearing_noise_std_temp = 1; + noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); +} + +void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, + const int &robotID) { + size_t idx = 0; + fgraph.addPrior(getSymbol(robotID, idx), pose_prior, + noise_model_prior_first_pose); + // here the insert also set the initial estimate for the optimization + fvalues.insert(getSymbol(robotID, idx), pose_prior); + ROS_INFO_STREAM("Setting prior for robot " << robotID + << " with pose: " << pose_prior); + + // only for active SLAM: + // -------------------------------- + // "fake" factor graph for information gain evaluation only + // fgraph_loop.addPrior(getSymbol(robotID, idx), pose_prior, + // noise_model_prior_first_pose); + // fvalues_loop.insert(getSymbol(robotID, idx), pose_prior); + // current_pose_global_ = pose_prior; + // -------------------------------- +} + +void SemanticFactorGraph::addKeyPoseAndBetween( + const size_t prevIdx, const size_t curIdx, const Pose3 &relativeMotion, + const std::array relativeMotionCov, + const Pose3 &poseEstimate, const int &robotID, const bool &loopClosureFound, + const SE3 &loop_closure_relative_pose, + const size_t &closure_matched_pose_idx) { + + // Calculate the covariance for the relative motion + Vector6 noise_vec; + if(use_odom_cov_in_nav_msg_) { + // Use the covariance provided + for (int i = 0; i < 6; ++i) { + // Swap from variance XYZRPY to standard deviation RPYXYZ + noise_vec(i) = std::sqrt(relativeMotionCov[(i + 3) % 6]); + } + } else { + // Scale covariance by travel distance + double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); + noise_vec = noise_model_odom_vec * relative_dist; + ROS_DEBUG_STREAM("Relative distance: " << relative_dist); + } + + // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance + fgraph.add(BetweenFactor( + getSymbol(robotID, prevIdx), getSymbol(robotID, curIdx), relativeMotion, + noiseModel::Diagonal::Sigmas(noise_vec))); + + // Only for active SLAM + // fgraph_loop.add(BetweenFactor( + // getSymbol(robotID,prevIdx), getSymbol(robotID,curIdx), + // relativeMotion, noiseModel::Diagonal::Sigmas(cur_noise_vec))); + + if (latest_to_idx_ < curIdx) { + latest_to_idx_ = curIdx; + } + + if (loopClosureFound) { + // add loop_closure_relative_pose to X(closure_matched_pose_idx) + gtsam::Pose3 est_loop_closure_pose; + + // get current estimate of X(closure_matched_pose_idx) + gtsam::Pose3 est_from_pose; + + if (closure_matched_pose_idx == 0) { + ROS_ERROR( + "ERROR: closure_matched_pose_idx is 0, this probably means this " + "param is not correctly set, check!!"); + } + bool cur_pose_valid = + getPose(closure_matched_pose_idx, robotID, est_from_pose); + if (!cur_pose_valid) { + ROS_ERROR_STREAM( + "ERROR: get closure_matched_pose_idx fail to fetch pose for pose_idx " + ": " + << closure_matched_pose_idx); + // here the insert also set the initial estimate for the optimization + fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); + // Only for active SLAM + // fvalues_loop.insert(getSymbol(robotID,curIdx), poseEstimate); + // current_pose_global_ = poseEstimate; + return; + } + + // TODO(xu:) check why est_from_pose is removed here, maybe because we are + // no longer treating loop closure as a prior, instead we are using between + // factor + // but we should still keep this since we use it as initial guess when + // adding the pose key to the factor graph + + // gtsam::Pose3 est_from_pose = + // fvalues.at(X(closure_matched_pose_idx)); multiply + // est_from_pose.matrix and loop_closure_relative_pose.matrix + // loop_closure_relative_pose will be used for adding loop closure between + // factor + auto est_pose_matrix = + est_from_pose.matrix() * loop_closure_relative_pose.matrix(); + est_loop_closure_pose = gtsam::Pose3(est_pose_matrix); + + ROS_INFO_STREAM("++++++++++++CLOSURE FACTOR BEING ADDED++++++++++++"); + ROS_INFO_STREAM("loop_closure_relative_pose" + << loop_closure_relative_pose.matrix()); + ROS_INFO_STREAM("est_loop_closure_pose" << est_loop_closure_pose.matrix()); + + // No longer treating loop closure as a prior. Instead, use it as a proper + // between factor add loop closure as a between factor + // loop_closure_relative_pose is from history loop closure key pose to + // current pose The correct interface for BetweenFactor is (pose1, + // pose2, H_pose2^pose1, noise) + fgraph.add( + BetweenFactor(getSymbol(robotID, closure_matched_pose_idx), + getSymbol(robotID, curIdx), + gtsam::Pose3(loop_closure_relative_pose.matrix()), + noise_model_closure)); + + // Only for active SLAM: + // -------------------------------- + // fgraph_loop.add(BetweenFactor( + // getSymbol(robotID, closure_matched_pose_idx), + // getSymbol(robotID,curIdx), + // gtsam::Pose3(loop_closure_relative_pose.matrix()), + // noise_model_closure)); + // -------------------------------- + + // here the insert also set the initial estimate for the optimization + fvalues.insert(getSymbol(robotID, curIdx), est_loop_closure_pose); + // Only for active SLAM: + // fvalues_loop.insert(getSymbol(robotID, curIdx),est_loop_closure_pose); + ROS_INFO_STREAM("loop_closure_relative_pose being added"); + // current_pose_global_ = est_loop_closure_pose; + } else { + // here the insert also set the initial estimate for the optimization + fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); + // Only for active SLAM + // fvalues_loop.insert(getSymbol(robotID, curIdx), poseEstimate); + // current_pose_global_ = poseEstimate; + } +} + +void SemanticFactorGraph::addPointLandmarkKey(const size_t ugvIdx, + const Point3 &ugv_position) { + fvalues.insert(U(ugvIdx), ugv_position); +} + +void SemanticFactorGraph::addRangeBearingFactor( + const size_t poseIdx, const size_t ugvIdx, + const Point3 &bearing_measurement, const double &range_measurement, + const int &robotID) { + // add bearing measurement with a covaraince + Unit3 bearing_measurement3D = Pose3().bearing(bearing_measurement); + // IMPORTANT: bearing vector should be expressed in body frame! + // ROS_INFO_STREAM_THROTTLE(1, + // "bearing vector should be expressed in body + // frame"); + + fgraph.add(BearingRangeFactor3D(getSymbol(robotID, poseIdx), U(ugvIdx), + bearing_measurement3D, range_measurement, + noise_model_bearing)); + // Only for active SLAM: + // fgraph_loop.add(BearingRangeFactor3D(X(poseIdx), + // U(ugvIdx), bearing_measurement3D, + // range_measurement, + // noise_model_bearing)); + if (latest_landmark_counter < ugvIdx) { + latest_landmark_counter = ugvIdx; + } +} + +void SemanticFactorGraph::addCylinderFactor(const size_t poseIdx, + const size_t cylIdx, + const Pose3 &pose, + const CylinderMeasurement &cylinder, + bool alreadyExists, + const int &robotID) { + // pose here is tf_map_to_sensor, not tf_sensor_to_map, hence it needs to be + // inverted + CylinderMeasurement c_local = cylinder.project(pose.inverse()); + fgraph.add(CylinderFactor(getSymbol(robotID, poseIdx), L(cylIdx), c_local, + noise_model_cylinder)); + if (!alreadyExists) { + fvalues.insert(L(cylIdx), cylinder); + } +} + +void SemanticFactorGraph::addCubeFactor( + const size_t poseIdx, const size_t cubeIdx, const Pose3 &pose, + const CubeMeasurement &cube_global_meas_raw, bool alreadyExists, + const int &robotID) { + // pose here is tf_map_to_sensor, not tf_sensor_to_map, hence it needs to be + // inverted + CubeMeasurement cube_global_meas = cube_global_meas_raw; + + // Temporarily handling the edge case of cuboid having yaw close to 0 or + // pi will have yaw values flipping / jumping back and forth between 0 and + // pi, which cause the optimization to break, hard coding it to be 0 for now + // Update: removed this since our process node can handle this properly + + CubeMeasurement cube_local_meas = cube_global_meas.project(pose.inverse()); + + // Calculate noise based on distance from the robot to the cube + double relative_dist = std::max(cube_local_meas.pose.translation().norm(), 0.1); + Vector9 noise_vec_scaled = noise_model_cube_vec * relative_dist; + + // Add the factor + fgraph.add(CubeFactor(getSymbol(robotID, poseIdx), C(cubeIdx), + cube_local_meas, noiseModel::Diagonal::Sigmas(noise_vec_scaled))); + + if (cube_local_meas.pose.translation().norm() > 50) { + ROS_WARN_THROTTLE(1, "cube_local_meas.pose.translation().norm() is larger " + "than 25 meters, maybe it is due to the front end keeping " + "track of observations over a long time or maybe it is because " + " the robot is moving fast!!"); + } + + if (!alreadyExists) { + // Initialize with first measurement of cube (in fixed world frame) + fvalues.insert(C(cubeIdx), cube_global_meas); + } +} + +void SemanticFactorGraph::addLoopClosureFactor(const Pose3 poseRelative, + const size_t prevIdx, + const size_t robotID1, + const size_t curIdx, + const size_t robotID2) { + // Correct interface for BetweenFactor(pose1, pose2, H_pose2^pose1, + // noise) poseRelative is H_query^candidate, i.e., from query pose to + // candidate (history) pose candidate is robotID1, prevIdx query is robotID2, + // curIdx + fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), + getSymbol(robotID2, curIdx), poseRelative, + noise_model_closure)); +} + +void SemanticFactorGraph::addRelativeMeasFactor(const Pose3 poseRelative, const size_t prevIdx, + const size_t robotID1, const size_t curIdx, const size_t robotID2) { + + // Scale noise by distance of the relative measurement + double relative_dist = std::max(poseRelative.translation().norm(), noise_floor); + Vector6 noise_vec_scaled = noise_model_rel_meas_vec * relative_dist; + + // Add the factor + fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), + getSymbol(robotID2, curIdx), poseRelative, + noiseModel::Diagonal::Sigmas(noise_vec_scaled))); +} + +void SemanticFactorGraph::solve() { + // ROS_INFO_STREAM("START SOLVING THE FACTOR GRAPH OPTIMIZATION PROBLEM"); + isam->update(fgraph, fvalues); + // Only for active SLAM: + // isam_loop->update(fgraph_loop, fvalues_loop); + + // Extract the result/current estimates + currEstimate = isam->calculateEstimate(); + + // Reset the newFactors and newValues list + fgraph.resize(0); + fvalues.clear(); +} + +CylinderMeasurement SemanticFactorGraph::getCylinder(const int idx) { + return currEstimate.at(L(idx)); +} + +CubeMeasurement SemanticFactorGraph::getCube(const int idx) { + return currEstimate.at(C(idx)); +} + +Point3 SemanticFactorGraph::getCentroidLandmark(const int idx) { + if (isam->valueExists(U(idx))) { + return currEstimate.at(U(idx)); + } else { + return Point3(); + } +} + +bool SemanticFactorGraph::getPose(const size_t idx, const int &robotID, + Pose3 &poseOut) { + if (robotID >= 0 || robotID < MAX_NUM_ROBOTS) { + if (isam->valueExists(getSymbol(robotID, idx))) { + poseOut = currEstimate.at(getSymbol(robotID, idx)); + return true; + } else { + printf( + "############# Error!!! Node of interest does not exist in factor " + "graph #############\n"); + std::cout << "the node is X(" << idx << ")" << std::endl; + poseOut = Pose3(); + return false; + } + // return currEstimate.at(getSymbol(robotID, idx)); + } else { + ROS_ERROR_STREAM("############# Error: " + << robotID << " is an invalid robotID !!! #############"); + // printf("############# Error: invalid robotID!!! #############\n"); + poseOut = Pose3(); + return false; + } +} + +Eigen::MatrixXd SemanticFactorGraph::getPoseCovariance(const int idx, + const int &robotID) { + if (robotID >= 0 || robotID < MAX_NUM_ROBOTS) { + return isam->marginalCovariance(getSymbol(robotID, idx)); + } else { + ROS_ERROR_STREAM("############# Error: " + << robotID << " is an invalid robotID !!! #############"); + return isam->marginalCovariance(getSymbol(robotID, idx)); + } +} + +Symbol SemanticFactorGraph::getSymbol(const int &robotID, const int idx) { + Symbol returned_symbol; + switch (robotID) { + case 0: + returned_symbol = X(idx); + break; + case 1: + returned_symbol = Y(idx); + break; + case 2: + returned_symbol = Z(idx); + break; + case 3: + returned_symbol = M(idx); + break; + case 4: + returned_symbol = N(idx); + break; + case 5: + returned_symbol = O(idx); + break; + case 6: + returned_symbol = P(idx); + break; + case 7: + returned_symbol = Q(idx); + break; + case 8: + returned_symbol = R(idx); + break; + case 9: + returned_symbol = S(idx); + break; + case 10: + returned_symbol = T(idx); + break; + case 11: + returned_symbol = V(idx); + break; + case 12: + returned_symbol = W(idx); + break; + default: + break; + } + return returned_symbol; +} + +Symbol SemanticFactorGraph::getSymbol(const int &robotID, const size_t idx) { + Symbol returned_symbol; + switch (robotID) { + case 0: + returned_symbol = X(idx); + break; + case 1: + returned_symbol = Y(idx); + break; + case 2: + returned_symbol = Z(idx); + break; + case 3: + returned_symbol = M(idx); + break; + case 4: + returned_symbol = N(idx); + break; + case 5: + returned_symbol = O(idx); + break; + case 6: + returned_symbol = P(idx); + break; + case 7: + returned_symbol = Q(idx); + break; + case 8: + returned_symbol = R(idx); + break; + case 9: + returned_symbol = S(idx); + break; + case 10: + returned_symbol = T(idx); + break; + case 11: + returned_symbol = V(idx); + break; + case 12: + returned_symbol = W(idx); + break; + default: + break; + } + return returned_symbol; +} + +// ONLY FOR ACTIVE SLAM, ALL FUNCTIONS BELOW +// ----------------------------------------------------- +// void SemanticFactorGraph::logEntropy() { +// // keep current covaraince matrix +// double sum_entropy_pose = 0.0; +// double sum_entropy_landmark = 0.0; +// size_t num_valid_poses = 0; +// size_t num_valid_landmarks = 0; +// // iterate through all latest_landmark_counter and latest_to_idx_ +// for (size_t i = 0; i < latest_to_idx_; i++) { +// // get the covariance matrix of the current pose +// if (isam->valueExists(X(i)) && isam_loop->valueExists(X(i))) { +// gtsam::Matrix cov = isam->marginalCovariance(X(i)); +// sum_entropy_pose += cov.trace(); +// num_valid_poses++; +// } +// } + +// // ROS_ERROR_STREAM("landmark marginal cov printing, num of landark is " +// // << latest_landmark_counter); +// for (size_t i = 0; i < latest_landmark_counter; i++) { +// // get the covariance matrix of the current pose +// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { +// // ROS_ERROR_STREAM("getting landmark cov "); +// gtsam::Matrix cov = isam->marginalCovariance(U(i)); +// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); +// sum_entropy_landmark += cov.trace(); +// num_valid_landmarks++; +// } +// } + +// std::ofstream entropy_log_file("/home/yuezhan/bags/entropy_log.txt", +// ios::app); if (entropy_log_file.fail()){ +// cout << "open file error!\n"; +// } +// ros::Time time_now = ros::Time::now(); +// // write a header for entropy log file if it is empty, header is TIME, +// ENTROPY_POSE, ENTROPY_LANDMARK, NUM_VALID_POSES if +// (entropy_log_file.tellp() == 0) { +// entropy_log_file << "TIME, ENTROPY_POSE, ENTROPY_LANDMARK, +// NUM_VALID_POSES, NUM_VALID_LANDMARKS" << std::endl; +// } +// entropy_log_file << (time_now-start_time_).toSec() << ", " << +// sum_entropy_pose << ", " << sum_entropy_landmark << ", " << num_valid_poses +// << ", " << num_valid_landmarks << std::endl; entropy_log_file.close(); + +// } + +// double SemanticFactorGraph::estimateClosureInfoGain( +// const std::vector &candidateTrajPoseIndices, +// const std::vector &travel_distances) { +// // candidateTrajPoseIndices is a vector of pose indices in the candidate +// // trajecotry from current pose to key pose 1, key pose 2, ..., key pose n + +// // travel_distances is a vector of travel distances between each pair of +// poses + +// // assert length of travel_distances is one less than length of +// // candidateTrajPoseIndices +// assert(travel_distances.size() == candidateTrajPoseIndices.size() - 1); + +// // safety check +// for (size_t i = 0; i < candidateTrajPoseIndices.size(); i++) { +// size_t thisPoseIdx = candidateTrajPoseIndices[i]; +// if (currEstimate.exists(X(thisPoseIdx)) == false) { +// ROS_ERROR_STREAM("current pose index is not in the graph, error!"); +// return -1; +// } +// } + +// NonlinearFactorGraph fgraph_loop_tmp; +// // iterate through all poses in the candidate trajectory +// for (size_t i = 0; i < candidateTrajPoseIndices.size() - 1; i++) { +// size_t currentPoseIdx = candidateTrajPoseIndices[i]; +// size_t keyPoseIdx = candidateTrajPoseIndices[i + 1]; +// auto motion_noise = noiseModel::Diagonal::Sigmas( +// noise_model_pose_vec_per_m * travel_distances[i]); + +// // add between factor between current pose and history pose +// Pose3 pose = currEstimate.at(X(currentPoseIdx)); +// Pose3 pose_history = currEstimate.at(X(keyPoseIdx)); +// // TODO: flip order of pose_history and pose, either way should work +// though Pose3 pose_relative = pose_history.between(pose); +// fgraph_loop_tmp.add(BetweenFactor(X(keyPoseIdx), +// X(currentPoseIdx), +// pose_relative, motion_noise)); +// } + +// // add fake loop factor +// ISAM2Result result = isam_loop->update(fgraph_loop_tmp); +// FactorIndices factor_ids = result.newFactorsIndices; + +// result.print(); + +// // keep current covaraince matrix +// double sum_entropy_pose = 0.0; +// double sum_entropy_fake_loop_pose = 0.0; +// double sum_entropy_landmark = 0.0; +// double sum_entropy_fake_loop_landmark = 0.0; +// // iterate through all latest_landmark_counter and latest_to_idx_ +// for (size_t i = 0; i < latest_to_idx_; i++) { +// // get the covariance matrix of the current pose +// gtsam::Matrix cov = isam->marginalCovariance(X(i)); +// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(X(i)); +// sum_entropy_pose += cov.trace(); +// sum_entropy_fake_loop_pose += cov_loop.trace(); +// } + +// // ROS_ERROR_STREAM("landmark marginal cov printing, num of landark is " +// // << latest_landmark_counter); +// for (size_t i = 0; i < latest_landmark_counter; i++) { +// // get the covariance matrix of the current pose +// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { +// // ROS_ERROR_STREAM("getting landmark cov "); +// gtsam::Matrix cov = isam->marginalCovariance(U(i)); +// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); +// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(U(i)); +// sum_entropy_landmark += cov.trace(); +// sum_entropy_fake_loop_landmark += cov_loop.trace(); +// // ROS_ERROR_STREAM("getting landmark cov ends "); +// } +// } +// // ROS_ERROR_STREAM("landmark marginal cov printing ends"); + +// double info_gain_pose = sum_entropy_pose - sum_entropy_fake_loop_pose; +// double info_gain_landmark = +// sum_entropy_landmark - sum_entropy_fake_loop_landmark; + +// ROS_ERROR_STREAM("sum_entropy_pose: " << sum_entropy_pose); +// ROS_ERROR_STREAM( +// "sum_entropy_fake_loop_pose: " << sum_entropy_fake_loop_pose); +// ROS_ERROR_STREAM("sum_entropy_landmark: " << sum_entropy_landmark); +// ROS_ERROR_STREAM( +// "sum_entropy_fake_loop_landmark: " << sum_entropy_fake_loop_landmark); +// ROS_ERROR_STREAM("info_gain_pose: " << info_gain_pose); +// ROS_ERROR_STREAM("info_gain_landmark: " << info_gain_landmark); + +// // remove fake loop factor +// ISAM2Result result_after_delete = +// isam_loop->update(NonlinearFactorGraph(), Values(), factor_ids); + +// bool sanity_check = true; +// // sanity check the covariance matrix +// if (sanity_check) { +// // keep current covaraince matrix +// sum_entropy_pose = 0.0; +// sum_entropy_fake_loop_pose = 0.0; +// sum_entropy_landmark = 0.0; +// sum_entropy_fake_loop_landmark = 0.0; +// // iterate through all latest_landmark_counter and latest_to_idx_ +// for (size_t i = 0; i < latest_to_idx_; i++) { +// // get the covariance matrix of the current pose +// if (isam->valueExists(X(i)) && isam_loop->valueExists(X(i))) { +// gtsam::Matrix cov = isam->marginalCovariance(X(i)); +// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(X(i)); +// sum_entropy_pose += cov.trace(); +// sum_entropy_fake_loop_pose += cov_loop.trace(); +// } +// } + +// for (size_t i = 0; i < latest_landmark_counter; i++) { +// // get the covariance matrix of the current pose +// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { +// gtsam::Matrix cov = isam->marginalCovariance(U(i)); +// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); +// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(U(i)); +// sum_entropy_landmark += cov.trace(); +// sum_entropy_fake_loop_landmark += cov_loop.trace(); +// } +// // else { +// // ROS_ERROR_STREAM("landmark marginal cov for landmark # " +// // << i << "not exist"); +// // } +// } +// double info_gain_pose = sum_entropy_pose - sum_entropy_fake_loop_pose; +// double info_gain_landmark = +// sum_entropy_landmark - sum_entropy_fake_loop_landmark; +// ROS_ERROR_STREAM( +// "++++++++++++++++++++++++++++++++++++++++SANITY " +// "CHECK+++++++++++++++++++++++"); + +// // ROS_ERROR_STREAM("sum_entropy_pose: " << sum_entropy_pose); +// // ROS_ERROR_STREAM( +// // "sum_entropy_fake_loop_pose: " << sum_entropy_fake_loop_pose); +// // ROS_ERROR_STREAM("sum_entropy_landmark: " << sum_entropy_landmark); +// // ROS_ERROR_STREAM( +// // "sum_entropy_fake_loop_landmark: " << +// // sum_entropy_fake_loop_landmark); +// ROS_ERROR_STREAM( +// "info_gain_pose difference (should be 0): " << info_gain_pose); +// ROS_ERROR_STREAM( +// "info_gain_landmark difference (should be 0): " << +// info_gain_landmark); +// bool is_equal = isam->equals(*isam_loop); +// ROS_ERROR_STREAM("two factor graphs should be equal, are they? >> " +// << is_equal); +// ROS_ERROR_STREAM( +// "++++++++++++++++++++++++++++++++++++++++SANITY " +// "CHECK+++++++++++++++++++++++"); +// } +// // total weighted info gain +// double total_info_gain = 10.0 * info_gain_pose + info_gain_landmark; +// return total_info_gain; +// } +// ----------------------------------------------------------------------- diff --git a/backend/sloam_msgs/msg/PoseMst.msg b/backend/sloam_msgs/msg/PoseMst.msg index 4ea02f1..bb0eb42 100755 --- a/backend/sloam_msgs/msg/PoseMst.msg +++ b/backend/sloam_msgs/msg/PoseMst.msg @@ -1,5 +1,6 @@ geometry_msgs/Pose pose # 56 bytes geometry_msgs/Pose relativeRawOdom # 56 bytes +float64[6] relativeRawOdomCov # 48 bytes time stamp # 8 bytes (theorized, but not proven) ROSCube[] cubes # 69 bytes each ROSCylinder[] cylinders # 37 bytes each diff --git a/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh index a415380..74c9fd2 100755 --- a/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh +++ b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh @@ -48,7 +48,7 @@ tmux split-window -h -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:1.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch relative_meas_gen odomRelativeMeasSync.launch" Enter tmux select-pane -t $SESSION_NAME:1.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play stuff_world-two_jackal_amcl.bag --clock -r $BAG_PLAY_RATE -s 0 --topic /jackal0/odometry/local_filtered /jackal1/odometry/local_filtered /gazebo/model_states /jackal0/odometry/local_filtered:=/robot0/odometry/local_filtered /jackal1/odometry/local_filtered:=/robot1/odometry/local_filtered" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 18; cd $BAG_DIR && rosbag play stuff_world-two_jackal_amcl.bag --clock -r $BAG_PLAY_RATE -s 0 --topic /jackal0/odometry/local_filtered /jackal1/odometry/local_filtered /gazebo/model_states /jackal0/odometry/local_filtered:=/robot0/odometry/local_filtered /jackal1/odometry/local_filtered:=/robot1/odometry/local_filtered" Enter tmux select-pane -t $SESSION_NAME:1.2 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch relative_meas_gen multiUGVToSlideSLAM.launch publish_for_rviz:=true" Enter tmux select-pane -t $SESSION_NAME:1.3 @@ -69,11 +69,11 @@ tmux split-window -v -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:2.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter tmux select-pane -t $SESSION_NAME:2.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=0 odom_topic:=/robot0/odometry/local_filtered enable_rviz:=true turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=0 odom_topic:=/robot0/odometry/local_filtered enable_rviz:=true turn_off_rel_inter_robot_factor:=true param_file:=sloam_multiUGV.yaml" Enter tmux select-pane -t $SESSION_NAME:2.2 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter tmux select-pane -t $SESSION_NAME:2.3 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=1 odom_topic:=/robot1/odometry/local_filtered turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=1 odom_topic:=/robot1/odometry/local_filtered turn_off_rel_inter_robot_factor:=true param_file:=sloam_multiUGV.yaml" Enter # Add window to easily kill all processes tmux new-window -t $SESSION_NAME -n "Kill" diff --git a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py index 7da6f41..5dc0d61 100755 --- a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py +++ b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py @@ -86,16 +86,18 @@ def create_marker_for_cuboid(self, model: ET.Element, robot_name: str, robot_pos # Transform pose into the relative frame of the robots cuboid.pose = transforms.calculate_relative_pose(robot_pose, pose) - # Add 0.5 m and 1 degree/0.017 rad (per unit distance) Gaussian noise to the cuboid pose + # Add 0.05 m and 1 degree/0.017 rad (per unit distance) Gaussian noise to the cuboid pose relative_dist = np.sqrt(cuboid.pose.position.x**2 + cuboid.pose.position.y**2 + cuboid.pose.position.z**2) - cuboid.pose = transforms.add_zero_mean_gaussian_noise_to_pose(cuboid.pose, 0.5 * relative_dist, 0.017 * relative_dist) + #if relative_dist > 5: + # return None # Return None if the cuboid isn't within 5 meters of the robot + cuboid.pose = transforms.add_zero_mean_gaussian_noise_to_pose(cuboid.pose, 0.05 * relative_dist, 0.017 * relative_dist) - # Extract the scale of the cuboid and add 0.05 m Gaussian noise (per unit distance) + # Extract the scale of the cuboid and add 0.01 m Gaussian noise (per unit distance) scale_vals = model.find('link').find('collision').find('geometry').find('box').find('size').text.strip().split(' ') - noise = np.random.normal(0, 0.05 * relative_dist, 3) - cuboid.scale.x = float(scale_vals[0] + noise[0]) - cuboid.scale.y = float(scale_vals[1] + noise[1]) - cuboid.scale.z = float(scale_vals[2] + noise[2]) + noise = np.random.normal(0, 0.01 * relative_dist, 3) + cuboid.scale.x = float(scale_vals[0]) + noise[0] + cuboid.scale.y = float(scale_vals[1]) + noise[1] + cuboid.scale.z = float(scale_vals[2]) + noise[2] # Set other values cuboid.lifetime = rospy.Duration(0) # 0 means forever @@ -126,10 +128,10 @@ def gazebo_callback(self, msg: ModelStates): # Calculate the relative measurement from jackal0 to jackal1 relative_meas: Pose = transforms.calculate_relative_pose(jackal0_pose, jackal1_pose) - # Add 0.06 m and 0.1 degree/0.0017 rad (per unit distance) Gaussian noise to the relative measurement + # Add 0.03 m and 0.06 degree/0.001 rad (per unit distance) Gaussian noise to the relative measurement # Current inspiration for this is Fig. 5 & 6 of AprilTag 2 paper. relative_dist = np.sqrt(relative_meas.position.x**2 + relative_meas.position.y**2 + relative_meas.position.z**2) - relative_meas = transforms.add_zero_mean_gaussian_noise_to_pose(relative_meas, 0.06 * relative_dist, 0.0017 * relative_dist) + relative_meas = transforms.add_zero_mean_gaussian_noise_to_pose(relative_meas, 0.03 * relative_dist, 0.001 * relative_dist) # Create the header for the messages header = Header() @@ -170,8 +172,12 @@ def gazebo_callback(self, msg: ModelStates): for model in world.findall('model'): name = model.get('name') if "_box_" in name: - jackal0_cuboids.markers.append(self.create_marker_for_cuboid(model, "jackal0", jackal0_pose)) - jackal1_cuboids.markers.append(self.create_marker_for_cuboid(model, "jackal1", jackal1_pose)) + jackal0_box = self.create_marker_for_cuboid(model, "jackal0", jackal0_pose) + jackal1_box = self.create_marker_for_cuboid(model, "jackal1", jackal1_pose) + if jackal0_box is not None: + jackal0_cuboids.markers.append(jackal0_box) + if jackal1_box is not None: + jackal1_cuboids.markers.append(jackal1_box) # Create the StampedRvizMarkerArray messages jackal0_array = StampedRvizMarkerArray() diff --git a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py index e939339..d100db9 100644 --- a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py +++ b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py @@ -52,7 +52,7 @@ def add_zero_mean_gaussian_noise_to_pose(pose: Pose, stddev_translation: float, # Extract quaternion and convert to euler angles q = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) - r = Rotation.from_quat(q, scalar_first=False) + r = Rotation.from_quat(q) euler_angles = r.as_euler('xyz', degrees=False) # Add rotation noise From 8852651d590b463cdb671012b73070cd1f41b4c6 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 3 Apr 2025 15:47:34 +0000 Subject: [PATCH 32/74] Consistent run files & comment tweak --- docker/run_container.sh | 6 +++--- docker/run_container_cam.sh | 4 ++-- docker/run_container_dbutter.sh | 2 +- frontend/relative_meas_gen/CMakeLists.txt | 16 ---------------- 4 files changed, 6 insertions(+), 22 deletions(-) diff --git a/docker/run_container.sh b/docker/run_container.sh index 0d80f29..538ac75 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -2,8 +2,7 @@ SlideSlamWs="$WS_PATH" # point to your workspace directory SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory -# xhost +local:root # for the lazy and reckless -docker run \ +docker run -it \ --name="slideslam_ros" \ --net="host" \ --privileged \ @@ -11,13 +10,14 @@ docker run \ --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ --env="QT_X11_NO_MITSHM=1" \ - --env="XAUTHORITY=$XAUTH" \ + --env="XAUTHORITY=/root/.Xauthority" \ --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ --volume="$BAGS_DIR:/opt/bags" \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ + --volume="$XAUTHORITY:/root/.Xauthority:ro" \ slideslam_ros_image \ bash diff --git a/docker/run_container_cam.sh b/docker/run_container_cam.sh index bb542a9..a778b2b 100755 --- a/docker/run_container_cam.sh +++ b/docker/run_container_cam.sh @@ -2,7 +2,6 @@ SlideSlamWs="/home/cam/Documents/slideslam_docker_ws" # point to your workspace SlideSlamCodeDir="/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR='/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/bags' # point to your bags / data directory -xhost +local:root # for the lazy and reckless docker run -it \ --name="slideslam_ros" \ --net="host" \ @@ -11,13 +10,14 @@ docker run -it \ --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ --env="QT_X11_NO_MITSHM=1" \ - --env="XAUTHORITY=$XAUTH" \ + --env="XAUTHORITY=/root/.Xauthority" \ --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ --volume="$BAGS_DIR:/opt/bags" \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ + --volume="$XAUTHORITY:/root/.Xauthority:ro" \ slideslam_ros_image \ bash diff --git a/docker/run_container_dbutter.sh b/docker/run_container_dbutter.sh index 10be7ca..f526b85 100755 --- a/docker/run_container_dbutter.sh +++ b/docker/run_container_dbutter.sh @@ -17,7 +17,7 @@ docker run -it \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ - --volume="$XAUTHORITY:/root/.Xauthority:ro" + --volume="$XAUTHORITY:/root/.Xauthority:ro" \ slideslam_ros_image \ bash diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index d4020bf..ad2a401 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -46,22 +46,6 @@ catkin_python_setup() ## Declare ROS messages, services and actions ## ################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -# ) - ## Generate services in the 'srv' folder # add_service_files( # FILES From f8eec016ab72a0824d93ac99060fdaac95ae1236 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 19:58:05 -0400 Subject: [PATCH 33/74] github actions test 4/7 17:57 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- docker/run_container.sh | 13 +++++++++++-- frontend/relative_meas_gen/CMakeLists.txt | 2 +- frontend/relative_meas_gen/tests/testMultiUGV.py | 1 - 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 4c22eee..b575d35 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -45,4 +45,4 @@ jobs: - name: Run SLIDE_SLAM Tests run: | - docker exec -it slideslam_ros sh -c "catkin_make run_tests" \ No newline at end of file + docker exec slideslam_ros sh -c "catkin_make run_tests" \ No newline at end of file diff --git a/docker/run_container.sh b/docker/run_container.sh index 538ac75..f30f207 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -2,7 +2,14 @@ SlideSlamWs="$WS_PATH" # point to your workspace directory SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory -docker run -it \ +DOCKER_RUN_FLAGS="" + +# If not running in CI, add -it +if [ -z "$CI" ]; then + DOCKER_RUN_FLAGS="-it" +fi + +docker run $DOCKER_RUN_FLAGS \ --name="slideslam_ros" \ --net="host" \ --privileged \ @@ -17,7 +24,9 @@ docker run -it \ --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ - --volume="$XAUTHORITY:/root/.Xauthority:ro" \ + if [ -n "$XAUTHORITY" ]; then + DOCKER_ARGS="$DOCKER_ARGS -v $XAUTHORITY:/root/.Xauthority:ro" + fi slideslam_ros_image \ bash diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index ad2a401..92be1c4 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -200,7 +200,7 @@ target_link_libraries(apriltag_meas_gen ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apr if (CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roscpp rostest) - catkin_add_gtest(apriltag_test test/apriltag_test.cpp src/apriltag_wrapper.cpp) + catkin_add_gtest(apriltag_test tests/apriltag_test.cpp src/apriltag_wrapper.cpp) target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES}) endif() diff --git a/frontend/relative_meas_gen/tests/testMultiUGV.py b/frontend/relative_meas_gen/tests/testMultiUGV.py index 279c5d3..bc1b666 100755 --- a/frontend/relative_meas_gen/tests/testMultiUGV.py +++ b/frontend/relative_meas_gen/tests/testMultiUGV.py @@ -39,4 +39,3 @@ def test_calculate_relative_pose(self): if __name__ == '__main__': import rostest rostest.rosrun('relative_meas_gen', 'test_multi_ugv', TestMultiUGV) - rostest.rosrun('relative_meas_gen', 'apriltag_test', 'Detect') \ No newline at end of file From f41ddc570d66b20f75b0374e1b35fa5986aa19d5 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 20:06:24 -0400 Subject: [PATCH 34/74] github actions test 4/7 20:06 --- docker/run_container.sh | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/docker/run_container.sh b/docker/run_container.sh index f30f207..5ba2a3b 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -9,6 +9,16 @@ if [ -z "$CI" ]; then DOCKER_RUN_FLAGS="-it" fi +EXTRA_VOLUMES="" +if [ -z "$CI" ]; then + EXTRA_VOLUMES+=" --env=\"DISPLAY=$DISPLAY\"" + EXTRA_VOLUMES+=" --env=\"QT_X11_NO_MITSHM=1\"" + EXTRA_VOLUMES+=" --env=\"XAUTHORITY=/root/.Xauthority\"" + EXTRA_VOLUMES+=" --volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\"" + EXTRA_VOLUMES+=" --volume=\"/home/$USER/.bash_aliases:/root/.bash_aliases\"" + EXTRA_VOLUMES+=" --volume=\"$XAUTHORITY:/root/.Xauthority:ro\"" +fi + docker run $DOCKER_RUN_FLAGS \ --name="slideslam_ros" \ --net="host" \ @@ -16,17 +26,11 @@ docker run $DOCKER_RUN_FLAGS \ --gpus="all" \ --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --env="XAUTHORITY=/root/.Xauthority" \ --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ --volume="$BAGS_DIR:/opt/bags" \ - --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --volume="/home/$USER/repos:/home/$USER/repos" \ - if [ -n "$XAUTHORITY" ]; then - DOCKER_ARGS="$DOCKER_ARGS -v $XAUTHORITY:/root/.Xauthority:ro" - fi + $EXTRA_VOLUMES \ slideslam_ros_image \ bash From 58fc7be6bfc1daaa19771ca50670face1b8a3e99 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 20:13:50 -0400 Subject: [PATCH 35/74] github actions test 4/7 8:13 --- docker/run_container.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/run_container.sh b/docker/run_container.sh index 5ba2a3b..0779df3 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -11,6 +11,7 @@ fi EXTRA_VOLUMES="" if [ -z "$CI" ]; then + EXTRA_VOLUMES+=" --gpus=\"all\"" EXTRA_VOLUMES+=" --env=\"DISPLAY=$DISPLAY\"" EXTRA_VOLUMES+=" --env=\"QT_X11_NO_MITSHM=1\"" EXTRA_VOLUMES+=" --env=\"XAUTHORITY=/root/.Xauthority\"" @@ -23,7 +24,6 @@ docker run $DOCKER_RUN_FLAGS \ --name="slideslam_ros" \ --net="host" \ --privileged \ - --gpus="all" \ --workdir="/opt/slideslam_docker_ws" \ --env="DISPLAY=$DISPLAY" \ --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ From 5c6732eba9ff6537c7ab021777dce7500b18a51b Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 20:51:01 -0400 Subject: [PATCH 36/74] github actions test 4/7 8:50 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- docker/run_container.sh | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index b575d35..d968b56 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -37,7 +37,7 @@ jobs: cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" chmod +x run_container.sh - ./run_container.sh --no-gpu + ./run_container.sh - name: Build SLIDE_SLAM inside Container run: | diff --git a/docker/run_container.sh b/docker/run_container.sh index 0779df3..f4ba0b8 100755 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -2,7 +2,7 @@ SlideSlamWs="$WS_PATH" # point to your workspace directory SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory -DOCKER_RUN_FLAGS="" +DOCKER_RUN_FLAGS="-d" # If not running in CI, add -it if [ -z "$CI" ]; then @@ -32,5 +32,5 @@ docker run $DOCKER_RUN_FLAGS \ --volume="/home/$USER/repos:/home/$USER/repos" \ $EXTRA_VOLUMES \ slideslam_ros_image \ - bash + bash -c "while true; do sleep 60; done" From 2177e19af751a59231aa16f265fe77a612b21246 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 20:57:59 -0400 Subject: [PATCH 37/74] github actions test 4/7 8:57 --- .github/workflows/SLIDE_SLAM_test.yml | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index d968b56..f82ca09 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -39,10 +39,8 @@ jobs: chmod +x run_container.sh ./run_container.sh - - name: Build SLIDE_SLAM inside Container - run: | - docker exec slideslam_ros sh -c "catkin_make && source devel/setup.bash" docker exec slideslam_ros sh -c "source devel/setup.bash" - - - name: Run SLIDE_SLAM Tests + - name: Build SLIDE_SLAM inside Container and run tests run: | + docker start slideslam_ros + docker exec slideslam_ros sh -c "catkin_make && source devel/setup.bash" docker exec slideslam_ros sh -c "catkin_make run_tests" \ No newline at end of file From da9c53698be2d00ac1877ce39d6b0d36a0dadae1 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 21:03:51 -0400 Subject: [PATCH 38/74] github actions test 4/7 9:03 --- .github/workflows/SLIDE_SLAM_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index f82ca09..cc17242 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,5 +42,5 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker exec slideslam_ros sh -c "catkin_make && source devel/setup.bash" - docker exec slideslam_ros sh -c "catkin_make run_tests" \ No newline at end of file + docker exec slideslam_ros sh -c "cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" + docker exec slideslam_ros sh -c "cd /opt/slideslam_docker_ws && catkin_make run_tests" \ No newline at end of file From 7957d6c4c1bf2c2a74d861bbbc0ff5c660f983e2 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 21:23:22 -0400 Subject: [PATCH 39/74] github actions test 4/7 9:23 --- .github/workflows/SLIDE_SLAM_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index cc17242..f552bcd 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,5 +42,5 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker exec slideslam_ros sh -c "cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" - docker exec slideslam_ros sh -c "cd /opt/slideslam_docker_ws && catkin_make run_tests" \ No newline at end of file + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" + docker exec slideslam_ros bash -c "cd /opt/slideslam_docker_ws && catkin_make run_tests" \ No newline at end of file From 04f9a84ef12e4091653b52003823a244b40159a3 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Mon, 7 Apr 2025 21:29:33 -0400 Subject: [PATCH 40/74] github actions test 4/7 9:29 --- .github/workflows/SLIDE_SLAM_test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index f552bcd..8c13395 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,5 +42,6 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "cd /opt/slideslam_docker_ws && catkin_make run_tests" \ No newline at end of file From a183fce9612219e933155d27a736de507401b5ea Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 14:38:49 -0400 Subject: [PATCH 41/74] github actions test 4/8 2:38 --- .github/workflows/SLIDE_SLAM_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 8c13395..03a95b9 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -43,5 +43,5 @@ jobs: run: | docker start slideslam_ros docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" - docker exec slideslam_ros bash -c "cd /opt/slideslam_docker_ws && catkin_make run_tests" \ No newline at end of file + docker exec slideslam_ros bash -c "cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests" From f82aa207c76e93b9f7fa56a8b94b9aa78a0367c0 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 14:45:44 -0400 Subject: [PATCH 42/74] github actions test 4/8 2:45 --- .github/workflows/SLIDE_SLAM_test.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 03a95b9..6bd81d4 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -41,7 +41,8 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | + - name: Build SLIDE_SLAM inside Container and run tests docker start slideslam_ros docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" - docker exec slideslam_ros bash -c "cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests" From bda2c0cf272f1c9bcaa458916daa14dd21cc2171 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 14:53:32 -0400 Subject: [PATCH 43/74] github actions test 4/8 2:53 --- .github/workflows/SLIDE_SLAM_test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 6bd81d4..e6db148 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -41,7 +41,6 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | - - name: Build SLIDE_SLAM inside Container and run tests docker start slideslam_ros docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" From da2cf809a24c48459b22cc47fd8952fbee598c50 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 15:00:05 -0400 Subject: [PATCH 44/74] github actions test 4/8 3:00 --- frontend/relative_meas_gen/tests/apriltag_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index d71420c..125fb18 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -30,7 +30,7 @@ TEST(apriltag_test, Detect) { std::cout << "Detected tag: " << test_tag.id << std::endl; std::cout << "Expected tag: 6" << std::endl; - EXPECT_EQ(6, test_tag.id); + EXPECT_EQ(5, test_tag.id); } From 5d33798734761e49e1678d79d6f8afd2c9f02084 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 15:05:58 -0400 Subject: [PATCH 45/74] github actions test 4/8 3:05 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index e6db148..c36c4ae 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -44,4 +44,4 @@ jobs: docker start slideslam_ros docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From cd70ecab09730366750fde93a1f48deff3a218d3 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 15:21:07 -0400 Subject: [PATCH 46/74] github actions test 4/8 3:21 --- .github/workflows/SLIDE_SLAM_test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index c36c4ae..a462e59 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,6 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros + docker cp ./src slideslam_ros:/opt/slideslam_docker_ws/ docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 3334ac9ab50ce6debc4853e0372cc5fa211f329c Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Tue, 8 Apr 2025 15:25:06 -0400 Subject: [PATCH 47/74] 4/8 5:25 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index a462e59..9c4f9b1 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp ./src slideslam_ros:/opt/slideslam_docker_ws/ + docker cp . slideslam_ros:/opt/slideslam_docker_ws/ docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From cdb2f5d6fae9f7ac02dd5fbc17166af9eff06c6a Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Tue, 8 Apr 2025 20:58:25 +0000 Subject: [PATCH 48/74] Remove unfinished covariance code, test case for utils.h --- backend/sloam/CMakeLists.txt | 5 +- backend/sloam/include/core/databaseManager.h | 1 - backend/sloam/include/core/input.h | 6 +- backend/sloam/include/core/robot.h | 5 +- backend/sloam/include/core/sloamNode.h | 1 - backend/sloam/include/factorgraph/graph.h | 1 - .../sloam/include/factorgraph/graphWrapper.h | 1 - backend/sloam/include/helpers/definitions.h | 2 - .../single_robot_sloam_test_outdoor.launch | 2 +- backend/sloam/params/sloam_multiUGV.yaml | 2 +- backend/sloam/src/core/databaseManager.cpp | 6 -- backend/sloam/src/core/input.cpp | 58 +++++++++---------- backend/sloam/src/core/inputNode.cpp | 14 +---- backend/sloam/src/core/robot.cpp | 24 +++----- backend/sloam/src/core/sloamNode.cpp | 16 +++-- backend/sloam/src/factorgraph/graph.cpp | 1 - .../sloam/src/factorgraph/graphWrapper.cpp | 4 +- backend/sloam/src/test/utils_test.cpp | 31 ++++++++++ .../bash_scripts/multi_UGV_run.sh | 4 +- 19 files changed, 92 insertions(+), 92 deletions(-) create mode 100644 backend/sloam/src/test/utils_test.cpp diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 2a14765..8664469 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -125,4 +125,7 @@ cc_binary( # Add Unit Tests catkin_add_gtest(input_test "src/test/input_test.cpp") -target_link_libraries(input_test sloam_core ${catkin_LIBRARIES}) \ No newline at end of file +target_link_libraries(input_test sloam_core ${catkin_LIBRARIES}) + +catkin_add_gtest(utils_test "src/test/utils_test.cpp") +target_link_libraries(utils_test sloam_core ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/backend/sloam/include/core/databaseManager.h b/backend/sloam/include/core/databaseManager.h index 29fab35..aaa1a70 100755 --- a/backend/sloam/include/core/databaseManager.h +++ b/backend/sloam/include/core/databaseManager.h @@ -46,7 +46,6 @@ struct PoseMstPair { SE3 keyPose; // the current pose of the robot SE3 relativeRawOdomMotion; // relative motion from last keyPose to current as // measured using odometry - std::array relativeRawOdomMotionCov; // Covariance for relativeRawOdomMotion ros::Time stamp; // the timestamp associated with the keyPose // measurement in robot frame diff --git a/backend/sloam/include/core/input.h b/backend/sloam/include/core/input.h index 77f8a44..2dd3890 100644 --- a/backend/sloam/include/core/input.h +++ b/backend/sloam/include/core/input.h @@ -11,7 +11,7 @@ class Input { double msg_delay_tolerance, float minOdomDistance_, int &meas_to_add); - std::array computeRelativeMotionCovariance( - const SE3 &T1, const std::array &cov1, - const SE3 &T2, const std::array &cov2); + // std::array computeRelativeMotionCovariance( + // const SE3 &T1, const std::array &cov1, + // const SE3 &T2, const std::array &cov2); }; \ No newline at end of file diff --git a/backend/sloam/include/core/robot.h b/backend/sloam/include/core/robot.h index bb9bbca..58a2147 100755 --- a/backend/sloam/include/core/robot.h +++ b/backend/sloam/include/core/robot.h @@ -28,7 +28,6 @@ #include #include #include -#include #include #include @@ -37,12 +36,10 @@ #include struct StampedSE3 { - StampedSE3(SE3 p, ros::Time s, std::array c) : pose(p), stamp(s), covariance(c) {}; StampedSE3(SE3 p, ros::Time s) : pose(p), stamp(s){}; StampedSE3() : pose(SE3()), stamp(ros::Time::now()){}; SE3 pose; ros::Time stamp; - std::array covariance; // Ordering XYZRPY }; struct Observation { @@ -105,7 +102,7 @@ class Robot { // Helper methods for converting messages to objects std::vector rosEllipsoid2EllipObj(const std::vector& msgs); std::vector rosCylinder2CylinderObj(const std::vector& rosCylinders); - std::array ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg); + // std::array ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg); // Logging methods void PrintLateMsgWarning(const std::string &msg_type); diff --git a/backend/sloam/include/core/sloamNode.h b/backend/sloam/include/core/sloamNode.h index 2430766..ea3ced5 100755 --- a/backend/sloam/include/core/sloamNode.h +++ b/backend/sloam/include/core/sloamNode.h @@ -63,7 +63,6 @@ class SLOAMNode : public sloam { // timestamp is used for visualization bool runSLOAMNode(const SE3 &relativeRawOdomMotion, - std::array relativeRawOdomMotionCov, const SE3 &prevKeyPose, const std::vector &cylindersBody, const std::vector &cubesBody, diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index 2044781..e0d4c0d 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -85,7 +85,6 @@ class SemanticFactorGraph { bool alreadyExists, const int& robotID); void addKeyPoseAndBetween( const size_t fromIdx, const size_t toIdx, const Pose3& relativeMotion, - const std::array relativeMotionCov, const Pose3& poseEstimate, const int& robotID, const bool& loopClosureFound = false, const SE3& loop_closure_pose = SE3(), diff --git a/backend/sloam/include/factorgraph/graphWrapper.h b/backend/sloam/include/factorgraph/graphWrapper.h index 392bcc6..f82b107 100755 --- a/backend/sloam/include/factorgraph/graphWrapper.h +++ b/backend/sloam/include/factorgraph/graphWrapper.h @@ -89,7 +89,6 @@ class SemanticFactorGraphWrapper : public SemanticFactorGraph { const std::vector &ellipsoid_matches, const std::vector &ellipsoids, const SE3 &relativeMotion, - const std::array relativeMotionCov, const SE3 &poseEstimates, const int &robotID, bool opt = true); diff --git a/backend/sloam/include/helpers/definitions.h b/backend/sloam/include/helpers/definitions.h index 3edd89f..14146bb 100755 --- a/backend/sloam/include/helpers/definitions.h +++ b/backend/sloam/include/helpers/definitions.h @@ -107,7 +107,6 @@ struct FeatureModelParams { * * @param stamp - the timestamp. * @param odomPose - the synced odometry. - * @param covariance - the covariance of the odometry. * @param relativePose - the relative measurement * between the current robot and robot assigned to * "robotIndex". @@ -123,7 +122,6 @@ struct FeatureModelParams { struct RelativeMeas { ros::Time stamp; SE3 odomPose; - std::array covariance; SE3 relativePose; int robotIndex; bool onlyUseOdom; diff --git a/backend/sloam/launch/single_robot_sloam_test_outdoor.launch b/backend/sloam/launch/single_robot_sloam_test_outdoor.launch index ea234d0..c82aaa4 100755 --- a/backend/sloam/launch/single_robot_sloam_test_outdoor.launch +++ b/backend/sloam/launch/single_robot_sloam_test_outdoor.launch @@ -2,7 +2,7 @@ - + diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index 5b4dd88..6fe329a 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -1,4 +1,4 @@ -enable_rviz: true +enable_rviz: false sloam: # ------- MOST IMPORTANT PARAMS TO TUNE: ------- use_slidematch: false # VERY IMPORANT: (recommended: false) place recognition algorithm choice, if true, use slidematch, if false, use slidegraph diff --git a/backend/sloam/src/core/databaseManager.cpp b/backend/sloam/src/core/databaseManager.cpp index b80dbb5..052b7a6 100755 --- a/backend/sloam/src/core/databaseManager.cpp +++ b/backend/sloam/src/core/databaseManager.cpp @@ -113,9 +113,6 @@ void databaseManager::poseMstCb_(const sloam_msgs::PoseMstBundle &msgs) { // construct a PoseMstPair struct poseMst.keyPose = pose; poseMst.relativeRawOdomMotion = relativeOdom; - for (int i = 0; i < 6; i++) { - poseMst.relativeRawOdomMotionCov[i] = singleMsg.relativeRawOdomCov[i]; - } poseMst.stamp = singleMsg.stamp; // convert msg to object for (int j = 0; j < singleMsg.cylinders.size(); j++) { @@ -241,9 +238,6 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { pmMsg.pose = ToRosPoseMsg(pmp.keyPose); pmMsg.stamp = pmp.stamp; pmMsg.relativeRawOdom = ToRosPoseMsg(pmp.relativeRawOdomMotion); - for (int i = 0; i < 6; i++) { - pmMsg.relativeRawOdomCov[i] = pmp.relativeRawOdomMotionCov[i]; - } pmMsg.cylinders = obj2RosObjMsg(pmp.cylinderMsts); pmMsg.cubes = obj2RosObjMsg(pmp.cubeMsts); pmMsg.ellipsoids = obj2RosObjMsg(pmp.ellipsoidMsts); diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp index 17c7830..104fbcf 100644 --- a/backend/sloam/src/core/input.cpp +++ b/backend/sloam/src/core/input.cpp @@ -105,41 +105,41 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, * @param cov2: The associated covariance for T2 * @return The covariance for the relative motion */ -std::array Input::computeRelativeMotionCovariance(const SE3 &T1, - const std::array &cov1, const SE3 &T2, const std::array &cov2) { +// std::array Input::computeRelativeMotionCovariance(const SE3 &T1, +// const std::array &cov1, const SE3 &T2, const std::array &cov2) { - std::array cov_rel; +// std::array cov_rel; - // Compute relative transformation - SE3 T_rel = T1.inverse() * T2; +// // Compute relative transformation +// SE3 T_rel = T1.inverse() * T2; - // Extract rotation and translation from T_rel - Eigen::Matrix3d R_rel = T_rel.rotationMatrix(); - Eigen::Vector3d t_rel = T_rel.translation(); +// // Extract rotation and translation from T_rel +// Eigen::Matrix3d R_rel = T_rel.rotationMatrix(); +// Eigen::Vector3d t_rel = T_rel.translation(); - // Compute Adjoint matrix - Eigen::Matrix Adj_T_rel = Eigen::Matrix::Zero(); - Adj_T_rel.block<3, 3>(0, 0) = R_rel; - Adj_T_rel.block<3, 3>(0, 3) = skewSymmetric(t_rel) * R_rel; - Adj_T_rel.block<3, 3>(3, 3) = R_rel; +// // Compute Adjoint matrix +// Eigen::Matrix Adj_T_rel = Eigen::Matrix::Zero(); +// Adj_T_rel.block<3, 3>(0, 0) = R_rel; +// Adj_T_rel.block<3, 3>(0, 3) = skewSymmetric(t_rel) * R_rel; +// Adj_T_rel.block<3, 3>(3, 3) = R_rel; - // Convert std::array to Eigen::Matrix - Eigen::Matrix Sigma1 = Eigen::Matrix::Zero(); - Eigen::Matrix Sigma2 = Eigen::Matrix::Zero(); +// // Convert std::array to Eigen::Matrix +// Eigen::Matrix Sigma1 = Eigen::Matrix::Zero(); +// Eigen::Matrix Sigma2 = Eigen::Matrix::Zero(); - for (int i = 0; i < 6; ++i) { - Sigma1(i, i) = cov1[i]; - Sigma2(i, i) = cov2[i]; - } +// for (int i = 0; i < 6; ++i) { +// Sigma1(i, i) = cov1[i]; +// Sigma2(i, i) = cov2[i]; +// } - // Compute the relative motion covariance - Eigen::Matrix Sigma1_transformed = Adj_T_rel * Sigma1 * Adj_T_rel.transpose(); - Eigen::Matrix Sigma_rel = Sigma2 - Sigma1_transformed; +// // Compute the relative motion covariance +// Eigen::Matrix Sigma1_transformed = Adj_T_rel * Sigma1 * Adj_T_rel.transpose(); +// Eigen::Matrix Sigma_rel = Sigma2 - Sigma1_transformed; - // Store result back in std::array - for (int i = 0; i < 6; ++i) { - cov_rel[i] = Sigma_rel(i, i); // Extract diagonal elements - } +// // Store result back in std::array +// for (int i = 0; i < 6; ++i) { +// cov_rel[i] = Sigma_rel(i, i); // Extract diagonal elements +// } - return cov_rel; -} \ No newline at end of file +// return cov_rel; +// } \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 1669b4b..8b39f74 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -58,10 +58,8 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { if (robot.robotOdomReceived_) { // if odom has been updated in sloam, use the relative odom pose and add // it to the previous sloam pose - SE3 latestRelativeMotionFactorGraph = - robot.robotLatestOdom_.pose.inverse() * cur_vio_odom.pose; - highFreqSLOAMPose = - robot.robotLastSLOAMKeyPose_ * latestRelativeMotionFactorGraph; + SE3 latestRelativeMotionFactorGraph = robot.robotLatestOdom_.pose.inverse() * cur_vio_odom.pose; + highFreqSLOAMPose = robot.robotLastSLOAMKeyPose_ * latestRelativeMotionFactorGraph; odom_stamp = cur_vio_odom.stamp; } else { // directly take current odometry and publish @@ -139,7 +137,6 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { StampedSE3 validStampedPose; validStampedPose.pose = relativeMeas.odomPose; validStampedPose.stamp = relativeMeas.stamp; - validStampedPose.covariance = relativeMeas.covariance; latestObservation.stampedPose = validStampedPose; // If we observed another robot (rather than being observed) @@ -169,10 +166,6 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; - // Estimate the covariance of the relative motion - std::array relativeRawOdomCovariance = computeRelativeMotionCovariance( - robot.robotLatestOdom_.pose, robot.robotLatestOdom_.covariance, - raw_vio_odom_used_for_sloam.pose, raw_vio_odom_used_for_sloam.covariance); // Get the Previous Key Pose SE3 prevKeyPose; @@ -186,7 +179,7 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { // Send the measurement to SLOAM SE3 keyPose; bool success = sloam_->runSLOAMNode( - relativeRawOdomMotion, relativeRawOdomCovariance, prevKeyPose, + relativeRawOdomMotion, prevKeyPose, latestObservation.cylinders, latestObservation.cubes, latestObservation.ellipsoids, latestObservation.stampedPose.stamp, keyPose, hostRobotID_); @@ -210,7 +203,6 @@ void InputManager::updateLastPose(const StampedSE3 &odom, const int &robotID) { robot.robotOdomReceived_ = true; robot.robotLatestOdom_.pose = odom.pose; robot.robotLatestOdom_.stamp = odom.stamp; - robot.robotLatestOdom_.covariance = odom.covariance; if (robot.robotKeyPoses_.size() == 0) { robot.robotLastSLOAMKeyPose_ = robot.robotLatestOdom_.pose; } else { diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 3cd574a..c2f6dca 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -93,11 +93,8 @@ void Robot::RobotOdomCb(const nav_msgs::OdometryConstPtr &odom_msg) { return; } - // Extract covariance - std::array covariance = ExtractCovarianceFromOdometry(*odom_msg); - // Add odometry to the queue, and pop the oldest one if the queue is too long - robotOdomQueue_.emplace_back(odom, odomStamp, covariance); + robotOdomQueue_.emplace_back(odom, odomStamp); if (robotOdomQueue_.size() > 10 * maxQueueSize_) robotOdomQueue_.pop_front(); } @@ -113,7 +110,6 @@ void Robot::RobotObservationCb(const sloam_msgs::SemanticMeasSyncOdom &observati cur_observation.stampedPose.pose = databaseManager::toSE3Pose(observation_msg.odometry.pose.pose); cur_observation.stampedPose.stamp = observation_msg.header.stamp; - cur_observation.stampedPose.covariance = ExtractCovarianceFromOdometry(observation_msg.odometry); std::vector scan_cubes_body; int total_cuboids = observation_msg.cuboid_factors.size(); @@ -168,12 +164,10 @@ void Robot::RobotRelativeMeasCb(const sloam_msgs::RelativeInterRobotMeasurementO cur_measurement.onlyUseOdom = false; cur_measurement.robotIndex = relativeMeas_msg.robotIdObserved; cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserver.pose.pose); - cur_measurement.covariance = ExtractCovarianceFromOdometry(relativeMeas_msg.odometryObserver); } else { cur_measurement.onlyUseOdom = true; cur_measurement.robotIndex = relativeMeas_msg.robotIdObserver; cur_measurement.odomPose = databaseManager::toSE3Pose(relativeMeas_msg.odometryObserved.pose.pose); - cur_measurement.covariance = ExtractCovarianceFromOdometry(relativeMeas_msg.odometryObserved); } robotRelativeMeasQueue_.push_back(cur_measurement); @@ -227,11 +221,11 @@ void Robot::PrintLateMsgWarning(const std::string &msg_type) { * @param odom_msg: The odometry message from which to extract the covariance. * @return A std::array containing the covariance diagonal values (in XYZRPY order) */ -std::array Robot::ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg) { - std::array covariance; - for (size_t i = 0; i < 6; i++) { - // Absolute value used in case some very small covariance values come in negative - covariance[i] = std::abs(odom_msg.pose.covariance[i * 6 + i]); - } - return covariance; -} \ No newline at end of file +// std::array Robot::ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg) { +// std::array covariance; +// for (size_t i = 0; i < 6; i++) { +// // Absolute value used in case some very small covariance values come in negative +// covariance[i] = std::abs(odom_msg.pose.covariance[i * 6 + i]); +// } +// return covariance; +// } \ No newline at end of file diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index 960bb0c..a4b6a17 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -24,7 +24,7 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) debugMode_ = nh_.param("debug_mode", false); if (debugMode_) { - ROS_DEBUG_STREAM("Running SLOAM in Debug Mode" << std::endl); + ROS_DEBUG("Running SLOAM in Debug Mode"); if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); @@ -126,7 +126,7 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) save_results_dir_ = ros::package::getPath("sloam") + "/results"; save_runtime_analysis_dir_ = save_results_dir_ + "/runtime_analysis"; runtime_analysis_file = save_runtime_analysis_dir_ + "/robot"+std::to_string(hostRobotID)+"_runtime_analysis.txt"; - ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file); + ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file << ""); } /** @@ -471,7 +471,7 @@ void SLOAMNode::intraLoopClosureThread_() { hostRobotID); factorGraphMtx_.unlock(); } else{ - ROS_DEBUG_STREAM("Tried loop closure but was not succesfull"); + ROS_DEBUG("Tried loop closure but was not succesful"); } } else { ROS_DEBUG("No loop closure candidate history key pose found"); @@ -852,7 +852,6 @@ void SLOAMNode::relInterRobotFactorThread_() { } bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, - std::array relativeRawOdomMotionCov, const SE3 &prevKeyPose, const std::vector &cylindersBodyIn, const std::vector &cubesBodyIn, @@ -887,7 +886,6 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, pmp.cylinderMsts = cylindersBodyIn; pmp.stamp = stamp; pmp.relativeRawOdomMotion = relativeRawOdomMotion; - pmp.relativeRawOdomMotionCov = relativeRawOdomMotionCov; pmp.cubeMsts = cubesBodyIn; pmp.ellipsoidMsts = ellipsoidBodyIn; dbManager.getHostRobotData().poseMstPacket.push_back(pmp); @@ -984,7 +982,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, semanticMap_, cube_semantic_map_, ellipsoid_semantic_map_, sloamOut.cylinderMatches, sloamOut.scanCylindersWorld, sloamOut.cubeMatches, sloamOut.scanCubesWorld, sloamOut.ellipsoidMatches, - sloamOut.scanEllipsoidsWorld, relativeRawOdomMotion, relativeRawOdomMotionCov, + sloamOut.scanEllipsoidsWorld, relativeRawOdomMotion, sloamOut.T_Map_Curr, robotID); double fg_optimization_end = ros::Time::now().toSec(); double time_diff = fg_optimization_end - fg_optimization_start; @@ -1034,7 +1032,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, iter->second.poseMstPacket[i].keyPose; SE3 relativeRawOdomMotionInRefFrame = iter->second.poseMstPacket[i].relativeRawOdomMotion; - std::array relativeRawOdomMotionCovInRefFrame = iter->second.poseMstPacket[i].relativeRawOdomMotionCov; + // transform the pose and landmark into host robot map frame std::vector CylinderMeasurement = iter->second.poseMstPacket[i].cylinderMsts; @@ -1088,8 +1086,8 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, semanticMap_, cube_semantic_map_, ellipsoid_semantic_map_, cylinderMatchIndices, cylinderInRefFrame, cubeMatchIndices, cubeInRefFrame, ellipsoidMatchIndices, ellipsoidInRefFrame, - relativeRawOdomMotionInRefFrame, relativeRawOdomMotionCovInRefFrame, - poseEstimateInRefFrame, curRobotID, false); + relativeRawOdomMotionInRefFrame, poseEstimateInRefFrame, + curRobotID, false); } factorGraph_.solve(); dbManager.updateFGBookmark(curSize, curRobotID); diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index b7b04f9..f21cfa6 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -74,7 +74,6 @@ void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, void SemanticFactorGraph::addKeyPoseAndBetween( const size_t prevIdx, const size_t curIdx, const Pose3 &relativeMotion, - const std::array relativeMotionCov, const Pose3 &poseEstimate, const int &robotID, const bool &loopClosureFound, const SE3 &loop_closure_relative_pose, const size_t &closure_matched_pose_idx) { diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index 2155e48..0857ebc 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -84,7 +84,6 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( const std::vector &scan_cubes_world, const std::vector &ellipse_matches, const std::vector &ellipses, const SE3 &relativeMotionSE3, - const std::array relativeMotionCov, const SE3 &poseEstimateSE3, const int &robotID, bool opt) { size_t pose_counter; @@ -102,8 +101,7 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( "ERROR: Factor graph optimization is done when adding the first " "pose prior, this may cause problems!!!"); } else { - addKeyPoseAndBetween(pose_counter - 1, pose_counter, relativeMotion, - relativeMotionCov, curr_pose, robotID); + addKeyPoseAndBetween(pose_counter - 1, pose_counter, relativeMotion, curr_pose, robotID); } const auto matchesMap = semanticMap.getMatchesMap(); const auto cubeMatchesMap = cube_semantic_map.getMatchesMap(); diff --git a/backend/sloam/src/test/utils_test.cpp b/backend/sloam/src/test/utils_test.cpp new file mode 100644 index 0000000..6723b03 --- /dev/null +++ b/backend/sloam/src/test/utils_test.cpp @@ -0,0 +1,31 @@ +#include +#include + +TEST(UtilsTest, SE3ToGTSAMPose3) { + // Define a sample SE3 transformation + Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + Eigen::Vector3d t(1.0, 2.0, 3.0); + Sophus::SE3d se3_pose(R, t); + + // Convert using the function + gtsam::Pose3 gtsam_pose = SE3ToGTSAMPose3(se3_pose); + + // Extract values from the output gtsam::Pose3 + gtsam::Rot3 gtsam_rot = gtsam_pose.rotation(); + gtsam::Point3 gtsam_trans = gtsam_pose.translation(); + + // Check rotation matrix equality + Eigen::Matrix3d expected_R = gtsam_rot.matrix(); + EXPECT_TRUE(expected_R.isApprox(R, 1e-15)) << "Rotation matrices do not match!"; + + // Check translation vector equality + EXPECT_NEAR(gtsam_trans.x(), t[0], 1e-15); + EXPECT_NEAR(gtsam_trans.y(), t[1], 1e-15); + EXPECT_NEAR(gtsam_trans.z(), t[2], 1e-15); +} + +int main(int argc, char **argv) { + // Initialize GoogleTest and run all tests + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh index 74c9fd2..df8a66e 100755 --- a/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh +++ b/frontend/relative_meas_gen/bash_scripts/multi_UGV_run.sh @@ -69,11 +69,11 @@ tmux split-window -v -t $SESSION_NAME tmux select-pane -t $SESSION_NAME:2.0 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter tmux select-pane -t $SESSION_NAME:2.1 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=0 odom_topic:=/robot0/odometry/local_filtered enable_rviz:=true turn_off_rel_inter_robot_factor:=true param_file:=sloam_multiUGV.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=0 odom_topic:=/robot0/odometry/local_filtered enable_rviz:=true turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter tmux select-pane -t $SESSION_NAME:2.2 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter tmux select-pane -t $SESSION_NAME:2.3 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=1 odom_topic:=/robot1/odometry/local_filtered turn_off_rel_inter_robot_factor:=true param_file:=sloam_multiUGV.yaml" Enter +tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 8; roslaunch sloam decentralized_sloam.launch hostRobot_ID:=1 odom_topic:=/robot1/odometry/local_filtered turn_off_rel_inter_robot_factor:=false param_file:=sloam_multiUGV.yaml" Enter # Add window to easily kill all processes tmux new-window -t $SESSION_NAME -n "Kill" From 21875c07572c3d8337a1d78f30805eb34ac7674d Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 10:34:26 -0400 Subject: [PATCH 49/74] github actions test 4/9 10:34 --- .github/workflows/SLIDE_SLAM_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 9c4f9b1..ff8d8da 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -44,5 +44,5 @@ jobs: docker start slideslam_ros docker cp . slideslam_ros:/opt/slideslam_docker_ws/ docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin build -DCMAKE_BUILD_TYPE=Debug --catkin-make-args tests && source devel/setup.bash" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin run_tests && catkin_test_results --verbose" From c6aecddd98dad1a71cfb857a64fdbfe8ed70ae91 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Wed, 9 Apr 2025 14:58:32 +0000 Subject: [PATCH 50/74] Fix bug with factors being added out of timestamp order --- backend/sloam/include/core/input.h | 3 - backend/sloam/params/sloam_multiUGV.yaml | 2 +- backend/sloam/src/core/databaseManager.cpp | 2 - backend/sloam/src/core/input.cpp | 66 +++++--------------- backend/sloam/src/core/inputNode.cpp | 1 - backend/sloam/src/core/place_recognition.cpp | 4 +- backend/sloam/src/core/robot.cpp | 2 +- backend/sloam/src/core/sloam.cpp | 10 +-- backend/sloam/src/core/sloamNode.cpp | 28 ++++----- backend/sloam/src/test/input_test.cpp | 25 ++++++++ 10 files changed, 61 insertions(+), 82 deletions(-) diff --git a/backend/sloam/include/core/input.h b/backend/sloam/include/core/input.h index 2dd3890..0f4f2c4 100644 --- a/backend/sloam/include/core/input.h +++ b/backend/sloam/include/core/input.h @@ -11,7 +11,4 @@ class Input { double msg_delay_tolerance, float minOdomDistance_, int &meas_to_add); - // std::array computeRelativeMotionCovariance( - // const SE3 &T1, const std::array &cov1, - // const SE3 &T2, const std::array &cov2); }; \ No newline at end of file diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index 6fe329a..5b4dd88 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -1,4 +1,4 @@ -enable_rviz: false +enable_rviz: true sloam: # ------- MOST IMPORTANT PARAMS TO TUNE: ------- use_slidematch: false # VERY IMPORANT: (recommended: false) place recognition algorithm choice, if true, use slidematch, if false, use slidegraph diff --git a/backend/sloam/src/core/databaseManager.cpp b/backend/sloam/src/core/databaseManager.cpp index 052b7a6..4f69654 100755 --- a/backend/sloam/src/core/databaseManager.cpp +++ b/backend/sloam/src/core/databaseManager.cpp @@ -197,7 +197,6 @@ void databaseManager::measureReceivedCommMsgSize(const sloam_msgs::PoseMstBundle for (int i = 0; i < msgs.poseMstPair.size(); i++) { cur_received_msg_size_bytes += 56; cur_received_msg_size_bytes += 56; - cur_received_msg_size_bytes += 48; // From covariance cur_received_msg_size_bytes += 8; // For the time stamp (theorized, not proven) cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].ellipsoids.size(); cur_received_msg_size_bytes += 69*msgs.poseMstPair[i].cubes.size(); @@ -248,7 +247,6 @@ void databaseManager::runCommunication_(const ros::TimerEvent &e) { cur_publish_msg_size_bytes += 56; cur_publish_msg_size_bytes += 56; cur_publish_msg_size_bytes += 8; // From time stamp (not proven) - cur_publish_msg_size_bytes += 48; // From covariance } for (int i = 0; i < robotMapDict_[curRobotID].size(); i++) { diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp index 104fbcf..957fa96 100644 --- a/backend/sloam/src/core/input.cpp +++ b/backend/sloam/src/core/input.cpp @@ -5,10 +5,13 @@ * to the factor graph next. It ensures they are added in order of timestamp * (assuming that each queue is ordered by timestamp). This prevents jumping * back and forth through time in the factor graph, which breaks covariances. + * To do this, it will pop measurements that are before the most recent factor. * * @param robotOdomQueue_: The deque of odometry measurements. * @param robotObservationQueue_: The queue of observation measurements. * @param robotRelativeMeasQueue_: The deque of relative measurements. + * @param robotLatestOdom_: The latest estimated pose from odometry used to + * generate a relativeRawOdomMotion for the factor graph. * @param current_time: The current time of the robot (in seconds) * @param msg_delay_tolerance: The amount of time to wait before adding * a measurement. @@ -29,6 +32,17 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, float minOdomDistance_, int &meas_to_add) { + // Pop all measurements that are before the most recent graph factor + while (!robotOdomQueue_.empty() && robotOdomQueue_.front().stamp < robotLatestOdom_.stamp) { + robotOdomQueue_.pop_front(); + } + while (!robotObservationQueue_.empty() && robotObservationQueue_.front().stampedPose.stamp < robotLatestOdom_.stamp) { + robotObservationQueue_.pop(); + } + while (!robotRelativeMeasQueue_.empty() && robotRelativeMeasQueue_.front().stamp < robotLatestOdom_.stamp) { + robotRelativeMeasQueue_.pop_front(); + } + // Create variables to hold statuses and values of valid measurements bool validObs = false; bool validRelMeas = false; @@ -92,54 +106,4 @@ void Input::PickNextMeasurementToAdd(std::deque &robotOdomQueue_, // If we got this far, then no odometry measurements are valid & moved enough meas_to_add = 0; return; -} - -/** - * @brief Computes the relative motion covariance from odometry pose T1 to odometry - * pose T2 given their respective covariance matrices. Note that cov2 includes - * the covariance of T1 plus any additional covariance from the relative motion. - * - * @param T1: The first odometry pose - * @param cov1: The associated covariance for T1 - * @param T2: The second odometry pose - * @param cov2: The associated covariance for T2 - * @return The covariance for the relative motion - */ -// std::array Input::computeRelativeMotionCovariance(const SE3 &T1, -// const std::array &cov1, const SE3 &T2, const std::array &cov2) { - -// std::array cov_rel; - -// // Compute relative transformation -// SE3 T_rel = T1.inverse() * T2; - -// // Extract rotation and translation from T_rel -// Eigen::Matrix3d R_rel = T_rel.rotationMatrix(); -// Eigen::Vector3d t_rel = T_rel.translation(); - -// // Compute Adjoint matrix -// Eigen::Matrix Adj_T_rel = Eigen::Matrix::Zero(); -// Adj_T_rel.block<3, 3>(0, 0) = R_rel; -// Adj_T_rel.block<3, 3>(0, 3) = skewSymmetric(t_rel) * R_rel; -// Adj_T_rel.block<3, 3>(3, 3) = R_rel; - -// // Convert std::array to Eigen::Matrix -// Eigen::Matrix Sigma1 = Eigen::Matrix::Zero(); -// Eigen::Matrix Sigma2 = Eigen::Matrix::Zero(); - -// for (int i = 0; i < 6; ++i) { -// Sigma1(i, i) = cov1[i]; -// Sigma2(i, i) = cov2[i]; -// } - -// // Compute the relative motion covariance -// Eigen::Matrix Sigma1_transformed = Adj_T_rel * Sigma1 * Adj_T_rel.transpose(); -// Eigen::Matrix Sigma_rel = Sigma2 - Sigma1_transformed; - -// // Store result back in std::array -// for (int i = 0; i < 6; ++i) { -// cov_rel[i] = Sigma_rel(i, i); // Extract diagonal elements -// } - -// return cov_rel; -// } \ No newline at end of file +} \ No newline at end of file diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 8b39f74..99f245a 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -166,7 +166,6 @@ void InputManager::RunInputNode(const ros::TimerEvent &e) { StampedSE3 raw_vio_odom_used_for_sloam = latestObservation.stampedPose; SE3 relativeRawOdomMotion = robot.robotLatestOdom_.pose.inverse() * raw_vio_odom_used_for_sloam.pose; - // Get the Previous Key Pose SE3 prevKeyPose; if (robot.robotKeyPoses_.size() > 0) { diff --git a/backend/sloam/src/core/place_recognition.cpp b/backend/sloam/src/core/place_recognition.cpp index 36bb34e..bcae51e 100755 --- a/backend/sloam/src/core/place_recognition.cpp +++ b/backend/sloam/src/core/place_recognition.cpp @@ -617,9 +617,9 @@ bool PlaceRecognition::findInterLoopClosureWithClipper( // make sure we have at least slidegraph_min_num_map_objects_to_start_ objects to do the matching in both the reference and query if (reference_objects_vector.size() >= slidegraph_min_num_map_objects_to_start_ && query_objects_vector.size() >= slidegraph_min_num_map_objects_to_start_) { // ROS_WARN_STREAM("[PlaceRecognition]: reference_objects_vector size is: " << reference_objects_vector.size() << " and query_objects_vector size is: " << query_objects_vector.size()); - ROS_WARN("Calling CLIPPER for inter loop closure, if anything bad happens, look into that piece of code..."); + //ROS_WARN("Calling CLIPPER for inter loop closure, if anything bad happens, look into that piece of code..."); found = semantic_clipper::run_semantic_clipper(reference_objects_vector, query_objects_vector, tfFromQueryToRef, sigma, epsilon, min_num_pairs, matching_threshold); - ROS_DEBUG("EXIT CLIPPER SUCCESSFULLY"); + //ROS_DEBUG("EXIT CLIPPER SUCCESSFULLY"); // get the inverse of the transformation matrix tfFromQueryToRef = tfFromQueryToRef.inverse(); } else { diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index c2f6dca..9820cf3 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -210,7 +210,7 @@ std::vector Robot::rosEllipsoid2EllipObj( } void Robot::PrintLateMsgWarning(const std::string &msg_type) { - ROS_WARN_STREAM_THROTTLE(10, msg_type << "message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); + ROS_WARN_STREAM_THROTTLE(10, msg_type << " message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); ROS_WARN_STREAM_THROTTLE(10, "Please increase `msg_delay_tolerance` to account for lag in your system"); } diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index ef5fa6f..dc763a7 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -117,10 +117,10 @@ void sloam::matchCubeModels(const std::vector &currObjects, size_t obj_counter = 0; if (currObjects.size() == 0) { - ROS_WARN("No cube detected in current scan!"); + ROS_WARN_THROTTLE(20, "No cube detected in current scan!"); return; } else if (mapObjects.size() == 0) { - ROS_WARN("No cube in submap!"); + ROS_WARN_THROTTLE(20, "No cube in submap!"); return; } @@ -152,7 +152,7 @@ void sloam::matchCubeModels(const std::vector &currObjects, } obj_counter++; } - std::cout << "total scan objects in matchCubeModels:" << obj_counter << '\n'; + //std::cout << "total scan objects in matchCubeModels:" << obj_counter << '\n'; } template @@ -161,10 +161,10 @@ void sloam::matchEllipsoidModels(const std::vector &currObjects, std::vector &matchIndices) { if (currObjects.size() == 0) { - ROS_WARN_STREAM_THROTTLE(1.0, "No Ellipsoid detected in current scan!"); + ROS_WARN_STREAM_THROTTLE(20.0, "No Ellipsoid detected in current scan!"); return; } else if (mapObjects.size() == 0) { - ROS_WARN("No Ellipsoid in submap!"); + ROS_WARN_THROTTLE(20.0, "No Ellipsoid in submap!"); return; } // TODO(ankit): Make ellip_match_search_threshold and valid_match_treshold a parameter diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index a4b6a17..e3e4447 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -367,10 +367,10 @@ void SLOAMNode::intraLoopClosureThread_() { ros::Duration(0.1).sleep(); continue; } else { - ROS_ERROR_THROTTLE( - 1.0, "isInLoopClosureRegion_ is true, attempting loop closure"); + ROS_INFO_THROTTLE(20.0, "isInLoopClosureRegion_ is true, attempting loop closure"); } - ROS_DEBUG("****** Starting Intra Loop Closure Thread *****"); + //ROS_DEBUG("****** Starting Intra Loop Closure Thread *****"); + // get latest pose and its observation to do loop closure semanticMapMtx_.lock(); int latestPoseIdx = semanticMap_.getLatestPoseIdx(hostRobotID); @@ -471,10 +471,10 @@ void SLOAMNode::intraLoopClosureThread_() { hostRobotID); factorGraphMtx_.unlock(); } else{ - ROS_DEBUG("Tried loop closure but was not succesful"); + ROS_DEBUG_THROTTLE(20.0, "Tried loop closure but was not succesful"); } } else { - ROS_DEBUG("No loop closure candidate history key pose found"); + ROS_DEBUG_THROTTLE(20.0, "No loop closure candidate history key pose found"); } } rate.sleep(); @@ -594,8 +594,8 @@ void SLOAMNode::interLoopClosureThread_() { dbMutex.unlock(); num_attempts_inter_loop_closure++; for (auto query_robot_id : robotIDLoopClosureToFind) { - ROS_INFO_STREAM("START TO FIND INTER LOOP CLOSURE BETWEEN ROBOTS: " - << query_robot_id << " AND " << dbManager.getHostRobotID()); + //ROS_INFO_STREAM("START TO FIND INTER LOOP CLOSURE BETWEEN ROBOTS: " + // << query_robot_id << " AND " << dbManager.getHostRobotID()); dbMutex.lock(); std::vector reference_map = dbManager.getRobotMap(dbManager.getHostRobotID()); @@ -751,8 +751,6 @@ void SLOAMNode::relInterRobotFactorThread_() { // Only run loop closures if we have relative inter-robot measurements waiting if (feasible_relative_meas_for_factors.size() > 0) { - ROS_DEBUG("****** Starting Relative Inter Robot Factor Generation Thread *****"); - // Prepare return variables int indexClosestHostRobot; int indexClosestOtherRobot; @@ -834,14 +832,12 @@ void SLOAMNode::relInterRobotFactorThread_() { // Keep track of total added if (matches_found > 0) { - ROS_INFO_STREAM("Success: " << matches_found << " Relative Inter Robot Measurement Factors added"); num_successful_rel_inter_robot_factor += matches_found; - } else { - ROS_DEBUG_STREAM("Added no Relative Inter Robot Measurement Factors"); + ROS_INFO_STREAM_THROTTLE(20, "Total of " << num_successful_rel_inter_robot_factor << " Relative Factors added"); } } else { - ROS_DEBUG_THROTTLE(5.0, "No Available Relative Inter Robot Measurements"); + ROS_DEBUG_THROTTLE(20.0, "No Available Relative Inter Robot Measurements"); } // Unlock the mutexes and sleep @@ -868,6 +864,8 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, if(packets.size() >= 1) { ros::Time last_stamp = dbManager.getHostRobotData().poseMstPacket.back().stamp; if (stamp < last_stamp) { + // Print out all available information on this data + ROS_ERROR_STREAM("Current stamp: " << stamp << " Last stamp: " << last_stamp); ROS_ERROR_STREAM("Adding a factor with a timestamp earlier than the last " "one in the factor graph! This should never happen."); } @@ -895,9 +893,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, if (isInLoopClosureRegion_) { // TODO(xu): record all the semantic measurements and corresponding pose // so as to add them after the robot exits the loop closure region - ROS_WARN_THROTTLE( - 1.0, - "isInLoopClosureRegion_ is true, clearing all measurements to avoid " + ROS_WARN_THROTTLE(20.0, "isInLoopClosureRegion_ is true, clearing all measurements to avoid " "polluting the map"); cylindersBody = std::vector(); cubesBody = std::vector(); diff --git a/backend/sloam/src/test/input_test.cpp b/backend/sloam/src/test/input_test.cpp index bffda58..b0b8a9f 100644 --- a/backend/sloam/src/test/input_test.cpp +++ b/backend/sloam/src/test/input_test.cpp @@ -16,6 +16,9 @@ class InputTest : public ::testing::Test { std::queue robotObservationQueue_10; std::deque robotRelativeMeasQueue_10; std::deque robotOdomQueue_Large; + std::deque robotOdomQueueFirstToPop_; + std::queue robotObservationQueueFirstToPop_; + std::deque robotRelativeMeasQueueFirstToPop_; // StampedSE3s StampedSE3 robotLatestOdom; @@ -26,6 +29,7 @@ class InputTest : public ::testing::Test { InputTest() { // Create various ROS times + ros::Time time_0(0.0); ros::Time time_1(1.0); ros::Time time_10(10.0); ros::Time time_100(100.0); @@ -37,6 +41,7 @@ class InputTest : public ::testing::Test { oneMeter.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); oneMeter_1 = StampedSE3(oneMeter, time_1); oneMeter_10 = StampedSE3(oneMeter, time_10); + robotLatestOdom.stamp = time_0; // 1 Queues all have stamps of timestep 1 robotOdomQueue_1.push_back(oneMeter_1); @@ -64,6 +69,17 @@ class InputTest : public ::testing::Test { for (int i = 0; i < 100; i++) { robotOdomQueue_Large.push_back(StampedSE3(oneMeter, ros::Time(i))); } + + // Add values to be popped + for (int i = 0; i < 12; i++) { + robotOdomQueueFirstToPop_.push_back(StampedSE3(oneMeter, ros::Time(i))); + Observation obs; + obs.stampedPose = StampedSE3(oneMeter, ros::Time(i)); + robotObservationQueueFirstToPop_.push(obs); + RelativeMeas relMeas; + relMeas.stamp = ros::Time(i); + robotRelativeMeasQueueFirstToPop_.push_back(relMeas); + } } }; @@ -121,6 +137,15 @@ TEST_F(InputTest, PickNextMeasurementToAdd) { input.PickNextMeasurementToAdd(robotOdomQueue_1, robotObservationQueue_10, robotRelativeMeasQueue_1, robotLatestOdom, 10.0, 8.0, 0.5, meas_to_add); EXPECT_EQ(meas_to_add, 3); + + // Make sure measurements before the most recent graph factor are popped + input.PickNextMeasurementToAdd(robotOdomQueueFirstToPop_, robotObservationQueueFirstToPop_, robotRelativeMeasQueueFirstToPop_, + identity_10, 12.0, 3.0, 0.5, meas_to_add); + EXPECT_EQ(meas_to_add, 0); + EXPECT_EQ(robotOdomQueueFirstToPop_.size(), 2); + EXPECT_EQ(robotObservationQueueFirstToPop_.size(), 2); + EXPECT_EQ(robotRelativeMeasQueueFirstToPop_.size(), 2); + EXPECT_EQ(robotOdomQueueFirstToPop_.front().stamp.toSec(), 10.0); } int main(int argc, char **argv) { From edef10df8f9c96012944b958183fa6eab1408d27 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:01:11 -0400 Subject: [PATCH 51/74] github actions test 4/9 11:01 --- .github/workflows/SLIDE_SLAM_test.yml | 6 +++--- frontend/relative_meas_gen/CMakeLists.txt | 4 +++- frontend/relative_meas_gen/package.xml | 3 +++ 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index ff8d8da..14d62ef 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp . slideslam_ros:/opt/slideslam_docker_ws/ + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin build -DCMAKE_BUILD_TYPE=Debug --catkin-make-args tests && source devel/setup.bash" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin run_tests && catkin_test_results --verbose" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" + docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 92be1c4..16df89a 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -24,7 +24,8 @@ find_package(catkin REQUIRED COMPONENTS ) catkin_package( - CATKIN_DEPENDS roscpp + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp sloam_msgs ) ## System dependencies are found with CMake's conventions @@ -121,6 +122,7 @@ include_directories( ) add_executable(apriltag_meas_gen src/apriltag_meas_gen.cpp src/apriltag_wrapper.cpp) +add_dependencies(apriltag_meas_gen ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) set_target_properties(apriltag_meas_gen PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) target_link_libraries(apriltag_meas_gen ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apriltag_LIBRARIES} yaml-cpp) diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index 0b230b5..e3038d0 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -54,6 +54,7 @@ rospy sloam_msgs std_msgs + geometry_msgs tf gazebo_msgs yaml-cpp @@ -64,6 +65,7 @@ roscpp rospy sloam_msgs + geometry_msgs gazebo_msgs std_msgs yaml-cpp @@ -75,6 +77,7 @@ roscpp rospy sloam_msgs + geometry_msgs gazebo_msgs std_msgs yaml-cpp From 310d299ffcaa83b93418ac765ba3876acd1b65a1 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:07:20 -0400 Subject: [PATCH 52/74] github actions test 4/9 11:07 --- .github/workflows/SLIDE_SLAM_test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 14d62ef..c36c4ae 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,6 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 1caafde78f8bb5d5a075e9597210366634f4e8a0 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:12:38 -0400 Subject: [PATCH 53/74] github actions test 4/9 11:21 --- .github/workflows/SLIDE_SLAM_test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index c36c4ae..cc47b48 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,6 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 05eb2bf1b7577e74b86cc2219ad740a52ae2b060 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:19:24 -0400 Subject: [PATCH 54/74] github actions test 4/9 11:19 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index cc47b48..d85cc7a 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws/src docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 3992b39052a990ff14baaabc60e408e2faa81a0f Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:27:27 -0400 Subject: [PATCH 55/74] github actions test 4/9 11:27 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index d85cc7a..9b7c7f5 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM slideslam_ros:/opt/slideslam_docker_ws/src + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/. slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From abb112e5f1370e41c1f005ee9731fbb7a9e9fa40 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Wed, 9 Apr 2025 15:29:52 +0000 Subject: [PATCH 56/74] Slight tweaks --- backend/sloam/src/core/inputNode.cpp | 7 ------- backend/sloam/src/core/robot.cpp | 19 ++----------------- 2 files changed, 2 insertions(+), 24 deletions(-) diff --git a/backend/sloam/src/core/inputNode.cpp b/backend/sloam/src/core/inputNode.cpp index 99f245a..5c2987d 100755 --- a/backend/sloam/src/core/inputNode.cpp +++ b/backend/sloam/src/core/inputNode.cpp @@ -30,16 +30,9 @@ InputManager::InputManager(ros::NodeHandle nh) nh_.param(idName, hostRobotID_, 0); nh_.param(node_name + "/turn_off_intra_loop_closure", turn_off_intra_loop_closure_, false); - nh_.param(node_name+"/turn_off_rel_inter_robot_factor", - turn_off_rel_inter_robot_factor, true); auto sloam_ptr = boost::make_shared(nh_); sloam_ = std::move(sloam_ptr); - - // Print information about parameters - if (turn_off_intra_loop_closure_) { - ROS_INFO_ONCE("Intra Loop closure is turned off, the default of the variable is false"); - } } void InputManager::RunInputNode(const ros::TimerEvent &e) { diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 9820cf3..0b3b2ba 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -45,6 +45,7 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { bool turn_off_rel_inter_robot_factor = nh_.param(node_name+"/turn_off_rel_inter_robot_factor", true); if (!turn_off_rel_inter_robot_factor) { + ROS_INFO("Relative Inter-Robot Factors are enabled!"); std::string relativeMeasSub_topic = "/relative_inter_robot_meas_sync"; RobotRelativeMeasSub_ = nh_.subscribe(relativeMeasSub_topic, 10, &Robot::RobotRelativeMeasCb, this); } @@ -212,20 +213,4 @@ std::vector Robot::rosEllipsoid2EllipObj( void Robot::PrintLateMsgWarning(const std::string &msg_type) { ROS_WARN_STREAM_THROTTLE(10, msg_type << " message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); ROS_WARN_STREAM_THROTTLE(10, "Please increase `msg_delay_tolerance` to account for lag in your system"); -} - -/** - * @brief This method extracts the covariance values from a - * nav_msgs::Odometry message. - * - * @param odom_msg: The odometry message from which to extract the covariance. - * @return A std::array containing the covariance diagonal values (in XYZRPY order) - */ -// std::array Robot::ExtractCovarianceFromOdometry(nav_msgs::Odometry odom_msg) { -// std::array covariance; -// for (size_t i = 0; i < 6; i++) { -// // Absolute value used in case some very small covariance values come in negative -// covariance[i] = std::abs(odom_msg.pose.covariance[i * 6 + i]); -// } -// return covariance; -// } \ No newline at end of file +} \ No newline at end of file From 70dd4a3c15792e1667e09bb80df9d4340848b438 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:33:29 -0400 Subject: [PATCH 57/74] github actions test 4/9 11:33 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 9b7c7f5..6ee477a 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/. slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM + docker exec slideslam_ros bash -c "tree" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 5690acad98c9a09b5cb6402f2ef5f5e9905700f1 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:40:45 -0400 Subject: [PATCH 58/74] github actions test 4/9 11:40 --- .github/workflows/SLIDE_SLAM_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 6ee477a..87a006d 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,7 +42,7 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros - docker exec slideslam_ros bash -c "tree" + docker exec slideslam_ros bash -c "find . -type d" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From 28322b152f4213622e6df0fd37ef9c8bc6e83c87 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:49:15 -0400 Subject: [PATCH 59/74] github actions test 4/9 11:49 --- .github/workflows/SLIDE_SLAM_test.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 87a006d..707619d 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -42,6 +42,9 @@ jobs: - name: Build SLIDE_SLAM inside Container and run tests run: | docker start slideslam_ros + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/backend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/frontend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM + docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/bags/. slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags docker exec slideslam_ros bash -c "find . -type d" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" From 7c0320eb12ddab4d2dd379ea3554b97c2c25bd8b Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 11:53:33 -0400 Subject: [PATCH 60/74] github actions test 4/9 11:53 --- .github/workflows/SLIDE_SLAM_test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 707619d..22a9568 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -44,7 +44,6 @@ jobs: docker start slideslam_ros docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/backend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/frontend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM - docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/bags/. slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags docker exec slideslam_ros bash -c "find . -type d" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" From 722a70ecde4be10b4d2d430be284a3b365353cf9 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 14:55:41 -0400 Subject: [PATCH 61/74] github actions test 4/9 2:55 --- .github/workflows/SLIDE_SLAM_test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index 22a9568..b1d4188 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -45,6 +45,5 @@ jobs: docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/backend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/frontend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker exec slideslam_ros bash -c "find . -type d" - docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths /opt/slideslam_docker_ws/src --ignore-src -r -y" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" From c01abf708b68400c41072f99e4a2b3885a67b980 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 15:09:03 -0400 Subject: [PATCH 62/74] github actions test 4/9 3:09 --- frontend/relative_meas_gen/tests/apriltag_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index 125fb18..d71420c 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -30,7 +30,7 @@ TEST(apriltag_test, Detect) { std::cout << "Detected tag: " << test_tag.id << std::endl; std::cout << "Expected tag: 6" << std::endl; - EXPECT_EQ(5, test_tag.id); + EXPECT_EQ(6, test_tag.id); } From 4878b29717f7f2b9309eb672bd6ee6a4ce81f3f1 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Wed, 9 Apr 2025 21:55:30 +0000 Subject: [PATCH 63/74] Test cases for parts of Relative Measurement Generation --- backend/sloam/CMakeLists.txt | 5 +- backend/sloam/include/core/sloam.h | 20 ++ backend/sloam/include/core/sloamNode.h | 8 +- .../sloam/include/factorgraph/graphWrapper.h | 3 +- backend/sloam/params/sloam_multiUGV.yaml | 3 +- backend/sloam/src/core/input.cpp | 2 +- backend/sloam/src/core/robot.cpp | 1 - backend/sloam/src/core/sloam.cpp | 134 +++++++++++ backend/sloam/src/core/sloamNode.cpp | 176 ++++----------- backend/sloam/src/test/sloam_test.cpp | 209 ++++++++++++++++++ 10 files changed, 418 insertions(+), 143 deletions(-) create mode 100644 backend/sloam/src/test/sloam_test.cpp diff --git a/backend/sloam/CMakeLists.txt b/backend/sloam/CMakeLists.txt index 8664469..4284410 100755 --- a/backend/sloam/CMakeLists.txt +++ b/backend/sloam/CMakeLists.txt @@ -128,4 +128,7 @@ catkin_add_gtest(input_test "src/test/input_test.cpp") target_link_libraries(input_test sloam_core ${catkin_LIBRARIES}) catkin_add_gtest(utils_test "src/test/utils_test.cpp") -target_link_libraries(utils_test sloam_core ${catkin_LIBRARIES}) \ No newline at end of file +target_link_libraries(utils_test sloam_core ${catkin_LIBRARIES}) + +catkin_add_gtest(sloam_test "src/test/sloam_test.cpp") +target_link_libraries(sloam_test sloam_core ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/backend/sloam/include/core/sloam.h b/backend/sloam/include/core/sloam.h index de7706a..c9a900b 100755 --- a/backend/sloam/include/core/sloam.h +++ b/backend/sloam/include/core/sloam.h @@ -12,6 +12,7 @@ #include #include +#include #include #include #include @@ -61,6 +62,15 @@ struct SloamOutput { SE3 T_Delta; }; +// Holds results from FindRelativeMeasurementMatch() +struct RelativeMeasMatch { + RelativeMeasMatch(int i, int h, int o) : index(i), + indexClosestHostRobot(h), indexClosestOtherRobot(o) {} + int index; + int indexClosestHostRobot; + int indexClosestOtherRobot; +}; + namespace sloam { class sloam { public: @@ -108,6 +118,16 @@ class sloam { std::vector getPrevGroundModel(); CloudT getPrevGroundFeatures(); + // Relative Inter-Robot Factor Generation + void FindRelativeMeasurementMatch(std::vector& pose_counter_robot_, + const std::unordered_map& robotDataDict_, + int hostRobotID, + std::vector& matches); + void GetIndexClosestPoseMstPair(std::deque &poseMstPacket, + ros::Time stamp, int &indexClosest, double &timeDiffClosest); + std::vector feasible_relative_meas_for_factors; // All measurements that could still be used to generate a factor + std::mutex feasRelMeasVectorMtx_; + private: // Pose of ANCHOR frame in the Map frame SE3 T_Map_Anchor_; diff --git a/backend/sloam/include/core/sloamNode.h b/backend/sloam/include/core/sloamNode.h index ea3ced5..8ec963f 100755 --- a/backend/sloam/include/core/sloamNode.h +++ b/backend/sloam/include/core/sloamNode.h @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -111,7 +110,7 @@ class SLOAMNode : public sloam { const std::vector> &landmarks); void publishMap_(const ros::Time stamp); void publishCubeMaps_(const ros::Time stamp); - void publishInterRobotFactors(const int &robotID); + void publishInterRobotFactors(); bool prepareInputs_(const SE3 relativeMotion, const SE3 prevKeyPose, CloudT::Ptr tree_cloud, CloudT::Ptr ground_cloud, @@ -121,8 +120,7 @@ class SLOAMNode : public sloam { void intraLoopClosureThread_(); void interLoopClosureThread_(); void relInterRobotFactorThread_(); - void GetIndexClosestPoseMstPair(std::deque &poseMstPacket, ros::Time stamp, int &indexClosest, double &timeDiffClosest); - + std::vector extractPosition( const std::vector &candidateCylinderObs, const std::vector &candidateCubeObs, @@ -189,9 +187,7 @@ class SLOAMNode : public sloam { // Relative Measurement Factor Generation ros::Time last_rel_inter_robot_factor_stamp_; std::thread relInterRobotFactorthread_; - std::vector feasible_relative_meas_for_factors; // All measurements that could still be used to generate a factor std::vector relative_inter_robot_factors; // All measurements that have been used to generate a factor - std::mutex feasRelMeasVectorMtx_; std::mutex relInterRobotFactorsMtx_; // For cuboid semantic landmarks diff --git a/backend/sloam/include/factorgraph/graphWrapper.h b/backend/sloam/include/factorgraph/graphWrapper.h index f82b107..021ca02 100755 --- a/backend/sloam/include/factorgraph/graphWrapper.h +++ b/backend/sloam/include/factorgraph/graphWrapper.h @@ -123,13 +123,14 @@ class SemanticFactorGraphWrapper : public SemanticFactorGraph { size_t pose_counter_robot1_; size_t pose_counter_robot2_; + size_t getPoseCounterById(const int &robotID) const; + std::vector pose_counter_robot_; private: size_t cyl_counter_; size_t cube_counter_; size_t point_landmark_counter_; - std::vector pose_counter_robot_; std::vector robot_prev_pose_; // ROS NodeHandle for getting rosparams diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index 5b4dd88..bc81fdc 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -12,7 +12,8 @@ sloam: main_node_rate: 5.0 # rate at which the main node runs, should not be less than the desired keyframe rate (e.g. 1-5 Hz is usually good) place_recognition_attempt_time_offset: 7.0 # (keep default unless you know what you are doing) time offset for place recognition attempts for different robot's nodes to avoid extra compute #----------------------------------------------- - intra_robot_place_recognition_frequency: 0.05 # [intra-robot place recog is default turned off] frequency at which place recognition is run within a robot + intra_robot_place_recognition_frequency: 0.05 # frequency at which place recognition is run within a robot + rel_inter_robot_factor_frequency: 1.0 # frequency at which relative inter-robot factors are attempted to be added number_of_robots: 2 robot_ns_prefix: /robot # robot_namespace will be robot_prefix + ROBOT_ID (e.g. /quadrotor1, /quadrotor2) debug_mode: true diff --git a/backend/sloam/src/core/input.cpp b/backend/sloam/src/core/input.cpp index 957fa96..e4c2fd0 100644 --- a/backend/sloam/src/core/input.cpp +++ b/backend/sloam/src/core/input.cpp @@ -17,7 +17,7 @@ * a measurement. * @param minOdomDistance_: The minimum distance of estimated odometry * movement to add a new odometry measurement. - * @param meas_to_add: Value will represent which measurement to add. + * @param[out] meas_to_add: Value will represent which measurement to add. * 0: None * 1: Odometry * 2: Observation diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index 0b3b2ba..bd55a93 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -45,7 +45,6 @@ Robot::Robot(const ros::NodeHandle &nh) : nh_(nh) { bool turn_off_rel_inter_robot_factor = nh_.param(node_name+"/turn_off_rel_inter_robot_factor", true); if (!turn_off_rel_inter_robot_factor) { - ROS_INFO("Relative Inter-Robot Factors are enabled!"); std::string relativeMeasSub_topic = "/relative_inter_robot_meas_sync"; RobotRelativeMeasSub_ = nh_.subscribe(relativeMeasSub_topic, 10, &Robot::RobotRelativeMeasCb, this); } diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index dc763a7..0c1ef10 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -305,4 +305,138 @@ bool sloam::RunSloam(SloamInput &in, SloamOutput &out) { } } // end of RunSloam +/** + * @brief Iterates through each relative measurement and checks + * if a corresponding pair of robot poses (within reasonable + * time) are found within the graph. Return all the matches. + * + * @param pose_counter_robot_: The pose counters for each robot, + * which tracks the number of pose variables in the factor graph. + * @param robotDataDict_: The robot data dictionary, which contains + * all recieved robot data for each robot. + * @param hostRobotID: The ID of the current robot. + * @param[out] matches: The vector of relative measurements that have + * matches onto poses for both robots in the factor graph. + */ +void sloam::FindRelativeMeasurementMatch(std::vector& pose_counter_robot_, + const std::unordered_map& robotDataDict_, + int hostRobotID, + std::vector& matches) { + // Lock mutexes + feasRelMeasVectorMtx_.lock(); + + // Maximum time difference between relative inter robot measurement and stamped pose + // in order to add measurement to the factor graph + double maxTimeDiff = 0.001; // 1 ms + + // Get the data of the host robot + std::deque poseMstPacketHost = robotDataDict_.at(hostRobotID).poseMstPacket; + + // Only run loop closures if we have relative inter-robot measurements waiting + if (feasible_relative_meas_for_factors.size() > 0) { + // Prepare return variables + int indexClosestHostRobot; + int indexClosestOtherRobot; + double timeDiffClosest; + + // Try to add each relative measurement + for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { + RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; + + // Make sure that the robot ID is not the same as the host robot + if (relativeMeas.robotIndex == hostRobotID) { + feasRelMeasVectorMtx_.unlock(); + throw std::runtime_error("robotIndex should not be the same as hostRobotID"); + } + + // Make sure that onlyUseOdom is not true + if (relativeMeas.onlyUseOdom) { + feasRelMeasVectorMtx_.unlock(); + throw std::runtime_error("onlyUseOdom measurements shouldn't get to this function"); + } + + // Iterate through the other robot's poses, and make sure one exists in the factor graph + // that is close enough in time to our measurement + size_t pose_counter_other = pose_counter_robot_[relativeMeas.robotIndex]; + std::deque poseMstPacketOther = robotDataDict_.at(relativeMeas.robotIndex).poseMstPacket; + GetIndexClosestPoseMstPair(poseMstPacketOther, relativeMeas.stamp, indexClosestOtherRobot, timeDiffClosest); + if (indexClosestOtherRobot == -1 || timeDiffClosest > maxTimeDiff || + indexClosestOtherRobot >= pose_counter_other) { + continue; + } + + // Iterate through our own poses, and make sure one exists in the factor graph thats close + // enough in time to our measurement + size_t pose_counter_host = pose_counter_robot_[hostRobotID]; + GetIndexClosestPoseMstPair(poseMstPacketHost, relativeMeas.stamp, indexClosestHostRobot, timeDiffClosest); + if (indexClosestHostRobot == -1 || timeDiffClosest > maxTimeDiff || + indexClosestHostRobot >= pose_counter_host) { + continue; + } + + // We found a match + matches.push_back(RelativeMeasMatch(i, indexClosestHostRobot, indexClosestOtherRobot)); + + // Remove this measurement from vector, so we don't use it again + feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); + i--; + } + + // Check which factors are no longer feasible so we can remove them + for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { + + // Get pose_counters (tracks number of poses added to factor graph) + RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; + size_t pose_counter_observed = pose_counter_robot_[relativeMeas.robotIndex]; + size_t pose_counter_host = pose_counter_robot_[hostRobotID]; + + // Get stamp of last poses if factor graphs aren't empty + ros::Time stamp_observed(0.0); + ros::Time stamp_host(0.0); + if (pose_counter_observed > 0) { + stamp_observed = robotDataDict_.at(relativeMeas.robotIndex).poseMstPacket[pose_counter_observed - 1].stamp; + } if (pose_counter_host > 0) { + stamp_host = poseMstPacketHost[pose_counter_host - 1].stamp; + } + + // If relative measurement stamp is earlier than both, then it can no longer be added + if (stamp_observed > relativeMeas.stamp && stamp_host > relativeMeas.stamp) { + feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); + i--; + } + } + } + + // Unlock mutexes + feasRelMeasVectorMtx_.unlock(); +} + +/** + * @brief This function searches through the deque of + * PoseMstPair and finds the message with the + * closest timestamp. It returns the corresponding + * index. If two entries have the same time difference, + * then the first index in deque is returned. + * + * @param std::deque &poseMstPacket : The queue to search. + * @param ros::Time stamp : The stamp we compare to. + * @param[out] int &indexClosets : Index in postMstPacket that is closest in time. + * Returns -1 if empty. + * @param[out] double &timeDiffClosest : The difference in time between stamp and + * the closest PoseMstPair (in seconds). + */ +void sloam::GetIndexClosestPoseMstPair(std::deque &poseMstPacket, + ros::Time stamp, int &indexClosest, double &timeDiffClosest) { + indexClosest = -1; + timeDiffClosest = std::numeric_limits::max(); + + for(int i = 0; i < poseMstPacket.size(); i++) { + double currTimeDiff = abs((poseMstPacket[i].stamp - stamp).toSec()); + if (currTimeDiff < timeDiffClosest) { + indexClosest = i; + timeDiffClosest = currTimeDiff; + } + } +} + } // namespace sloam diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index e3e4447..82c7b21 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -126,7 +126,7 @@ SLOAMNode::SLOAMNode(const ros::NodeHandle &nh) save_results_dir_ = ros::package::getPath("sloam") + "/results"; save_runtime_analysis_dir_ = save_results_dir_ + "/runtime_analysis"; runtime_analysis_file = save_runtime_analysis_dir_ + "/robot"+std::to_string(hostRobotID)+"_runtime_analysis.txt"; - ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file << ""); + ROS_DEBUG_STREAM("THE RUNTIME ANALYSIS FILE IS: " << runtime_analysis_file); } /** @@ -215,7 +215,11 @@ void SLOAMNode::publishCubeMaps_(const ros::Time stamp) { pubSubmapCubeModel_.publish(cubeSubMapTMarkerArray); } -void SLOAMNode::publishInterRobotFactors(const int &robotID) { +/** + * @brief This method publishes the inter-robot factors for + * visualization in RViz. + */ +void SLOAMNode::publishInterRobotFactors() { relInterRobotFactorsMtx_.lock(); @@ -280,7 +284,7 @@ void SLOAMNode::publishResults_(const SloamInput &sloamIn, const int &robotID) { publishMap_(stamp); publishCubeMaps_(stamp); - publishInterRobotFactors(robotID); + publishInterRobotFactors(); std::vector allLandmarks; std::vector allLabels; @@ -693,39 +697,8 @@ void SLOAMNode::interLoopClosureThread_() { } /** - * @brief This function searches through the deque of - * PoseMstPair and finds the message with the - * closest timestamp. It returns the corresponding - * index. If two entries have the same time difference, - * then the first index is returned. - * - * @param std::deque &poseMstPacket : The queue to search. - * @param ros::Time stamp : The stamp we compare to. - * @return int &indexClosets : Index in postMstPacket that is closest in time. - * Returns -1 if empty. - * @return double &timeDiffClosest : The difference in time between stamp and - * the closest PoseMstPair (in seconds). - */ -void SLOAMNode::GetIndexClosestPoseMstPair(std::deque &poseMstPacket, ros::Time stamp, int &indexClosest, double &timeDiffClosest) { - indexClosest = -1; - timeDiffClosest = std::numeric_limits::max(); - - for(int i = 0; i < poseMstPacket.size(); i++) { - double currTimeDiff = abs((poseMstPacket[i].stamp - stamp).toSec()); - if (currTimeDiff < timeDiffClosest) { - indexClosest = i; - timeDiffClosest = currTimeDiff; - } - } -} - -/** - * This function handles relative inter-robot factor generation - * by iterating through each relative measurement and seeing - * if the corresponding pair of robot poses (within reasonable - * time) are found within the graph. If so, its added to the - * factor graph. Does this at a rate of - * rel_inter_robot_factor_frequency_ Hz. + * @brief This function handles relative inter-robot factor generation. + * Does this at a rate of rel_inter_robot_factor_frequency_ Hz. * * NOTE: Currently relative inter-robot loop closures detected * by other robots are NOT sent to other robots, should fix @@ -736,113 +709,52 @@ void SLOAMNode::relInterRobotFactorThread_() { double desired_interval = 1.0 / rel_inter_robot_factor_frequency_; ros::Rate rate(desired_interval); - // Maximum time difference between relative inter robot measurement and stamped pose - // in order to add measurement to the factor graph - double maxTimeDiff = 0.001; // 1 ms + // Setup variable to hold all matches + std::vector matches; while (ros::ok()) { - // Lock mutexes dbMutex.lock(); - feasRelMeasVectorMtx_.lock(); - - // Retrive poses from host robot - std::deque poseMstPacket = dbManager.getHostRobotData().poseMstPacket; - - // Only run loop closures if we have relative inter-robot measurements waiting - if (feasible_relative_meas_for_factors.size() > 0) { - // Prepare return variables - int indexClosestHostRobot; - int indexClosestOtherRobot; - double timeDiffClosest; - int matches_found = 0; - - // Try to add each relative measurement - for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { - RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; - - // Iterate through the other robot's poses, and make sure one exists in the factor graph - // that is close enough in time to our measurement - size_t pose_counter_other = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); - GetIndexClosestPoseMstPair(dbManager.getRobotDataByID(relativeMeas.robotIndex).poseMstPacket, - relativeMeas.stamp, indexClosestOtherRobot, timeDiffClosest); - if (indexClosestOtherRobot == -1 || timeDiffClosest > maxTimeDiff || - indexClosestOtherRobot >= pose_counter_other) { - continue; - } - - // Iterate through our own poses, and make sure one exists in the factor graph thats close - // enough in time to our measurement - size_t pose_counter_host = factorGraph_.getPoseCounterById(hostRobotID); - GetIndexClosestPoseMstPair(poseMstPacket, relativeMeas.stamp, indexClosestHostRobot, timeDiffClosest); - if (indexClosestHostRobot == -1 || timeDiffClosest > maxTimeDiff || - indexClosestHostRobot >= pose_counter_host) { - continue; - } - - // We found a match, so add it to the factor graph - matches_found += 1; - factorGraphMtx_.lock(); - gtsam::Pose3 relativeMeasPose = SE3ToGTSAMPose3(relativeMeas.relativePose); - factorGraph_.addRelativeMeasFactor(relativeMeasPose, indexClosestHostRobot, - hostRobotID, indexClosestOtherRobot, relativeMeas.robotIndex); - factorGraphMtx_.unlock(); - //ROS_DEBUG_STREAM("Rel-Inter-Robot Factor added between Robot #" << hostRobotID << " Pose " << indexClosestHostRobot - // << "and Robot #" << relativeMeas.robotIndex << " Pose " << indexClosestOtherRobot); - - // Save this factor separately so that we can visualize it - RelativeInterRobotFactor factor; - factor.hostPoseIndex = indexClosestHostRobot; - factor.hostRobotID = hostRobotID; - factor.observedPoseIndex = indexClosestOtherRobot; - factor.observedRobotID = relativeMeas.robotIndex; - factor.relativePose = relativeMeas.relativePose; - factor.stamp = relativeMeas.stamp; - relInterRobotFactorsMtx_.lock(); - relative_inter_robot_factors.push_back(factor); - relInterRobotFactorsMtx_.unlock(); - - // Remove this measurement from vector, so we don't use it again - feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); - i--; - } - - // Check which factors are no longer feasible so we can remove them - for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { + + // Get all relative measurements that can be added as factors + FindRelativeMeasurementMatch(factorGraph_.pose_counter_robot_, + dbManager.getRobotDataDict(), hostRobotID, matches); - // Get pose_counter (tracks number of poses added to factor graph) - RelativeMeas relativeMeas = feasible_relative_meas_for_factors[i]; - size_t pose_counter = factorGraph_.getPoseCounterById(relativeMeas.robotIndex); + // Add each match to the graph + for (int i = 0; i < matches.size(); i++) { - // If factor graph for this robot isn't empty - if (pose_counter > 0) { + // Extract the relative measurement associated with this match + RelativeMeas relativeMeas = feasible_relative_meas_for_factors[matches[i].index]; + + // Add to the factor graph + factorGraphMtx_.lock(); + gtsam::Pose3 relativeMeasPose = SE3ToGTSAMPose3(relativeMeas.relativePose); + factorGraph_.addRelativeMeasFactor(relativeMeasPose, matches[i].indexClosestHostRobot, + hostRobotID, matches[i].indexClosestOtherRobot, relativeMeas.robotIndex); + factorGraphMtx_.unlock(); + + // Save this factor separately so that we can visualize it + RelativeInterRobotFactor factor; + factor.hostPoseIndex = matches[i].indexClosestHostRobot; + factor.hostRobotID = hostRobotID; + factor.observedPoseIndex = matches[i].indexClosestOtherRobot; + factor.observedRobotID = relativeMeas.robotIndex; + factor.relativePose = relativeMeas.relativePose; + factor.stamp = relativeMeas.stamp; + relInterRobotFactorsMtx_.lock(); + relative_inter_robot_factors.push_back(factor); + relInterRobotFactorsMtx_.unlock(); + } - // Get timestamp of most recent pose added to factor graph from observed robot - ros::Time stamp = dbManager.getRobotDataByID(relativeMeas.robotIndex).poseMstPacket[pose_counter - 1].stamp; + // Keep track of all added factors + num_successful_rel_inter_robot_factor += matches.size(); + ROS_INFO_STREAM_THROTTLE(20, "Total of " << num_successful_rel_inter_robot_factor << " Relative Factors added"); - // If this timestamp is later than relative measurement timestamp - if (stamp > relativeMeas.stamp) { - // This relative measurement is no longer feasible, as other robot - // simply doesn't have a pose factor to connect this measurement to - feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); - i--; - } - } - } - - // Keep track of total added - if (matches_found > 0) { - num_successful_rel_inter_robot_factor += matches_found; - ROS_INFO_STREAM_THROTTLE(20, "Total of " << num_successful_rel_inter_robot_factor << " Relative Factors added"); - } - } - else { - ROS_DEBUG_THROTTLE(20.0, "No Available Relative Inter Robot Measurements"); - } + // Empty out the matches for the next iteration + matches.clear(); // Unlock the mutexes and sleep dbMutex.unlock(); - feasRelMeasVectorMtx_.unlock(); rate.sleep(); } } diff --git a/backend/sloam/src/test/sloam_test.cpp b/backend/sloam/src/test/sloam_test.cpp new file mode 100644 index 0000000..82183a2 --- /dev/null +++ b/backend/sloam/src/test/sloam_test.cpp @@ -0,0 +1,209 @@ +#include +#include +#include +#include "ros/time.h" +#include "sloam.h" + +class SloamTest : public ::testing::Test { +protected: + // GetIndexClosestPoseMstPair + int indexClosest; + double timeDiffClosest; + sloam::sloam sloamInstance; + std::deque poseMstPacket; + + // FindRelativeMeasurementMatch + std::unordered_map robotDataDict_; + std::vector matches; +}; + +TEST_F(SloamTest, GetIndexClosestPoseMstPair) { + // Test with an empty deque + ros::Time stamp(10, 0); + sloamInstance.GetIndexClosestPoseMstPair(poseMstPacket, stamp, indexClosest, timeDiffClosest); + EXPECT_EQ(indexClosest, -1); + EXPECT_DOUBLE_EQ(timeDiffClosest, std::numeric_limits::max()); + + // Test with a single element + PoseMstPair p1; + p1.stamp = ros::Time(5, 0); + poseMstPacket.push_back(p1); + + sloamInstance.GetIndexClosestPoseMstPair(poseMstPacket, stamp, indexClosest, timeDiffClosest); + EXPECT_EQ(indexClosest, 0); + EXPECT_DOUBLE_EQ(timeDiffClosest, 5.0); + + // Test with multiple elements + p1.stamp = ros::Time(5, 0); + PoseMstPair p2; p2.stamp = ros::Time(15, 0); + PoseMstPair p3; p3.stamp = ros::Time(12, 0); + poseMstPacket = {p1, p2, p3}; + + stamp = ros::Time(11, 0); + sloamInstance.GetIndexClosestPoseMstPair(poseMstPacket, stamp, indexClosest, timeDiffClosest); + EXPECT_EQ(indexClosest, 2); + EXPECT_DOUBLE_EQ(timeDiffClosest, 1.0); + + // Test tie-breaking (choosing the first occurrence) + stamp = ros::Time(13, 500000000); + sloamInstance.GetIndexClosestPoseMstPair(poseMstPacket, stamp, indexClosest, timeDiffClosest); + EXPECT_EQ(indexClosest, 1); + EXPECT_DOUBLE_EQ(timeDiffClosest, 1.5); + + stamp = ros::Time(13, 400000000); + sloamInstance.GetIndexClosestPoseMstPair(poseMstPacket, stamp, indexClosest, timeDiffClosest); + EXPECT_EQ(indexClosest, 2); + EXPECT_DOUBLE_EQ(timeDiffClosest, 1.4); +}; + +TEST_F(SloamTest, FindRelativeMeasurementMatch) { + // Define host robot + int hostRobotID = 0; + + // Initialize all values to empty + std::vector pose_counter_robot_(2, 0); + robotData hostRobotData; + robotData otherRobotData; + robotDataDict_[0] = hostRobotData; + robotDataDict_[1] = otherRobotData; + sloamInstance.feasible_relative_meas_for_factors.clear(); + + // Test with empty input + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_TRUE(matches.empty()); + + // Test to make sure appropriate errors are raised for when robotIndex == hostRobotID + RelativeMeas meas; + meas.robotIndex = 0; + sloamInstance.feasible_relative_meas_for_factors.push_back(meas); + EXPECT_THROW(sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, + hostRobotID, matches), std::runtime_error); + sloamInstance.feasible_relative_meas_for_factors.clear(); + + // Test to make sure error is raised when useOnlyOdom == true + meas = RelativeMeas(); + meas.robotIndex = 1; + meas.onlyUseOdom = true; + sloamInstance.feasible_relative_meas_for_factors.push_back(meas); + EXPECT_THROW(sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, + hostRobotID, matches), std::runtime_error); + sloamInstance.feasible_relative_meas_for_factors.clear(); + + // Test with single relative measurement and no matching poses + meas.stamp = ros::Time(5, 0); + meas.onlyUseOdom = false; + sloamInstance.feasible_relative_meas_for_factors.push_back(meas); + + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_TRUE(matches.empty()); + + // Test with single relative measurement with a match + PoseMstPair hostPose; + hostPose.stamp = ros::Time(5, 0); + hostRobotData.poseMstPacket.push_back(hostPose); + pose_counter_robot_[0] = 1; + + PoseMstPair otherPose; + otherPose.stamp = ros::Time(5, 0); + otherRobotData.poseMstPacket.push_back(otherPose); + pose_counter_robot_[1] = 1; + + robotDataDict_[0] = hostRobotData; + robotDataDict_[1] = otherRobotData; + + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_EQ(matches.size(), 1); + EXPECT_EQ(matches[0].indexClosestHostRobot, 0); + EXPECT_EQ(matches[0].indexClosestOtherRobot, 0); + EXPECT_EQ(sloamInstance.feasible_relative_meas_for_factors.size(), 0); + + // Test with multiple measurements + matches.clear(); + sloamInstance.feasible_relative_meas_for_factors.clear(); + + RelativeMeas meas2; + meas2.stamp = ros::Time(7, 0); + meas2.robotIndex = 1; + meas2.onlyUseOdom = false; + sloamInstance.feasible_relative_meas_for_factors.push_back(meas); + sloamInstance.feasible_relative_meas_for_factors.push_back(meas2); + + PoseMstPair hostPose2; + hostPose2.stamp = ros::Time(7, 0); + hostRobotData.poseMstPacket.push_back(hostPose2); + pose_counter_robot_[0]++; + + PoseMstPair otherPose2; + otherPose2.stamp = ros::Time(7, 0); + otherRobotData.poseMstPacket.push_back(otherPose2); + pose_counter_robot_[1]++; + + robotDataDict_[0] = hostRobotData; + robotDataDict_[1] = otherRobotData; + + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_EQ(matches.size(), 2); + EXPECT_EQ(matches[0].indexClosestHostRobot, 0); + EXPECT_EQ(matches[0].indexClosestOtherRobot, 0); + EXPECT_EQ(matches[1].indexClosestHostRobot, 1); + EXPECT_EQ(matches[1].indexClosestOtherRobot, 1); + EXPECT_EQ(sloamInstance.feasible_relative_meas_for_factors.size(), 0); + + // Test not adding measurement exceeding time threshold (1ms) + matches.clear(); + sloamInstance.feasible_relative_meas_for_factors.clear(); + + RelativeMeas meas3; + meas3.stamp = ros::Time(10, 0); + meas3.robotIndex = 1; + meas3.onlyUseOdom = false; + sloamInstance.feasible_relative_meas_for_factors.push_back(meas3); + + PoseMstPair latehostPose; + latehostPose.stamp = ros::Time(9, 8000000); + hostRobotData.poseMstPacket.push_back(latehostPose); + pose_counter_robot_[0]++; + + PoseMstPair lateOtherPose; + lateOtherPose.stamp = ros::Time(10, 2000000); + otherRobotData.poseMstPacket.push_back(lateOtherPose); + pose_counter_robot_[1]++; + + robotDataDict_[0] = hostRobotData; + robotDataDict_[1] = otherRobotData; + + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_TRUE(matches.empty()); + EXPECT_EQ(sloamInstance.feasible_relative_meas_for_factors.size(), 1); + + // Test removing outdated measurements + matches.clear(); + sloamInstance.feasible_relative_meas_for_factors.clear(); + + RelativeMeas staleMeas; + staleMeas.stamp = ros::Time(2, 0); + staleMeas.robotIndex = 1; + staleMeas.onlyUseOdom = false; + sloamInstance.feasible_relative_meas_for_factors.push_back(staleMeas); + + hostPose.stamp = ros::Time(4, 0); + otherPose.stamp = ros::Time(4, 0); + hostRobotData.poseMstPacket = {hostPose}; + otherRobotData.poseMstPacket = {otherPose}; + + pose_counter_robot_[0] = 1; + pose_counter_robot_[1] = 1; + robotDataDict_[0] = hostRobotData; + robotDataDict_[1] = otherRobotData; + + sloamInstance.FindRelativeMeasurementMatch(pose_counter_robot_, robotDataDict_, hostRobotID, matches); + EXPECT_TRUE(matches.empty()); + EXPECT_TRUE(sloamInstance.feasible_relative_meas_for_factors.empty()); +}; + + +int main(int argc, char **argv) { + // Initialize GoogleTest and run all tests + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From e0efb9fd17f1a0680569414e25a4a05d9091b18c Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 18:01:38 -0400 Subject: [PATCH 64/74] github actions test 4/9 6:01 --- .github/workflows/SLIDE_SLAM_test.yml | 1 - frontend/relative_meas_gen/CMakeLists.txt | 4 ++-- frontend/relative_meas_gen/package.xml | 4 ++++ frontend/relative_meas_gen/tests/apriltag_test.cpp | 12 ++++-------- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index b1d4188..c094aed 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -44,6 +44,5 @@ jobs: docker start slideslam_ros docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/backend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/frontend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM - docker exec slideslam_ros bash -c "find . -type d" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash" docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose" diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index 16df89a..f3e7114 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp sloam_msgs + CATKIN_DEPENDS roscpp sloam_msgs roslib ) ## System dependencies are found with CMake's conventions @@ -204,7 +204,7 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(apriltag_test tests/apriltag_test.cpp src/apriltag_wrapper.cpp) target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) - target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES}) + target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES} ${roslib_LIBRARIES}) endif() ## Add folders to be run by python nosetests diff --git a/frontend/relative_meas_gen/package.xml b/frontend/relative_meas_gen/package.xml index e3038d0..84cdbcd 100644 --- a/frontend/relative_meas_gen/package.xml +++ b/frontend/relative_meas_gen/package.xml @@ -59,6 +59,7 @@ gazebo_msgs yaml-cpp sensor_msgs + roslib apriltag gtest nav_msgs @@ -70,6 +71,7 @@ std_msgs yaml-cpp sensor_msgs + roslib apriltag gtest message_runtime @@ -86,6 +88,8 @@ message_filters python-numpy tf + roslib + rostest gtest diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index d71420c..36b35fd 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -1,19 +1,15 @@ #include +#include #include #include #include TEST(apriltag_test, Detect) { - std::string path = "/home/cam/Documents/slideslam_docker_ws/src/SLIDE_SLAM/frontend/relative_meas_gen/tests/apriltag_test_pic.jpg"; - - //std::string package_path = ros::package::getPath("relative_meas_gen"); - //std::string path = package_path + "/test/apriltag_test_pic.jpg"; + std::string package_path = ros::package::getPath("relative_meas_gen"); + std::string image_path = package_path + "/tests/apriltag_test_pic.jpg"; - - std::cout << path << std::endl; - cv::Mat matrix = cv::imread(path, cv::IMREAD_COLOR).clone(); - std::cout << matrix.cols << std::endl; + cv::Mat matrix = cv::imread(image_path, cv::IMREAD_COLOR).clone(); cv::Mat image; cv::cvtColor(matrix, image, cv::COLOR_BGR2GRAY); From 92bc059c4ecf39a7dce554d052ba78e715961863 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 18:09:32 -0400 Subject: [PATCH 65/74] github actions test 4/9 6:09 --- frontend/relative_meas_gen/tests/apriltag_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index 36b35fd..c0db1ae 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -26,7 +26,7 @@ TEST(apriltag_test, Detect) { std::cout << "Detected tag: " << test_tag.id << std::endl; std::cout << "Expected tag: 6" << std::endl; - EXPECT_EQ(6, test_tag.id); + EXPECT_EQ(5, test_tag.id); } From 14b7b5f8b0a71d8968b920a51d51f61ba9ac83bf Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Wed, 9 Apr 2025 18:17:39 -0400 Subject: [PATCH 66/74] github actions test 4/9 6:17 --- frontend/relative_meas_gen/tests/apriltag_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index c0db1ae..36b35fd 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -26,7 +26,7 @@ TEST(apriltag_test, Detect) { std::cout << "Detected tag: " << test_tag.id << std::endl; std::cout << "Expected tag: 6" << std::endl; - EXPECT_EQ(5, test_tag.id); + EXPECT_EQ(6, test_tag.id); } From 1c875e6ae9ec340bb55fc359f9f3863fb7d7748b Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 02:29:00 +0000 Subject: [PATCH 67/74] Fix bug of removing RelativeMeas before using them --- backend/sloam/include/core/sloam.h | 6 ++++-- backend/sloam/src/core/sloam.cpp | 2 +- backend/sloam/src/core/sloamNode.cpp | 3 +-- backend/sloam/src/test/sloam_test.cpp | 5 ++++- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/backend/sloam/include/core/sloam.h b/backend/sloam/include/core/sloam.h index c9a900b..9380b33 100755 --- a/backend/sloam/include/core/sloam.h +++ b/backend/sloam/include/core/sloam.h @@ -64,8 +64,10 @@ struct SloamOutput { // Holds results from FindRelativeMeasurementMatch() struct RelativeMeasMatch { - RelativeMeasMatch(int i, int h, int o) : index(i), - indexClosestHostRobot(h), indexClosestOtherRobot(o) {} + RelativeMeasMatch(RelativeMeas m, int i, int h, int o) : + meas(m), index(i), indexClosestHostRobot(h), + indexClosestOtherRobot(o) {} + RelativeMeas meas; int index; int indexClosestHostRobot; int indexClosestOtherRobot; diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index 0c1ef10..2fc0fe4 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -375,7 +375,7 @@ void sloam::FindRelativeMeasurementMatch(std::vector& pose_counter_robot } // We found a match - matches.push_back(RelativeMeasMatch(i, indexClosestHostRobot, indexClosestOtherRobot)); + matches.push_back(RelativeMeasMatch(relativeMeas, i, indexClosestHostRobot, indexClosestOtherRobot)); // Remove this measurement from vector, so we don't use it again feasible_relative_meas_for_factors.erase(feasible_relative_meas_for_factors.begin() + i); diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index 82c7b21..5f33e7a 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -724,7 +724,7 @@ void SLOAMNode::relInterRobotFactorThread_() { for (int i = 0; i < matches.size(); i++) { // Extract the relative measurement associated with this match - RelativeMeas relativeMeas = feasible_relative_meas_for_factors[matches[i].index]; + RelativeMeas relativeMeas = matches[i].meas; // Add to the factor graph factorGraphMtx_.lock(); @@ -777,7 +777,6 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, ros::Time last_stamp = dbManager.getHostRobotData().poseMstPacket.back().stamp; if (stamp < last_stamp) { // Print out all available information on this data - ROS_ERROR_STREAM("Current stamp: " << stamp << " Last stamp: " << last_stamp); ROS_ERROR_STREAM("Adding a factor with a timestamp earlier than the last " "one in the factor graph! This should never happen."); } diff --git a/backend/sloam/src/test/sloam_test.cpp b/backend/sloam/src/test/sloam_test.cpp index 82183a2..ae6b424 100644 --- a/backend/sloam/src/test/sloam_test.cpp +++ b/backend/sloam/src/test/sloam_test.cpp @@ -115,6 +115,7 @@ TEST_F(SloamTest, FindRelativeMeasurementMatch) { EXPECT_EQ(matches.size(), 1); EXPECT_EQ(matches[0].indexClosestHostRobot, 0); EXPECT_EQ(matches[0].indexClosestOtherRobot, 0); + EXPECT_EQ(matches[0].meas.stamp.toSec(), meas.stamp.toSec()); EXPECT_EQ(sloamInstance.feasible_relative_meas_for_factors.size(), 0); // Test with multiple measurements @@ -122,7 +123,7 @@ TEST_F(SloamTest, FindRelativeMeasurementMatch) { sloamInstance.feasible_relative_meas_for_factors.clear(); RelativeMeas meas2; - meas2.stamp = ros::Time(7, 0); + meas2.stamp = ros::Time(7, 1000); meas2.robotIndex = 1; meas2.onlyUseOdom = false; sloamInstance.feasible_relative_meas_for_factors.push_back(meas); @@ -145,8 +146,10 @@ TEST_F(SloamTest, FindRelativeMeasurementMatch) { EXPECT_EQ(matches.size(), 2); EXPECT_EQ(matches[0].indexClosestHostRobot, 0); EXPECT_EQ(matches[0].indexClosestOtherRobot, 0); + EXPECT_EQ(matches[0].meas.stamp.toSec(), meas.stamp.toSec()); EXPECT_EQ(matches[1].indexClosestHostRobot, 1); EXPECT_EQ(matches[1].indexClosestOtherRobot, 1); + EXPECT_EQ(matches[1].meas.stamp.toSec(), meas2.stamp.toSec()); EXPECT_EQ(sloamInstance.feasible_relative_meas_for_factors.size(), 0); // Test not adding measurement exceeding time threshold (1ms) From 93cce8c2af1c24ae3b2814f6d2b973daac3e6e45 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 02:38:31 +0000 Subject: [PATCH 68/74] Remove cov from PoseMst.msg & raise errors in unfinished files --- backend/sloam_msgs/msg/PoseMst.msg | 1 - frontend/relative_meas_gen/scripts/dummyRelMeas.py | 3 +++ frontend/relative_meas_gen/scripts/odomGPSSync.py | 3 +++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/backend/sloam_msgs/msg/PoseMst.msg b/backend/sloam_msgs/msg/PoseMst.msg index bb0eb42..4ea02f1 100755 --- a/backend/sloam_msgs/msg/PoseMst.msg +++ b/backend/sloam_msgs/msg/PoseMst.msg @@ -1,6 +1,5 @@ geometry_msgs/Pose pose # 56 bytes geometry_msgs/Pose relativeRawOdom # 56 bytes -float64[6] relativeRawOdomCov # 48 bytes time stamp # 8 bytes (theorized, but not proven) ROSCube[] cubes # 69 bytes each ROSCylinder[] cylinders # 37 bytes each diff --git a/frontend/relative_meas_gen/scripts/dummyRelMeas.py b/frontend/relative_meas_gen/scripts/dummyRelMeas.py index b1e47f6..d7d8320 100755 --- a/frontend/relative_meas_gen/scripts/dummyRelMeas.py +++ b/frontend/relative_meas_gen/scripts/dummyRelMeas.py @@ -107,6 +107,9 @@ def publish_relative_meas(self, observer_msg, observed_msg): rospy.init_node(node_name) meas_gen = DummyRelativeMeasGen() + # NOTE: NODE NOT FULLY IMPLEMENTED/TESTED + raise NotImplementedError("This node is not fully implemented or tested. Please check the code and test it before use.") + # Run until shutdown while not rospy.is_shutdown(): rospy.spin() diff --git a/frontend/relative_meas_gen/scripts/odomGPSSync.py b/frontend/relative_meas_gen/scripts/odomGPSSync.py index 01fd662..c02a7b7 100755 --- a/frontend/relative_meas_gen/scripts/odomGPSSync.py +++ b/frontend/relative_meas_gen/scripts/odomGPSSync.py @@ -52,6 +52,9 @@ def publish_sync(self, msg_odom: nav_msgs.msg.Odometry, msg_gps: sensor_msgs.msg rospy.init_node(node_name) sync = OdomGPSSync() + # NOTE: NODE NOT FULLY IMPLEMENTED/TESTED + raise NotImplementedError("This node is not fully implemented or tested. Please check the code and test it before use.") + # Run until shutdown while not rospy.is_shutdown(): rospy.spin() From 053f64ddededb2bf362e4aef27286ba23ffd0ff4 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 03:40:16 +0000 Subject: [PATCH 69/74] Add comments --- .../relative_meas_gen/scripts/multiUGVToSlideSLAM.py | 2 +- .../src/relative_meas_gen/transforms.py | 11 ++++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py index 5dc0d61..48d2407 100755 --- a/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py +++ b/frontend/relative_meas_gen/scripts/multiUGVToSlideSLAM.py @@ -135,7 +135,7 @@ def gazebo_callback(self, msg: ModelStates): # Create the header for the messages header = Header() - header.stamp = rospy.Time.now() + header.stamp = rospy.Time.now() # Assume recieve time is close to publish time header.seq = self.sequence_number header.frame_id = "jackal0/base_link" diff --git a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py index d100db9..5732124 100644 --- a/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py +++ b/frontend/relative_meas_gen/src/relative_meas_gen/transforms.py @@ -2,8 +2,17 @@ from scipy.spatial.transform import Rotation from geometry_msgs.msg import Pose -# This method calculates the pose of "pose2" in the frame of "pose1" def calculate_relative_pose(pose1: Pose, pose2: Pose) -> Pose: + """ + This method calculates the pose of "pose2" in the frame of "pose1" + + Args: + pose1 (Pose): The first pose. + pose2 (Pose): The second pose. + + Returns: + Pose: The relative pose of pose2 in the frame of pose1. + """ p1 = np.array([pose1.position.x, pose1.position.y, pose1.position.z]) p2 = np.array([pose2.position.x, pose2.position.y, pose2.position.z]) From 005a6a8355b92904650de01be7b0dd1f6f08cef9 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 14:07:39 +0000 Subject: [PATCH 70/74] Remove graphImportant.cpp --- backend/sloam/src/factorgraph/graph.cpp | 61 +- .../sloam/src/factorgraph/graph_important.cpp | 663 ------------------ 2 files changed, 24 insertions(+), 700 deletions(-) delete mode 100755 backend/sloam/src/factorgraph/graph_important.cpp diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index f21cfa6..26213df 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -20,36 +20,26 @@ SemanticFactorGraph::SemanticFactorGraph() { // Get the start time start_time_ = ros::Time::now(); - // VERY IMPORTANT: the first three corresponds to Rotation and the last three - // corresponds to Translation - Vector6 noise_vec_prior_first_pose; - noise_vec_prior_first_pose << 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, - 0.00001; - noise_model_prior_first_pose = - noiseModel::Diagonal::Sigmas(noise_vec_prior_first_pose); - - // Landmark Noise model - // TODO(xu:) update the cylinder measurement noise - noise_model_cylinder = - noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); - - // TODO(xu:) update the cuboid measurement noise - double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); - double stddev_pos = 1.0; // 3.0; - Vector9 noise_vec; - noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, - stddev_pos, stddev_pos, stddev_pos, stddev_pos; - noise_model_cube = noiseModel::Diagonal::Sigmas(noise_vec); + // For all Pose3 Noise models note that the first three values + // correspond to RPY and the last three corresponds to XYZ. + // Proof from Frank Daellart here: + // https://github.com/borglab/gtsam/issues/205?utm_source=chatgpt.com + + // ================ First Pose Noise models ================ + noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); + + // ================= Loop Closure models ================= + // It is assumed to have 0.01 of the noise of the odometry + noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); + + // ================ Landmark Noise models ================ + + // TODO: update the cylinder measurement noise + noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); // For range and bearing (ellipsoid objects) measurements double bearing_noise_std_temp = 1; noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); - - Vector6 noise_model_pose_vec; - noise_model_pose_vec << 0.01, 0.01, 0.01, 0.02, 0.02, 0.02; // 2% drift per dist traveled & 0.5 degree - noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec); - noise_model_closure = - noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01); } void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, @@ -78,21 +68,18 @@ void SemanticFactorGraph::addKeyPoseAndBetween( const SE3 &loop_closure_relative_pose, const size_t &closure_matched_pose_idx) { - Vector6 cur_noise_vec; - // scale covariance by travel distance so that motion uncertainty is properly - // accounted for - cur_noise_vec = noise_model_pose->sigmas(); - double relative_dist = relativeMotion.translation().norm(); - // clip the relative dist to avoid numerical issues when optimizing - if (relative_dist < 0.1) { - relative_dist = 0.1; - } - Vector6 noise_vec_scaled_by_travel_dist = cur_noise_vec * relative_dist; + // Calculate the covariance for the relative motion + Vector6 noise_vec; + + // Scale covariance by travel distance + double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); + noise_vec = noise_model_odom_vec * relative_dist; + ROS_DEBUG_STREAM("Relative distance: " << relative_dist); // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance fgraph.add(BetweenFactor( getSymbol(robotID, prevIdx), getSymbol(robotID, curIdx), relativeMotion, - noiseModel::Diagonal::Sigmas(noise_vec_scaled_by_travel_dist))); + noiseModel::Diagonal::Sigmas(noise_vec))); // Only for active SLAM // fgraph_loop.add(BetweenFactor( diff --git a/backend/sloam/src/factorgraph/graph_important.cpp b/backend/sloam/src/factorgraph/graph_important.cpp deleted file mode 100755 index 06023f7..0000000 --- a/backend/sloam/src/factorgraph/graph_important.cpp +++ /dev/null @@ -1,663 +0,0 @@ -/** -* This file is part of SlideSLAM -* -* Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari -* -* TODO: License information -* -*/ - -#include -#include -#include - -SemanticFactorGraph::SemanticFactorGraph() { - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 1; - isam_params.relinearizeThreshold = 0.1; - isam = new ISAM2(isam_params); - - // Get the start time - start_time_ = ros::Time::now(); - - // For all Pose3 Noise models note that the first three values - // correspond to RPY and the last three corresponds to XYZ. - // Proof from Frank Daellart here: - // https://github.com/borglab/gtsam/issues/205?utm_source=chatgpt.com - - // ================ First Pose Noise models ================ - noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); - - // ================= Loop Closure models ================= - // It is assumed to have 0.01 of the noise of the odometry - noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); - - // ================ Landmark Noise models ================ - - // TODO(xu:) update the cuboid measurement noise - // double stddev_angle = 5 * (3.14 / 180); // 120.0 * (3.14 / 180); - // double stddev_pos = 1.0; // 3.0; - // Vector9 noise_vec; - // noise_vec << stddev_angle, stddev_angle, stddev_angle, stddev_pos, stddev_pos, - // stddev_pos, stddev_pos, stddev_pos, stddev_pos; - - // TODO(xu:) update the cylinder measurement noise - noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); - - // For range and bearing (ellipsoid objects) measurements - double bearing_noise_std_temp = 1; - noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); -} - -void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, - const int &robotID) { - size_t idx = 0; - fgraph.addPrior(getSymbol(robotID, idx), pose_prior, - noise_model_prior_first_pose); - // here the insert also set the initial estimate for the optimization - fvalues.insert(getSymbol(robotID, idx), pose_prior); - ROS_INFO_STREAM("Setting prior for robot " << robotID - << " with pose: " << pose_prior); - - // only for active SLAM: - // -------------------------------- - // "fake" factor graph for information gain evaluation only - // fgraph_loop.addPrior(getSymbol(robotID, idx), pose_prior, - // noise_model_prior_first_pose); - // fvalues_loop.insert(getSymbol(robotID, idx), pose_prior); - // current_pose_global_ = pose_prior; - // -------------------------------- -} - -void SemanticFactorGraph::addKeyPoseAndBetween( - const size_t prevIdx, const size_t curIdx, const Pose3 &relativeMotion, - const std::array relativeMotionCov, - const Pose3 &poseEstimate, const int &robotID, const bool &loopClosureFound, - const SE3 &loop_closure_relative_pose, - const size_t &closure_matched_pose_idx) { - - // Calculate the covariance for the relative motion - Vector6 noise_vec; - if(use_odom_cov_in_nav_msg_) { - // Use the covariance provided - for (int i = 0; i < 6; ++i) { - // Swap from variance XYZRPY to standard deviation RPYXYZ - noise_vec(i) = std::sqrt(relativeMotionCov[(i + 3) % 6]); - } - } else { - // Scale covariance by travel distance - double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); - noise_vec = noise_model_odom_vec * relative_dist; - ROS_DEBUG_STREAM("Relative distance: " << relative_dist); - } - - // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance - fgraph.add(BetweenFactor( - getSymbol(robotID, prevIdx), getSymbol(robotID, curIdx), relativeMotion, - noiseModel::Diagonal::Sigmas(noise_vec))); - - // Only for active SLAM - // fgraph_loop.add(BetweenFactor( - // getSymbol(robotID,prevIdx), getSymbol(robotID,curIdx), - // relativeMotion, noiseModel::Diagonal::Sigmas(cur_noise_vec))); - - if (latest_to_idx_ < curIdx) { - latest_to_idx_ = curIdx; - } - - if (loopClosureFound) { - // add loop_closure_relative_pose to X(closure_matched_pose_idx) - gtsam::Pose3 est_loop_closure_pose; - - // get current estimate of X(closure_matched_pose_idx) - gtsam::Pose3 est_from_pose; - - if (closure_matched_pose_idx == 0) { - ROS_ERROR( - "ERROR: closure_matched_pose_idx is 0, this probably means this " - "param is not correctly set, check!!"); - } - bool cur_pose_valid = - getPose(closure_matched_pose_idx, robotID, est_from_pose); - if (!cur_pose_valid) { - ROS_ERROR_STREAM( - "ERROR: get closure_matched_pose_idx fail to fetch pose for pose_idx " - ": " - << closure_matched_pose_idx); - // here the insert also set the initial estimate for the optimization - fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); - // Only for active SLAM - // fvalues_loop.insert(getSymbol(robotID,curIdx), poseEstimate); - // current_pose_global_ = poseEstimate; - return; - } - - // TODO(xu:) check why est_from_pose is removed here, maybe because we are - // no longer treating loop closure as a prior, instead we are using between - // factor - // but we should still keep this since we use it as initial guess when - // adding the pose key to the factor graph - - // gtsam::Pose3 est_from_pose = - // fvalues.at(X(closure_matched_pose_idx)); multiply - // est_from_pose.matrix and loop_closure_relative_pose.matrix - // loop_closure_relative_pose will be used for adding loop closure between - // factor - auto est_pose_matrix = - est_from_pose.matrix() * loop_closure_relative_pose.matrix(); - est_loop_closure_pose = gtsam::Pose3(est_pose_matrix); - - ROS_INFO_STREAM("++++++++++++CLOSURE FACTOR BEING ADDED++++++++++++"); - ROS_INFO_STREAM("loop_closure_relative_pose" - << loop_closure_relative_pose.matrix()); - ROS_INFO_STREAM("est_loop_closure_pose" << est_loop_closure_pose.matrix()); - - // No longer treating loop closure as a prior. Instead, use it as a proper - // between factor add loop closure as a between factor - // loop_closure_relative_pose is from history loop closure key pose to - // current pose The correct interface for BetweenFactor is (pose1, - // pose2, H_pose2^pose1, noise) - fgraph.add( - BetweenFactor(getSymbol(robotID, closure_matched_pose_idx), - getSymbol(robotID, curIdx), - gtsam::Pose3(loop_closure_relative_pose.matrix()), - noise_model_closure)); - - // Only for active SLAM: - // -------------------------------- - // fgraph_loop.add(BetweenFactor( - // getSymbol(robotID, closure_matched_pose_idx), - // getSymbol(robotID,curIdx), - // gtsam::Pose3(loop_closure_relative_pose.matrix()), - // noise_model_closure)); - // -------------------------------- - - // here the insert also set the initial estimate for the optimization - fvalues.insert(getSymbol(robotID, curIdx), est_loop_closure_pose); - // Only for active SLAM: - // fvalues_loop.insert(getSymbol(robotID, curIdx),est_loop_closure_pose); - ROS_INFO_STREAM("loop_closure_relative_pose being added"); - // current_pose_global_ = est_loop_closure_pose; - } else { - // here the insert also set the initial estimate for the optimization - fvalues.insert(getSymbol(robotID, curIdx), poseEstimate); - // Only for active SLAM - // fvalues_loop.insert(getSymbol(robotID, curIdx), poseEstimate); - // current_pose_global_ = poseEstimate; - } -} - -void SemanticFactorGraph::addPointLandmarkKey(const size_t ugvIdx, - const Point3 &ugv_position) { - fvalues.insert(U(ugvIdx), ugv_position); -} - -void SemanticFactorGraph::addRangeBearingFactor( - const size_t poseIdx, const size_t ugvIdx, - const Point3 &bearing_measurement, const double &range_measurement, - const int &robotID) { - // add bearing measurement with a covaraince - Unit3 bearing_measurement3D = Pose3().bearing(bearing_measurement); - // IMPORTANT: bearing vector should be expressed in body frame! - // ROS_INFO_STREAM_THROTTLE(1, - // "bearing vector should be expressed in body - // frame"); - - fgraph.add(BearingRangeFactor3D(getSymbol(robotID, poseIdx), U(ugvIdx), - bearing_measurement3D, range_measurement, - noise_model_bearing)); - // Only for active SLAM: - // fgraph_loop.add(BearingRangeFactor3D(X(poseIdx), - // U(ugvIdx), bearing_measurement3D, - // range_measurement, - // noise_model_bearing)); - if (latest_landmark_counter < ugvIdx) { - latest_landmark_counter = ugvIdx; - } -} - -void SemanticFactorGraph::addCylinderFactor(const size_t poseIdx, - const size_t cylIdx, - const Pose3 &pose, - const CylinderMeasurement &cylinder, - bool alreadyExists, - const int &robotID) { - // pose here is tf_map_to_sensor, not tf_sensor_to_map, hence it needs to be - // inverted - CylinderMeasurement c_local = cylinder.project(pose.inverse()); - fgraph.add(CylinderFactor(getSymbol(robotID, poseIdx), L(cylIdx), c_local, - noise_model_cylinder)); - if (!alreadyExists) { - fvalues.insert(L(cylIdx), cylinder); - } -} - -void SemanticFactorGraph::addCubeFactor( - const size_t poseIdx, const size_t cubeIdx, const Pose3 &pose, - const CubeMeasurement &cube_global_meas_raw, bool alreadyExists, - const int &robotID) { - // pose here is tf_map_to_sensor, not tf_sensor_to_map, hence it needs to be - // inverted - CubeMeasurement cube_global_meas = cube_global_meas_raw; - - // Temporarily handling the edge case of cuboid having yaw close to 0 or - // pi will have yaw values flipping / jumping back and forth between 0 and - // pi, which cause the optimization to break, hard coding it to be 0 for now - // Update: removed this since our process node can handle this properly - - CubeMeasurement cube_local_meas = cube_global_meas.project(pose.inverse()); - - // Calculate noise based on distance from the robot to the cube - double relative_dist = std::max(cube_local_meas.pose.translation().norm(), 0.1); - Vector9 noise_vec_scaled = noise_model_cube_vec * relative_dist; - - // Add the factor - fgraph.add(CubeFactor(getSymbol(robotID, poseIdx), C(cubeIdx), - cube_local_meas, noiseModel::Diagonal::Sigmas(noise_vec_scaled))); - - if (cube_local_meas.pose.translation().norm() > 50) { - ROS_WARN_THROTTLE(1, "cube_local_meas.pose.translation().norm() is larger " - "than 25 meters, maybe it is due to the front end keeping " - "track of observations over a long time or maybe it is because " - " the robot is moving fast!!"); - } - - if (!alreadyExists) { - // Initialize with first measurement of cube (in fixed world frame) - fvalues.insert(C(cubeIdx), cube_global_meas); - } -} - -void SemanticFactorGraph::addLoopClosureFactor(const Pose3 poseRelative, - const size_t prevIdx, - const size_t robotID1, - const size_t curIdx, - const size_t robotID2) { - // Correct interface for BetweenFactor(pose1, pose2, H_pose2^pose1, - // noise) poseRelative is H_query^candidate, i.e., from query pose to - // candidate (history) pose candidate is robotID1, prevIdx query is robotID2, - // curIdx - fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), - getSymbol(robotID2, curIdx), poseRelative, - noise_model_closure)); -} - -void SemanticFactorGraph::addRelativeMeasFactor(const Pose3 poseRelative, const size_t prevIdx, - const size_t robotID1, const size_t curIdx, const size_t robotID2) { - - // Scale noise by distance of the relative measurement - double relative_dist = std::max(poseRelative.translation().norm(), noise_floor); - Vector6 noise_vec_scaled = noise_model_rel_meas_vec * relative_dist; - - // Add the factor - fgraph.add(BetweenFactor(getSymbol(robotID1, prevIdx), - getSymbol(robotID2, curIdx), poseRelative, - noiseModel::Diagonal::Sigmas(noise_vec_scaled))); -} - -void SemanticFactorGraph::solve() { - // ROS_INFO_STREAM("START SOLVING THE FACTOR GRAPH OPTIMIZATION PROBLEM"); - isam->update(fgraph, fvalues); - // Only for active SLAM: - // isam_loop->update(fgraph_loop, fvalues_loop); - - // Extract the result/current estimates - currEstimate = isam->calculateEstimate(); - - // Reset the newFactors and newValues list - fgraph.resize(0); - fvalues.clear(); -} - -CylinderMeasurement SemanticFactorGraph::getCylinder(const int idx) { - return currEstimate.at(L(idx)); -} - -CubeMeasurement SemanticFactorGraph::getCube(const int idx) { - return currEstimate.at(C(idx)); -} - -Point3 SemanticFactorGraph::getCentroidLandmark(const int idx) { - if (isam->valueExists(U(idx))) { - return currEstimate.at(U(idx)); - } else { - return Point3(); - } -} - -bool SemanticFactorGraph::getPose(const size_t idx, const int &robotID, - Pose3 &poseOut) { - if (robotID >= 0 || robotID < MAX_NUM_ROBOTS) { - if (isam->valueExists(getSymbol(robotID, idx))) { - poseOut = currEstimate.at(getSymbol(robotID, idx)); - return true; - } else { - printf( - "############# Error!!! Node of interest does not exist in factor " - "graph #############\n"); - std::cout << "the node is X(" << idx << ")" << std::endl; - poseOut = Pose3(); - return false; - } - // return currEstimate.at(getSymbol(robotID, idx)); - } else { - ROS_ERROR_STREAM("############# Error: " - << robotID << " is an invalid robotID !!! #############"); - // printf("############# Error: invalid robotID!!! #############\n"); - poseOut = Pose3(); - return false; - } -} - -Eigen::MatrixXd SemanticFactorGraph::getPoseCovariance(const int idx, - const int &robotID) { - if (robotID >= 0 || robotID < MAX_NUM_ROBOTS) { - return isam->marginalCovariance(getSymbol(robotID, idx)); - } else { - ROS_ERROR_STREAM("############# Error: " - << robotID << " is an invalid robotID !!! #############"); - return isam->marginalCovariance(getSymbol(robotID, idx)); - } -} - -Symbol SemanticFactorGraph::getSymbol(const int &robotID, const int idx) { - Symbol returned_symbol; - switch (robotID) { - case 0: - returned_symbol = X(idx); - break; - case 1: - returned_symbol = Y(idx); - break; - case 2: - returned_symbol = Z(idx); - break; - case 3: - returned_symbol = M(idx); - break; - case 4: - returned_symbol = N(idx); - break; - case 5: - returned_symbol = O(idx); - break; - case 6: - returned_symbol = P(idx); - break; - case 7: - returned_symbol = Q(idx); - break; - case 8: - returned_symbol = R(idx); - break; - case 9: - returned_symbol = S(idx); - break; - case 10: - returned_symbol = T(idx); - break; - case 11: - returned_symbol = V(idx); - break; - case 12: - returned_symbol = W(idx); - break; - default: - break; - } - return returned_symbol; -} - -Symbol SemanticFactorGraph::getSymbol(const int &robotID, const size_t idx) { - Symbol returned_symbol; - switch (robotID) { - case 0: - returned_symbol = X(idx); - break; - case 1: - returned_symbol = Y(idx); - break; - case 2: - returned_symbol = Z(idx); - break; - case 3: - returned_symbol = M(idx); - break; - case 4: - returned_symbol = N(idx); - break; - case 5: - returned_symbol = O(idx); - break; - case 6: - returned_symbol = P(idx); - break; - case 7: - returned_symbol = Q(idx); - break; - case 8: - returned_symbol = R(idx); - break; - case 9: - returned_symbol = S(idx); - break; - case 10: - returned_symbol = T(idx); - break; - case 11: - returned_symbol = V(idx); - break; - case 12: - returned_symbol = W(idx); - break; - default: - break; - } - return returned_symbol; -} - -// ONLY FOR ACTIVE SLAM, ALL FUNCTIONS BELOW -// ----------------------------------------------------- -// void SemanticFactorGraph::logEntropy() { -// // keep current covaraince matrix -// double sum_entropy_pose = 0.0; -// double sum_entropy_landmark = 0.0; -// size_t num_valid_poses = 0; -// size_t num_valid_landmarks = 0; -// // iterate through all latest_landmark_counter and latest_to_idx_ -// for (size_t i = 0; i < latest_to_idx_; i++) { -// // get the covariance matrix of the current pose -// if (isam->valueExists(X(i)) && isam_loop->valueExists(X(i))) { -// gtsam::Matrix cov = isam->marginalCovariance(X(i)); -// sum_entropy_pose += cov.trace(); -// num_valid_poses++; -// } -// } - -// // ROS_ERROR_STREAM("landmark marginal cov printing, num of landark is " -// // << latest_landmark_counter); -// for (size_t i = 0; i < latest_landmark_counter; i++) { -// // get the covariance matrix of the current pose -// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { -// // ROS_ERROR_STREAM("getting landmark cov "); -// gtsam::Matrix cov = isam->marginalCovariance(U(i)); -// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); -// sum_entropy_landmark += cov.trace(); -// num_valid_landmarks++; -// } -// } - -// std::ofstream entropy_log_file("/home/yuezhan/bags/entropy_log.txt", -// ios::app); if (entropy_log_file.fail()){ -// cout << "open file error!\n"; -// } -// ros::Time time_now = ros::Time::now(); -// // write a header for entropy log file if it is empty, header is TIME, -// ENTROPY_POSE, ENTROPY_LANDMARK, NUM_VALID_POSES if -// (entropy_log_file.tellp() == 0) { -// entropy_log_file << "TIME, ENTROPY_POSE, ENTROPY_LANDMARK, -// NUM_VALID_POSES, NUM_VALID_LANDMARKS" << std::endl; -// } -// entropy_log_file << (time_now-start_time_).toSec() << ", " << -// sum_entropy_pose << ", " << sum_entropy_landmark << ", " << num_valid_poses -// << ", " << num_valid_landmarks << std::endl; entropy_log_file.close(); - -// } - -// double SemanticFactorGraph::estimateClosureInfoGain( -// const std::vector &candidateTrajPoseIndices, -// const std::vector &travel_distances) { -// // candidateTrajPoseIndices is a vector of pose indices in the candidate -// // trajecotry from current pose to key pose 1, key pose 2, ..., key pose n - -// // travel_distances is a vector of travel distances between each pair of -// poses - -// // assert length of travel_distances is one less than length of -// // candidateTrajPoseIndices -// assert(travel_distances.size() == candidateTrajPoseIndices.size() - 1); - -// // safety check -// for (size_t i = 0; i < candidateTrajPoseIndices.size(); i++) { -// size_t thisPoseIdx = candidateTrajPoseIndices[i]; -// if (currEstimate.exists(X(thisPoseIdx)) == false) { -// ROS_ERROR_STREAM("current pose index is not in the graph, error!"); -// return -1; -// } -// } - -// NonlinearFactorGraph fgraph_loop_tmp; -// // iterate through all poses in the candidate trajectory -// for (size_t i = 0; i < candidateTrajPoseIndices.size() - 1; i++) { -// size_t currentPoseIdx = candidateTrajPoseIndices[i]; -// size_t keyPoseIdx = candidateTrajPoseIndices[i + 1]; -// auto motion_noise = noiseModel::Diagonal::Sigmas( -// noise_model_pose_vec_per_m * travel_distances[i]); - -// // add between factor between current pose and history pose -// Pose3 pose = currEstimate.at(X(currentPoseIdx)); -// Pose3 pose_history = currEstimate.at(X(keyPoseIdx)); -// // TODO: flip order of pose_history and pose, either way should work -// though Pose3 pose_relative = pose_history.between(pose); -// fgraph_loop_tmp.add(BetweenFactor(X(keyPoseIdx), -// X(currentPoseIdx), -// pose_relative, motion_noise)); -// } - -// // add fake loop factor -// ISAM2Result result = isam_loop->update(fgraph_loop_tmp); -// FactorIndices factor_ids = result.newFactorsIndices; - -// result.print(); - -// // keep current covaraince matrix -// double sum_entropy_pose = 0.0; -// double sum_entropy_fake_loop_pose = 0.0; -// double sum_entropy_landmark = 0.0; -// double sum_entropy_fake_loop_landmark = 0.0; -// // iterate through all latest_landmark_counter and latest_to_idx_ -// for (size_t i = 0; i < latest_to_idx_; i++) { -// // get the covariance matrix of the current pose -// gtsam::Matrix cov = isam->marginalCovariance(X(i)); -// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(X(i)); -// sum_entropy_pose += cov.trace(); -// sum_entropy_fake_loop_pose += cov_loop.trace(); -// } - -// // ROS_ERROR_STREAM("landmark marginal cov printing, num of landark is " -// // << latest_landmark_counter); -// for (size_t i = 0; i < latest_landmark_counter; i++) { -// // get the covariance matrix of the current pose -// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { -// // ROS_ERROR_STREAM("getting landmark cov "); -// gtsam::Matrix cov = isam->marginalCovariance(U(i)); -// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); -// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(U(i)); -// sum_entropy_landmark += cov.trace(); -// sum_entropy_fake_loop_landmark += cov_loop.trace(); -// // ROS_ERROR_STREAM("getting landmark cov ends "); -// } -// } -// // ROS_ERROR_STREAM("landmark marginal cov printing ends"); - -// double info_gain_pose = sum_entropy_pose - sum_entropy_fake_loop_pose; -// double info_gain_landmark = -// sum_entropy_landmark - sum_entropy_fake_loop_landmark; - -// ROS_ERROR_STREAM("sum_entropy_pose: " << sum_entropy_pose); -// ROS_ERROR_STREAM( -// "sum_entropy_fake_loop_pose: " << sum_entropy_fake_loop_pose); -// ROS_ERROR_STREAM("sum_entropy_landmark: " << sum_entropy_landmark); -// ROS_ERROR_STREAM( -// "sum_entropy_fake_loop_landmark: " << sum_entropy_fake_loop_landmark); -// ROS_ERROR_STREAM("info_gain_pose: " << info_gain_pose); -// ROS_ERROR_STREAM("info_gain_landmark: " << info_gain_landmark); - -// // remove fake loop factor -// ISAM2Result result_after_delete = -// isam_loop->update(NonlinearFactorGraph(), Values(), factor_ids); - -// bool sanity_check = true; -// // sanity check the covariance matrix -// if (sanity_check) { -// // keep current covaraince matrix -// sum_entropy_pose = 0.0; -// sum_entropy_fake_loop_pose = 0.0; -// sum_entropy_landmark = 0.0; -// sum_entropy_fake_loop_landmark = 0.0; -// // iterate through all latest_landmark_counter and latest_to_idx_ -// for (size_t i = 0; i < latest_to_idx_; i++) { -// // get the covariance matrix of the current pose -// if (isam->valueExists(X(i)) && isam_loop->valueExists(X(i))) { -// gtsam::Matrix cov = isam->marginalCovariance(X(i)); -// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(X(i)); -// sum_entropy_pose += cov.trace(); -// sum_entropy_fake_loop_pose += cov_loop.trace(); -// } -// } - -// for (size_t i = 0; i < latest_landmark_counter; i++) { -// // get the covariance matrix of the current pose -// if (isam->valueExists(U(i)) && isam_loop->valueExists(U(i))) { -// gtsam::Matrix cov = isam->marginalCovariance(U(i)); -// // ROS_ERROR_STREAM("landmark marginal cov: " << cov); -// gtsam::Matrix cov_loop = isam_loop->marginalCovariance(U(i)); -// sum_entropy_landmark += cov.trace(); -// sum_entropy_fake_loop_landmark += cov_loop.trace(); -// } -// // else { -// // ROS_ERROR_STREAM("landmark marginal cov for landmark # " -// // << i << "not exist"); -// // } -// } -// double info_gain_pose = sum_entropy_pose - sum_entropy_fake_loop_pose; -// double info_gain_landmark = -// sum_entropy_landmark - sum_entropy_fake_loop_landmark; -// ROS_ERROR_STREAM( -// "++++++++++++++++++++++++++++++++++++++++SANITY " -// "CHECK+++++++++++++++++++++++"); - -// // ROS_ERROR_STREAM("sum_entropy_pose: " << sum_entropy_pose); -// // ROS_ERROR_STREAM( -// // "sum_entropy_fake_loop_pose: " << sum_entropy_fake_loop_pose); -// // ROS_ERROR_STREAM("sum_entropy_landmark: " << sum_entropy_landmark); -// // ROS_ERROR_STREAM( -// // "sum_entropy_fake_loop_landmark: " << -// // sum_entropy_fake_loop_landmark); -// ROS_ERROR_STREAM( -// "info_gain_pose difference (should be 0): " << info_gain_pose); -// ROS_ERROR_STREAM( -// "info_gain_landmark difference (should be 0): " << -// info_gain_landmark); -// bool is_equal = isam->equals(*isam_loop); -// ROS_ERROR_STREAM("two factor graphs should be equal, are they? >> " -// << is_equal); -// ROS_ERROR_STREAM( -// "++++++++++++++++++++++++++++++++++++++++SANITY " -// "CHECK+++++++++++++++++++++++"); -// } -// // total weighted info gain -// double total_info_gain = 10.0 * info_gain_pose + info_gain_landmark; -// return total_info_gain; -// } -// ----------------------------------------------------------------------- From 7afdd84944d1f7e3f868c535cede6bce5cf705ac Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 15:26:09 +0000 Subject: [PATCH 71/74] Fix bug causing covariances to not load properly --- backend/sloam/include/factorgraph/graph.h | 4 +--- backend/sloam/params/sloam_multiUGV.yaml | 7 +++--- backend/sloam/src/factorgraph/graph.cpp | 22 ------------------- .../sloam/src/factorgraph/graphWrapper.cpp | 22 ++++++++++++++++++- 4 files changed, 25 insertions(+), 30 deletions(-) diff --git a/backend/sloam/include/factorgraph/graph.h b/backend/sloam/include/factorgraph/graph.h index e0d4c0d..1d3f44a 100755 --- a/backend/sloam/include/factorgraph/graph.h +++ b/backend/sloam/include/factorgraph/graph.h @@ -122,12 +122,10 @@ class SemanticFactorGraph { protected: // Noise models (per unit of relative distance) - double noise_floor = 0.1; // Clipping noises prevents numerical issues + double noise_floor = 0.01; // Clipping noises prevents numerical issues boost::shared_ptr noise_model_prior_first_pose; Vector6 noise_model_prior_first_pose_vec; - // If true, ignore noise_model_odom_vec and use the cov from the nav message instead - bool use_odom_cov_in_nav_msg_; Vector6 noise_model_odom_vec; boost::shared_ptr noise_model_closure; diff --git a/backend/sloam/params/sloam_multiUGV.yaml b/backend/sloam/params/sloam_multiUGV.yaml index bc81fdc..33a6cdd 100755 --- a/backend/sloam/params/sloam_multiUGV.yaml +++ b/backend/sloam/params/sloam_multiUGV.yaml @@ -62,7 +62,6 @@ sloam: # VERY IMPORTANT: For Pose3 Noise models, ordering is RPYXYZ. # Values are standard deviation of the noise in radians and meters respectively noise_model_prior_first_pose_vec: [0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001] - use_odom_cov_in_nav_msg: false # If true, ignore noise_model_odom_vec and use the covariance included in the nav_msgs/Odometry message - noise_model_odom_vec: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - noise_model_cube_vec: [0.017, 0.017, 0.017, 0.05, 0.05, 0.05, 0.01, 0.01, 0.01] - noise_model_rel_meas_vec: [0.001, 0.001, 0.001, 0.03, 0.03, 0.03] + noise_model_odom_vec: [0.01, 0.01, 0.01, 0.02, 0.02, 0.02] # Assume 2% m drift and 0.5 degrees per dist traveled + noise_model_cube_vec: [0.017, 0.017, 0.017, 0.05, 0.05, 0.05, 0.01, 0.01, 0.01] # 0.01 m scale, 0.05 m pos, and 1 degree per unit distance + noise_model_rel_meas_vec: [0.001, 0.001, 0.001, 0.03, 0.03, 0.03] # 0.03 m and 0.06 degree per unit distance diff --git a/backend/sloam/src/factorgraph/graph.cpp b/backend/sloam/src/factorgraph/graph.cpp index 26213df..c1c1b3d 100755 --- a/backend/sloam/src/factorgraph/graph.cpp +++ b/backend/sloam/src/factorgraph/graph.cpp @@ -19,27 +19,6 @@ SemanticFactorGraph::SemanticFactorGraph() { // Get the start time start_time_ = ros::Time::now(); - - // For all Pose3 Noise models note that the first three values - // correspond to RPY and the last three corresponds to XYZ. - // Proof from Frank Daellart here: - // https://github.com/borglab/gtsam/issues/205?utm_source=chatgpt.com - - // ================ First Pose Noise models ================ - noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); - - // ================= Loop Closure models ================= - // It is assumed to have 0.01 of the noise of the odometry - noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); - - // ================ Landmark Noise models ================ - - // TODO: update the cylinder measurement noise - noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); - - // For range and bearing (ellipsoid objects) measurements - double bearing_noise_std_temp = 1; - noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); } void SemanticFactorGraph::setPriors(const Pose3 &pose_prior, @@ -74,7 +53,6 @@ void SemanticFactorGraph::addKeyPoseAndBetween( // Scale covariance by travel distance double relative_dist = std::max(relativeMotion.translation().norm(), noise_floor); noise_vec = noise_model_odom_vec * relative_dist; - ROS_DEBUG_STREAM("Relative distance: " << relative_dist); // noiseModel::Diagonal::Sigmas() takes in standard deviation, not variance fgraph.add(BetweenFactor( diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index 0857ebc..aa91b5c 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -29,7 +29,6 @@ SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(const ros::NodeHandle &nh std::vector noise_model_rel_meas_temp; nh_.param("factor_graph/noise_model_prior_first_pose_vec", noise_model_prior_first_pose_temp, std::vector(6, 0.000001)); - nh_.param("factor_graph/use_odom_cov_in_nav_msg", use_odom_cov_in_nav_msg_, true); nh_.param("factor_graph/noise_model_odom_vec", noise_model_odom_temp, std::vector(6, 0.1)); nh_.param("factor_graph/noise_model_cube_vec", noise_model_cube_temp, std::vector(9, 0.1)); nh_.param("factor_graph/noise_model_rel_meas_vec", noise_model_rel_meas_temp, std::vector(6, 0.1)); @@ -42,6 +41,27 @@ SemanticFactorGraphWrapper::SemanticFactorGraphWrapper(const ros::NodeHandle &nh for(int i = 0; i < 9; i++) { noise_model_cube_vec(i) = noise_model_cube_temp[i]; } + + // For all Pose3 Noise models note that the first three values + // correspond to RPY and the last three corresponds to XYZ. + // Proof from Frank Daellart here: + // https://github.com/borglab/gtsam/issues/205?utm_source=chatgpt.com + + // ================ First Pose Noise models ================ + noise_model_prior_first_pose = noiseModel::Diagonal::Sigmas(noise_model_prior_first_pose_vec); + + // ================= Loop Closure models ================= + // It is assumed to have 0.01 of the noise of the odometry + noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_odom_vec * 0.01); + + // ================ Landmark Noise models ================ + + // TODO: update the cylinder measurement noise + noise_model_cylinder = noiseModel::Diagonal::Sigmas(100 * Vector7::Ones() * 4); + + // For range and bearing (ellipsoid objects) measurements + double bearing_noise_std_temp = 1; + noise_model_bearing = noiseModel::Isotropic::Sigma(3, bearing_noise_std_temp); } // for active team localization From b607e5e2cf449292bebe8a5c9d369a1367927a41 Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 16:38:16 +0000 Subject: [PATCH 72/74] WARN message tweaks --- backend/sloam/src/core/robot.cpp | 4 ++-- backend/sloam/src/core/sloam.cpp | 2 +- backend/sloam/src/core/sloamNode.cpp | 1 + backend/sloam/src/factorgraph/graphWrapper.cpp | 3 --- 4 files changed, 4 insertions(+), 6 deletions(-) diff --git a/backend/sloam/src/core/robot.cpp b/backend/sloam/src/core/robot.cpp index bd55a93..1a687d2 100755 --- a/backend/sloam/src/core/robot.cpp +++ b/backend/sloam/src/core/robot.cpp @@ -210,6 +210,6 @@ std::vector Robot::rosEllipsoid2EllipObj( } void Robot::PrintLateMsgWarning(const std::string &msg_type) { - ROS_WARN_STREAM_THROTTLE(10, msg_type << " message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); - ROS_WARN_STREAM_THROTTLE(10, "Please increase `msg_delay_tolerance` to account for lag in your system"); + ROS_WARN_STREAM_ONCE(msg_type << " message arrived after " << msg_delay_tolerance << " seconds, dicarding..."); + ROS_WARN_STREAM_ONCE("To avoid this, increase `msg_delay_tolerance` to account for lag in your system"); } \ No newline at end of file diff --git a/backend/sloam/src/core/sloam.cpp b/backend/sloam/src/core/sloam.cpp index 2fc0fe4..48f4fe1 100755 --- a/backend/sloam/src/core/sloam.cpp +++ b/backend/sloam/src/core/sloam.cpp @@ -382,7 +382,7 @@ void sloam::FindRelativeMeasurementMatch(std::vector& pose_counter_robot i--; } - // Check which factors are no longer feasible so we can remove them + // Check which measurements are no longer feasible so we can remove them for(int i = 0; i < feasible_relative_meas_for_factors.size(); i++) { // Get pose_counters (tracks number of poses added to factor graph) diff --git a/backend/sloam/src/core/sloamNode.cpp b/backend/sloam/src/core/sloamNode.cpp index 5f33e7a..7f2480f 100644 --- a/backend/sloam/src/core/sloamNode.cpp +++ b/backend/sloam/src/core/sloamNode.cpp @@ -798,6 +798,7 @@ bool SLOAMNode::runSLOAMNode(const SE3 &relativeRawOdomMotion, pmp.cubeMsts = cubesBodyIn; pmp.ellipsoidMsts = ellipsoidBodyIn; dbManager.getHostRobotData().poseMstPacket.push_back(pmp); + std::vector cylindersBody; std::vector cubesBody; std::vector ellipsoidBody; diff --git a/backend/sloam/src/factorgraph/graphWrapper.cpp b/backend/sloam/src/factorgraph/graphWrapper.cpp index aa91b5c..1ad52aa 100755 --- a/backend/sloam/src/factorgraph/graphWrapper.cpp +++ b/backend/sloam/src/factorgraph/graphWrapper.cpp @@ -111,9 +111,6 @@ bool SemanticFactorGraphWrapper::addSLOAMObservation( gtsam::Pose3 relativeMotion(relativeMotionSE3.matrix()); pose_counter = pose_counter_robot_[robotID]; - // ROS_INFO_STREAM("################ ROBOT " << robotID << " pose counter: " - // << "################"); - if (pose_counter == 0) { // set priors for the first pose setPriors(curr_pose, robotID); From a632815de411dcd001c9085e88622989253f820b Mon Sep 17 00:00:00 2001 From: Daniel Butterfield Date: Thu, 10 Apr 2025 17:02:06 +0000 Subject: [PATCH 73/74] Refactor test files --- frontend/relative_meas_gen/CMakeLists.txt | 17 ++++------------- .../relative_meas_gen/tests/apriltag_test.cpp | 4 ---- .../relative_meas_gen/tests/apriltag_test.test | 3 --- .../python_tests.launch} | 0 4 files changed, 4 insertions(+), 20 deletions(-) delete mode 100644 frontend/relative_meas_gen/tests/apriltag_test.test rename frontend/relative_meas_gen/{launch/testCases.launch => tests/python_tests.launch} (100%) diff --git a/frontend/relative_meas_gen/CMakeLists.txt b/frontend/relative_meas_gen/CMakeLists.txt index f3e7114..6e0081e 100644 --- a/frontend/relative_meas_gen/CMakeLists.txt +++ b/frontend/relative_meas_gen/CMakeLists.txt @@ -193,24 +193,15 @@ target_link_libraries(apriltag_meas_gen ${catkin_LIBRARIES} ${OpenCV_LIBS} ${apr ## Testing ## ############# -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_relative_meas_gen.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - if (CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roscpp rostest) + find_package(rostest REQUIRED) + # Add C++ tests catkin_add_gtest(apriltag_test tests/apriltag_test.cpp src/apriltag_wrapper.cpp) target_link_libraries(apriltag_test ${catkin_LIBRARIES} GTest::GTest GTest::Main) target_link_libraries(apriltag_test ${OpenCV_LIBS} ${apriltag_LIBRARIES} ${roslib_LIBRARIES}) -endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest(launch/testCases.launch) + # Add Python tests + add_rostest(tests/python_tests.launch) endif() \ No newline at end of file diff --git a/frontend/relative_meas_gen/tests/apriltag_test.cpp b/frontend/relative_meas_gen/tests/apriltag_test.cpp index 36b35fd..b8047c3 100644 --- a/frontend/relative_meas_gen/tests/apriltag_test.cpp +++ b/frontend/relative_meas_gen/tests/apriltag_test.cpp @@ -23,11 +23,7 @@ TEST(apriltag_test, Detect) { std::vector tags = ExtractAprilTags(image, intrinsics, tagsize); apriltag_wrapper test_tag = tags[0]; - std::cout << "Detected tag: " << test_tag.id << std::endl; - std::cout << "Expected tag: 6" << std::endl; - EXPECT_EQ(6, test_tag.id); - } int main(int argc, char **argv) { diff --git a/frontend/relative_meas_gen/tests/apriltag_test.test b/frontend/relative_meas_gen/tests/apriltag_test.test deleted file mode 100644 index 53b4f37..0000000 --- a/frontend/relative_meas_gen/tests/apriltag_test.test +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/frontend/relative_meas_gen/launch/testCases.launch b/frontend/relative_meas_gen/tests/python_tests.launch similarity index 100% rename from frontend/relative_meas_gen/launch/testCases.launch rename to frontend/relative_meas_gen/tests/python_tests.launch From 0fa7384b6bcaffe4851a022ee1424e703a935dd2 Mon Sep 17 00:00:00 2001 From: cjackson357 Date: Thu, 10 Apr 2025 16:03:35 -0400 Subject: [PATCH 74/74] github actions test 4/10 4:03 --- .github/workflows/SLIDE_SLAM_test.yml | 4 +- ...n_container.sh => run_container_github.sh} | 20 +------- .../relative_meas_gen/bash_scripts/frames.gv | 47 ------------------- .../bash_scripts/tmux_relative_meas.sh | 4 +- .../src/apriltag_meas_gen.cpp | 2 +- 5 files changed, 5 insertions(+), 72 deletions(-) rename docker/{run_container.sh => run_container_github.sh} (52%) delete mode 100644 frontend/relative_meas_gen/bash_scripts/frames.gv diff --git a/.github/workflows/SLIDE_SLAM_test.yml b/.github/workflows/SLIDE_SLAM_test.yml index c094aed..a8c63ef 100644 --- a/.github/workflows/SLIDE_SLAM_test.yml +++ b/.github/workflows/SLIDE_SLAM_test.yml @@ -36,8 +36,8 @@ jobs: run: | cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker export WS_PATH="$(pwd)" - chmod +x run_container.sh - ./run_container.sh + chmod +x run_container_github.sh + ./run_container_github.sh - name: Build SLIDE_SLAM inside Container and run tests run: | diff --git a/docker/run_container.sh b/docker/run_container_github.sh similarity index 52% rename from docker/run_container.sh rename to docker/run_container_github.sh index f4ba0b8..3520ce5 100755 --- a/docker/run_container.sh +++ b/docker/run_container_github.sh @@ -2,25 +2,7 @@ SlideSlamWs="$WS_PATH" # point to your workspace directory SlideSlamCodeDir="$WS_PATH/src/SLIDE_SLAM" # point to code dir with SLIDE_SLAM BAGS_DIR="$WS_PATH/src/SLIDE_SLAM/bags" # point to your bags / data directory -DOCKER_RUN_FLAGS="-d" - -# If not running in CI, add -it -if [ -z "$CI" ]; then - DOCKER_RUN_FLAGS="-it" -fi - -EXTRA_VOLUMES="" -if [ -z "$CI" ]; then - EXTRA_VOLUMES+=" --gpus=\"all\"" - EXTRA_VOLUMES+=" --env=\"DISPLAY=$DISPLAY\"" - EXTRA_VOLUMES+=" --env=\"QT_X11_NO_MITSHM=1\"" - EXTRA_VOLUMES+=" --env=\"XAUTHORITY=/root/.Xauthority\"" - EXTRA_VOLUMES+=" --volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\"" - EXTRA_VOLUMES+=" --volume=\"/home/$USER/.bash_aliases:/root/.bash_aliases\"" - EXTRA_VOLUMES+=" --volume=\"$XAUTHORITY:/root/.Xauthority:ro\"" -fi - -docker run $DOCKER_RUN_FLAGS \ +docker run -d \ --name="slideslam_ros" \ --net="host" \ --privileged \ diff --git a/frontend/relative_meas_gen/bash_scripts/frames.gv b/frontend/relative_meas_gen/bash_scripts/frames.gv deleted file mode 100644 index d1c381a..0000000 --- a/frontend/relative_meas_gen/bash_scripts/frames.gv +++ /dev/null @@ -1,47 +0,0 @@ -digraph G { -"wilbur/base" -> "wilbur/base_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/odom" -> "wilbur/base"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 125.356 Hz\nMost recent transform: 1684528100.338 ( -0.958 sec old)\nBuffer length: 2.457 sec\n"]; -"wilbur/base_link" -> "wilbur/chassis_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/chassis_link" -> "wilbur/diff_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/chassis_link" -> "wilbur/embedded_imu_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_bottom_screw_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/forward_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_bottom_screw_frame" -> "wilbur/forward_camera_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_color_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_depth_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_aligned_depth_to_color_frame" -> "wilbur/forward_color_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_depth_frame" -> "wilbur/forward_depth_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/base_link" -> "wilbur/sensor_plate_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_depth_frame" -> "wilbur/forward_left_ir_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_left_ir_frame" -> "wilbur/forward_left_ir_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_depth_frame" -> "wilbur/forward_right_ir_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_right_ir_frame" -> "wilbur/forward_right_ir_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/imu_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/ouster_center_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/ouster_front_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/stereo_left_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/stereo_left_link" -> "wilbur/stereo_left_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/stereo_right_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/stereo_right_link" -> "wilbur/stereo_right_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/chassis_link" -> "wilbur/top_chassis_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/sensor_plate_link" -> "wilbur/ublox_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/imu_link" -> "wilbur/imu_link_base"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/ouster_center_link" -> "wilbur/os_imu"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/ouster_center_link" -> "wilbur/os_lidar"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_aligned_depth_to_color_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_infra1_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_infra1_frame" -> "wilbur/forward_infra1_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_link" -> "wilbur/forward_infra2_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/forward_infra2_frame" -> "wilbur/forward_infra2_optical_frame"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1684528099.380 sec old)\nBuffer length: 0.000 sec\n"]; -"wilbur/chassis_link" -> "wilbur/left_diff_unit_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 32.220 Hz\nMost recent transform: 1684528099.344 ( 0.036 sec old)\nBuffer length: 2.421 sec\n"]; -"wilbur/chassis_link" -> "wilbur/right_diff_unit_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 32.220 Hz\nMost recent transform: 1684528099.344 ( 0.036 sec old)\nBuffer length: 2.421 sec\n"]; -"wilbur/map" -> "wilbur/odom"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 10.435 Hz\nMost recent transform: 1684528099.301 ( 0.078 sec old)\nBuffer length: 2.300 sec\n"]; -"wilbur/left_diff_unit_link" -> "wilbur/front_left_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; -"wilbur/right_diff_unit_link" -> "wilbur/front_right_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; -"wilbur/left_diff_unit_link" -> "wilbur/rear_left_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; -"wilbur/right_diff_unit_link" -> "wilbur/rear_right_wheel_link"[label="Broadcaster: /play_1743431957317480563\nAverage rate: 20.417 Hz\nMost recent transform: 1684528099.360 ( 0.020 sec old)\nBuffer length: 2.400 sec\n"]; -edge [style=invis]; - subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; -"Recorded at time: 1684528099.380"[ shape=plaintext ] ; - }->"wilbur/map"; -} \ No newline at end of file diff --git a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh index 29af786..86701a7 100755 --- a/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh +++ b/frontend/relative_meas_gen/bash_scripts/tmux_relative_meas.sh @@ -2,7 +2,7 @@ # Set parameters SESSION_NAME=slide_slam_nodes -BAG_PLAY_RATE=4.0 +BAG_PLAY_RATE=1.0 BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/' SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311" @@ -44,8 +44,6 @@ tmux split-window -h -t $SESSION_NAME #tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam decentralized_sloam.launch enable_rviz:=false" Enter tmux select-pane -t $SESSION_NAME:1.1 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /tf /tf_static /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter -tmux select-pane -t $SESSION_NAME:1.2 -tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 4; roslaunch relative_meas_gen CoPeD_relative_meas_gen.launch" Enter tmux select-layout -t $SESSION_NAME tiled # Add window for roscore diff --git a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp index 1ffbd05..7daea83 100755 --- a/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp +++ b/frontend/relative_meas_gen/src/apriltag_meas_gen.cpp @@ -178,7 +178,7 @@ Eigen::Matrix4d ApriltagMeasurer::CalculateRelativeTransformation(Eigen::Matrix4 Eigen::Matrix4d H_cam_to_tag, Eigen::Matrix4d H_observedBot_to_tag) { // Calculate transformation from bot_to_tag Eigen::Matrix4d H_bot_to_tag = H_hostBot_to_cam * H_cam_to_tag; - + // Invert to get tag to observedBot Eigen::Matrix4d T_tag_to_observedBot = H_observedBot_to_tag.inverse();