Revision history [back]

Hi,

A peculiar bug occurred while making a simple listener/publisher for a visualization application.

I am using boost::function to pass a lambda-function; which might be a bit odd, but I do not see how this could give me a segmentation error or whether it is something different..

So the following gives a segmentation error when it tries to publish the message:

#include ....

int main(int argc, char** argv) {
    ros::init(argc, argv, "inertial_odom_demo_node");
    ros::NodeHandle n; 

    auto pose_pub   = n.advertise<geometry_msgs::PoseStamped>("/pose", 10); 
    odometry inertial_odom; 

    boost::function<void (const sensor_msgs::Imu&)> callback = 
        [&] (const sensor_msgs::Imu& msg) {
            Eigen::Vector3d a, g;
            a << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z;
            g << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z;
            uint64_t t = msg.header.stamp.toNSec(); 

            if (inertial_odom.imu(a,g,t))
            {
                auto state_vector = inertial_odom.pose();
                geometry_msgs::PoseStamped poseStamped;            
                poseStamped.header.frame_id = "map";
                poseStamped.header.stamp = msg.header.stamp;
                poseStamped.pose.position.x = state_vector(0);
                poseStamped.pose.position.y = state_vector(1);
                poseStamped.pose.position.z = state_vector(2);
                poseStamped.pose.orientation.w = state_vector(3);
                poseStamped.pose.orientation.x = state_vector(4);
                poseStamped.pose.orientation.y = state_vector(5);
                poseStamped.pose.orientation.z = state_vector(6);

                pose_pub.publish(poseStamped);
            }
    };
    auto imu_sub    = n.subscribe<sensor_msgs::Imu>("/imu", 1000, callback);
    ros::spin();
}

while the following does not:

#include ....

int main(int argc, char** argv) {
    ros::init(argc, argv, "inertial_odom_demo_node");
    ros::NodeHandle n; 

    auto pose_pub   = n.advertise<geometry_msgs::PoseStamped>("/pose", 10);

    geometry_msgs::PoseStamped poseStamped;
    poseStamped.header.frame_id = "map";

    odometry inertial_odom; 

    boost::function<void (const sensor_msgs::Imu&)> callback = 
        [&] (const sensor_msgs::Imu& msg) {
            Eigen::Vector3d a, g;
            a << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z;
            g << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z;
            uint64_t t = msg.header.stamp.toNSec(); 

            if (inertial_odom.imu(a,g,t))
            {
                auto state_vector = inertial_odom.pose();
                poseStamped.header.stamp = msg.header.stamp;
                poseStamped.pose.position.x = state_vector(0);
                poseStamped.pose.position.y = state_vector(1);
                poseStamped.pose.position.z = state_vector(2);
                poseStamped.pose.orientation.w = state_vector(3);
                poseStamped.pose.orientation.x = state_vector(4);
                poseStamped.pose.orientation.y = state_vector(5);
                poseStamped.pose.orientation.z = state_vector(6);

                pose_pub.publish(poseStamped);
            }
    };
    auto imu_sub    = n.subscribe<sensor_msgs::Imu>("/imu", 1000, callback);
    ros::spin();
}

I toyed around with different message-type, but it does not change the result... Any ideas?