ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Issue with robot_pose_ekf

asked 2014-10-10 06:39:26 -0600

Nithin gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-10-10 08:01:54 -0600

Tom Moore gravatar image

updated 2014-10-10 09:32:37 -0600

Looks like it's blowing up trying to invert a matrix. If I had to guess, your measurement covariances are wrong and are making the innovation covariance singular (i.e., uninvertible). You have two main issues that I can see after a quick review:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex++)
{
    odom.pose.covariance[iIndex]=0.001;
}

In this code block, you're not setting the diagonal indices. You're setting all the indices. The inverse of that matrix alone is singular. What you want is this:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex+=7) // Note the index increment is now 7
{
    odom.pose.covariance[iIndex]=0.001;
}

Second, you've zeroed out the covariance matrix for your IMU measurement. No sensor can be completely without error, which is what a zero covariance implies.

for(iIndex=0;iIndex <9;iIndex++)
{
  yaw_sens.orientation_covariance[iIndex]=0;
}

Even though this might not cause the inversion issue you originally posted, you should change it to

for(iIndex=0;iIndex <9;iIndex+=4) // Note the index increment is now 4
{
  yaw_sens.orientation_covariance[iIndex]=0.1; // Or whatever you want. Just make it non-zero and positive.
}

I'm pretty sure the covariance arrays in messages default to all zeros anyway, so you shouldn't have to handle the off-diagonal elements.

edit flag offensive delete link more

Comments

Thanks for the instructions. Will try this and update if this solves my issue

Nithin gravatar image Nithin  ( 2014-10-10 11:39:25 -0600 )edit

Thanks Moore. The issue got resolved after making changes as you suggested. Thanks a lot.

Regards, Nithin

Nithin gravatar image Nithin  ( 2014-10-12 23:43:02 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2014-10-10 06:39:26 -0600

Seen: 476 times

Last updated: Oct 10 '14