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

Merge pull request #10 from artivis/mrpt_free

Browse Source 
[WIP] Remove MRPT dependency
This commit is contained in:
Javier G. Monroy
2018-11-21 13:17:48 +01:00
committed by GitHub
parent 06301fd315 1bc891bf58
commit d09068cfff
6 changed files with 1020 additions and 993 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
CMakeLists.txt
View File

@@ -19,22 +19,12 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs sensor_msgs
std_msgs std_msgs
tf tf
cmake_modules
) )
set(MRPT_DONT_USE_DBG_LIBS 1)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system) find_package(Boost REQUIRED COMPONENTS system)
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED) find_package(Eigen3 REQUIRED)
find_package(MRPT REQUIRED base obs) # maps slam
#include_directories(${MRPT_INCLUDE_DIRS})
MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS})
#link_directories(${MRPT_LIBRARY_DIRS})
MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
@@ -46,10 +36,10 @@ MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
## CATKIN_DEPENDS: catkin_packages dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
LIBRARIES ${PROJECT_NAME} LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
DEPENDS MRPT DEPENDS #Eigen3
) )
## Specify additional locations of header files ## Specify additional locations of header files
@@ -60,15 +50,12 @@ include_directories(
SYSTEM SYSTEM
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${EIGEN_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}
${MRPT_INCLUDE_DIRS}
) )
## Declare a cpp library ## Declare a cpp library
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp)
src/CLaserOdometry2D.cpp target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS})
## Declare a cpp executable ## Declare a cpp executable
add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp) add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)

5
README.md
View File

