erik-nelson / blam

Other
785 stars 347 forks source link

blam_slam running error #36

Open junliu111 opened 5 years ago

junliu111 commented 5 years ago

after running the demo code roslaunch blam_example test_online.launch, it shows the following error. I have source the internal/devel.zsh file.

The system is ubuntu: 18.04.1, ros: melodic.

SUMMARY

PARAMETERS

NODES /blam/ blam_slam (blam_slam/blam_slam_node)

ROS_MASTER_URI=http://localhost:11311

process[blam/blam_slam-1]: started with pid [26609] [ INFO] [1541090716.155156906]: /blam/blam_slam/BlamSlam: Registering online callbacks. blam_slam_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/octree/include/pcl/octree/impl/octree_pointcloud.hpp:688: void pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyforPoint(const PointT&, pcl::octree::OctreeKey&) const [with PointT = pcl::PointXYZ; LeafContainerT = pcl::octree::OctreeContainerPointIndices; BranchContainerT = pcl::octree::OctreeContainerEmpty; OctreeT = pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty>]: Assertion key_arg.x <= this->max_key_.x' failed. [blam/blam_slam-1] process has died [pid 26609, exit code -6, cmd /home/jun/catkin_ws/src/blam/internal/devel/lib/blam_slam/blam_slam_node ~pcld:=/velodyne_points __name:=blam_slam __log:=/home/jun/.ros/log/4cd720f2-ddec-11e8-bc37-e4a7a0130394/blam-blam_slam-1.log]. log file: /home/jun/.ros/log/4cd720f2-ddec-11e8-bc37-e4a7a0130394/blam-blam_slam-1*.log all processes on machine have died, roslaunch will exit shutting down processing monitor... ... shutting down processing monitor complete done

Anyone has any idea about how could this happen? Thanks.

thorstink commented 5 years ago

I have the same issue on ubuntu 18.04 using melodic...

aj-n commented 5 years ago

I am also encountering this issue on 18.04 / melodic using bslam_offline

uzdry commented 5 years ago

Same problem here with both online and offline. Is there anyone that was able to solve it?

nevermoredanny commented 5 years ago

PointCloudMapper.cc change

if (!mapoctree->isVoxelOccupiedAtPoint(p)) {

to:

double min_x, min_y, min_z, max_x, max_y, max_z; mapoctree->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);

if (!isInBox || !mapoctree->isVoxelOccupiedAtPoint(p)) {

vahidbehtajisiahkal commented 5 years ago

Thanks, it works well