Skip to content
This repository was archived by the owner on Jun 5, 2025. It is now read-only.

fix crashes in AMCL#1152

Merged
mikeferguson merged 6 commits intoros-planning:noetic-develfrom
easylyou:noetic-devel
Nov 11, 2021
Merged

fix crashes in AMCL#1152
mikeferguson merged 6 commits intoros-planning:noetic-develfrom
easylyou:noetic-devel

Conversation

@easylyou
Copy link
Contributor

fix #1151 about move_base crashes.

if the LaserScan.msg is malformed, the move_base will crash.
sensor_msgs/LaserScan.msg:
std_msgs/Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment #it seems the value is below zero or beyond INT_MAX, roscore will throw a runtime_error.
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

in roscpp_core/rostime/include/ros/impl/duration.h
if (sec64 < std::numeric_limits<int32_t>::min() || sec64 > std::numeric_limits<int32_t>::max())
throw std::runtime_error("Duration is out of dual 32-bit range")

in roscpp_core/rostime/include/ros/impl/time.h
if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Time is out of dual 32-bit range");

before fix:
image

after fix:
image

@easylyou
Copy link
Contributor Author

easylyou commented Aug 3, 2021

fix #1151 about amcl crash.
It only need a few cycles to check some obvious malformed values, which prevents some crashes from these malformed messages.

    // in amcl_node.cpp    laserReceived()
    if(laser_max_range_ > 0.0)
      ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    else
      ldata.range_max = laser_scan->range_max

    //ldata.range_max is from laser_scan. It could be any value, such as zero.
    //in amcl_laser.cpp LikelihoodFieldModel()
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set){

      double z_rand_mult = 1.0/data->range_max; //if range_max is zero, z_rand_mult will be inf.

      pz += self->z_rand * z_rand_mult;
      assert(pz <= 1.0);
      assert(pz >= 0.0);
}

@mikeferguson
Copy link
Contributor

Just going to wait for CI to complete, then I'll merge this

@mikeferguson mikeferguson changed the title fix: catch runtime_error from roscore fix crashes in AMCL Nov 11, 2021
@mikeferguson mikeferguson merged commit 19eddca into ros-planning:noetic-devel Nov 11, 2021
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

[amcl, move_base] crashed with malformed messages from laser sensor (/scan topic)

2 participants