0
$\begingroup$

Rosanswers logo

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));
     double dy = (distance*cos(dtheta)); 

     theta =  (theta+dtheta); //fmodf((theta+dtheta),(2*M_PI));                   
     x = x + dx;
     y = y + dy;

     ROS_INFO("x=%e  y=%e theta=%f ddtheta=%f ",x,y,theta, dtheta);

     timeold = msg.header.stamp;    // new old time

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = sin(theta/2);          // set yaw
     quaternion.w = cos(theta/2);

     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 = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0.0; 
     odom_trans.transform.rotation = quaternion;

     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.x  = distance;
     odometry.pose.pose.position.y  = y;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = x;
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = ((theta)*(M_PI/180));  // convert to radians

     pubOdometry.publish(odometry);

}


private:

   double x,y,count;
   float motor_position_joint1_new;
   float motor_position_joint1_old;
   float motor_position_joint2_new;
   float motor_position_joint2_old;
   float theta;
   ros::Time timeold;
   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Publisher  pubJointState;
   ros::Subscriber subInitialPose;
   ros::Subscriber subStateJoint1;
   ros::Subscriber subStateJoint2;



};  // Enodof Class



int main(int argc, char **argv)
{ 
  ros::init(argc, argv, "robot_odometry");

  PublishOdometry sp;

  // send static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  ros::spin();

  return 0;
}

Originally posted by rnunziata on ROS Answers with karma: 713 on 2013-09-25

Post score: 0

Original comments

Comment by Hendrik Wiese on 2013-09-25:
Where's your pubJointState defined? Can you give us that code, too?

Comment by rnunziata on 2013-09-25:
I will post the whole program. Please note I have other odometry errors that I am working though so the code has a few calculation errors.

Comment by rnunziata on 2013-09-25:
The model I am using is called rrbot and it exist in gazebo as a sdf file and is added via the model tab. The rviz side is a urdf counter part all names are the same.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

Sorry for my inattention. As @hendrik-seveq-wiese suspected, most probably uninitialized pubJointState publisher is causing the problem. Here is a similar question.

OLD answer:

As I mentioned in the answer to your other question, the problem is in the type of the callback argument. Thus, instead of

void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)

use:

void handelerInitialPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)

The PoseWithCovarianceStampedConstPtr is a typedef like this:

typedef boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped> geometry_msgs::PoseWithCovarianceStampedConstPtr

and is generated automagically by ROS message generator.

Originally posted by Boris with karma: 3060 on 2013-09-25

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by rnunziata on 2013-09-26:
I will make these changes but that alone should not cause this error as passing by value is not prohibited.

Comment by rnunziata on 2013-09-26:
Sorry...you I was not advertising as you pointed out in related question.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.