Program dies when use_pixel is used

[abylikhsanov]

I think this is where the problem lies:

navigation_layers/range_sensor_layer/src/range_sensor_layer.cpp

Line 403 in 8899b6e

if(use_decay_)

@DLu

[abylikhsanov]

Actually, the program dies when does this line:

navigation_layers/range_sensor_layer/src/range_sensor_layer.cpp

Line 376 in 8899b6e

setCost(std::get<0>(it_map->first), std::get<1>(it_map->first), costmap_2d::FREE_SPACE);

[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.