Add core_navigation pack

core_navigation/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(core_navigation)
+
+set (CMAKE_CXX_STANDARD 11)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  geometry_utils
+  parameter_utils
+  geometry_msgs
+  sensor_msgs
+  tf2_ros
+  tf
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    roscpp
+    geometry_utils
+    parameter_utils
+    geometry_msgs
+    sensor_msgs
+    tf2_ros
+    tf
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/CoreNav.cpp)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp)
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)

core_navigation/config/parameters.yaml

@@ -0,0 +1,57 @@
+init:
+  position:
+    x: 0.691948429336368
+    y: -1.395751807594636
+    z: 312.7822375757000
+
+  velocity:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 1.605702911834783
+
+  bias:
+    accel:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    gyro:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+
+imu:
+  publish_hz: 50
+  sensor_pub_rate: 125
+  topic: /imu/dataAdis
+  noise:
+    accel_noise_sigma: 0.00005
+    gyro_noise_sigma: 0.0000024241
+    accel_bias_rw_sigma: 0.07
+    gyro_bias_rw_sigma: 0.15
+
+
+odo:
+  topic: /husky_velocity_controller/odom
+  updates:
+    do_zupt: false
+    do_zaru: false
+
+
+joint:
+  topic: /joint_states
+
+frames:
+  frame_id_out: map
+  frame_id_fixed: odom
+  frame_id_imu: imu_link
+  frame_id_odo: odom_link
+
+wheel:
+  T_r_: 0.125
+  s_or_: 0.0
+  s_delta_or_: 0.0

core_navigation/include/core_navigation/CoreNav.h

@@ -0,0 +1,204 @@
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ *    1. Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *
+ *    3. Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Please contact the author(s) of this library if you have any questions.
+ * Authors: Cagri, Ryan
+ */
+
+#ifndef core_navigation_H
+#define core_navigation_H
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/JointState.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_utils/Transform3.h>
+#include <geometry_msgs/PointStamped.h>
+#include <message_filters/time_synchronizer.h>
+
+#include <core_navigation/InsConst.h>
+#include <core_navigation/InsUtils.h>
+
+class CoreNav {
+public:
+
+typedef sensor_msgs::Imu ImuData;
+typedef nav_msgs::Odometry OdoData;
+typedef sensor_msgs::JointState JointData;
+typedef geometry_msgs::PoseStamped PoseData;
+
+typedef Eigen::VectorXd Vector;
+typedef Eigen::MatrixXd Matrix;
+
+typedef Eigen::Matrix<double, 3, 1> Vector3;
+typedef Eigen::Matrix<double, 4, 1> Vector4;
+typedef Eigen::Matrix<double, 6, 1> Vector6;
+typedef Eigen::Matrix<double, 13, 1> Vector13;
+typedef Eigen::Matrix<double, 15, 1> Vector15;
+
+typedef Eigen::Matrix<double, 3, 3> Matrix3;
+Matrix3 CbnMinus;
+Matrix3 eye3=Eigen::Matrix3d::Identity();
+Matrix3 zeros3=Eigen::Matrix3d::Zero(3,3);
+
+CoreNav::Vector13 odo;
+CoreNav::Vector6 imu;
+CoreNav::Vector4 joint;
+
+CoreNav();
+~CoreNav();
+
+// Calls LoadParameters and RegisterCallbacks. Fails on failure of either.
+bool Initialize(const ros::NodeHandle& n);
+
+private:
+// Node initialization
+bool Init(const ros::NodeHandle& n);
+bool LoadParameters(const ros::NodeHandle& n);
+bool RegisterCallbacks(const ros::NodeHandle& n);
+
+// Publish estimated  states.
+void PublishStates(const CoreNav::Vector3& states, const ros::Publisher& pub);
+
+void ImuCallback(const ImuData& imu_data_);
+void OdoCallback(const OdoData& odo_data_);
+void JointCallBack(const JointData& joint_data_);
+
+void Update(const CoreNav::Vector13& odo);
+void Propagate(const CoreNav::Vector6& imu, const CoreNav::Vector4& joint);
+
+//Zero vel update
+void zupt(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 errorStates, Eigen::MatrixXd P);
+// Zero ang. update
+void zaru(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 errorStates, Eigen::MatrixXd P, const CoreNav::Vector3 omega_b_ib);
+
+CoreNav::Vector3 calc_gravity(const double latitude, const double height);
+CoreNav::Matrix3 skew_symm(const CoreNav::Vector3 vec);
+CoreNav::Matrix3 eul_to_dcm(double phi, double theta, double psi);
+CoreNav::Vector3 dcm_to_eul(CoreNav::Matrix3 dcm);
+CoreNav::Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, CoreNav::Vector3 insLLHPlus, CoreNav::Vector3 insVelPlus, double dt,CoreNav::Matrix3 CbnPlus, double omega_ie, CoreNav::Vector3 omega_n_in,Vector3 f_ib_b,double gravity);
+CoreNav::Matrix calc_Q(double R_N, double R_E, CoreNav::Vector3 insLLHPlus, double dt, CoreNav::Matrix3 CbnPlus, CoreNav::Vector3 f_ib_b);
+
+CoreNav::Vector6 getImuData(const ImuData& imu_data_);
+CoreNav::Vector4 getJointData(const JointData &joint_data_);
+CoreNav::Vector13 getOdoData(const OdoData& odo_data_);
+
+// The node's name.
+std::string name_;
+
+// Subscriber
+ros::Subscriber imu_sub_;
+ros::Subscriber odo_sub_;
+ros::Subscriber joint_sub_;
+
+// Publisher.
+ros::Publisher position_pub_, velocity_pub_, attitude_pub_, enu_pub_;
+tf::TransformBroadcaster transformed_states_tf_broad;
+
+OdoData odo_data_;
+OdoData odo_data_prev_;
+ImuData imu_data_;
+JointData joint_data_;
+
+bool has_odo_ = false;
+bool has_joint_ = false;
+bool has_imu_ = false;
+bool first_odo_ = true;
+bool first_joint_ = true;
+bool first_imu_ = true;
+
+// Most recent time stamp for publishers.
+ros::Time stamp_;
+
+// Coordinate frames.
+std::string frame_id_out_;
+std::string frame_id_imu_;
+std::string frame_id_odo_;
+std::string frame_id_fixed_;
+
+// update rate [hz]
+unsigned int publish_hz_;
+unsigned int sensor_pub_rate_;
+
+// sub. topics
+std::string imu_topic_;
+std::string odo_topic_;
+std::string joint_topic_;
+
+// For initialization.
+bool initialized_;
+
+// Filter vars.
+int num_states_ = 15;
+CoreNav::Vector15 error_states_; // {pos., vel, att, ba, bg}
+CoreNav::Vector3 ba_;
+CoreNav::Vector3 bg_;
+CoreNav::Vector3 ins_att_, ins_vel_, ins_pos_, ins_enu_;
+CoreNav::Vector4 Z_;
+CoreNav::Matrix P_, Q_, STM_;
+Eigen::Matrix<double, 4, 4> R_;
+Eigen::Matrix<double, 3, 3> R_zupt;
+Eigen::Matrix<double, 3, 3> R_zaru;
+Eigen::Matrix<double, 15, 4> K_;
+Eigen::Matrix<double, 15, 3> K_zupt;
+Eigen::Matrix<double, 15, 3> K_zaru;
+Eigen::Matrix<double, 4, 15> H_;
+Eigen::Matrix<double, 3, 15> H_zupt;
+Eigen::Matrix<double, 3, 15> H_zaru;
+
+CoreNav::Vector3 H11_, H12_, H21_, H31_, H32_, H24_, H41_, H42_;
+double z11_, z21_, z31_, z41_;
+double rearVel_, headRate_, T_r_, s_or_, s_delta_or_;
+
+CoreNav::Vector3 omega_b_ib_, omega_b_ib_prev_, omega_n_ie_;
+CoreNav::Vector3 f_ib_b_, f_ib_b_prev_, omega_n_en_, omega_n_in_, grav_;
+CoreNav::Matrix3 Omega_b_ib_, Omega_n_ie_, Omega_n_en_;
+
+// imu noise params
+double accel_sigma_, accel_rw_, gyro_sigma_, gyro_rw_;
+
+// filter noise params
+double position_noise_, attitude_noise_, velocity_noise_, bias_noise_;
+
+// initial pose
+double init_x, init_y, init_z, init_vx, init_vy, init_vz, psiEst;
+double init_roll, init_pitch, init_yaw, sigma_x, sigma_y;
+double sigma_z, sigma_vx, sigma_vy, prev_stamp_, up_time_;
+double sigma_vz, sigma_roll, sigma_pitch, sigma_yaw;
+double imu_stamp_curr_, imu_stamp_prev_, odo_stamp_curr_, odo_stamp_prev_;
+double joint_stamp_curr_, joint_stamp_prev_;
+double dt_odo_, dt_imu_;
+int count=0;
+
+};
+#endif

