Ros Quickies

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(roveo_sensor_calibration)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

# find_package(Boost REQUIRED COMPONENTS system)
# find_package(Ceres REQUIRED)



catkin_package(
)

###########
## Build ##
###########

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
)

#SET( SRC_FILES
#    src/gauss_multi.cpp
#    src/gp.cpp
#    )
# refer this as: ${SRC_FILES}

#add_library(vins_lib
#    src/estimator/parameters.cpp
#    src/estimator/estimator.cpp
#)

add_executable(lidar_imu_calib src/lidar_imu_calib.cpp)
target_link_libraries(lidar_imu_calib
    #vins_lib
    ${catkin_LIBRARIES} ${PCL_LIBRARIES}
    )

main-fusion.cpp

#include <iostream>
#include <string>
using namespace std;

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

class Fusion
{
    public:
    Fusion()
    {
        //---- Read params
        ros::NodeHandle private_nh("~");
        private_nh.param("ODOMETRY_TOPIC_NAME", ODOMETRY_TOPIC_NAME, std::string("/base_pose_in_map"));

        private_nh.param("gyro_bias", gyro_bias, -1.0f );
        private_nh.param("accelerometer_bias", accelerometer_bias, -1.0f );
        
        /*
        // switch to this format of params
          private_nh.param<double>("clustTor_search", clustTor_search, 0.1);
          private_nh.param<bool>("useTurn", useTurn, false);


        */
        
        // yaml read
        string config_file;
        if( !private_nh.hasParam( "config_file" ) ) {
            cout << "cannot file param `config_file`" << endl;
            exit(1);
        }
        private_nh.param("config_file", config_file, std::string("no file") );
        auto cfg = YAMLConfigReader( config_file );



        //---- Subscribes and Publishers
        cout  << "Subscribe ODOMETRY_TOPIC_NAME=" << ODOMETRY_TOPIC_NAME  << endl;
        sub_lidar_odom = nh_.subscribe(ODOMETRY_TOPIC_NAME, 100, &Fusion::lidar_odom_callback, this);

        cout << "Advertise OUTPUT_ODOM_TOPIC_NAME=" << OUTPUT_ODOM_TOPIC_NAME << endl;
        pub_base_pose_in_map_100hz = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(OUTPUT_ODOM_TOPIC_NAME, 100);

    }

    //---- callbacks
    void lidar_odom_callback( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr msg )
    {
    }

    private:
    // ros
    ros::NodeHandle nh_;
    ros::Subscriber sub_lidar_odom;
    ros::Publisher pub_base_pose_in_map_100hz;

    // global variables
    float gyro_bias, accelerometer_bias;
    string ODOMETRY_TOPIC_NAME, OUTPUT_ODOM_TOPIC_NAME;


    // helper functions
};

int main(int argc, char ** argv )
{
    ros::init(argc, argv, "fusion_node");
    std::cout<<"+++ Fusion Node +++ \n";

    Fusion fusion_;

    ros::spin();
    return 0;
}

noide.launch

<launch>



    <node pkg="upsample_odom_imu_deadrecon" type="upsample_odom_ekf_v2" name="upsample_odom_ekf_v2" output="screen">
        <!-- TOPIC NAMES
        -->
        <param name="ODOMETRY_TOPIC_NAME" value="/base_pose_in_map" />

      
        <!-- sample parameters -->
        <param name="gyro_bias" value="-1.33371e-05" />
        <param name="accelerometer_bias" value="-0.648316" />

    </node>
    
    <arg name="lidar_count" default="1"/>
    <group if="$(eval arg('lidar_count') == 2)">
        <node  name="run_me" pkg="sample_p" type="run_me" output="screen" />
    </group>

</launch>

tf.cpp

    // Includes 
    #include <tf/transform_listener.h>
    #include <tf/transform_datatypes.h>
    #include <tf/transform_broadcaster.h>

    


    // TF TO eIGEN.
    // _t: time at which you want the transform
    // wait_for_t: The amount of time to wait for transform to be available
    // src_frame: Source frame 
    // target_frame: Target frame
    // @returns The transform src_T_target at Eigen::Matrix4d
    Matrix4d tf_waitForTransform( ros::Time _t, ros::Duration wait_for_, string src_frame, string target_frame )
    {
        // get from TF: imu_T_baseframe
        cout << "tf.waitForTransform\n";
        tf::TransformListener tf_listener;
        if(!tf_listener.waitForTransform(target_frame,  src_frame, _t, wait_for_))
        {
            ROS_ERROR("Failed to find transform between map_frame and base_frame");
            exit(1);
        }

        tf::StampedTransform transform_base_T_imu;
        tf_listener.lookupTransform(target_frame,  src_frame, _t, transform_base_T_imu);

        // tf --> Eigen
        tf::Quaternion quat = transform_base_T_imu.getRotation();
        tf::Vector3 origin = transform_base_T_imu.getOrigin();

        // cout <<transform_base_T_imu << endl;
        Matrix4d dst;
        PoseManipUtils::raw_xyzw_to_eigenmat(
            Vector4d(quat.x(), quat.y(), quat.z(), quat.w() ),
            Vector3d( origin.x(), origin.y(), origin.z() ),
            dst );

        return dst;
    }



    // Publishes the TF 
    void publish_base_pose_as_TF( ros::Time ekf__timestamp, const Matrix4d& ekf___w_T_baseframe, 
                                  const string frame_id,  const string child_frame_id )
    {
        Vector4d quat_xyzw; Vector3d tr;
        PoseManipUtils::eigenmat_to_raw_xyzw( ekf___w_T_baseframe, quat_xyzw, tr  );

        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(tr(0), tr(1), tr(2)) );
        transform.setRotation( tf::Quaternion( quat_xyzw(0), quat_xyzw(1), quat_xyzw(2), quat_xyzw(3) ) );

        br.sendTransform(tf::StampedTransform(transform, ekf__timestamp, frame_id, child_frame_id) );

    }