Getting odd error from odometry node

edit

I get this error , if I publish joint state data to robot state publisher. If I do not make this call my other publish calls work. Note: there is no robot_odometry log file generated.

[FATAL] [1380131167.482613461, 4.361000000]: ASSERTION FAILED
    file = /opt/ros/hydro/include/ros/publisher.h
    line = 102
    cond = false
    message = 
[FATAL] [1380131167.482767124, 4.361000000]: Call to publish() on an invalid Publisher
[FATAL] [1380131167.482799967, 4.361000000]: 

[robot_odometry-2] process has died [pid 27476, exit code -5, cmd /home/viki/catkin_ws/devel/lib/rrbot_control/robot_odometry __name:=robot_odometry __log:=/home/viki/.ros/log/2fab056a-260a-11e3-b2e4-28cfe95d78b1/robot_odometry-2.log].
log file: /home/viki/.ros/log/2fab056a-260a-11e3-b2e4-28cfe95d78b1/robot_odometry-2*.log




   #include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>



class PublishOdometry
{
public:
  PublishOdometry()
  {

     count=0;
     motor_position_joint1_new =0;
     motor_position_joint1_old =0;
     motor_position_joint2_new =0;
     motor_position_joint2_old =0;
     x = 0;               
     y = 0;
     theta = 0;

     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, 
           &PublishOdometry::handelerInitialPose,this);

     // Listen for motor control response
     subStateJoint1 = n.subscribe("/rrbot/joint1_position_controller/state", 10, 
           &PublishOdometry::handelerJointState1,this);

     subStateJoint2 = n.subscribe("/rrbot/joint2_position_controller/state", 10, 
           &PublishOdometry::handelerJointState2,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     odom_broadcaster.sendTransform(odom_trans);

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose.pose.position = msg.pose.pose.position;
     odometry.pose.pose.orientation = msg.pose.pose.orientation;

     pubOdometry.publish(odometry);

     ROS_INFO("handelerInitialPose ");
}

void handelerJointState1(const control_msgs::JointControllerState msg)
{
     motor_position_joint1_new = msg.process_value;
     calculatedOdometry(msg);
     calculateJointState(msg,"joint1");
}

void handelerJointState2(const control_msgs::JointControllerState msg)
{
     motor_position_joint2_new =  msg.process_value;
     calculatedOdometry(msg);  
     calculateJointState(msg,"joint2");
}

void calculateJointState(const control_msgs::JointControllerState msg, const std::string name)
{
     sensor_msgs::JointState jointStateMsg;
     jointStateMsg.name.resize(1);
     jointStateMsg.position.resize(1);
     jointStateMsg.velocity.resize(1);
     jointStateMsg.header.frame_id = "base_link";
     jointStateMsg.header.stamp = msg.header.stamp;
     jointStateMsg.name[0] = name;
     jointStateMsg.position[0] = msg.process_value;
     jointStateMsg.velocity[0] = msg.process_value_dot;
     pubJointState.publish(jointStateMsg);
}


void calculatedOdometry(const control_msgs::JointControllerState msg)
{
     double wheel_track = 0.4;   //distance between two wheels

     count = count +1;
     if (count<2) return;  // make sure we have both - should replace with TimeSynchronizer filter

     count =0;

     ROS_INFO("J1PV=%f  J2PV=%f",motor_position_joint1_new,motor_position_joint2_new);


     if (timeold.toSec()==0)  // skip first set of joint cmd
     {
       timeold = msg.header.stamp;
       motor_position_joint1_old = motor_position_joint1_new;
       motor_position_joint2_old = motor_position_joint2_new;
       return; 
     }


     float j1 = (motor_position_joint1_new - motor_position_joint1_old); 
     float j2 = (motor_position_joint2_new - motor_position_joint2_old);

     motor_position_joint1_old = motor_position_joint1_new;
     motor_position_joint2_old = motor_position_joint2_new;

     double  dt = (msg.header.stamp - timeold).toSec();  // elapsed time in seconds
     float   distance =    ( ((j1 + j2)*dt) / 2.0);             // distance in meters
     float   dtheta   =    ( ((j1 - j2)*dt) / wheel_track);     // rotation

     double dx = (distance*sin(dtheta ...