Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Issue with robot_pose_ekf

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:

    <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 pkg="tf" type="static_transform_publisher" name="base_imu" args="0 0 0 0 0 0 base_footprint yaw 100"/>


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);


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

    //Publish calibrated Yaw data into yaw topic
    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

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex++)

    //set the velocity
    odom.child_frame_id = "base_footprint";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy; //vy;  //as it is differential drive
    odom.twist.twist.angular.z = vth;

    //publish the message

}`void security_robot_yaw_publisher(ros::Publisher *yaw_pub) {

tf::TransformBroadcaster yaw_broadcaster;
sensor_msgs::Imu yaw_sens;
//if(calibrated_yaw !=0)

    geometry_msgs::Quaternion yaw_quat_1 = tf::createQuaternionMsgFromYaw(calibrated_yaw);

    //first, we'll publish the transform over tf
        /*geometry_msgs::TransformStamped yaw_trans;
        yaw_trans.header.stamp = current_time;
        yaw_trans.header.frame_id = "yaw";
        yaw_trans.child_frame_id = "base_link";

    yaw_trans.transform.rotation = yaw_quad_1;

        //send the transform

    //tf::Quaternion yaw_quad_2 = tf::createQuaternionFromRPY(1,1,2);
    yaw_sens.header.stamp    = ros::Time::now();
    yaw_sens.header.frame_id = "yaw";

    yaw_sens.orientation = yaw_quat_1;

    for(iIndex=0;iIndex <9;iIndex++)




Thanks in Advance, Nithin