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
#include
#include
class SensorStatePublisher
{
public:
SensorStatePublisher()
{
state_listener = nh.subscribe("/range_data",100000, &SensorStatePublisher::sensor_msg_callback, this);
ir_1_publisher = nh.advertise("/ir_1_data", 100000);
ir_2_publisher = nh.advertise("/ir_2_data", 100000);
ir_3_publisher = nh.advertise("/ir_3_data", 100000);
ir_4_publisher = nh.advertise("/ir_4_data", 100000);
ir_5_publisher = nh.advertise("/ir_5_data", 100000);
ir_6_publisher = nh.advertise("/ir_6_data", 100000);
ir_7_publisher = nh.advertise("/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 _sensor_ranges;
// Range Messages, one for every Sensor
std::vector 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. ;)
↧