From fb5334d54271259eba40949865ff28c783d94fbb Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Wed, 16 Aug 2017 10:09:20 +0200 Subject: [PATCH] extend rf2o namespace to class --- include/rf2o_laser_odometry/CLaserOdometry2D.h | 4 +++- src/CLaserOdometry2D.cpp | 4 ++++ src/CLaserOdometry2DNode.cpp | 8 ++++++-- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 1176eef..04fb276 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -34,6 +34,7 @@ #include namespace rf2o { + template inline int sign(T x) { return x<0 ? -1:1; } @@ -61,7 +62,6 @@ inline Eigen::Matrix matrixYaw(const T yaw) { return matrixRollPitchYaw(0, 0, yaw); } -} // namespace rf2o class CLaserOdometry2D { @@ -175,4 +175,6 @@ protected: void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/); }; +} /* namespace rf2o */ + #endif // CLaserOdometry2D_H diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 294c3bd..5f9be46 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -17,6 +17,8 @@ #include "rf2o_laser_odometry/CLaserOdometry2D.h" +namespace rf2o { + // -------------------------------------------- // CLaserOdometry2D //--------------------------------------------- @@ -951,3 +953,5 @@ void CLaserOdometry2D::PoseUpdate() ang_speed = sum2 / last_m_ang_speeds.size(); */ } + +} /* namespace rf2o */ diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 4f2d67b..942c000 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -20,6 +20,8 @@ #include #include +namespace rf2o { + class CLaserOdometry2DNode : CLaserOdometry2D { public: @@ -249,6 +251,8 @@ void CLaserOdometry2DNode::publish() odom_pub.publish(odom); } +} /* namespace rf2o */ + //----------------------------------------------------------------------------------- // MAIN //----------------------------------------------------------------------------------- @@ -256,7 +260,7 @@ int main(int argc, char** argv) { ros::init(argc, argv, "RF2O_LaserOdom"); - CLaserOdometry2DNode myLaserOdomNode; + rf2o::CLaserOdometry2DNode myLaserOdomNode; ros::TimerOptions timer_opt; timer_opt.oneshot = false; @@ -264,7 +268,7 @@ int main(int argc, char** argv) timer_opt.callback_queue = ros::getGlobalCallbackQueue(); 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(); ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);