MAPIRlab · Aug 31, 2017 · Aug 31, 2017 · Aug 31, 2017 · Feb 1, 2018 · Feb 9, 2018
Showing with 64 additions and 33 deletions.

+50 −12 CMakeLists.txt

+9 −16 package.xml

+3 −3 src/CLaserOdometry2D.cpp

+2 −2 src/CLaserOdometry2DNode.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,24 +1,30 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.11 FATAL_ERROR)
 project(rf2o_laser_odometry)
-set (CMAKE_CXX_STANDARD 14)  # Require C++14
+
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+if(COMPILER_SUPPORTS_CXX11)
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+else()
+  message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS  
-  roscpp
-  rospy
+  cmake_modules
   nav_msgs
+  roscpp
   sensor_msgs
   std_msgs
   tf
-  cmake_modules
 )
 
 ## System dependencies are found with CMake's conventions
-find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Boost REQUIRED COMPONENTS system)
 
-find_package(Eigen3 REQUIRED)
+find_package(Eigen REQUIRED)
 
 ###################################
 ## catkin specific configuration ##
@@ -30,10 +36,10 @@ find_package(Eigen3 REQUIRED)
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
- INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
+ INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
- DEPENDS #Eigen3
+ CATKIN_DEPENDS cmake_modules nav_msgs roscpp sensor_msgs std_msgs tf
+ DEPENDS Eigen
 )
 
 ## Specify additional locations of header files
@@ -43,14 +49,46 @@ include_directories(include)
 include_directories(
   SYSTEM
   ${catkin_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
+  #${Boost_INCLUDE_DIRS}
+  ${EIGEN_INCLUDE_DIRS}
 )
 
 ## Declare a cpp library
 add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
+#Require C++11
+if(CMAKE_VERSION VERSION_LESS "3.1")
+  set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c++11")
+else()
+  set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11)
+  set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
+  set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_EXTENSIONS OFF)
+endif()
+
 ## Declare a cpp executable
 add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)
 target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+#Require C++11
+if(CMAKE_VERSION VERSION_LESS "3.1")
+  set_target_properties(rf2o_laser_odometry_node PROPERTIES COMPILE_FLAGS "-std=c++11")
+else()
+  set_property(TARGET rf2o_laser_odometry_node PROPERTY CXX_STANDARD 11)
+  set_property(TARGET rf2o_laser_odometry_node PROPERTY CXX_STANDARD_REQUIRED ON)
+  set_property(TARGET rf2o_laser_odometry_node PROPERTY CXX_EXTENSIONS OFF)
+endif()
+
+################
+##  Install   ##
+################
+
+install(TARGETS ${PROJECT_NAME} rf2o_laser_odometry_node
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
diff --git a/package.xml b/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
 
   <name>rf2o_laser_odometry</name>
   <version>1.0.0</version>
@@ -13,25 +13,18 @@
   <maintainer email="jgmonroy@uma.es">Javier G. Monroy</maintainer>
   <author email="marianojt@uma.es"> Mariano Jaimez</author>
   <author email="jgmonroy@uma.es"> Javier G. Monroy</author>
+
   <license>GPL v3</license> 
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>std_msgs</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. -->
-                                               <!-- https://github.com/ros/cmake_modules/blob/0.3-devel/README.md -->
-  <build_depend>eigen</build_depend>
+  <depend>cmake_modules</depend>
+  <depend>nav_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</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>eigen</run_depend>
+  <depend>eigen</depend>
 
 </package>
diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp
@@ -156,7 +156,7 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
   kai_loc_old_ = MatrixS31::Zero();
 
   module_initialized = true;
-  last_odom_time = ros::Time::now();
+  last_odom_time = scan.header.stamp;
 }
 
 const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getIncrement() const
@@ -292,7 +292,7 @@ void CLaserOdometry2D::createImagePyramid()
         //Inner pixels
         if ((u>1)&&(u<cols_i-2))
         {
-          if (dcenter > 0.f)
+          if (std::isfinite(dcenter) && dcenter > 0.f)
           {
             float sum = 0.f;
             float weight = 0.f;
@@ -316,7 +316,7 @@ void CLaserOdometry2D::createImagePyramid()
         //Boundary
         else
         {
-          if (dcenter > 0.f)
+          if (std::isfinite(dcenter) && dcenter > 0.f)
           {
             float sum = 0.f;
             float weight = 0.f;

diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp
@@ -220,7 +220,7 @@ void CLaserOdometry2DNode::publish()
   {
     //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
     geometry_msgs::TransformStamped odom_trans;
-    odom_trans.header.stamp = ros::Time::now();
+    odom_trans.header.stamp = last_odom_time;
     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);
@@ -235,7 +235,7 @@ void CLaserOdometry2DNode::publish()
   //-------------------------------------------------
   //ROS_INFO("[rf2o] Publishing Odom Topic");
   nav_msgs::Odometry odom;
-  odom.header.stamp = ros::Time::now();
+  odom.header.stamp = last_odom_time;
   odom.header.frame_id = odom_frame_id;
   //set the position
   odom.pose.pose.position.x = robot_pose_.translation()(0);