@@ -6,8 +6,3 @@ RF2O is a fast and precise method to estimate the planar motion of a lidar from
Its very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precission, makes RF2O a suitable method for those robotic applications that require planar odometry. Its very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precission, makes RF2O a suitable method for those robotic applications that require planar odometry.
For full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.isa.uma.es/work/rf2o For full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.isa.uma.es/work/rf2o
# Requirements
RF2O core is implemented within the **Mobile Robot Programming Toolkit** [MRPT](http://www.mrpt.org/), so it is necessary to install this powerful library (see instructions [here](http://www.mrpt.org/download-mrpt/))
So far RF2O has been tested with the Ubuntu official repository version (MRPT v1.3), and we are working to update it to MRPT v.1.9

241
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -11,153 +11,170 @@
* *
* Maintainer: Javier G. Monroy * Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/ * MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */ ******************************************************************************************** */
#ifndef CLaserOdometry2D_H #ifndef CLaserOdometry2D_H
#define CLaserOdometry2D_H #define CLaserOdometry2D_H
#include <ros/ros.h> // std header
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
// MRPT related headers
#include <mrpt/version.h>
#if MRPT_VERSION>=0x130
# include <mrpt/obs/CObservation2DRangeScan.h>
# include <mrpt/obs/CObservationOdometry.h>
typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan;
#else
# include <mrpt/slam/CObservation2DRangeScan.h>
# include <mrpt/slam/CObservationOdometry.h>
typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan;
#endif
#if MRPT_VERSION<0x150
# include <mrpt/system/threads.h>
#endif
#include <mrpt/system/os.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/utils.h>
//#include <mrpt/opengl.h>
#include <mrpt/math/CHistogram.h>
#include <boost/bind.hpp>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
#include <numeric> #include <numeric>
// ROS headers
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
// Eigen headers
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions>
namespace rf2o {
template <typename T>
inline T sign(const T x) { return x<T(0) ? -1:1; }
template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar
getYaw(const Eigen::MatrixBase<Derived>& r)
{
return std::atan2( r(1, 0), r(0, 0) );
}
template<typename T>
inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
const T pitch,
const T yaw)
{
const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
return (az * ay * ax).toRotationMatrix().matrix();
}
template<typename T>
inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw)
{
return matrixRollPitchYaw<T>(0, 0, yaw);
}
class CLaserOdometry2D class CLaserOdometry2D
{ {
public: public:
CLaserOdometry2D();
~CLaserOdometry2D();
void Init(const sensor_msgs::LaserScan& scan, using Scalar = float;
const geometry_msgs::Pose& initial_robot_pose);
bool is_initialized(); using Pose2d = Eigen::Isometry2d;
using Pose3d = Eigen::Isometry3d;
using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;
void odometryCalculation(const sensor_msgs::LaserScan& scan); CLaserOdometry2D();
virtual ~CLaserOdometry2D() = default;
void setLaserPose(const mrpt::poses::CPose3D& laser_pose); void init(const sensor_msgs::LaserScan& scan,
const geometry_msgs::Pose& initial_robot_pose);
const mrpt::poses::CPose3D& getIncrement() const; bool is_initialized();
const Eigen::Matrix<float, 3, 3>& getIncrementCovariance() const; bool odometryCalculation(const sensor_msgs::LaserScan& scan);
mrpt::poses::CPose3D& getPose(); void setLaserPose(const Pose3d& laser_pose);
const mrpt::poses::CPose3D& getPose() const;
const Pose3d& getIncrement() const;
const IncrementCov& getIncrementCovariance() const;
Pose3d& getPose();
const Pose3d& getPose() const;
protected: protected:
bool verbose,module_initialized,first_laser_scan; bool verbose, module_initialized, first_laser_scan;
// Internal Data // Internal Data
std::vector<Eigen::MatrixXf> range; std::vector<Eigen::MatrixXf> range;
std::vector<Eigen::MatrixXf> range_old; std::vector<Eigen::MatrixXf> range_old;
std::vector<Eigen::MatrixXf> range_inter; std::vector<Eigen::MatrixXf> range_inter;
std::vector<Eigen::MatrixXf> range_warped; std::vector<Eigen::MatrixXf> range_warped;
std::vector<Eigen::MatrixXf> xx; std::vector<Eigen::MatrixXf> xx;
std::vector<Eigen::MatrixXf> xx_inter; std::vector<Eigen::MatrixXf> xx_inter;
std::vector<Eigen::MatrixXf> xx_old; std::vector<Eigen::MatrixXf> xx_old;
std::vector<Eigen::MatrixXf> xx_warped; std::vector<Eigen::MatrixXf> xx_warped;
std::vector<Eigen::MatrixXf> yy; std::vector<Eigen::MatrixXf> yy;
std::vector<Eigen::MatrixXf> yy_inter; std::vector<Eigen::MatrixXf> yy_inter;
std::vector<Eigen::MatrixXf> yy_old; std::vector<Eigen::MatrixXf> yy_old;
std::vector<Eigen::MatrixXf> yy_warped; std::vector<Eigen::MatrixXf> yy_warped;
std::vector<Eigen::MatrixXf> transformations; std::vector<Eigen::MatrixXf> transformations;
Eigen::MatrixXf range_wf; Eigen::MatrixXf range_wf;
Eigen::MatrixXf dtita; Eigen::MatrixXf dtita;
Eigen::MatrixXf dt; Eigen::MatrixXf dt;
Eigen::MatrixXf rtita; Eigen::MatrixXf rtita;
Eigen::MatrixXf normx, normy, norm_ang; Eigen::MatrixXf normx, normy, norm_ang;
Eigen::MatrixXf weights; Eigen::MatrixXf weights;
Eigen::MatrixXi null; Eigen::MatrixXi null;
Eigen::MatrixXf A,Aw; Eigen::MatrixXf A,Aw;
Eigen::MatrixXf B,Bw; Eigen::MatrixXf B,Bw;
Eigen::Matrix<float, 3, 1> Var; //3 unknowns: vx, vy, w
Eigen::Matrix<float, 3, 3> cov_odo;
MatrixS31 Var; //3 unknowns: vx, vy, w
IncrementCov cov_odo;
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan float fps; //In Hz
float fps; //In Hz float fovh; //Horizontal FOV
float fovh; //Horizontal FOV unsigned int cols;
unsigned int cols; unsigned int cols_i;
unsigned int cols_i; unsigned int width;
unsigned int width; unsigned int ctf_levels;
unsigned int ctf_levels; unsigned int image_level, level;
unsigned int image_level, level; unsigned int num_valid_range;
unsigned int num_valid_range; unsigned int iter_irls;
unsigned int iter_irls; float g_mask[5];
float g_mask[5];
double lin_speed, ang_speed; double lin_speed, ang_speed;
//mrpt::gui::CDisplayWindowPlots window; ros::WallDuration m_runtime;
mrpt::utils::CTicTac m_clock; ros::Time last_odom_time, current_scan_time;
float m_runtime;
ros::Time last_odom_time, current_scan_time;
mrpt::math::CMatrixFloat31 kai_abs; MatrixS31 kai_abs_;
mrpt::math::CMatrixFloat31 kai_loc; MatrixS31 kai_loc_;
mrpt::math::CMatrixFloat31 kai_loc_old; MatrixS31 kai_loc_old_;
mrpt::math::CMatrixFloat31 kai_loc_level; MatrixS31 kai_loc_level_;
mrpt::poses::CPose3D last_increment; Pose3d last_increment_;
Pose3d laser_pose_on_robot_;
Pose3d laser_pose_on_robot_inv_;
Pose3d laser_pose_;
Pose3d laser_oldpose_;
Pose3d robot_pose_;
Pose3d robot_oldpose_;
mrpt::poses::CPose3D laser_pose_on_robot; bool test;
mrpt::poses::CPose3D laser_pose_on_robot_inv; std::vector<double> last_m_lin_speeds;
std::vector<double> last_m_ang_speeds;
mrpt::poses::CPose3D laser_pose; // Methods
mrpt::poses::CPose3D laser_oldpose; void createImagePyramid();
mrpt::poses::CPose3D robot_pose; void calculateCoord();
mrpt::poses::CPose3D robot_oldpose; void performWarping();
bool test; void calculaterangeDerivativesSurface();
std::vector<double> last_m_lin_speeds; void computeNormals();
std::vector<double> last_m_ang_speeds; void computeWeights();
void findNullPoints();
void solveSystemOneLevel();
// Methods void solveSystemNonLinear();
void createImagePyramid(); bool filterLevelSolution();
void calculateCoord(); void PoseUpdate();
void performWarping(); void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
void calculaterangeDerivativesSurface();
void computeNormals();
void computeWeights();
void findNullPoints();
void solveSystemOneLevel();
void solveSystemNonLinear();
void filterLevelSolution();
void PoseUpdate();
void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan);
}; };
#endif } /* namespace rf2o */
#endif // CLaserOdometry2D_H

