Issue with robot_pose_ekf

edit

Hello Everyone,

Currently I am trying to use robot_pose_ekf package to get odometry data fused with yaw. But while launching the file i am getting the below error

robot_pose_ekf: /tmp/buildd/ros-hydro-bfl-0.7.0-3precise-20140617-0122/src/wrappers/matrix/matrix_BOOST.cpp:283: virtual MatrixWrapper::Matrix MatrixWrapper::Matrix::inverse() const: Assertion `res == 0' failed.

I have enabled the debug for robot_pose_ekf and i can see data for both odo and yaw in the respective temporary files /tmp/odom and /tmp/imu_data which prompts me to assume the data is getting published correctly.

If i disable the odo_used parameter in the launch file for robot_pose_ekf , the package doesnt hang.

Attahing the launch files for robot_pose_ekf and the corresponding publiher file Launch File:

<launch>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
            <param name="output_frame" value="odom"/>
            <param name="freq" value="30.0"/>
            <param name="sensor_timeout" value="4.0"/>
            <param name="odom_used" value="true"/>
            <param name="imu_used" value="true"/>
            <param name="vo_used" value="false"/>
            <param name="debug" value="true"/>
            <param name="self_diagnose" value="false"/>
    </node>

    <node pkg="tf" type="static_transform_publisher" name="base_imu" args="0 0 0 0 0 0 base_footprint yaw 100"/>

</launch>

Publisher Code: `int main(int argc, char** argv){

ros::init(argc, argv, "security_robot_publisher");
ros::NodeHandle security_robot_publisher_Node;
    //Subscribe to ODO data
    ucRespMsg= security_robot_publisher_Node.subscribe("uc0Response", 100, ucCommandCallback);

//Publisher for odo data
ros::Publisher odom_pub = security_robot_publisher_Node.advertise<nav_msgs::Odometry>("odom", 50);  
//Publisher for calibrated yaw
ros::Publisher yaw_pub = security_robot_publisher_Node.advertise<sensor_msgs::Imu>("imu_data",50);  

current_time = ros::Time::now();
last_time = ros::Time::now();

ros::Rate r(100.0);

while(security_robot_publisher_Node.ok()){

        ros::spinOnce();     
    current_time = ros::Time::now();          
    //Publish Odometry data into odo topic
    security_robot_odo_publisher(&odom_pub);

    //Publish calibrated Yaw data into yaw topic
    security_robot_yaw_publisher(&yaw_pub);
        r.sleep();
    last_time = current_time;
}

}

void security_robot_odo_publisher(ros::Publisher *odom_pub) { tf::TransformBroadcaster odom_broadcaster;

double x = 0.0;
double y = 0.0;
double th = 0.0;

double vx = 0.3;//0.1;
double vy = -0.1;
double vth = 0.3;//0.1;

double delta_x = 0;
double delta_y = 0;
double delta_th = 0;
double dt = (current_time -last_time).toSec();  


rwheel_abs_ticks_pres = rwheel_abs_ticks;
lwheel_abs_ticks_pres = lwheel_abs_ticks;

Dleft = (2*(pi)*(radius)*(lwheel_abs_ticks_pres - lwheel_abs_ticks_prev))/N;
Dright = (2*(pi)*(radius)*(rwheel_abs_ticks_pres - rwheel_abs_ticks_prev))/N;

Dcentre = (Dleft +Dright)/2;

ROS_INFO("DL:%lf, DR:%lf, DC:%lf",Dleft,Dright,Dcentre);
ROS_INFO("x:%lf, y:%lf, th:%lf",x,y,th);

delta_x = Dcentre*cos(th);
delta_y = Dcentre*sin(th);
    delta_th = (Dright - Dleft)/wheel_base;

    x += delta_x;
    y += delta_y;
    th += delta_th;

ROS_INFO("Dx:%lf, dy:%lf, Dth:%lf",delta_x,delta_y,delta_th);
ROS_INFO("x:%lf, y:%lf, th:%lf",x,y,th);

lwheel_abs_ticks_prev = lwheel_abs_ticks_pres;
rwheel_abs_ticks_prev = rwheel_abs_ticks_pres;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom ...