Segmentation Fault

edit

Hi Everyone,

sorry for an other segmentaion fault question, but i really dont see it. Im generating on an arduino a message which has an array that holds multiple sensor readings. The arduino is publishing with 13.7Hz. On the ROS side i subscribe to that data and want to create for each sensor reading a single range message with an unique frame_id for a later transform.

Little Ros Graph: /serial_node -> /range_data -> /sensor_state_publisher -> /ir_1_data , ir_2_data, ir_3_data, ir_4_data, ir_5_data , ir_6_data, ir_7_data

Every now and then i get a segmention fault error, when starting the node on the ros side. I tried to deallocate the used arrays in the destructor, to clear them and to rize them. When im changing the average rate to a higher value, the segmentation error accures more often. I also tried to set up the buffer size. Nothing helped. Did someone see my mistake, or is the hole programm bs and not usable for my need.

Here my code:

#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
    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_publishe");
    SensorStatePublisher statePublisher;
    ros::Rate r(10);
    while(ros::ok())
    {
        statePublisher.populate_range_msg();
        statePublisher.publish_range_msg();
        r.sleep();
        ros::spinOnce();
    }
    return 0;
}

Thanks for every help and excuse my little noob cpp/ros programm. ;)