From 16ad22aa66814a511063b73810579410a839c840 Mon Sep 17 00:00:00 2001 From: Tian Bo Date: Wed, 21 Aug 2019 13:35:56 +0800 Subject: [PATCH] bug fix. laser pose update according to neomanic repo. handle .inf data --- src/CLaserOdometry2D.cpp | 6 +++--- src/CLaserOdometry2DNode.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 6894730..f3816da 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -156,7 +156,7 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan, kai_loc_old_ = MatrixS31::Zero(); module_initialized = true; - last_odom_time = ros::Time::now(); + last_odom_time = scan.header.stamp; } const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getIncrement() const @@ -292,7 +292,7 @@ void CLaserOdometry2D::createImagePyramid() //Inner pixels if ((u>1)&&(u 0.f) + if (std::isfinite(dcenter) && dcenter > 0.f) { float sum = 0.f; float weight = 0.f; @@ -316,7 +316,7 @@ void CLaserOdometry2D::createImagePyramid() //Boundary else { - if (dcenter > 0.f) + if (std::isfinite(dcenter) && dcenter > 0.f) { float sum = 0.f; float weight = 0.f; diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 942c000..b6620cf 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -104,7 +104,6 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() : initial_robot_pose.pose.pose.orientation.z = 0; } - setLaserPoseFromTf(); //Init variables module_initialized = false; @@ -196,6 +195,7 @@ void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& } else { + setLaserPoseFromTf(); init(last_scan, initial_robot_pose.pose.pose); first_laser_scan = false; } @@ -218,9 +218,9 @@ void CLaserOdometry2DNode::publish() //--------------------------------------- if (publish_tf) { - //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]"); + ROS_DEBUG("[rf2o] Publishing TF: [base_link] to [odom]"); geometry_msgs::TransformStamped odom_trans; - odom_trans.header.stamp = ros::Time::now(); + odom_trans.header.stamp = last_odom_time; odom_trans.header.frame_id = odom_frame_id; odom_trans.child_frame_id = base_frame_id; odom_trans.transform.translation.x = robot_pose_.translation()(0); @@ -233,9 +233,9 @@ void CLaserOdometry2DNode::publish() //next, we'll publish the odometry message over ROS //------------------------------------------------- - //ROS_INFO("[rf2o] Publishing Odom Topic"); + ROS_DEBUG ("[rf2o] Publishing Odom Topic"); nav_msgs::Odometry odom; - odom.header.stamp = ros::Time::now(); + odom.header.stamp = last_odom_time; odom.header.frame_id = odom_frame_id; //set the position odom.pose.pose.position.x = robot_pose_.translation()(0);