7
package.xml
View File

@@ -24,15 +24,14 @@
<build_depend>tf</build_depend> <build_depend>tf</build_depend>
<build_depend>cmake_modules</build_depend> <!-- A common repository for CMake Modules which are not distributed with CMake but are commonly used by ROS packages. --> <build_depend>cmake_modules</build_depend> <!-- A common repository for CMake Modules which are not distributed with CMake but are commonly used by ROS packages. -->
<!-- https://github.com/ros/cmake_modules/blob/0.3-devel/README.md --> <!-- https://github.com/ros/cmake_modules/blob/0.3-devel/README.md -->
<build_depend>mrpt</build_depend> <!-- Depend on mrpt system pkgs: http://www.mrpt.org/ --> <build_depend>eigen</build_depend>
<run_depend>nav_msgs</run_depend> <run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend> <run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend> <run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend> <run_depend>tf</run_depend>
<run_depend>cmake_modules</run_depend> <!-- For aditional dependencies such as Eigen --> <run_depend>cmake_modules</run_depend> <!-- For aditional dependencies such as Eigen -->
<run_depend>mrpt</run_depend> <!-- Depend on mrpt system pkgs --> <run_depend>eigen</run_depend>
</package> </package>

1497
src/CLaserOdometry2D.cpp
View File

File diff suppressed because it is too large Load Diff

234
src/CLaserOdometry2DNode.cpp
View File

