mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

[rf2o] New "verbose" parameter to control Verbosity

Browse Source
This commit is contained in:
jgmonroy
2017-08-03 17:50:53 +02:00
parent 543caed2e8
commit 7998dec88d
2 changed files with 18 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -23,7 +23,6 @@
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
// MRPT related headers
// MRPT related headers // MRPT related headers
#include <mrpt/version.h> #include <mrpt/version.h>
#if MRPT_VERSION>=0x130 #if MRPT_VERSION>=0x130
@@ -72,7 +71,7 @@ public:
protected: protected:
ros::NodeHandle n; ros::NodeHandle n;
sensor_msgs::LaserScan last_scan; sensor_msgs::LaserScan last_scan;
bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized; bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized, verbose;
tf::TransformListener tf_listener; //Do not put inside the callback tf::TransformListener tf_listener; //Do not put inside the callback
tf::TransformBroadcaster odom_broadcaster; tf::TransformBroadcaster odom_broadcaster;
nav_msgs::Odometry initial_robot_pose; nav_msgs::Odometry initial_robot_pose;

24
src/CLaserOdometry2D.cpp
View File

@@ -40,6 +40,7 @@ CLaserOdometry2D::CLaserOdometry2D()
pn.param<bool>("publish_tf", publish_tf, true); pn.param<bool>("publish_tf", publish_tf, true);
pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
pn.param<double>("freq",freq,10.0); pn.param<double>("freq",freq,10.0);
pn.param<bool>("verbose", verbose, true);
//Publishers and Subscribers //Publishers and Subscribers
//-------------------------- //--------------------------
@@ -88,7 +89,8 @@ bool CLaserOdometry2D::scan_available()
void CLaserOdometry2D::Init() void CLaserOdometry2D::Init()
{ {
//Got an initial scan laser, obtain its parametes //Got an initial scan laser, obtain its parametes
ROS_INFO("Got first Laser Scan .... Configuring node"); if (verbose)
ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node");
width = last_scan.ranges.size(); // Num of samples (size) of the scan laser width = last_scan.ranges.size(); // Num of samples (size) of the scan laser
cols = width; // Max reolution. Should be similar to the width parameter cols = width; // Max reolution. Should be similar to the width parameter
fovh = fabs(last_scan.angle_max - last_scan.angle_min); // Horizontal Laser's FOV fovh = fabs(last_scan.angle_max - last_scan.angle_min); // Horizontal Laser's FOV
@@ -282,7 +284,8 @@ void CLaserOdometry2D::odometryCalculation()
} }
m_runtime = 1000*m_clock.Tac(); m_runtime = 1000*m_clock.Tac();
ROS_INFO("Time odometry (ms): %f", m_runtime); if (verbose)
ROS_INFO("[rf2o] execution time (ms): %f", m_runtime);
//Update poses //Update poses
PoseUpdate(); PoseUpdate();
@@ -653,7 +656,7 @@ void CLaserOdometry2D::solveSystemOneLevel()
} }
// Solves the system by considering the Cauchy M-estimatorrobust-function // Solves the system by considering the Cauchy M-estimator robust-function
void CLaserOdometry2D::solveSystemNonLinear() void CLaserOdometry2D::solveSystemNonLinear()
{ {
A.resize(num_valid_range,3); Aw.resize(num_valid_range,3); A.resize(num_valid_range,3); Aw.resize(num_valid_range,3);
@@ -745,7 +748,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
kai_loc_level = Var; kai_loc_level = Var;
std::cout << endl << "COV_ODO: " << cov_odo << endl; if (verbose)
std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl;
} }
void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan) void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
@@ -846,7 +850,8 @@ void CLaserOdometry2D::filterLevelSolution()
SelfAdjointEigenSolver<MatrixXf> eigensolver(cov_odo); SelfAdjointEigenSolver<MatrixXf> eigensolver(cov_odo);
if (eigensolver.info() != Success) if (eigensolver.info() != Success)
{ {
printf("Eigensolver couldn't find a solution. Pose is not updated"); if (verbose)
printf("[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
return; return;
} }
@@ -949,8 +954,8 @@ void CLaserOdometry2D::PoseUpdate()
kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi); kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi);
kai_loc_old(2) = kai_abs(2); kai_loc_old(2) = kai_abs(2);
if (verbose)
ROS_INFO("LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
// GET ROBOT POSE from LASER POSE // GET ROBOT POSE from LASER POSE
@@ -981,6 +986,7 @@ void CLaserOdometry2D::PoseUpdate()
//Compose Transformations //Compose Transformations
robot_pose = laser_pose + LaserPoseOnTheRobot_inv; robot_pose = laser_pose + LaserPoseOnTheRobot_inv;
if (verbose)
ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw()); ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
@@ -1103,7 +1109,7 @@ int main(int argc, char** argv)
//Main Loop //Main Loop
//---------- //----------
ROS_INFO("initialization complete...Looping"); ROS_INFO("[rf2o] initialization complete...Looping");
ros::Rate loop_rate(myLaserOdom.freq); ros::Rate loop_rate(myLaserOdom.freq);
while (ros::ok()) while (ros::ok())
{ {
@@ -1116,7 +1122,7 @@ int main(int argc, char** argv)
} }
else else
{ {
ROS_WARN("Waiting for laser_scans....") ; ROS_WARN("[rf2o] Waiting for laser_scans....") ;
} }
loop_rate.sleep(); loop_rate.sleep();