Program dies when use_pixel is used

[abylikhsanov]

I think this is where the problem lies: https://github.com/DLu/navigation_layers/blob/8899b6e9504165469468cc966f9030305ed5d827/range_sensor_layer/src/range_sensor_layer.cpp#L403

@DLu

[abylikhsanov]

Actually, the program dies when does this line: https://github.com/DLu/navigation_layers/blob/8899b6e9504165469468cc966f9030305ed5d827/range_sensor_layer/src/range_sensor_layer.cpp#L376

[abylikhsanov]

Well it is obvious I think, the market_point_history is being cleared before accessing

[jhbirdchoi]

Hi...same issue~

[jhbirdchoi]

Hi

I think this is a problem caused by referring to marked_point_history_ in several places.

if (use_decay_)
{
  std::pair<unsigned int, unsigned int> coordinate_pair(x, y);
  // If the point has a score high enough to be marked in the costmap, we add it's time to the marked_point_history
  if (c > to_cost(mark_threshold_)) {
    marked_point_history_[coordinate_pair] = last_reading_time_.toSec();

ROS_INFO("CHECK::update_cell is called x = %d y = %d\n", x, y); // If the point score is not high enough, we try to find it in the mark history point. // In the case we find it in the marked_point_history // we clear it from the map so we won't checked already cleared point } else if (c < to_cost(clear_threshold_)) { ROS_INFO("CHECK::update_cell is called CLEAR x = %d y = %d\n", x, y); // If the point score is not high enough, we try to find it in the mark history point. std::map<std::pair<unsigned int, unsigned int>, double>::iterator it_clear; it_clear = marked_point_history_.find(coordinate_pair); if (it_clear != marked_point_history_.end()) marked_point_history_.erase(it_clear); } }

void RangeSensorLayer::removeOutdatedReadings() { std::map<std::pair<unsigned int, unsigned int>, double>::iterator it_map;

ROS_INFO("CHECK::removeOutdaredReadings\n"); double removal_time = last_reading_time_.toSec() - pixel_decay_; for (it_map = marked_point_history_.begin() ; it_map != marked_point_history_.end() && marked_point_history_.size() != 0 ; it_map++ ) { if (it_map->second < removal_time) { ROS_INFO("CHECK::removeOutdaredReading size = %d\n", (unsigned int)marked_point_history_.size()); setCost(std::get<0>(it_map->first), std::get<1>(it_map->first), costmap_2d::FREE_SPACE); ROS_INFO("CHECK::removeOutdaredReadings2 = %d - %d\n", std::get<0>(it_map->first), std::get<1>(it_map->first)); marked_point_history_.erase(it_map); ROS_INFO("CHECK::removeOutdaredReadings ERASED\n"); } } ROS_INFO("CHECK::removeOutdaredReadings _END\n"); }

After the marked_point_histor is removed by clear_threshold, it is also referred to by pixel_decay.

The simplest way is to use a lock. I will update after testing.

thanks.