Ask Your Question
0

Getting odd error from odometry node

asked 2013-09-25 07:50:54 -0500

rnunziata gravatar image

updated 2013-09-25 09:03:24 -0500

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

Comments

Where's your `pubJointState` defined? Can you give us that code, too?

Hendrik Wiese gravatar imageHendrik Wiese ( 2013-09-25 08:55:43 -0500 )edit

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.

rnunziata gravatar imagernunziata ( 2013-09-25 09:02:30 -0500 )edit

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.

rnunziata gravatar imagernunziata ( 2013-09-25 09:20:17 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-09-25 15:48:17 -0500

updated 2013-09-25 16:10:11 -0500

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.

edit flag offensive delete link more

Comments

I will make these changes but that alone should not cause this error as passing by value is not prohibited.

rnunziata gravatar imagernunziata ( 2013-09-26 03:52:45 -0500 )edit

Sorry...you I was not advertising as you pointed out in related question.

rnunziata gravatar imagernunziata ( 2013-09-26 04:33:05 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-09-25 07:50:54 -0500

Seen: 444 times

Last updated: Sep 25 '13