Revision history [back]

Hey Everyone,

found my Problem. As Hendrix said, the Problem was that i wasn't checking the vector size in the publish_range_msg method. The Problem was that in the first moment i have not received any data through my subscriber. In that case the elements of my range_msgs vector were 0. That causes the segmentation fault. So here the Solution for everyone who has a similar approach :

#include <ros/ros.h>
#include <range_msgs/SensorStates.h>
#include <sensor_msgs/Range.h>

class SensorStatePublisher
{
public:
    SensorStatePublisher()
    {
        state_listener = nh.subscribe<range_msgs::SensorStates>("/range_data",100000, &SensorStatePublisher::sensor_msg_callback, this);
        ir_1_publisher = nh.advertise<sensor_msgs::Range>("/ir_1_data", 100000);
        ir_2_publisher = nh.advertise<sensor_msgs::Range>("/ir_2_data", 100000);
        ir_3_publisher = nh.advertise<sensor_msgs::Range>("/ir_3_data", 100000);
        ir_4_publisher = nh.advertise<sensor_msgs::Range>("/ir_4_data", 100000);
        ir_5_publisher = nh.advertise<sensor_msgs::Range>("/ir_5_data", 100000);
        ir_6_publisher = nh.advertise<sensor_msgs::Range>("/ir_6_data", 100000);
        ir_7_publisher = nh.advertise<sensor_msgs::Range>("/ir_7_data", 100000);
    }

    void sensor_msg_callback(const range_msgs::SensorStatesConstPtr &msg);
    void publish_range_msg(void);
    void populate_range_msg(void);
private:
    ros::NodeHandle nh;
    ros::Subscriber state_listener;
    ros::Publisher ir_1_publisher,
                   ir_2_publisher,
                   ir_3_publisher,
                   ir_4_publisher,
                   ir_5_publisher,
                   ir_6_publisher,
                   ir_7_publisher;

    // Message Elements
    std_msgs::Header _header;
    int _radiation_type;
    float _field_of_view, _min_range, _max_range;
    std::vector<float> _sensor_ranges;

    // Range Messages, one for every Sensor
    std::vector<sensor_msgs::Range> range_msgs;
protected:
};

void SensorStatePublisher::sensor_msg_callback(const range_msgs::SensorStatesConstPtr &msg)
{
    _sensor_ranges.clear();
    // Save All Metadata
    _header = msg->header;
    _field_of_view = msg->field_of_view;
    _min_range = msg->min_range;
    _max_range = msg->max_range;

    // Save Every Arrayelement Of The Message
    for (int i = 0; i < msg->range.size() ; ++i) {
        _sensor_ranges.push_back(msg->range[i]);
    }
    //_sensor_ranges.resize(msg->range.size());
}

void SensorStatePublisher::populate_range_msg(void)
{
    range_msgs.clear();
    // Populate New Range Message With Saved Data
    sensor_msgs::Range new_range;
    new_range.header = _header;
    new_range.field_of_view = _field_of_view;
    new_range.min_range = _min_range;
    new_range.max_range = _max_range;

    // Save Data In An Array of Ranges
    for (int i = 0; i < _sensor_ranges.size(); ++i){
        std::ostringstream oss;
        oss << "ir_" << i+1 << "_link";
        new_range.header.frame_id = oss.str();
        new_range.range = _sensor_ranges.at(i);
        range_msgs.push_back(new_range);
    }
    //range_msgs.resize(_sensor_ranges.size());
}

void SensorStatePublisher::publish_range_msg(void)
{
    // Publish All Elements of the Range Array
    ros::Publisher *pubs[7];
    pubs[0]=&ir_1_publisher;
    pubs[1]=&ir_2_publisher;
    pubs[2]=&ir_3_publisher;
    pubs[3]=&ir_4_publisher;
    pubs[4]=&ir_5_publisher;
    pubs[5]=&ir_6_publisher;
    pubs[6]=&ir_7_publisher;

    const int n = _sensor_ranges.size();
    ROS_INFO("publishing %d range msgs",n);
    for (int i = 0; i < _sensor_ranges.size(); ++i){
        pubs[i]->publish(range_msgs[i]);
    }

/*    ir_1_publisher.publish(range_msgs[0]);
    ir_2_publisher.publish(range_msgs[1]);
    ir_3_publisher.publish(range_msgs[2]);
    ir_4_publisher.publish(range_msgs[3]);
    ir_5_publisher.publish(range_msgs[4]);
    ir_6_publisher.publish(range_msgs[5]);
    ir_7_publisher.publish(range_msgs[6]);*/
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sensor_state_publisher_node");
    SensorStatePublisher statePublisher;
    ros::Rate r(10);
    while(ros::ok())
    {
        statePublisher.populate_range_msg();
        statePublisher.publish_range_msg();
        r.sleep();
        ros::spinOnce();
    }
    return 0;
}