diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index ef0c16bbb..5460288ae 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -619,7 +619,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.57079632679 + 0 @@ -644,6 +644,7 @@ /fix_velocity 33.774497 -84.405001 + 90 309.0 0.001 0.001 0.001 0.0001 0.0001 0.0001 diff --git a/igvc_gazebo/config/ramp_lane.csv b/igvc_gazebo/config/ramp_lane.csv index 754f29810..12e8078bf 100644 --- a/igvc_gazebo/config/ramp_lane.csv +++ b/igvc_gazebo/config/ramp_lane.csv @@ -1 +1 @@ -33.7746465553, -84.4049956986 +33.7744970036, -84.4048174628 diff --git a/igvc_gazebo/config/waypoints_autonav_0.csv b/igvc_gazebo/config/waypoints_autonav_0.csv index 164e7ba1e..a6bc5d29f 100644 --- a/igvc_gazebo/config/waypoints_autonav_0.csv +++ b/igvc_gazebo/config/waypoints_autonav_0.csv @@ -1,5 +1,5 @@ -33.7746360976, -84.4047548331 -33.7745538754, -84.4048924451 -33.7744382483, -84.4048926838 -33.7743553358, -84.4047560492 -33.7744950498, -84.4052488655 \ No newline at end of file +33.7742906118346, -84.40482946425239 +33.7744032479795, -84.40493470388343 +33.7744015516094, -84.40507763991296 +33.7742906402805, -84.40517679775147 +33.7747034526282, -84.40500116144553 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_autonav_1.csv b/igvc_gazebo/config/waypoints_autonav_1.csv index 8dfe5de29..f1b77a699 100644 --- a/igvc_gazebo/config/waypoints_autonav_1.csv +++ b/igvc_gazebo/config/waypoints_autonav_1.csv @@ -1,5 +1,5 @@ -33.7743553358, -84.4047560492 -33.7744382483, -84.4048926838 -33.7745538754, -84.4048924451 -33.7746360976, -84.4047548331 -33.7744950498, -84.4052488655 \ No newline at end of file +33.7742906402805, -84.40517679775147 +33.7744015516094, -84.40507763991296 +33.7744032479795, -84.40493470388343 +33.7742906118346, -84.40482946425239 +33.7747034526282, -84.40500116144553 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_0.csv b/igvc_gazebo/config/waypoints_qual_0.csv index ff7d58f9e..1658be81a 100644 --- a/igvc_gazebo/config/waypoints_qual_0.csv +++ b/igvc_gazebo/config/waypoints_qual_0.csv @@ -1,2 +1,2 @@ -33.7744970057, -84.4052924784 -33.7747806719, -84.4052299548 +33.7747404520, -84.4050010479 +33.7746932055, -84.4046567163 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_1.csv b/igvc_gazebo/config/waypoints_qual_1.csv index 28f8812fa..c0015a95f 100644 --- a/igvc_gazebo/config/waypoints_qual_1.csv +++ b/igvc_gazebo/config/waypoints_qual_1.csv @@ -1,2 +1,2 @@ -33.774496988, -84.4052330973 -33.774781346, -84.4051866739 +33.774690857, -84.4050010059 +33.774655721, -84.4046558027 diff --git a/igvc_gazebo/config/waypoints_qual_10.csv b/igvc_gazebo/config/waypoints_qual_10.csv index 8723c3eca..ec587bc4d 100644 --- a/igvc_gazebo/config/waypoints_qual_10.csv +++ b/igvc_gazebo/config/waypoints_qual_10.csv @@ -1 +1 @@ -33.7744969822, -84.4047958812 +33.7743347316, -84.4046771296 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_2.csv b/igvc_gazebo/config/waypoints_qual_2.csv index 28b45bc39..3d4aa93bb 100644 --- a/igvc_gazebo/config/waypoints_qual_2.csv +++ b/igvc_gazebo/config/waypoints_qual_2.csv @@ -1 +1 @@ -33.7744969834, -84.405184529 +33.7746187315, -84.404655538 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_3.csv b/igvc_gazebo/config/waypoints_qual_3.csv index afbbf34c6..d8647ff8f 100644 --- a/igvc_gazebo/config/waypoints_qual_3.csv +++ b/igvc_gazebo/config/waypoints_qual_3.csv @@ -1 +1,3 @@ -33.774496984, -84.4051467422 +33.774623239, -84.4051035645 +33.774623238, -84.4048984439 +33.774623236, -84.4046771293 diff --git a/igvc_gazebo/config/waypoints_qual_4.csv b/igvc_gazebo/config/waypoints_qual_4.csv index fa82f3a13..b7a016e68 100644 --- a/igvc_gazebo/config/waypoints_qual_4.csv +++ b/igvc_gazebo/config/waypoints_qual_4.csv @@ -1 +1 @@ -33.7744969817, -84.4051089608 +33.7745781594, -84.4046771290 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_5.csv b/igvc_gazebo/config/waypoints_qual_5.csv index 1384e5ef5..6fbb07f1f 100644 --- a/igvc_gazebo/config/waypoints_qual_5.csv +++ b/igvc_gazebo/config/waypoints_qual_5.csv @@ -1 +1 @@ -33.774496985, -84.4050387849 +33.774528570, -84.4046771297 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_6.csv b/igvc_gazebo/config/waypoints_qual_6.csv index 1f37e34be..883f2011a 100644 --- a/igvc_gazebo/config/waypoints_qual_6.csv +++ b/igvc_gazebo/config/waypoints_qual_6.csv @@ -1 +1 @@ -33.7744969811, -84.4049826404 +33.7744970156, -84.4046771288 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_7.csv b/igvc_gazebo/config/waypoints_qual_7.csv index ec17128de..bcb4d72b2 100644 --- a/igvc_gazebo/config/waypoints_qual_7.csv +++ b/igvc_gazebo/config/waypoints_qual_7.csv @@ -1 +1 @@ -33.774496983,-84.4049524208 +33.774469970, -84.4046771283 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_8.csv b/igvc_gazebo/config/waypoints_qual_8.csv index e69de29bb..e96153ae2 100644 --- a/igvc_gazebo/config/waypoints_qual_8.csv +++ b/igvc_gazebo/config/waypoints_qual_8.csv @@ -0,0 +1 @@ +33.77441587480856, -84.40467713087766 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_9.csv b/igvc_gazebo/config/waypoints_qual_9.csv index 90bb1d363..b3ae0d6a9 100644 --- a/igvc_gazebo/config/waypoints_qual_9.csv +++ b/igvc_gazebo/config/waypoints_qual_9.csv @@ -1 +1 @@ -33.7744969833, -84.4048390622 +33.7743662886, -84.4046771292 \ No newline at end of file diff --git a/igvc_gazebo/launch/igvc_control.launch b/igvc_gazebo/launch/igvc_control.launch index 3cb0f5cc8..6335ae228 100644 --- a/igvc_gazebo/launch/igvc_control.launch +++ b/igvc_gazebo/launch/igvc_control.launch @@ -17,7 +17,7 @@ - + diff --git a/igvc_gazebo/launch/qualification.launch b/igvc_gazebo/launch/qualification.launch index c4ed94d61..28a478b54 100644 --- a/igvc_gazebo/launch/qualification.launch +++ b/igvc_gazebo/launch/qualification.launch @@ -19,11 +19,11 @@ - - + + - + diff --git a/igvc_gazebo/nodes/ground_truth/main.cpp b/igvc_gazebo/nodes/ground_truth/main.cpp index c733c7609..5b13c6cd8 100644 --- a/igvc_gazebo/nodes/ground_truth/main.cpp +++ b/igvc_gazebo/nodes/ground_truth/main.cpp @@ -43,8 +43,8 @@ void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg) // set up the correct header result.header = msg->header; - result.child_frame_id = "/base_footprint"; - result.header.frame_id = "/odom"; + result.child_frame_id = "base_footprint"; + result.header.frame_id = "odom"; tf::Quaternion quat; tf::Vector3 pos; diff --git a/igvc_navigation/config/ekf_localization_node_params.yaml b/igvc_navigation/config/ekf_localization_node_params.yaml index 9396e976d..2a3320ba6 100644 --- a/igvc_navigation/config/ekf_localization_node_params.yaml +++ b/igvc_navigation/config/ekf_localization_node_params.yaml @@ -148,6 +148,13 @@ imu0_queue_size: 5 #imu0_twist_rejection_threshold: 0.8 # #imu0_linear_acceleration_rejection_threshold: 0.8 # +imu1: /magnetometer +imu1_config: [ false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. imu0_remove_gravitational_acceleration: true diff --git a/igvc_navigation/launch/3D_nav_simulation.launch b/igvc_navigation/launch/3D_nav_simulation.launch new file mode 100644 index 000000000..960acb7c8 --- /dev/null +++ b/igvc_navigation/launch/3D_nav_simulation.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/igvc_navigation/src/wheel_odometer/Odometer.cpp b/igvc_navigation/src/wheel_odometer/Odometer.cpp index 33726eb23..7503185bd 100644 --- a/igvc_navigation/src/wheel_odometer/Odometer.cpp +++ b/igvc_navigation/src/wheel_odometer/Odometer.cpp @@ -1,9 +1,4 @@ #include "Odometer.h" -#include -#include -#include -#include -#include "Odometer.h" double wheel_separation; @@ -78,7 +73,7 @@ void Odometer::enc_callback(const igvc_msgs::velocity_pair& msg) odom.header.seq = seq++; // setting reference frames - odom.header.frame_id = "wheel_odom"; + odom.header.frame_id = "odom"; odom.child_frame_id = "base_footprint"; // set time then publish @@ -99,7 +94,19 @@ Odometer::Odometer(ros::NodeHandle& nh) // initialize position - map published is relative to position at time t=0 x = 0; y = 0; - yaw = 0; + + sensor_msgs::Imu initImuMsg; + sensor_msgs::ImuConstPtr msg = ros::topic::waitForMessage("/magnetometer", ros::Duration(1)); + if (msg == NULL) + { + ROS_INFO("No messages received"); + } + else + { + initImuMsg = *msg; + } + tf::Quaternion orientation(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); + yaw = tf::getYaw(orientation); } int main(int argc, char** argv) diff --git a/igvc_navigation/src/wheel_odometer/Odometer.h b/igvc_navigation/src/wheel_odometer/Odometer.h index 6c441ee3b..6e2b5ee1c 100644 --- a/igvc_navigation/src/wheel_odometer/Odometer.h +++ b/igvc_navigation/src/wheel_odometer/Odometer.h @@ -3,6 +3,9 @@ #include #include #include +#include +#include +#include class Odometer { diff --git a/igvc_perception/CMakeLists.txt b/igvc_perception/CMakeLists.txt index b1a49812a..08caa1dd1 100644 --- a/igvc_perception/CMakeLists.txt +++ b/igvc_perception/CMakeLists.txt @@ -171,15 +171,7 @@ include_directories( # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_subdirectory(src/tests) -endif () - +add_subdirectory(src/slam) add_subdirectory(src/pointcloud_filter) add_subdirectory(src/robot_pose_type_converter) add_subdirectory(src/slope_filter) diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h new file mode 100644 index 000000000..650863a2a --- /dev/null +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -0,0 +1,66 @@ +#ifndef SRC_MAGPOSEFACTOR_H +#define SRC_MAGPOSEFACTOR_H + +#include +#include + +class MagPoseFactor : public gtsam::NoiseModelFactor1 +{ + const gtsam::Point3 measured_; ///< The measured magnetometer values + const gtsam::Point3 nM_; ///< Local magnetic field (mag output units) + const gtsam::Point3 bias_; ///< bias + +public: + /** + * Constructor of factor that estimates nav to body rotation bRn + * @param key of the unknown Pose X in the factor graph + * @param measured magnetometer reading, a 3-vector + * @param scale by which a unit vector is scaled to yield a magnetometer reading + * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm + * @param bias of the magnetometer, modeled as purely additive (after scaling) + * @param model of the additive Gaussian noise that is assumed + */ + MagPoseFactor(gtsam::Key key, const gtsam::Point3& measured, double scale, const gtsam::Unit3& direction, + const gtsam::Point3& bias, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor1(model, key) + , // + measured_(measured) + , nM_(scale * direction) + , bias_(bias) + { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const + { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new MagPoseFactor(*this))); + } + + /** + * @brief vector of errors + */ + gtsam::Vector evaluateError(const gtsam::Pose3& nRb, boost::optional H) const override + { + // measured bM = nRbÕ * nM + b + gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H, boost::none) + bias_; + // H is 3x3 from above unrotate operation, but needs to be 3x6, ie. set the 3x3 block on the right + // to all zeros. + if (H) + { + H->conservativeResize(3, 6); + H->rightCols(3).setZero(); + } + const auto error = hx - measured_; + return error; + } + + void print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const override + { + std::cout << s << "MagPoseFactor on " << keyFormatter(key()) << "\n"; + std::cout << s << " Mag measurement: " << measured_.transpose() << std::endl; + noiseModel_->print(" noise model: "); + } +}; + +#endif // SRC_MAGPOSEFACTOR_H diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h new file mode 100644 index 000000000..2ea9114fd --- /dev/null +++ b/igvc_perception/include/slam/slam.h @@ -0,0 +1,113 @@ +#ifndef SRC_SLAM_H +#define SRC_SLAM_H + +#include +#include +#include +// Pose2 == (Point3, Rot3) +#include +// PriorFactor == Initial Pose +#include +// BetweenFactor == Odom measurement +#include +// The factor graph we are creating. Nonlinear since angle measurements are nonlinear. +#include +#include +// Helps initialize initial guess +#include +// Custom Mag Factor based around Pose3 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Convert between geodetic coordinates to local cartesian coordinates +#include +#include + +// Uncomment to enable debugging and have additional print statements +//#define _DEBUG 1 + +class Slam +{ +public: + Slam(); + +private: + ros::NodeHandle pnh_; + + ros::Subscriber odom_sub_; + ros::Subscriber imu_sub_; + ros::Subscriber gps_sub_; + ros::Subscriber mag_sub_; + ros::Subscriber wheel_sub_; + + ros::Publisher location_pub_; + ros::Publisher gps_location_pub_; + + void gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps); + void imuCallback(const sensor_msgs::Imu &msg); + void magCallback(const sensor_msgs::MagneticField &msg); + void integrateAndAddIMUFactor(); + void wheelOdomCallback(const nav_msgs::Odometry &msg); + void addWheelOdomFactor(); + void optimize(); + void initializeImuParams(); + void initializePriors(); + void initializeNoiseMatrices(); + void initializeDirectionOfLocalMagField(); + void addMagFactor(); + void updateTransform(const nav_msgs::Odometry &pos); + static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang, + const gtsam::Matrix &cov); + + // Defining some types + typedef gtsam::noiseModel::Diagonal noiseDiagonal; + typedef gtsam::Vector3 Vec3; + + // Defining some variables + gtsam::Values graphValues, result_; +#if defined(_DEBUG) + gtsam::Values history_; +#endif + + gtsam::NonlinearFactorGraph graph_; + tf2_ros::TransformBroadcaster world_transform_broadcaster_; + unsigned long curr_index_; + noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_, odometryNoise; + gtsam::Unit3 local_mag_field_; + gtsam::Point3 curr_mag_reading_; + gtsam::Pose3 curr_wheelOdom_reading_; + gtsam::ISAM2 isam_; + gtsam::PreintegratedImuMeasurements accum_; + ros::Time last_imu_measurement_; + bool imu_connected_, imu_update_available_, firstReading; + double scale_; + GeographicLib::LocalCartesian origin_ENU; + gtsam::Rot3 initOrientation; + + const double KGRAVITY = 9.81; + size_t n_discard_frames = 20; + + double g_x = 0; + double g_y = 0; + double g_theta = 0; + double g_xVar = 0; + double g_yVar = 0; + double g_zVar = 0; + double g_thetaVariance = 0; + ros::Time g_last_time; // Last time callback was called (to calculate delta t) +}; + +#endif // SRC_SLAM_H diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h new file mode 100644 index 000000000..2fc98ffb5 --- /dev/null +++ b/igvc_perception/include/slam/type_conversions.h @@ -0,0 +1,21 @@ +#ifndef SRC_TYPE_CONVERSIONS_H +#define SRC_TYPE_CONVERSIONS_H + +#include +#include +// Pose2 == (Point3, Rot3) +#include +#include +#include + +class Conversion +{ +public: + static gtsam::Pose3 odomMsgToGtsamPose3(const nav_msgs::Odometry &msg); + static gtsam::Pose2 odomMsgToGtsamPose2(const nav_msgs::Odometry &msg); + static gtsam::Point3 odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg); + static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); + static geometry_msgs::PoseWithCovariance gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos, const gtsam::Matrix &cov); +}; + +#endif // SRC_TYPE_CONVERSIONS_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch new file mode 100644 index 000000000..a6a1735ee --- /dev/null +++ b/igvc_perception/launch/slam.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/igvc_perception/package.xml b/igvc_perception/package.xml index 0d189c4ad..3d4d78f7a 100644 --- a/igvc_perception/package.xml +++ b/igvc_perception/package.xml @@ -46,6 +46,7 @@ grid_map_core grid_map_ros grid_map_msgs + geographiclib image_transport cv_bridge @@ -60,7 +61,6 @@ cv_bridge pcl_conversions image_geometry - geographiclib robot_localization pluginlib python-pytorch-pip diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt new file mode 100644 index 000000000..cce444459 --- /dev/null +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -0,0 +1,33 @@ +# Find and add Boost +find_package(Boost REQUIRED) +find_package(Boost COMPONENTS system filesystem timer REQUIRED) +include_directories(${Boost_INCLUDE_DIR}) +list(APPEND NEEDED_LIBRARIES ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) + +# Find Eigen +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) + +# Find GeographicLib helpful: "sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/" +# Geographiclib installs FindGeographicLib.cmake to this non-standard location +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) +include_directories(${GeographicLib_INCLUDE_DIR}) + +# Find GTSAM components +find_package(GTSAM REQUIRED) # Uses installed package +if(GTSAM_FOUND) + get_target_property(GTSAM_INCLUDE_DIR gtsam INTERFACE_INCLUDE_DIRECTORIES) + set(GTSAM_LIBRARIES gtsam) + include_directories(${GTSAM_INCLUDE_DIR}) + message(STATUS "Found gtsam, compiling slam") + add_executable(slam + main.cpp + slam.cpp + type_conversions.cpp) + target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) + target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES} ${GeographicLib_LIBRARIES}) + add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +else() + message(WARNING "Could not find gtsam. Not compiling slam!") +endif() \ No newline at end of file diff --git a/igvc_perception/src/slam/main.cpp b/igvc_perception/src/slam/main.cpp new file mode 100644 index 000000000..f88f37f0d --- /dev/null +++ b/igvc_perception/src/slam/main.cpp @@ -0,0 +1,9 @@ +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "slam"); + Slam slam_node; + ros::spin(); +} \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp new file mode 100644 index 000000000..8f6e7c96e --- /dev/null +++ b/igvc_perception/src/slam/slam.cpp @@ -0,0 +1,396 @@ +#include + +// Using statements +using gtsam::symbol_shorthand::B; // bias measurements +using gtsam::symbol_shorthand::V; // velocity measurements +using gtsam::symbol_shorthand::X; // pose measurements + +/** + * Initializes the factor graph and all the subscribers and publishers + */ +Slam::Slam() : pnh_{ "~" } +{ + imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::imuCallback, this); + gps_sub_ = pnh_.subscribe("/fix", 1, &Slam::gpsCallback, this); + wheel_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::wheelOdomCallback, this); + mag_sub_ = pnh_.subscribe("/magnetometer_mag", 100, &Slam::magCallback, this); + + location_pub_ = pnh_.advertise("/slam/position", 1); + gps_location_pub_ = pnh_.advertise("/odometry/gps", 1); + + curr_index_ = 0; + imu_connected_ = false; + imu_update_available_ = false; + + initializeDirectionOfLocalMagField(); + initializeNoiseMatrices(); + initializeImuParams(); + initializePriors(); +} + +/** + * The gpsCallback adds GPS measurements to the factor graph, publishes GPS odom measurements. + * Also adds magnetometer factors, wheel odom measurements, and imu factors, then + * @param gps A NavSatFix message derived from GPS measurements (published by /fix) + */ +void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps) +{ + // get the starting location as the origin + double lat, lon, h, E, N, U; + lat = gps->latitude; + lon = gps->longitude; + h = gps->altitude; +#if defined(_DEBUG) + ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); +#endif + if (firstReading) + { + origin_ENU = GeographicLib::LocalCartesian(lat, lon, h, GeographicLib::Geocentric::WGS84()); + firstReading = false; + } + origin_ENU.Forward(lat, lon, h, E, N, U); + + nav_msgs::Odometry odom; + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = "odom"; + + // set the position + odom.pose.pose.position.x = E; + odom.pose.pose.position.y = N; + odom.pose.pose.position.z = U; + gps_location_pub_.publish(odom); + + gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(E, N, U), gps_noise_); + graph_.add(gps_factor); + + addMagFactor(); + addWheelOdomFactor(); + + if (imu_update_available_) + { + integrateAndAddIMUFactor(); + imu_update_available_ = false; + } +} + +/** + * The imuCallback adds IMU measurements to the accumulator when it receives an IMU measurement + * @param msg An imu sensor message (published by yostlab_driver_node) + */ +void Slam::imuCallback(const sensor_msgs::Imu &msg) +{ + ros::Time curr_time = ros::Time::now(); + Vec3 measured_acc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); + Vec3 measured_omega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); + + if (!imu_connected_) + { + imu_connected_ = true; + accum_.integrateMeasurement(measured_acc, measured_omega, 0.005); + } + else + { + double deltaT = (curr_time - last_imu_measurement_).toSec(); + if (deltaT > 0) + accum_.integrateMeasurement(measured_acc, measured_omega, deltaT); + } + imu_update_available_ = true; + last_imu_measurement_ = curr_time; +} + +/** + * The magCallback updates the curr_mag_reading_ with its most recent value + * @param msg An MagneticField sensor message (published by yostlab_driver_node) + */ +void Slam::magCallback(const sensor_msgs::MagneticField &msg) +{ + curr_mag_reading_ = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); +} + +/** + * Adds the magFactor to the factor graph + */ +void Slam::addMagFactor() +{ + MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, local_mag_field_, + gtsam::Point3(1e-9, 1e-9, 1e-9), mag_noise_); + graph_.add(mag_factor); +} + +/** + * The wheelOdomCallback updates the curr_mag_reading_ with its most recent value + * @param msg An odometry sensor message + */ +void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) +{ + if (g_last_time.sec == 0) + { + g_last_time = ros::Time::now(); + } + ros::Duration delta_t = msg.header.stamp - g_last_time; + double dt = delta_t.toSec(); + + // the local frame velocities + double vx = msg.twist.twist.linear.x; + double vy = msg.twist.twist.linear.y; // currently zero with jessi but that will change with swervi + + // update the relative position from the previous factor + g_x += vx * dt * cos(g_theta) - vy * dt * sin(g_theta); + g_y += vx * dt * sin(g_theta) + vy * dt * cos(g_theta); + + g_theta += dt * msg.twist.twist.angular.z; + g_xVar += dt * msg.twist.covariance[0]; + g_yVar += dt * msg.twist.covariance[7]; + g_zVar += dt * msg.twist.covariance[14]; + g_thetaVariance += dt * msg.twist.covariance[35]; + + g_last_time = msg.header.stamp; +#if defined(_DEBUG) + ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " + << curr_wheelOdom_reading_.theta()); +#endif +} + +/** + * Adds the wheelOdom measurement to the factor graph + */ +void Slam::addWheelOdomFactor() +{ + gtsam::Pose3 betweenPose(gtsam::Rot3::Rz(g_theta), gtsam::Point3(g_x, g_y, 0.0)); + + auto factor = gtsam::BetweenFactor( + X(curr_index_), X(curr_index_ + 1), betweenPose, + noiseDiagonal::Sigmas( + (gtsam::Vector(6) << g_xVar, g_yVar, g_zVar, g_thetaVariance * 2, g_thetaVariance * 2, g_thetaVariance) + .finished())); + + graph_.add(factor); + + g_x = 0; + g_y = 0; + g_theta = 0; + g_xVar = 0; + g_yVar = 0; + g_zVar = 0; + g_thetaVariance = 0; +} + +/** + * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph and optimizes + * graph. + */ +void Slam::integrateAndAddIMUFactor() +{ + if (accum_.preintMeasCov().trace() != 0) + { + // Add bias factor + auto factor = boost::make_shared >( + B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); + graph_.add(factor); + graphValues.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + + // Add imu factor + gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), + accum_); + + graph_.add(imufac); + auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate + newPoseEstimate = + gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); + graphValues.insert(X(curr_index_ + 1), newPoseEstimate); + + Vec3 last_vel = result_.at(V(curr_index_)); + last_vel += accum_.deltaVij(); + graphValues.insert(V(curr_index_ + 1), last_vel); + + curr_index_++; + optimize(); + } + accum_.resetIntegration(); +} + +/** + * Triggers ISAM2 to optimize the current graph and publish the current estimated pose + */ +void Slam::optimize() +{ +#if defined(_DEBUG) + static int iteration = 0; + ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_); +#endif + // Adds new factors, updating the ISAM solution and relinearizing as needed. + isam_.update(graph_, graphValues); + +#if defined(_DEBUG) + // Add initial estimates to history_ + history_.insert(graphValues); + graph_.printErrors(history_); + + ROS_INFO_STREAM("printErrors:"); + isam_.getFactorsUnsafe().printErrors(history_); + + ROS_INFO_STREAM("isam_ graph"); + isam_.getFactorsUnsafe().print(); +#endif + + result_ = isam_.calculateEstimate(); + + gtsam::Matrix covariance = isam_.marginalCovariance(X(curr_index_)); + +#if defined(_DEBUG) + graph_.printErrors(graphValues); + + // Update variables with optimized ones + history_.update(result_); +#endif + + // Clear the objects holding new factors and node values for the next iteration + graph_.resize(0); + graphValues.clear(); + + // Discard first frames, while we wait for pose to converge + if (curr_index_ < n_discard_frames) + { + ROS_INFO("Discarding initial poses..."); + return; + } + + auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose + Vec3 curr_vel = result_.at(V(curr_index_)); + + // converts linear velocity to the local frame + auto global_to_local = curr_pose.rotation().toQuaternion().inverse(); + curr_vel = global_to_local._transformVector(curr_vel); + + Vec3 curr_ang = accum_.deltaRij().rpy() / accum_.deltaTij(); + auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang, covariance); +#if defined(_DEBUG) + ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); +#endif + + location_pub_.publish(odom_message); + updateTransform(odom_message); +} + +void Slam::updateTransform(const nav_msgs::Odometry &pos) +{ + geometry_msgs::TransformStamped tfLink; + tfLink.header.stamp = pos.header.stamp; + tfLink.header.frame_id = pos.header.frame_id; + tfLink.child_frame_id = pos.child_frame_id; + + tfLink.transform.translation.x = pos.pose.pose.position.x; + tfLink.transform.translation.y = pos.pose.pose.position.y; + tfLink.transform.translation.z = pos.pose.pose.position.z; + tfLink.transform.rotation = pos.pose.pose.orientation; + world_transform_broadcaster_.sendTransform(tfLink); +} + +/** + * Sets the IMU Params + */ +void Slam::initializeImuParams() +{ + // Should be replaced with actual imu measurements. Values should come from the launch file. + auto params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); + params->setAccelerometerCovariance(gtsam::I_3x3 * 0.1); + params->setGyroscopeCovariance(gtsam::I_3x3 * 0.1); + params->setIntegrationCovariance(gtsam::I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(Vec3(0, 0, 0)); + accum_ = gtsam::PreintegratedImuMeasurements(params); +} + +/** + * Adds initial values of variables in the factor graph. + */ +void Slam::initializePriors() +{ + // Adding Initial Position (Pose + Covariance Matrix) + sensor_msgs::Imu initImuMsg; + sensor_msgs::ImuConstPtr msg = ros::topic::waitForMessage("/magnetometer", ros::Duration(1)); + if (msg == NULL) + { + ROS_INFO("No messages received"); + } + else + { + initImuMsg = *msg; + } + initOrientation = gtsam::Rot3::Quaternion(initImuMsg.orientation.w, initImuMsg.orientation.x, + initImuMsg.orientation.y, initImuMsg.orientation.z); + gtsam::Pose3 priorPose = gtsam::Pose3(initOrientation, gtsam::Point3()); + noiseDiagonal::shared_ptr poseNoise = + noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); + graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); +#if defined(_DEBUG) + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); +#endif + graphValues.insert(X(0), priorPose); + + // Adding Initial Velocity (Pose + Covariance Matrix) + Vec3 priorVel(0.0, 0.0, 0.0); + noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); + graph_.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); +#if defined(_DEBUG) + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on v0"); +#endif + graphValues.insert(V(0), priorVel); + + // Adding Bias Prior + noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); + gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); + graph_.push_back(biasprior); +#if defined(_DEBUG) + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on B0"); +#endif + graphValues.insert(B(0), gtsam::imuBias::ConstantBias()); + + // Set flag for first reading + firstReading = true; + + optimize(); + ROS_INFO_STREAM("Priors Initialized."); +} + +/** + * Initializes the shared noise matrices from parameters in the launch file + */ +void Slam::initializeNoiseMatrices() +{ + double bias_noise = pnh_.param("biasNoiseConst", 0.03); + double gps_xy_noise = pnh_.param("gpsXYNoiseConstant", 0.15); + double gps_z_noise = pnh_.param("gpsZNoiseConstant", 0.15); + double mag_noise = pnh_.param("magNoiseConstant", 0.00000005); + gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); + mag_noise_ = noiseDiagonal::Sigmas(Vec3::Constant(mag_noise)); + bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); +} + +/** + * Initialize the local magnetic field direction from launch params + */ +void Slam::initializeDirectionOfLocalMagField() +{ + std::vector lmg = + pnh_.param("localMagneticField", std::vector{ 0.0000227095, 0.0000020783, -0.0000432753 }); + local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); + scale_ = Vec3(lmg[0], lmg[1], lmg[2]).norm(); + ROS_INFO_STREAM("Initializing Direction of Local Magnetic Field"); +} + +nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang, + const gtsam::Matrix &cov) +{ + nav_msgs::Odometry msg; + msg.header.stamp = ros::Time::now(); + msg.child_frame_id = "base_footprint"; + msg.header.frame_id = "odom"; + msg.pose = Conversion::gtsamPose3ToPose3Msg(pos, cov); + msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); + msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); + + msg.twist.covariance = msg.pose.covariance; + return msg; +} \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp new file mode 100644 index 000000000..dfc76989a --- /dev/null +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -0,0 +1,79 @@ +#include + +gtsam::Pose3 Conversion::odomMsgToGtsamPose3(const nav_msgs::Odometry &msg) +{ + return gtsam::Pose3(gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z), + gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)); +} + +gtsam::Pose2 Conversion::odomMsgToGtsamPose2(const nav_msgs::Odometry &msg) +{ + gtsam::Rot3 twist = gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z); + + return gtsam::Pose2(twist.yaw(), gtsam::Point2(msg.pose.pose.position.x, msg.pose.pose.position.y)); +} + +geometry_msgs::PoseWithCovariance Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos, const gtsam::Matrix &cov) +{ + geometry_msgs::PoseWithCovariance gPos; + gPos.pose.position.x = pos.x(); + gPos.pose.position.y = pos.y(); + gPos.pose.position.z = pos.z(); + gtsam::Vector3 r = pos.rotation().xyz(); + gPos.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); + + gPos.covariance[0] = cov(0); + gPos.covariance[1] = cov(1); + gPos.covariance[2] = cov(2); + gPos.covariance[3] = cov(3); + gPos.covariance[4] = cov(4); + gPos.covariance[5] = cov(5); + gPos.covariance[6] = cov(6); + gPos.covariance[7] = cov(7); + gPos.covariance[8] = cov(8); + gPos.covariance[9] = cov(9); + gPos.covariance[10] = cov(10); + gPos.covariance[11] = cov(11); + gPos.covariance[12] = cov(12); + gPos.covariance[13] = cov(13); + gPos.covariance[14] = cov(14); + gPos.covariance[15] = cov(15); + gPos.covariance[16] = cov(16); + gPos.covariance[17] = cov(17); + gPos.covariance[18] = cov(18); + gPos.covariance[19] = cov(19); + gPos.covariance[20] = cov(20); + gPos.covariance[21] = cov(21); + gPos.covariance[22] = cov(22); + gPos.covariance[23] = cov(23); + gPos.covariance[24] = cov(24); + gPos.covariance[25] = cov(25); + gPos.covariance[26] = cov(26); + gPos.covariance[27] = cov(27); + gPos.covariance[28] = cov(28); + gPos.covariance[29] = cov(29); + gPos.covariance[30] = cov(30); + gPos.covariance[31] = cov(31); + gPos.covariance[32] = cov(32); + gPos.covariance[33] = cov(33); + gPos.covariance[34] = cov(34); + gPos.covariance[35] = cov(35); + + return gPos; +} + +geometry_msgs::Vector3 Conversion::gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec) +{ + geometry_msgs::Vector3 gVec; + gVec.x = vec.x(); + gVec.y = vec.y(); + gVec.z = vec.z(); + return gVec; +} + +gtsam::Point3 Conversion::odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg) +{ + return gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); +} \ No newline at end of file diff --git a/igvc_perception/src/tests/CMakeLists.txt b/igvc_perception/src/tests/CMakeLists.txt deleted file mode 100644 index e69de29bb..000000000 diff --git a/igvc_platform/src/imu/YostLabDriver.cpp b/igvc_platform/src/imu/YostLabDriver.cpp index 58b8c4c4a..0c2824e92 100644 --- a/igvc_platform/src/imu/YostLabDriver.cpp +++ b/igvc_platform/src/imu/YostLabDriver.cpp @@ -293,12 +293,12 @@ void YostLabDriver::createAndPublishIMUMessage(std::vector &parsed_val) magnet_msg.magnetic_field.x = compass_raw[0]; magnet_msg.magnetic_field.y = compass_raw[1]; magnet_msg.magnetic_field.z = compass_raw[2]; - magnet_msg.magnetic_field_covariance = { 1e-5, 0, 0, 0, 1e-5, 0, 0, 0, 1e-5 }; + magnet_msg.magnetic_field_covariance = { 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 }; sensor_temp_ = parsed_val[13]; imu_pub_.publish(imu_msg); - magnet_pub_.publish(magnet_msg); + magnet_pub_.publish(magnet_msg); // Published in teslas lastUpdateTime_ = ros::Time::now(); last_quat_ = quat; msg_counter_++; diff --git a/install_dependencies.sh b/install_dependencies.sh index bf11b81ab..48b58ba44 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -13,3 +13,10 @@ fi rosdep update rosdep install -iy --from-paths ../../src --skip-keys='kindr serial' pip3 install --no-cache-dir torch torchvision + +## GTSAM +# Add PPA +sudo add-apt-repository -y ppa:borglab/gtsam-release-4.0 +sudo apt update # not necessary since Bionic +# Install: +sudo apt install -y libgtsam-dev libgtsam-unstable-dev \ No newline at end of file