core_navigation/include/core_navigation/InsConst.h

@@ -0,0 +1,22 @@
+/**
+ * @file   InsConst.h
+ * @brief  House values that are repeatedly used
+ * @author Cagri, Ryan
+ */
+
+#pragma once
+
+namespace INS {
+
+const double omega_ie = 7.292115e-5;   // rotation of Earth in rad/sec
+const double Ro = 6378137.0;   //WGS84 Equatorial Radius
+const double Rp = 6356752.31425; //WGS84 Polar Radius
+const double flat= 1.0/298.257223563; // WGS84 Earth flattening
+const double ecc = 0.0818191909425; // WGS84 Eccentricity 0.0818191908426
+const double t_const = pow((1.0 - flat),2.0); // constant
+const double wheel_radius=0.1651; //meters TODO ~0.12
+const double wheelbase=0.555; //meters TODO
+const double gravity= 9.80665;
+const double PI = 3.14159265358979;
+
+}

core_navigation/include/core_navigation/InsUtils.h

@@ -0,0 +1,43 @@
+/**
+ * @file   InsUtils.h
+ * @brief  Tools required to process inertial data
+ * @author Cagri, Ryan
+ */
+
+#pragma once
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Float64.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Point.h>
+#include <sensor_msgs/JointState.h>
+
+#include <Eigen/Dense>
+
+#include <core_navigation/InsConst.h>
+
+namespace INS {
+
+typedef Eigen::VectorXd Vector;
+typedef Eigen::MatrixXd Matrix;
+
+typedef Eigen::Matrix<double, 3, 1> Vector3;
+typedef Eigen::Matrix<double, 6, 1> Vector6;
+typedef Eigen::Matrix<double, 3, 3> Matrix3;
+
+Vector3 calc_gravity(const double latitude, const double height);
+
+Matrix3 skew_symm(const Vector3 vec);
+
+Matrix3 eul_to_dcm(double phi, double theta, double psi);
+
+Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, Vector3 insLLHPlus, Vector3 insVelPlus, double dt, Matrix3 CbnPlus, double omega_ie,Vector3 omega_n_in,Vector3 f_ib_b,double gravity);
+
+Matrix calc_Q(double R_N, double R_E, Vector3 insLLHPlus, double dt, Matrix3 CbnPlus,Vector3 f_ib_b);
+
+}