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

27
CMakeLists.txt
View File

@@ -18,23 +18,13 @@ find_package(catkin REQUIRED COMPONENTS
nav_msgs
sensor_msgs
std_msgs
tf
tf
cmake_modules
)
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
#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 ##
@@ -46,10 +36,10 @@ MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
DEPENDS MRPT
DEPENDS #Eigen3
)
## Specify additional locations of header files
@@ -60,15 +50,12 @@ include_directories(
SYSTEM
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${EIGEN_INCLUDE_DIRS}
${MRPT_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
## Declare a cpp library
add_library(${PROJECT_NAME}
src/CLaserOdometry2D.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS})
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/CLaserOdometry2DNode.cpp)

7
README.md
View File

@@ -5,9 +5,4 @@ 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.
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
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

241
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -11,153 +11,170 @@
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */
#ifndef CLaserOdometry2D_H
#define CLaserOdometry2D_H
#include <ros/ros.h>
#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>
// std header
#include <iostream>
#include <fstream>
#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
{
public:
CLaserOdometry2D();
~CLaserOdometry2D();
void Init(const sensor_msgs::LaserScan& scan,
const geometry_msgs::Pose& initial_robot_pose);
using Scalar = float;
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();
const mrpt::poses::CPose3D& getPose() const;
void setLaserPose(const Pose3d& laser_pose);
const Pose3d& getIncrement() const;
const IncrementCov& getIncrementCovariance() const;
Pose3d& getPose();
const Pose3d& getPose() const;
protected:
bool verbose,module_initialized,first_laser_scan;
bool verbose, module_initialized, first_laser_scan;
// Internal Data
std::vector<Eigen::MatrixXf> range;
std::vector<Eigen::MatrixXf> range_old;
std::vector<Eigen::MatrixXf> range_inter;
std::vector<Eigen::MatrixXf> range_warped;
std::vector<Eigen::MatrixXf> xx;
std::vector<Eigen::MatrixXf> xx_inter;
std::vector<Eigen::MatrixXf> xx_old;
std::vector<Eigen::MatrixXf> xx_warped;
std::vector<Eigen::MatrixXf> yy;
std::vector<Eigen::MatrixXf> yy_inter;
std::vector<Eigen::MatrixXf> yy_old;
std::vector<Eigen::MatrixXf> yy_warped;
std::vector<Eigen::MatrixXf> transformations;
// Internal Data
std::vector<Eigen::MatrixXf> range;
std::vector<Eigen::MatrixXf> range_old;
std::vector<Eigen::MatrixXf> range_inter;
std::vector<Eigen::MatrixXf> range_warped;
std::vector<Eigen::MatrixXf> xx;
std::vector<Eigen::MatrixXf> xx_inter;
std::vector<Eigen::MatrixXf> xx_old;
std::vector<Eigen::MatrixXf> xx_warped;
std::vector<Eigen::MatrixXf> yy;
std::vector<Eigen::MatrixXf> yy_inter;
std::vector<Eigen::MatrixXf> yy_old;
std::vector<Eigen::MatrixXf> yy_warped;
std::vector<Eigen::MatrixXf> transformations;
Eigen::MatrixXf range_wf;
Eigen::MatrixXf dtita;
Eigen::MatrixXf dt;
Eigen::MatrixXf rtita;
Eigen::MatrixXf normx, normy, norm_ang;
Eigen::MatrixXf weights;
Eigen::MatrixXi null;
Eigen::MatrixXf range_wf;
Eigen::MatrixXf dtita;
Eigen::MatrixXf dt;
Eigen::MatrixXf rtita;
Eigen::MatrixXf normx, normy, norm_ang;
Eigen::MatrixXf weights;
Eigen::MatrixXi null;
Eigen::MatrixXf A,Aw;
Eigen::MatrixXf B,Bw;
Eigen::Matrix<float, 3, 1> Var; //3 unknowns: vx, vy, w
Eigen::Matrix<float, 3, 3> cov_odo;
Eigen::MatrixXf A,Aw;
Eigen::MatrixXf B,Bw;
MatrixS31 Var; //3 unknowns: vx, vy, w
IncrementCov cov_odo;
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
float fps; //In Hz
float fovh; //Horizontal FOV
unsigned int cols;
unsigned int cols_i;
unsigned int width;
unsigned int ctf_levels;
unsigned int image_level, level;
unsigned int num_valid_range;
unsigned int iter_irls;
float g_mask[5];
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
float fps; //In Hz
float fovh; //Horizontal FOV
unsigned int cols;
unsigned int cols_i;
unsigned int width;
unsigned int ctf_levels;
unsigned int image_level, level;
unsigned int num_valid_range;
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, current_scan_time;
ros::WallDuration m_runtime;
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;
MatrixS31 kai_abs_;
MatrixS31 kai_loc_;
MatrixS31 kai_loc_old_;
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;
mrpt::poses::CPose3D laser_pose_on_robot_inv;
bool test;
std::vector<double> last_m_lin_speeds;
std::vector<double> last_m_ang_speeds;
mrpt::poses::CPose3D laser_pose;
mrpt::poses::CPose3D laser_oldpose;
mrpt::poses::CPose3D robot_pose;
mrpt::poses::CPose3D robot_oldpose;
bool test;
std::vector<double> last_m_lin_speeds;
std::vector<double> last_m_ang_speeds;
// Methods
void createImagePyramid();
void calculateCoord();
void performWarping();
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);
// Methods
void createImagePyramid();
void calculateCoord();
void performWarping();
void calculaterangeDerivativesSurface();
void computeNormals();
void computeWeights();
void findNullPoints();
void solveSystemOneLevel();
void solveSystemNonLinear();
bool filterLevelSolution();
void PoseUpdate();
void Reset(const Pose3d& 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>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 -->
<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>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>cmake_modules</run_depend> <!-- For aditional dependencies such as Eigen -->
<run_depend>mrpt</run_depend> <!-- Depend on mrpt system pkgs -->
<run_depend>cmake_modules</run_depend> <!-- For aditional dependencies such as Eigen -->
<run_depend>eigen</run_depend>
</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
* MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
@@ -18,12 +20,14 @@
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
namespace rf2o {
class CLaserOdometry2DNode : CLaserOdometry2D
{
public:
CLaserOdometry2DNode();
~CLaserOdometry2DNode();
~CLaserOdometry2DNode() = default;
void process(const ros::TimerEvent &);
void publish();
@@ -32,7 +36,7 @@ public:
public:
bool publish_tf,new_scan_available;
bool publish_tf, new_scan_available;
double freq;
@@ -63,57 +67,52 @@ public:
CLaserOdometry2DNode::CLaserOdometry2DNode() :
CLaserOdometry2D()
{
ROS_INFO("Initializing RF2O node...");
ROS_INFO("Initializing RF2O node...");
//Read Parameters
//----------------
ros::NodeHandle pn("~");
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>("base_frame_id", base_frame_id, "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
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<double>("freq",freq,10.0);
pn.param<bool>("verbose", verbose, true);
//Read Parameters
//----------------
ros::NodeHandle pn("~");
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>("base_frame_id", base_frame_id, "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
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<double>("freq",freq,10.0);
pn.param<bool>("verbose", verbose, true);
//Publishers and Subscribers
//--------------------------
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
//Publishers and Subscribers
//--------------------------
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
//init pose??
if (init_pose_from_topic != "")
{
initPose_sub = n.subscribe<nav_msgs::Odometry>(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;
}
//init pose??
if (init_pose_from_topic != "")
{
initPose_sub = n.subscribe<nav_msgs::Odometry>(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;
}
setLaserPoseFromTf();
setLaserPoseFromTf();
//Init variables
module_initialized = false;
first_laser_scan = true;
//Init variables
module_initialized = false;
first_laser_scan = true;
ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
}
CLaserOdometry2DNode::~CLaserOdometry2DNode()
{
//
}
bool CLaserOdometry2DNode::setLaserPoseFromTf()
{
bool retrieved = false;
@@ -124,28 +123,31 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
transform.setIdentity();
try
{
tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
retrieved = true;
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;
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];
//TF:transform -> Eigen::Isometry3d
const tf::Matrix3x3 &basis = transform.getBasis();
mrpt::math::CMatrixDouble33 R;
Eigen::Matrix3d 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);
for(int c = 0; c < 3; c++)
R(r,c) = basis[r][c];
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);
@@ -154,21 +156,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
bool CLaserOdometry2DNode::scan_available()
{
return new_scan_available;
return new_scan_available;
}
void CLaserOdometry2DNode::process(const ros::TimerEvent&)
{
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
//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....") ;
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)
{
if (GT_pose_initialized)
{
//Keep in memory the last received laser_scan
last_scan = *new_scan;
current_scan_time = last_scan.header.stamp;
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)
{
//copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++)
range_wf(i) = new_scan->ranges[i];
new_scan_available = true;
}
else
{
Init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
//Initialize module on first scan
if (!first_laser_scan)
{
//copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++)
range_wf(i) = new_scan->ranges[i];
new_scan_available = true;
}
else
{
init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
}
}
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;
}
//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()
@@ -216,17 +218,17 @@ void CLaserOdometry2DNode::publish()
//---------------------------------------
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);
//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_.translation()(0);
odom_trans.transform.translation.y = robot_pose_.translation()(1);
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
//send the transform
odom_broadcaster.sendTransform(odom_trans);
}
//next, we'll publish the odometry message over ROS
@@ -236,10 +238,10 @@ void CLaserOdometry2DNode::publish()
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.x = robot_pose_.translation()(0);
odom.pose.pose.position.y = robot_pose_.translation()(1);
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
odom.child_frame_id = base_frame_id;
odom.twist.twist.linear.x = lin_speed; //linear speed
@@ -249,27 +251,29 @@ void CLaserOdometry2DNode::publish()
odom_pub.publish(odom);
}
} /* namespace rf2o */
//-----------------------------------------------------------------------------------
// MAIN
//-----------------------------------------------------------------------------------
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;
timer_opt.oneshot = false;
timer_opt.autostart = true;
timer_opt.callback_queue = ros::getGlobalCallbackQueue();
timer_opt.tracked_object = ros::VoidConstPtr();
ros::TimerOptions timer_opt;
timer_opt.oneshot = false;
timer_opt.autostart = true;
timer_opt.callback_queue = ros::getGlobalCallbackQueue();
timer_opt.tracked_object = ros::VoidConstPtr();
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
timer_opt.callback = boost::bind(&rf2o::CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
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;
}