From 241358540a9bdc635d3df2eb1aa5c3272e3a4975 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Tue, 16 May 2017 11:20:04 +0200 Subject: [PATCH 01/11] wip separating node from computation --- CMakeLists.txt | 9 +- .../rf2o_laser_odometry/CLaserOdometry2D.h | 38 +-- src/CLaserOdometry2D.cpp | 316 ++++-------------- src/CLaserOdometry2DNode.cpp | 261 +++++++++++++++ 4 files changed, 354 insertions(+), 270 deletions(-) create mode 100644 src/CLaserOdometry2DNode.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e49d4df..2620f43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,8 +52,15 @@ include_directories( ${EIGEN_INCLUDE_DIRS} ) +## Declare a cpp library +add_library(${PROJECT_NAME} + src/CLaserOdometry2D.cpp +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + ## Declare a cpp executable -add_executable(rf2o_laser_odometry_node src/CLaserOdometry2D.cpp) +add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp) +target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME}) ## Specify libraries to link a library or executable target against target_link_libraries(rf2o_laser_odometry_node diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 200f148..576546f 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -59,19 +59,13 @@ class CLaserOdometry2D public: CLaserOdometry2D(); ~CLaserOdometry2D(); + + void Init(const sensor_msgs::LaserScan& scan, + const geometry_msgs::Pose& initial_robot_pose); + bool is_initialized(); - bool scan_available(); - void Init(); - void odometryCalculation(); - - std::string laser_scan_topic; - std::string odom_topic; - bool publish_tf; - std::string base_frame_id; - std::string odom_frame_id; - std::string init_pose_from_topic; - double freq; +<<<<<<< HEAD protected: ros::NodeHandle n; sensor_msgs::LaserScan last_scan; @@ -79,14 +73,15 @@ protected: tf::TransformListener tf_listener; //Do not put inside the callback tf::TransformBroadcaster odom_broadcaster; nav_msgs::Odometry initial_robot_pose; +======= + void odometryCalculation(const sensor_msgs::LaserScan& scan); +>>>>>>> wip separating node from computation - //Subscriptions & Publishers - ros::Subscriber laser_sub, initPose_sub; - ros::Publisher odom_pub; + void setLaserPose(const tf::Transform& laser_pose); - //CallBacks - void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan); - void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose); +protected: + + bool module_initialized,first_laser_scan; // Internal Data std::vector range; @@ -130,17 +125,22 @@ protected: unsigned int iter_irls; float g_mask[5]; + double lin_speed, ang_speed; + //mrpt::gui::CDisplayWindowPlots window; mrpt::utils::CTicTac m_clock; float m_runtime; - ros::Time last_odom_time; + ros::Time last_odom_time, current_scan_time; mrpt::math::CMatrixFloat31 kai_abs; mrpt::math::CMatrixFloat31 kai_loc; mrpt::math::CMatrixFloat31 kai_loc_old; mrpt::math::CMatrixFloat31 kai_loc_level; - mrpt::poses::CPose3D laser_pose; + mrpt::poses::CPose3D laser_pose_on_robot; + mrpt::poses::CPose3D laser_pose_on_robot_inv; + + mrpt::poses::CPose3D laser_pose; mrpt::poses::CPose3D laser_oldpose; mrpt::poses::CPose3D robot_pose; mrpt::poses::CPose3D robot_oldpose; diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index a4803d9..a7eab3b 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -27,106 +27,57 @@ using namespace Eigen; //--------------------------------------------- CLaserOdometry2D::CLaserOdometry2D() -{ - ROS_INFO("Initializing RF2O node..."); - - //Read Parameters - //---------------- - ros::NodeHandle pn("~"); - pn.param("laser_scan_topic",laser_scan_topic,"/laser_scan"); - pn.param("odom_topic", odom_topic, "/odom_rf2o"); - pn.param("base_frame_id", base_frame_id, "/base_link"); - pn.param("odom_frame_id", odom_frame_id, "/odom"); - pn.param("publish_tf", publish_tf, true); - pn.param("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); - pn.param("freq",freq,10.0); - pn.param("verbose", verbose, true); - - //Publishers and Subscribers - //-------------------------- - odom_pub = pn.advertise(odom_topic, 5); - laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2D::LaserCallBack,this); - - //init pose?? - if (init_pose_from_topic != "") - { - initPose_sub = n.subscribe(init_pose_from_topic,1,&CLaserOdometry2D::initPoseCallBack,this); - GT_pose_initialized = false; - } - else - { - GT_pose_initialized = true; - initial_robot_pose.pose.pose.position.x = 0; - initial_robot_pose.pose.pose.position.y = 0; - initial_robot_pose.pose.pose.position.z = 0; - initial_robot_pose.pose.pose.orientation.w = 0; - initial_robot_pose.pose.pose.orientation.x = 0; - initial_robot_pose.pose.pose.orientation.y = 0; - initial_robot_pose.pose.pose.orientation.z = 0; - } - - //Init variables - module_initialized = false; - first_laser_scan = true; +{ + // } CLaserOdometry2D::~CLaserOdometry2D() { + // } +void CLaserOdometry2D::setLaserPose(const tf::Transform& laser_pose) +{ + //Set laser pose on the robot + + //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) + const tf::Vector3 &t = laser_pose.getOrigin(); + laser_pose_on_robot.x() = t[0]; + laser_pose_on_robot.y() = t[1]; + laser_pose_on_robot.z() = t[2]; + const tf::Matrix3x3 &basis = laser_pose.getBasis(); + mrpt::math::CMatrixDouble33 R; + for(int r = 0; r < 3; r++) + for(int c = 0; c < 3; c++) + R(r,c) = basis[r][c]; + laser_pose_on_robot.setRotationMatrix(R); + + laser_pose_on_robot_inv = laser_pose_on_robot; + laser_pose_on_robot_inv.inverse(); +} bool CLaserOdometry2D::is_initialized() { return module_initialized; } -bool CLaserOdometry2D::scan_available() -{ - return new_scan_available; -} - -void CLaserOdometry2D::Init() +void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, + const geometry_msgs::Pose& initial_robot_pose) { //Got an initial scan laser, obtain its parametes if (verbose) ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node"); width = last_scan.ranges.size(); // Num of samples (size) of the scan laser + 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(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV ctf_levels = 5; // Coarse-to-Fine levels iter_irls = 5; //Num iterations to solve iterative reweighted least squares - //Set laser pose on the robot (through tF) - // This allow estimation of the odometry with respect to the robot base reference system. - mrpt::poses::CPose3D LaserPoseOnTheRobot; - tf::StampedTransform transform; - try - { - tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); - } - catch (tf::TransformException &ex) - { - ROS_ERROR("%s",ex.what()); - ros::Duration(1.0).sleep(); - } - - //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) - const tf::Vector3 &t = transform.getOrigin(); - LaserPoseOnTheRobot.x() = t[0]; - LaserPoseOnTheRobot.y() = t[1]; - LaserPoseOnTheRobot.z() = t[2]; - const tf::Matrix3x3 &basis = transform.getBasis(); - mrpt::math::CMatrixDouble33 R; - for(int r = 0; r < 3; r++) - for(int c = 0; c < 3; c++) - R(r,c) = basis[r][c]; - LaserPoseOnTheRobot.setRotationMatrix(R); - - //Robot initial pose (see MQTT:bridge) mrpt::poses::CPose3D robotInitialPose; - geometry_msgs::Pose _src = initial_robot_pose.pose.pose; + geometry_msgs::Pose _src = initial_robot_pose; robotInitialPose.x(_src.position.x); robotInitialPose.y(_src.position.y); @@ -141,13 +92,10 @@ void CLaserOdometry2D::Init() robotInitialPose.setYawPitchRoll(yaw,pitch,roll); //robotInitialPose.phi(yaw); - - //Set the initial pose laser_pose = robotInitialPose + LaserPoseOnTheRobot; laser_oldpose = robotInitialPose + LaserPoseOnTheRobot; - // Init module (internal) //------------------------ range_wf.setSize(1, width); @@ -227,12 +175,16 @@ void CLaserOdometry2D::Init() } -void CLaserOdometry2D::odometryCalculation() +void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) { //================================================================================== // DIFERENTIAL ODOMETRY MULTILEVEL //================================================================================== + //copy laser scan to internal variable + for (unsigned int i = 0; i Filter (not downsampling); if (i == 0) { for (unsigned int u = 0; u < cols_i; u++) - { + { const float dcenter = range_wf(u); - + //Inner pixels if ((u>1)&&(u 0.f) - { + { float sum = 0.f; float weight = 0.f; @@ -351,16 +302,16 @@ void CLaserOdometry2D::createImagePyramid() else { if (dcenter > 0.f) - { + { float sum = 0.f; float weight = 0.f; - for (int l=-2; l<3; l++) + for (int l=-2; l<3; l++) { const int indu = u+l; if ((indu>=0)&&(indu0)&&(u 0.f) - { + { float sum = 0.f; float weight = 0.f; @@ -416,18 +367,18 @@ void CLaserOdometry2D::createImagePyramid() else { if (dcenter > 0.f) - { + { float sum = 0.f; float weight = 0.f; const unsigned int cols_i2 = range[i_1].cols(); - for (int l=-2; l<3; l++) + for (int l=-2; l<3; l++) { const int indu = u2+l; if ((indu>=0)&&(indu 0.f) { @@ -466,7 +417,7 @@ void CLaserOdometry2D::createImagePyramid() void CLaserOdometry2D::calculateCoord() -{ +{ for (unsigned int u = 0; u < cols_i; u++) { if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f)) @@ -486,12 +437,12 @@ void CLaserOdometry2D::calculateCoord() void CLaserOdometry2D::calculaterangeDerivativesSurface() -{ +{ //The gradient size ir reserved at the maximum size (at the constructor) //Compute connectivity rtita.resize(1,cols_i); //Defined in a different way now, without inversion - rtita.assign(1.f); + rtita.assign(1.f); for (unsigned int u = 0; u < cols_i-1; u++) { @@ -569,15 +520,15 @@ void CLaserOdometry2D::computeWeights() { //The maximum weight size is reserved at the constructor weights.assign(0.f); - + //Parameters for error_linearization const float kdtita = 1.f; const float kdt = kdtita/square(fps); const float k2d = 0.2f; - + for (unsigned int u = 1; u < cols_i-1; u++) if (null(u) == 0) - { + { // Compute derivatives //----------------------------------------------------------------------- const float ini_dtita = range_old[image_level](u+1) - range_old[image_level](u-1); @@ -640,7 +591,7 @@ void CLaserOdometry2D::solveSystemOneLevel() cont++; } - + //Solve the linear system of equations using a minimum least squares method MatrixXf AtA, AtB; AtA.multiply_AtA(A); @@ -767,7 +718,7 @@ void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan) void CLaserOdometry2D::performWarping() { - Matrix3f acu_trans; + Matrix3f acu_trans; acu_trans.setIdentity(); for (unsigned int i=1; i<=level; i++) acu_trans = transformations[i-1]*acu_trans; @@ -780,7 +731,7 @@ void CLaserOdometry2D::performWarping() const float kdtita = cols_lim/fovh; for (unsigned int j = 0; j 0.f) { //Transform point to the warped reference frame @@ -822,7 +773,7 @@ void CLaserOdometry2D::performWarping() //Scale the averaged range and compute coordinates for (unsigned int u = 0; u 0.f) { const float tita = -0.5f*fovh + float(u)/kdtita; @@ -848,13 +799,13 @@ void CLaserOdometry2D::filterLevelSolution() // Calculate Eigenvalues and Eigenvectors //---------------------------------------------------------- SelfAdjointEigenSolver eigensolver(cov_odo); - if (eigensolver.info() != Success) + if (eigensolver.info() != Success) { if (verbose) printf("[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); return; } - + //First, we have to describe both the new linear and angular speeds in the "eigenvector" basis //------------------------------------------------------------------------------------------------- Matrix Bii; @@ -958,52 +909,25 @@ void CLaserOdometry2D::PoseUpdate() ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); - // GET ROBOT POSE from LASER POSE - //------------------------------ - mrpt::poses::CPose3D LaserPoseOnTheRobot_inv; - tf::StampedTransform transform; - try - { - tf_listener.lookupTransform(last_scan.header.frame_id, base_frame_id, ros::Time(0), transform); - } - catch (tf::TransformException &ex) - { - ROS_ERROR("%s",ex.what()); - ros::Duration(1.0).sleep(); - } - - //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) - const tf::Vector3 &t = transform.getOrigin(); - LaserPoseOnTheRobot_inv.x() = t[0]; - LaserPoseOnTheRobot_inv.y() = t[1]; - LaserPoseOnTheRobot_inv.z() = t[2]; - const tf::Matrix3x3 &basis = transform.getBasis(); - mrpt::math::CMatrixDouble33 R; - for(int r = 0; r < 3; r++) - for(int c = 0; c < 3; c++) - R(r,c) = basis[r][c]; - LaserPoseOnTheRobot_inv.setRotationMatrix(R); - //Compose Transformations robot_pose = laser_pose + LaserPoseOnTheRobot_inv; if (verbose) ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw()); - // Estimate linear/angular speeds (mandatory for base_local_planner) // last_scan -> the last scan received // last_odom_time -> The time of the previous scan lasser used to estimate the pose //------------------------------------------------------------------------------------- - double time_inc_sec = (last_scan.header.stamp - last_odom_time).toSec(); - last_odom_time = last_scan.header.stamp; - double lin_speed = acu_trans(0,2) / time_inc_sec; + double time_inc_sec = (current_scan_time - last_odom_time).toSec(); + last_odom_time = current_scan_time; + lin_speed = acu_trans(0,2) / time_inc_sec; //double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec; double ang_inc = robot_pose.yaw() - robot_oldpose.yaw(); if (ang_inc > 3.14159) ang_inc -= 2*3.14159; if (ang_inc < -3.14159) ang_inc += 2*3.14159; - double ang_speed = ang_inc/time_inc_sec; + ang_speed = ang_inc/time_inc_sec; robot_oldpose = robot_pose; //filter speeds @@ -1020,112 +944,4 @@ void CLaserOdometry2D::PoseUpdate() double sum2 = std::accumulate(last_m_ang_speeds.begin(), last_m_ang_speeds.end(), 0.0); ang_speed = sum2 / last_m_ang_speeds.size(); */ - - //first, we'll publish the odometry over tf - //--------------------------------------- - if (publish_tf) - { - //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]"); - geometry_msgs::TransformStamped odom_trans; - odom_trans.header.stamp = ros::Time::now(); - odom_trans.header.frame_id = odom_frame_id; - odom_trans.child_frame_id = base_frame_id; - odom_trans.transform.translation.x = robot_pose.x(); - odom_trans.transform.translation.y = robot_pose.y(); - odom_trans.transform.translation.z = 0.0; - odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); - //send the transform - odom_broadcaster.sendTransform(odom_trans); - } - - //next, we'll publish the odometry message over ROS - //------------------------------------------------- - //ROS_INFO("[rf2o] Publishing Odom Topic"); - nav_msgs::Odometry odom; - odom.header.stamp = ros::Time::now(); - odom.header.frame_id = odom_frame_id; - //set the position - odom.pose.pose.position.x = robot_pose.x(); - odom.pose.pose.position.y = robot_pose.y(); - odom.pose.pose.position.z = 0.0; - odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); - //set the velocity - odom.child_frame_id = base_frame_id; - odom.twist.twist.linear.x = lin_speed; //linear speed - odom.twist.twist.linear.y = 0.0; - odom.twist.twist.angular.z = ang_speed; //angular speed - //publish the message - odom_pub.publish(odom); -} - - - -//----------------------------------------------------------------------------------- -// CALLBACKS -//----------------------------------------------------------------------------------- - -void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) -{ - if (GT_pose_initialized) - { - //Keep in memory the last received laser_scan - last_scan = *new_scan; - - //Initialize module on first scan - if (first_laser_scan) - { - Init(); - first_laser_scan = false; - } - else - { - //copy laser scan to internal variable - for (unsigned int i = 0; iranges[i]; - new_scan_available = true; - } - } -} - - -void CLaserOdometry2D::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose) -{ - //Initialize module on first GT pose. Else do Nothing! - if (!GT_pose_initialized) - { - initial_robot_pose = *new_initPose; - GT_pose_initialized = true; - } -} - -//----------------------------------------------------------------------------------- -// MAIN -//----------------------------------------------------------------------------------- -int main(int argc, char** argv) -{ - ros::init(argc, argv, "RF2O_LaserOdom"); - - CLaserOdometry2D myLaserOdom; - - //Main Loop - //---------- - ROS_INFO("[rf2o] initialization complete...Looping"); - ros::Rate loop_rate(myLaserOdom.freq); - while (ros::ok()) - { - ros::spinOnce(); //Check for new laser scans - - if( myLaserOdom.is_initialized() && myLaserOdom.scan_available() ) - { - //Process odometry estimation - myLaserOdom.odometryCalculation(); - } - else - { - ROS_WARN("[rf2o] Waiting for laser_scans....") ; - } - - loop_rate.sleep(); - } - return(0); } diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp new file mode 100644 index 0000000..97ebc3c --- /dev/null +++ b/src/CLaserOdometry2DNode.cpp @@ -0,0 +1,261 @@ +/** **************************************************************************************** +* This node presents a fast and precise method to estimate the planar motion of a lidar +* from consecutive range scans. It is very useful for the estimation of the robot odometry from +* 2D laser range measurements. +* This module is developed for mobile robots with innacurate or inexistent built-in odometry. +* It allows the estimation of a precise odometry with low computational cost. +* For more information, please refer to: +* +* Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16. +* Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217 +* +* Maintainer: Javier G. Monroy +* MAPIR group: http://mapir.isa.uma.es/ +******************************************************************************************** */ + +#include "rf2o_laser_odometry/CLaserOdometry2D.h" +using namespace mrpt; +using namespace mrpt::math; +using namespace mrpt::utils; +using namespace mrpt::poses; +using namespace std; +using namespace Eigen; + +class CLaserOdometry2DNode : CLaserOdometry2D +{ +public: + + CLaserOdometry2DNode(); + ~CLaserOdometry2DNode(); + + void process(); + void publish(); + +public: + + bool verbose,publish_tf,new_scan_available; + + double freq; + + std::string laser_scan_topic; + std::string odom_topic; + std::string base_frame_id; + std::string odom_frame_id; + std::string init_pose_from_topic; + + ros::NodeHandle n; + sensor_msgs::LaserScan last_scan; + bool GT_pose_initialized; + tf::TransformListener tf_listener; //Do not put inside the callback + tf::TransformBroadcaster odom_broadcaster; + nav_msgs::Odometry initial_robot_pose; + + //Subscriptions & Publishers + ros::Subscriber laser_sub, initPose_sub; + ros::Publisher odom_pub; + + bool scan_available(); + + //CallBacks + void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan); + void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose); +}; + +CLaserOdometry2DNode::CLaserOdometry2DNode() : + CLaserOdometry2D() +{ + ROS_INFO("Initializing RF2O node..."); + + //Read Parameters + //---------------- + ros::NodeHandle pn("~"); + pn.param("laser_scan_topic",laser_scan_topic,"/laser_scan"); + pn.param("odom_topic", odom_topic, "/odom_rf2o"); + pn.param("base_frame_id", base_frame_id, "/base_link"); + pn.param("odom_frame_id", odom_frame_id, "/odom"); + pn.param("publish_tf", publish_tf, true); + pn.param("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); + pn.param("freq",freq,10.0); + pn.param("verbose", verbose, true); + + //Publishers and Subscribers + //-------------------------- + odom_pub = pn.advertise(odom_topic, 5); + laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); + + //init pose?? + if (init_pose_from_topic != "") + { + initPose_sub = n.subscribe(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this); + GT_pose_initialized = false; + } + else + { + GT_pose_initialized = true; + initial_robot_pose.pose.pose.position.x = 0; + initial_robot_pose.pose.pose.position.y = 0; + initial_robot_pose.pose.pose.position.z = 0; + initial_robot_pose.pose.pose.orientation.w = 0; + initial_robot_pose.pose.pose.orientation.x = 0; + initial_robot_pose.pose.pose.orientation.y = 0; + initial_robot_pose.pose.pose.orientation.z = 0; + } + + // Set laser pose on the robot (through tF) + // This allow estimation of the odometry with respect to the robot base reference system. + tf::StampedTransform transform; + transform.setIdentity(); + try + { + tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); + } + catch (tf::TransformException &ex) + { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + } + + setLaserPose(transform); + + //Init variables + module_initialized = false; + first_laser_scan = true; +} + +CLaserOdometry2DNode::~CLaserOdometry2DNode() +{ + // +} + +bool CLaserOdometry2DNode::scan_available() +{ + return new_scan_available; +} + +void CLaserOdometry2DNode::process() +{ + if( is_initialized() && scan_available() ) + { + //Process odometry estimation + odometryCalculation(last_scan); + publish(); + new_scan_available = false; //avoids the possibility to run twice on the same laser scan + } + else + { + ROS_WARN("Waiting for laser_scans....") ; + } +} + +//----------------------------------------------------------------------------------- +// CALLBACKS +//----------------------------------------------------------------------------------- + +void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) +{ + if (GT_pose_initialized) + { + //Keep in memory the last received laser_scan + last_scan = *new_scan; + current_scan_time = last_scan.header.stamp; + + //Initialize module on first scan + if (first_laser_scan) + { + Init(last_scan, initial_robot_pose.pose.pose); + first_laser_scan = false; + } + else + { + //copy laser scan to internal variable + for (unsigned int i = 0; iranges[i]; + new_scan_available = true; + } + } +} + +void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose) +{ + //Initialize module on first GT pose. Else do Nothing! + if (!GT_pose_initialized) + { + initial_robot_pose = *new_initPose; + GT_pose_initialized = true; + } +} + +void CLaserOdometry2DNode::publish() +{ + //first, we'll publish the odometry over tf + //--------------------------------------- + if (publish_tf) + { + //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]"); + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = ros::Time::now(); + odom_trans.header.frame_id = odom_frame_id; + odom_trans.child_frame_id = base_frame_id; + odom_trans.transform.translation.x = robot_pose.x(); + odom_trans.transform.translation.y = robot_pose.y(); + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); + //send the transform + odom_broadcaster.sendTransform(odom_trans); + } + + //next, we'll publish the odometry message over ROS + //------------------------------------------------- + //ROS_INFO("[rf2o] Publishing Odom Topic"); + nav_msgs::Odometry odom; + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = odom_frame_id; + //set the position + odom.pose.pose.position.x = robot_pose.x(); + odom.pose.pose.position.y = robot_pose.y(); + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); + //set the velocity + odom.child_frame_id = base_frame_id; + odom.twist.twist.linear.x = lin_speed; //linear speed + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.angular.z = ang_speed; //angular speed + //publish the message + odom_pub.publish(odom); +} + +//void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan) +//{ +// //Set the initial pose +// laser_pose = ini_pose; +// laser_oldpose = ini_pose; + +// //readLaser(scan); +// createImagePyramid(); +// //readLaser(scan); +// createImagePyramid(); +//} + +//----------------------------------------------------------------------------------- +// MAIN +//----------------------------------------------------------------------------------- +int main(int argc, char** argv) +{ + ros::init(argc, argv, "RF2O_LaserOdom"); + + CLaserOdometry2DNode myLaserOdomNode; + + //Main Loop + //---------- + ROS_INFO("initialization complete...Looping"); + ros::Rate loop_rate(myLaserOdomNode.freq); + while (ros::ok()) + { + ros::spinOnce(); //Check for new laser scans + + myLaserOdomNode.process(); + + loop_rate.sleep(); + } + return(0); +} From c3d22b8780bfaf02cacd339e67bf64ce409a1196 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Tue, 16 May 2017 11:51:09 +0200 Subject: [PATCH 02/11] add func get Pose/Increment --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 8 ++++++ src/CLaserOdometry2D.cpp | 26 ++++++++++++++++++- src/CLaserOdometry2DNode.cpp | 12 --------- 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 576546f..e460566 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -79,6 +79,12 @@ protected: void setLaserPose(const tf::Transform& laser_pose); + mrpt::poses::CPose3D& getIncrement(); + const mrpt::poses::CPose3D& getIncrement() const; + + mrpt::poses::CPose3D& getPose(); + const mrpt::poses::CPose3D& getPose() const; + protected: bool module_initialized,first_laser_scan; @@ -137,6 +143,8 @@ protected: mrpt::math::CMatrixFloat31 kai_loc_old; mrpt::math::CMatrixFloat31 kai_loc_level; + mrpt::poses::CPose3D last_increment; + mrpt::poses::CPose3D laser_pose_on_robot; mrpt::poses::CPose3D laser_pose_on_robot_inv; diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index a7eab3b..9cd98f5 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -26,7 +26,9 @@ using namespace Eigen; // CLaserOdometry2D //--------------------------------------------- -CLaserOdometry2D::CLaserOdometry2D() +CLaserOdometry2D::CLaserOdometry2D() : + module_initialized(false), + first_laser_scan(true) { // } @@ -175,6 +177,26 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, } +mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() +{ + return last_increment; +} + +const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const +{ + return last_increment; +} + +mrpt::poses::CPose3D& CLaserOdometry2D::getPose() +{ + return robot_pose; +} + +const mrpt::poses::CPose3D& CLaserOdometry2D::getPose() const +{ + return robot_pose; +} + void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) { //================================================================================== @@ -896,6 +918,8 @@ void CLaserOdometry2D::PoseUpdate() poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps); laser_pose = laser_pose + CPose3D(pose_aux_2D); + last_increment = pose_aux_2D; + // Compute kai_loc_old diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 97ebc3c..039940d 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -224,18 +224,6 @@ void CLaserOdometry2DNode::publish() odom_pub.publish(odom); } -//void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan) -//{ -// //Set the initial pose -// laser_pose = ini_pose; -// laser_oldpose = ini_pose; - -// //readLaser(scan); -// createImagePyramid(); -// //readLaser(scan); -// createImagePyramid(); -//} - //----------------------------------------------------------------------------------- // MAIN //----------------------------------------------------------------------------------- From dec57a6461df3a21367a905b80bd9d0afcd0dcea Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Tue, 16 May 2017 12:13:19 +0200 Subject: [PATCH 03/11] remove all using namespace --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 4 +- src/CLaserOdometry2D.cpp | 62 +++++++++---------- src/CLaserOdometry2DNode.cpp | 6 -- 3 files changed, 31 insertions(+), 41 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index e460566..a2b2436 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -32,7 +32,7 @@ #else # include # include - using namespace mrpt::slam; + #endif #if MRPT_VERSION<0x150 @@ -169,7 +169,7 @@ protected: void solveSystemNonLinear(); void filterLevelSolution(); void PoseUpdate(); - void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan); + void Reset(mrpt::poses::CPose3D ini_pose, mrpt::poses::CObservation2DRangeScan scan); }; #endif diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 9cd98f5..472fd98 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -14,13 +14,6 @@ ******************************************************************************************** */ #include "rf2o_laser_odometry/CLaserOdometry2D.h" -using namespace mrpt; -using namespace mrpt::math; -using namespace mrpt::utils; -using namespace mrpt::poses; -using namespace std; -using namespace Eigen; - // -------------------------------------------- // CLaserOdometry2D @@ -468,8 +461,8 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface() for (unsigned int u = 0; u < cols_i-1; u++) { - const float dist = square(xx_inter[image_level](u+1) - xx_inter[image_level](u)) - + square(yy_inter[image_level](u+1) - yy_inter[image_level](u)); + const float dist = mrpt::math::square(xx_inter[image_level](u+1) - xx_inter[image_level](u)) + + mrpt::math::square(yy_inter[image_level](u+1) - yy_inter[image_level](u)); if (dist > 0.f) rtita(u) = sqrt(dist); } @@ -545,7 +538,7 @@ void CLaserOdometry2D::computeWeights() //Parameters for error_linearization const float kdtita = 1.f; - const float kdt = kdtita/square(fps); + const float kdt = kdtita/mrpt::math::square(fps); const float k2d = 0.2f; for (unsigned int u = 1; u < cols_i-1; u++) @@ -559,7 +552,7 @@ void CLaserOdometry2D::computeWeights() const float dtitat = ini_dtita - final_dtita; const float dtita2 = dtita(u+1) - dtita(u-1); - const float w_der = kdt*square(dt(u)) + kdtita*square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2)); + const float w_der = kdt*mrpt::math::square(dt(u)) + kdtita*mrpt::math::square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2)); weights(u) = sqrt(1.f/w_der); } @@ -615,13 +608,13 @@ void CLaserOdometry2D::solveSystemOneLevel() } //Solve the linear system of equations using a minimum least squares method - MatrixXf AtA, AtB; + Eigen::MatrixXf AtA, AtB; AtA.multiply_AtA(A); AtB.multiply_AtB(A,B); Var = AtA.ldlt().solve(AtB); //Covariance matrix calculation Cov Order -> vx,vy,wz - MatrixXf res(num_valid_range,1); + Eigen::MatrixXf res(num_valid_range,1); res = A*Var - B; cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); @@ -657,13 +650,13 @@ void CLaserOdometry2D::solveSystemNonLinear() } //Solve the linear system of equations using a minimum least squares method - MatrixXf AtA, AtB; + Eigen::MatrixXf AtA, AtB; AtA.multiply_AtA(A); AtB.multiply_AtB(A,B); Var = AtA.ldlt().solve(AtB); //Covariance matrix calculation Cov Order -> vx,vy,wz - MatrixXf res(num_valid_range,1); + Eigen::MatrixXf res(num_valid_range,1); res = A*Var - B; //cout << endl << "max res: " << res.maxCoeff(); //cout << endl << "min res: " << res.minCoeff(); @@ -684,7 +677,7 @@ void CLaserOdometry2D::solveSystemNonLinear() const float k = 10.f/aver_dt; //200 //float energy = 0.f; //for (unsigned int i=0; i Bii; - Matrix kai_b; + Eigen::Matrix Bii; + Eigen::Matrix kai_b; Bii = eigensolver.eigenvectors(); kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level); //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too //------------------------------------------------------------------------------------------------- - CMatrixFloat31 kai_loc_sub; + mrpt::math::CMatrixFloat31 kai_loc_sub; //Important: we have to substract the solutions from previous levels - Matrix3f acu_trans; + Eigen::Matrix3f acu_trans; acu_trans.setIdentity(); for (unsigned int i=0; i 1.f) kai_loc_sub(2) = 0.f; else - kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*sign(acu_trans(1,0)); + kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0)); kai_loc_sub += kai_loc_old; - Matrix kai_b_old; + Eigen::Matrix kai_b_old; kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub); //Filter speed const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level)); - Matrix kai_b_fil; + Eigen::Matrix kai_b_fil; for (unsigned int i=0; i<3; i++) { kai_b_fil(i,0) = (kai_b(i,0) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i,0))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df); @@ -868,7 +863,7 @@ void CLaserOdometry2D::filterLevelSolution() } //Transform filtered speed to local reference frame and compute transformation - Matrix kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); + Eigen::Matrix kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); //transformation const float incrx = kai_loc_fil(0)/fps; @@ -887,7 +882,7 @@ void CLaserOdometry2D::PoseUpdate() { //First, compute the overall transformation //--------------------------------------------------- - Matrix3f acu_trans; + Eigen::Matrix3f acu_trans; acu_trans.setIdentity(); for (unsigned int i=1; i<=ctf_levels; i++) acu_trans = transformations[i-1]*acu_trans; @@ -900,7 +895,7 @@ void CLaserOdometry2D::PoseUpdate() if (acu_trans(0,0) > 1.f) kai_loc(2) = 0.f; else - kai_loc(2) = fps*acos(acu_trans(0,0))*sign(acu_trans(1,0)); + kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0)); //cout << endl << "Arc cos (incr tita): " << kai_loc(2); @@ -914,6 +909,7 @@ void CLaserOdometry2D::PoseUpdate() // Update poses //------------------------------------------------------- laser_oldpose = laser_pose; + math::CMatrixDouble33 aux_acu = acu_trans; poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps); laser_pose = laser_pose + CPose3D(pose_aux_2D); @@ -945,7 +941,7 @@ void CLaserOdometry2D::PoseUpdate() double time_inc_sec = (current_scan_time - last_odom_time).toSec(); last_odom_time = current_scan_time; lin_speed = acu_trans(0,2) / time_inc_sec; - //double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec; + //double lin_speed = sqrt( mrpt::math::square(robot_oldpose.x()-robot_pose.x()) + mrpt::math::square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec; double ang_inc = robot_pose.yaw() - robot_oldpose.yaw(); if (ang_inc > 3.14159) ang_inc -= 2*3.14159; diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 039940d..f1be914 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -14,12 +14,6 @@ ******************************************************************************************** */ #include "rf2o_laser_odometry/CLaserOdometry2D.h" -using namespace mrpt; -using namespace mrpt::math; -using namespace mrpt::utils; -using namespace mrpt::poses; -using namespace std; -using namespace Eigen; class CLaserOdometry2DNode : CLaserOdometry2D { From 5893d1a3e906254ea17893119eb68f9871c38938 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Tue, 16 May 2017 12:56:24 +0200 Subject: [PATCH 04/11] moving headers & setLaserPose with CPose3D arg --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 5 +- src/CLaserOdometry2D.cpp | 15 +---- src/CLaserOdometry2DNode.cpp | 59 ++++++++++++++----- 3 files changed, 47 insertions(+), 32 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index a2b2436..4f70b5b 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -17,11 +17,8 @@ #define CLaserOdometry2D_H #include -#include -#include #include #include -#include // MRPT related headers #include @@ -77,7 +74,7 @@ protected: void odometryCalculation(const sensor_msgs::LaserScan& scan); >>>>>>> wip separating node from computation - void setLaserPose(const tf::Transform& laser_pose); + void setLaserPose(const mrpt::poses::CPose3D& laser_pose); mrpt::poses::CPose3D& getIncrement(); const mrpt::poses::CPose3D& getIncrement() const; diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 472fd98..5d3d0a0 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -32,22 +32,11 @@ CLaserOdometry2D::~CLaserOdometry2D() // } -void CLaserOdometry2D::setLaserPose(const tf::Transform& laser_pose) +void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& laser_pose) { //Set laser pose on the robot - //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) - const tf::Vector3 &t = laser_pose.getOrigin(); - laser_pose_on_robot.x() = t[0]; - laser_pose_on_robot.y() = t[1]; - laser_pose_on_robot.z() = t[2]; - const tf::Matrix3x3 &basis = laser_pose.getBasis(); - mrpt::math::CMatrixDouble33 R; - for(int r = 0; r < 3; r++) - for(int c = 0; c < 3; c++) - R(r,c) = basis[r][c]; - laser_pose_on_robot.setRotationMatrix(R); - + laser_pose_on_robot = laser_pose; laser_pose_on_robot_inv = laser_pose_on_robot; laser_pose_on_robot_inv.inverse(); } diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index f1be914..f3b5ccb 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -15,6 +15,9 @@ #include "rf2o_laser_odometry/CLaserOdometry2D.h" +#include +#include + class CLaserOdometry2DNode : CLaserOdometry2D { public: @@ -25,6 +28,8 @@ public: void process(); void publish(); + bool setLaserPoseFromTf(); + public: bool verbose,publish_tf,new_scan_available; @@ -95,21 +100,7 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() : initial_robot_pose.pose.pose.orientation.z = 0; } - // Set laser pose on the robot (through tF) - // This allow estimation of the odometry with respect to the robot base reference system. - tf::StampedTransform transform; - transform.setIdentity(); - try - { - tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); - } - catch (tf::TransformException &ex) - { - ROS_ERROR("%s",ex.what()); - ros::Duration(1.0).sleep(); - } - - setLaserPose(transform); + setLaserPoseFromTf(); //Init variables module_initialized = false; @@ -121,6 +112,44 @@ CLaserOdometry2DNode::~CLaserOdometry2DNode() // } +bool CLaserOdometry2DNode::setLaserPoseFromTf() +{ + bool retrieved = false; + + // Set laser pose on the robot (through tF) + // This allow estimation of the odometry with respect to the robot base reference system. + tf::StampedTransform transform; + transform.setIdentity(); + try + { + tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); + retrieved = true; + } + catch (tf::TransformException &ex) + { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + retrieved = false; + } + + //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) + mrpt::poses::CPose3D laser_tf; + const tf::Vector3 &t = transform.getOrigin(); + laser_tf.x() = t[0]; + laser_tf.y() = t[1]; + laser_tf.z() = t[2]; + const tf::Matrix3x3 &basis = transform.getBasis(); + mrpt::math::CMatrixDouble33 R; + for(int r = 0; r < 3; r++) + for(int c = 0; c < 3; c++) + R(r,c) = basis[r][c]; + laser_tf.setRotationMatrix(R); + + setLaserPose(laser_tf); + + return retrieved; +} + bool CLaserOdometry2DNode::scan_available() { return new_scan_available; From 7333f65490cca6898da63dcb1ebf11829fbfb942 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Thu, 22 Jun 2017 21:21:32 +0200 Subject: [PATCH 05/11] rm one more using namespace and use typedef to deal with diff mrpt version --- include/rf2o_laser_odometry/CLaserOdometry2D.h | 5 +++-- src/CLaserOdometry2D.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 4f70b5b..7feb83a 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -25,10 +25,11 @@ #if MRPT_VERSION>=0x130 # include # include - using namespace mrpt::obs; + typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan; #else # include # include + typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan; #endif @@ -166,7 +167,7 @@ protected: void solveSystemNonLinear(); void filterLevelSolution(); void PoseUpdate(); - void Reset(mrpt::poses::CPose3D ini_pose, mrpt::poses::CObservation2DRangeScan scan); + void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan); }; #endif diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 5d3d0a0..d3dc87d 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -708,7 +708,7 @@ void CLaserOdometry2D::solveSystemNonLinear() std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl; } -void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, mrpt::poses::CObservation2DRangeScan scan) +void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan) { //Set the initial pose laser_pose = ini_pose; From 92b817eb17afefd2a2bd3d857cfd42dd747dbe71 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Thu, 22 Jun 2017 21:22:24 +0200 Subject: [PATCH 06/11] macro to find apropriate sign func depending on mrpt version --- src/CLaserOdometry2D.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index d3dc87d..014f0a2 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -834,8 +834,14 @@ void CLaserOdometry2D::filterLevelSolution() kai_loc_sub(1) = -fps*acu_trans(1,2); if (acu_trans(0,0) > 1.f) kai_loc_sub(2) = 0.f; - else - kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0)); + else + { +#if MRPT_VERSION>=0x130 + kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0)); +#else + kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0)); +#endif + } kai_loc_sub += kai_loc_old; Eigen::Matrix kai_b_old; @@ -884,8 +890,13 @@ void CLaserOdometry2D::PoseUpdate() if (acu_trans(0,0) > 1.f) kai_loc(2) = 0.f; else + { +#if MRPT_VERSION>=0x130 + kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0)); +#else kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0)); - +#endif + } //cout << endl << "Arc cos (incr tita): " << kai_loc(2); float phi = laser_pose.yaw(); From 576817a8b64bf0c37bd020c59b909f984abf657d Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sat, 12 Aug 2017 18:11:54 +0200 Subject: [PATCH 07/11] fixes after rebasing --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 18 ++------ src/CLaserOdometry2D.cpp | 41 ++++++++----------- 2 files changed, 22 insertions(+), 37 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 7feb83a..d01b86d 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -40,7 +40,7 @@ #include #include #include -#include +//#include #include #include @@ -63,17 +63,7 @@ public: bool is_initialized(); -<<<<<<< HEAD -protected: - ros::NodeHandle n; - sensor_msgs::LaserScan last_scan; - bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized, verbose; - tf::TransformListener tf_listener; //Do not put inside the callback - tf::TransformBroadcaster odom_broadcaster; - nav_msgs::Odometry initial_robot_pose; -======= void odometryCalculation(const sensor_msgs::LaserScan& scan); ->>>>>>> wip separating node from computation void setLaserPose(const mrpt::poses::CPose3D& laser_pose); @@ -85,7 +75,7 @@ protected: protected: - bool module_initialized,first_laser_scan; + bool verbose,module_initialized,first_laser_scan; // Internal Data std::vector range; @@ -101,7 +91,7 @@ protected: std::vector yy_old; std::vector yy_warped; std::vector transformations; - + Eigen::MatrixXf range_wf; Eigen::MatrixXf dtita; Eigen::MatrixXf dt; @@ -160,7 +150,7 @@ protected: void calculateCoord(); void performWarping(); void calculaterangeDerivativesSurface(); - void computeNormals(); + void computeNormals(); void computeWeights(); void findNullPoints(); void solveSystemOneLevel(); diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 014f0a2..ad4b1e1 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -50,9 +50,9 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, const geometry_msgs::Pose& initial_robot_pose) { //Got an initial scan laser, obtain its parametes - if (verbose) - ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node"); - width = last_scan.ranges.size(); // Num of samples (size) of the scan laser + ROS_INFO_COND(verbose, "[rf2o] Got first Laser Scan .... Configuring node"); + + width = scan.ranges.size(); // Num of samples (size) of the scan laser cols = width; // Max reolution. Should be similar to the width parameter fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV @@ -77,8 +77,8 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, //robotInitialPose.phi(yaw); //Set the initial pose - laser_pose = robotInitialPose + LaserPoseOnTheRobot; - laser_oldpose = robotInitialPose + LaserPoseOnTheRobot; + laser_pose = robotInitialPose + laser_pose_on_robot; + laser_oldpose = robotInitialPose + laser_pose_on_robot; // Init module (internal) //------------------------ @@ -240,8 +240,8 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) } m_runtime = 1000*m_clock.Tac(); - if (verbose) - ROS_INFO("[rf2o] execution time (ms): %f", m_runtime); + + ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", m_runtime); //Update poses PoseUpdate(); @@ -704,8 +704,7 @@ void CLaserOdometry2D::solveSystemNonLinear() cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); kai_loc_level = Var; - if (verbose) - std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl; + ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO: " << cov_odo); } void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan) @@ -723,7 +722,7 @@ void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeS void CLaserOdometry2D::performWarping() { - Matrix3f acu_trans; + Eigen::Matrix3f acu_trans; acu_trans.setIdentity(); for (unsigned int i=1; i<=level; i++) @@ -804,11 +803,10 @@ void CLaserOdometry2D::filterLevelSolution() { // Calculate Eigenvalues and Eigenvectors //---------------------------------------------------------- - SelfAdjointEigenSolver eigensolver(cov_odo); - if (eigensolver.info() != Success) + Eigen::SelfAdjointEigenSolver eigensolver(cov_odo); + if (eigensolver.info() != Eigen::Success) { - if (verbose) - printf("[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); + ROS_INFO_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); return; } @@ -910,9 +908,9 @@ void CLaserOdometry2D::PoseUpdate() //------------------------------------------------------- laser_oldpose = laser_pose; - math::CMatrixDouble33 aux_acu = acu_trans; - poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps); - laser_pose = laser_pose + CPose3D(pose_aux_2D); + mrpt::math::CMatrixDouble33 aux_acu = acu_trans; + mrpt::poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps); + laser_pose = laser_pose + mrpt::poses::CPose3D(pose_aux_2D); last_increment = pose_aux_2D; @@ -925,14 +923,11 @@ void CLaserOdometry2D::PoseUpdate() kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi); kai_loc_old(2) = kai_abs(2); - if (verbose) - ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); - + ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); //Compose Transformations - robot_pose = laser_pose + LaserPoseOnTheRobot_inv; - if (verbose) - ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw()); + robot_pose = laser_pose + laser_pose_on_robot_inv; + ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw()); // Estimate linear/angular speeds (mandatory for base_local_planner) // last_scan -> the last scan received From 223855db6daf506eab5c20b167905252113acd34 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sat, 12 Aug 2017 18:13:39 +0200 Subject: [PATCH 08/11] timer rather than spinOnce --- src/CLaserOdometry2DNode.cpp | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index f3b5ccb..678ba81 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -25,14 +25,14 @@ public: CLaserOdometry2DNode(); ~CLaserOdometry2DNode(); - void process(); + void process(const ros::TimerEvent &); void publish(); bool setLaserPoseFromTf(); public: - bool verbose,publish_tf,new_scan_available; + bool publish_tf,new_scan_available; double freq; @@ -79,7 +79,7 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() : //Publishers and Subscribers //-------------------------- - odom_pub = pn.advertise(odom_topic, 5); + odom_pub = pn.advertise(odom_topic, 5); laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); //init pose?? @@ -104,7 +104,9 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() : //Init variables module_initialized = false; - first_laser_scan = true; + first_laser_scan = true; + + ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic()); } CLaserOdometry2DNode::~CLaserOdometry2DNode() @@ -155,7 +157,7 @@ bool CLaserOdometry2DNode::scan_available() return new_scan_available; } -void CLaserOdometry2DNode::process() +void CLaserOdometry2DNode::process(const ros::TimerEvent&) { if( is_initialized() && scan_available() ) { @@ -256,17 +258,18 @@ int main(int argc, char** argv) CLaserOdometry2DNode myLaserOdomNode; - //Main Loop - //---------- - ROS_INFO("initialization complete...Looping"); - ros::Rate loop_rate(myLaserOdomNode.freq); - while (ros::ok()) - { - ros::spinOnce(); //Check for new laser scans + ros::TimerOptions timer_opt; + timer_opt.oneshot = false; + timer_opt.autostart = true; + timer_opt.callback_queue = ros::getGlobalCallbackQueue(); + timer_opt.tracked_object = ros::VoidConstPtr(); - myLaserOdomNode.process(); + timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1); + timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime(); - loop_rate.sleep(); - } - return(0); + ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt); + + ros::spin(); + + return EXIT_SUCCESS; } From 4d3cd73e91835cd2ae8d7b77940e442b5cef84ac Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sat, 12 Aug 2017 18:14:13 +0200 Subject: [PATCH 09/11] test general case first --- src/CLaserOdometry2DNode.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 678ba81..1310a8e 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -185,18 +185,18 @@ void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& current_scan_time = last_scan.header.stamp; //Initialize module on first scan - if (first_laser_scan) - { - Init(last_scan, initial_robot_pose.pose.pose); - first_laser_scan = false; - } - else + if (!first_laser_scan) { //copy laser scan to internal variable for (unsigned int i = 0; iranges[i]; new_scan_available = true; } + else + { + Init(last_scan, initial_robot_pose.pose.pose); + first_laser_scan = false; + } } } From 07c9a7b46c165479c29a23a807e9c2b724857597 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sat, 12 Aug 2017 18:16:01 +0200 Subject: [PATCH 10/11] fixe cmake --- CMakeLists.txt | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2620f43..ac5f2b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,15 +14,17 @@ find_package(catkin REQUIRED COMPONENTS tf ) +set(MRPT_DONT_USE_DBG_LIBS 1) + ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) find_package(cmake_modules REQUIRED) find_package(Eigen3 REQUIRED) -find_package(MRPT REQUIRED base obs maps slam) +find_package(MRPT REQUIRED base obs) # maps slam #include_directories(${MRPT_INCLUDE_DIRS}) -#MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS}) +MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS}) #link_directories(${MRPT_LIBRARY_DIRS}) -#MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBS}) +MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES}) @@ -37,9 +39,9 @@ find_package(MRPT REQUIRED base obs maps slam) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES laser_odometry + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf - #DEPENDS system_lib + DEPENDS MRPT ) ## Specify additional locations of header files @@ -47,25 +49,19 @@ catkin_package( include_directories(include) include_directories( + SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} + ${MRPT_INCLUDE_DIRS} ) ## Declare a cpp library add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS}) ## Declare a cpp executable add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp) -target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME}) - -## Specify libraries to link a library or executable target against -target_link_libraries(rf2o_laser_odometry_node - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${EIGEN_LIBRARIES} - ${MRPT_LIBS} -) +target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME} ${catkin_LIBRARIES}) From 737f44b0db0bd56ab413ece35daed47e12d810a1 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sat, 12 Aug 2017 18:26:39 +0200 Subject: [PATCH 11/11] add getIncrementCovariance & rm return by ref getIncrement --- include/rf2o_laser_odometry/CLaserOdometry2D.h | 3 ++- src/CLaserOdometry2D.cpp | 7 +++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index d01b86d..6985391 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -67,9 +67,10 @@ public: void setLaserPose(const mrpt::poses::CPose3D& laser_pose); - mrpt::poses::CPose3D& getIncrement(); const mrpt::poses::CPose3D& getIncrement() const; + const Eigen::Matrix& getIncrementCovariance() const; + mrpt::poses::CPose3D& getPose(); const mrpt::poses::CPose3D& getPose() const; diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index ad4b1e1..12c4c5b 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -158,15 +158,14 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, last_odom_time = ros::Time::now(); } - -mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() +const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const { return last_increment; } -const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const +const Eigen::Matrix& CLaserOdometry2D::getIncrementCovariance() const { - return last_increment; + return cov_odo; } mrpt::poses::CPose3D& CLaserOdometry2D::getPose()