search

at-wat / mcl_3dl

A ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that uses a reference pointcloud as a map.
BSD 3-Clause "New" or "Revised" License
476 stars 117 forks source link

Invalid initial pose hangs the node #316

Closed ykoga-kyutech closed 4 years ago

ykoga-kyutech commented 4 years ago

Invalid initial pose like non-unit quaternion of the orientation hangs the mcl_3dl node. This occurs when the following invalid initial pose is published.

$ rostopic pub -s /initialpose geometry_msgs/PoseWithCovarianceStamped "header:
  seq: 0
  stamp: 'now'
  frame_id: 'map'
pose:
  pose:
    position: {x: 0.0, y: 0.0, z: 0.0}
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}  # INVALID
  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.03]"

Adding a validation for the initial pose is required to avoid it.