@@ -11,6 +11,8 @@
* *
* Maintainer: Javier G. Monroy * Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/ * MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */ ******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h" #include "rf2o_laser_odometry/CLaserOdometry2D.h"
@@ -18,12 +20,14 @@
#include <tf/transform_broadcaster.h> #include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
namespace rf2o {
class CLaserOdometry2DNode : CLaserOdometry2D class CLaserOdometry2DNode : CLaserOdometry2D
{ {
public: public:
CLaserOdometry2DNode(); CLaserOdometry2DNode();
~CLaserOdometry2DNode(); ~CLaserOdometry2DNode() = default;
void process(const ros::TimerEvent &); void process(const ros::TimerEvent &);
void publish(); void publish();
@@ -32,7 +36,7 @@ public:
public: public:
bool publish_tf,new_scan_available; bool publish_tf, new_scan_available;
double freq; double freq;
@@ -63,57 +67,52 @@ public:
CLaserOdometry2DNode::CLaserOdometry2DNode() : CLaserOdometry2DNode::CLaserOdometry2DNode() :
CLaserOdometry2D() CLaserOdometry2D()
{ {
ROS_INFO("Initializing RF2O node..."); ROS_INFO("Initializing RF2O node...");
//Read Parameters //Read Parameters
//---------------- //----------------
ros::NodeHandle pn("~"); ros::NodeHandle pn("~");
pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan"); pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o"); pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
pn.param<std::string>("base_frame_id", base_frame_id, "/base_link"); pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom"); pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
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); pn.param<bool>("verbose", verbose, true);
//Publishers and Subscribers //Publishers and Subscribers
//-------------------------- //--------------------------
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5); odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
//init pose?? //init pose??
if (init_pose_from_topic != "") if (init_pose_from_topic != "")
{ {
initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this); initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
GT_pose_initialized = false; GT_pose_initialized = false;
} }
else else
{ {
GT_pose_initialized = true; GT_pose_initialized = true;
initial_robot_pose.pose.pose.position.x = 0; initial_robot_pose.pose.pose.position.x = 0;
initial_robot_pose.pose.pose.position.y = 0; initial_robot_pose.pose.pose.position.y = 0;
initial_robot_pose.pose.pose.position.z = 0; initial_robot_pose.pose.pose.position.z = 0;
initial_robot_pose.pose.pose.orientation.w = 0; initial_robot_pose.pose.pose.orientation.w = 0;
initial_robot_pose.pose.pose.orientation.x = 0; initial_robot_pose.pose.pose.orientation.x = 0;
initial_robot_pose.pose.pose.orientation.y = 0; initial_robot_pose.pose.pose.orientation.y = 0;
initial_robot_pose.pose.pose.orientation.z = 0; initial_robot_pose.pose.pose.orientation.z = 0;
} }
setLaserPoseFromTf(); setLaserPoseFromTf();
//Init variables //Init variables
module_initialized = false; module_initialized = false;
first_laser_scan = true; first_laser_scan = true;
ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic()); ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
} }
CLaserOdometry2DNode::~CLaserOdometry2DNode()
{
//
}
bool CLaserOdometry2DNode::setLaserPoseFromTf() bool CLaserOdometry2DNode::setLaserPoseFromTf()
{ {
bool retrieved = false; bool retrieved = false;
@@ -124,28 +123,31 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
transform.setIdentity(); transform.setIdentity();
try try
{ {
tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
retrieved = true; retrieved = true;
} }
catch (tf::TransformException &ex) catch (tf::TransformException &ex)
{ {
ROS_ERROR("%s",ex.what()); ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
retrieved = false; retrieved = false;
} }
//TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge) //TF:transform -> Eigen::Isometry3d
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(); const tf::Matrix3x3 &basis = transform.getBasis();
mrpt::math::CMatrixDouble33 R; Eigen::Matrix3d R;
for(int r = 0; r < 3; r++) for(int r = 0; r < 3; r++)
for(int c = 0; c < 3; c++) for(int c = 0; c < 3; c++)
R(r,c) = basis[r][c]; R(r,c) = basis[r][c];
laser_tf.setRotationMatrix(R);
Pose3d laser_tf(R);
const tf::Vector3 &t = transform.getOrigin();
laser_tf.translation()(0) = t[0];
laser_tf.translation()(1) = t[1];
laser_tf.translation()(2) = t[2];
setLaserPose(laser_tf); setLaserPose(laser_tf);
@@ -154,21 +156,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
bool CLaserOdometry2DNode::scan_available() bool CLaserOdometry2DNode::scan_available()
{ {
return new_scan_available; return new_scan_available;
} }
void CLaserOdometry2DNode::process(const ros::TimerEvent&) void CLaserOdometry2DNode::process(const ros::TimerEvent&)
{ {
if( is_initialized() && scan_available() ) if( is_initialized() && scan_available() )
{ {
//Process odometry estimation //Process odometry estimation
odometryCalculation(last_scan); odometryCalculation(last_scan);
publish(); publish();
new_scan_available = false; //avoids the possibility to run twice on the same laser scan new_scan_available = false; //avoids the possibility to run twice on the same laser scan
} }
else else
{ {
ROS_WARN("Waiting for laser_scans....") ; ROS_WARN("Waiting for laser_scans....") ;
} }
} }
@@ -178,36 +180,36 @@ void CLaserOdometry2DNode::process(const ros::TimerEvent&)
void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
{ {
if (GT_pose_initialized) if (GT_pose_initialized)
{ {
//Keep in memory the last received laser_scan //Keep in memory the last received laser_scan
last_scan = *new_scan; last_scan = *new_scan;
current_scan_time = last_scan.header.stamp; current_scan_time = last_scan.header.stamp;
//Initialize module on first scan //Initialize module on first scan
if (!first_laser_scan) if (!first_laser_scan)
{ {
//copy laser scan to internal variable //copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++) for (unsigned int i = 0; i<width; i++)
range_wf(i) = new_scan->ranges[i]; range_wf(i) = new_scan->ranges[i];
new_scan_available = true; new_scan_available = true;
}
else
{
Init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
} }
else
{
init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
}
} }
void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose) void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
{ {
//Initialize module on first GT pose. Else do Nothing! //Initialize module on first GT pose. Else do Nothing!
if (!GT_pose_initialized) if (!GT_pose_initialized)
{ {
initial_robot_pose = *new_initPose; initial_robot_pose = *new_initPose;
GT_pose_initialized = true; GT_pose_initialized = true;
} }
} }
void CLaserOdometry2DNode::publish() void CLaserOdometry2DNode::publish()
@@ -216,17 +218,17 @@ void CLaserOdometry2DNode::publish()
//--------------------------------------- //---------------------------------------
if (publish_tf) if (publish_tf)
{ {
//ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]"); //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
geometry_msgs::TransformStamped odom_trans; geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now(); odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = odom_frame_id; odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id; odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = robot_pose.x(); odom_trans.transform.translation.x = robot_pose_.translation()(0);
odom_trans.transform.translation.y = robot_pose.y(); odom_trans.transform.translation.y = robot_pose_.translation()(1);
odom_trans.transform.translation.z = 0.0; odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
//send the transform //send the transform
odom_broadcaster.sendTransform(odom_trans); odom_broadcaster.sendTransform(odom_trans);
} }
//next, we'll publish the odometry message over ROS //next, we'll publish the odometry message over ROS
@@ -236,10 +238,10 @@ void CLaserOdometry2DNode::publish()
odom.header.stamp = ros::Time::now(); odom.header.stamp = ros::Time::now();
odom.header.frame_id = odom_frame_id; odom.header.frame_id = odom_frame_id;
//set the position //set the position
odom.pose.pose.position.x = robot_pose.x(); odom.pose.pose.position.x = robot_pose_.translation()(0);
odom.pose.pose.position.y = robot_pose.y(); odom.pose.pose.position.y = robot_pose_.translation()(1);
odom.pose.pose.position.z = 0.0; odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
//set the velocity //set the velocity
odom.child_frame_id = base_frame_id; odom.child_frame_id = base_frame_id;
odom.twist.twist.linear.x = lin_speed; //linear speed odom.twist.twist.linear.x = lin_speed; //linear speed
@@ -249,27 +251,29 @@ void CLaserOdometry2DNode::publish()
odom_pub.publish(odom); odom_pub.publish(odom);
} }
} /* namespace rf2o */
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
// MAIN // MAIN
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "RF2O_LaserOdom"); ros::init(argc, argv, "RF2O_LaserOdom");
CLaserOdometry2DNode myLaserOdomNode; rf2o::CLaserOdometry2DNode myLaserOdomNode;
ros::TimerOptions timer_opt; ros::TimerOptions timer_opt;
timer_opt.oneshot = false; timer_opt.oneshot = false;
timer_opt.autostart = true; timer_opt.autostart = true;
timer_opt.callback_queue = ros::getGlobalCallbackQueue(); timer_opt.callback_queue = ros::getGlobalCallbackQueue();
timer_opt.tracked_object = ros::VoidConstPtr(); timer_opt.tracked_object = ros::VoidConstPtr();
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1); timer_opt.callback = boost::bind(&rf2o::CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime(); timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt); ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
ros::spin(); ros::spin();
return EXIT_SUCCESS; return EXIT_SUCCESS;
} }