Removed scan_rectifier in favor of using laser_geometry in scans_merger

tysik · Jul 19, 2017 · b20e6b2 · b20e6b2
1 parent df8913a
commit b20e6b2
Show file tree

Hide file tree

Showing 11 changed files with 85 additions and 587 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -3,7 +3,7 @@ project(obstacle_detector)
 
 set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ")
 
-find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_generation)
+find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_generation)
 find_package(Armadillo REQUIRED)
 find_package(Boost 1.54.0 REQUIRED system)
 
@@ -12,19 +12,15 @@ generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui 
-  CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_runtime
+  LIBRARIES scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui
+  CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_runtime
 )
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
 #
 # Build libs
 #
-add_library(scan_rectifier src/scan_rectifier.cpp)
-target_link_libraries(scan_rectifier ${catkin_LIBRARIES})
-add_dependencies(scan_rectifier ${catkin_EXPORTED_TARGETS})
-
 add_library(scans_merger src/scans_merger.cpp)
 target_link_libraries(scans_merger ${catkin_LIBRARIES})
 add_dependencies(scans_merger ${catkin_EXPORTED_TARGETS})
@@ -44,9 +40,6 @@ add_dependencies(obstacle_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}
 #
 # Build nodes
 #
-add_executable(scan_rectifier_node src/nodes/scan_rectifier_node.cpp)
-target_link_libraries(scan_rectifier_node scan_rectifier)
-
 add_executable(scans_merger_node src/nodes/scans_merger_node.cpp)
 target_link_libraries(scans_merger_node scans_merger)
 
@@ -63,12 +56,11 @@ target_link_libraries(obstacle_publisher_node obstacle_publisher)
 # Build nodelets
 #
 add_library(${PROJECT_NAME}_nodelets
-  src/nodelets/scan_rectifier_nodelet.cpp
   src/nodelets/scans_merger_nodelet.cpp
   src/nodelets/obstacle_extractor_nodelet.cpp
   src/nodelets/obstacle_tracker_nodelet.cpp
   src/nodelets/obstacle_publisher_nodelet.cpp)
-target_link_libraries(${PROJECT_NAME}_nodelets scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher)
+target_link_libraries(${PROJECT_NAME}_nodelets scans_merger obstacle_extractor obstacle_tracker obstacle_publisher)
 
 #
 # Build rviz plugins
@@ -102,15 +94,15 @@ add_dependencies(${PROJECT_NAME}_gui ${catkin_EXPORTED_TARGETS})
 #
 # Install libraries
 #
-install(TARGETS scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui
+install(TARGETS scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
 
 #
 # Install nodes
 #
-install(TARGETS scan_rectifier_node scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node
+install(TARGETS scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 #

diff --git a/include/obstacle_detector/scan_rectifier.h b/include/obstacle_detector/scan_rectifier.h
diff --git a/include/obstacle_detector/scans_merger.h b/include/obstacle_detector/scans_merger.h
@@ -41,6 +41,7 @@
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud.h>
 #include <tf/transform_listener.h>
+#include <laser_geometry/laser_geometry.h>
 
 namespace obstacle_detector
 {
@@ -57,9 +58,7 @@ class ScansMerger
 
   void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); }
 
-  void publishScan();
-  void publishPCL();
-  void publishAll();
+  void publishMessages();
 
   ros::NodeHandle nh_;
   ros::NodeHandle nh_local_;
@@ -71,18 +70,16 @@ class ScansMerger
   ros::Publisher scan_pub_;
   ros::Publisher pcl_pub_;
 
+  tf::TransformListener tf_ls_;
+  laser_geometry::LaserProjection projector_;
+
   bool front_scan_received_;
   bool rear_scan_received_;
   bool front_scan_error_;
   bool rear_scan_error_;
 
-  ros::Time stamp_;
-
-  geometry_msgs::Pose2D front_tf_;
-  geometry_msgs::Pose2D rear_tf_;
-
-  std::vector<float> ranges_;
-  std::vector<geometry_msgs::Point32> points_;
+  sensor_msgs::PointCloud front_pcl_;
+  sensor_msgs::PointCloud rear_pcl_;
 
   // Parameters
   bool p_active_;
@@ -98,6 +95,7 @@ class ScansMerger
   double p_max_y_range_;
   double p_min_y_range_;
 
+  std::string p_fixed_frame_id_;
   std::string p_target_frame_id_;
   std::string p_front_scan_frame_id_;
   std::string p_rear_scan_frame_id_;

diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
@@ -1,13 +1,5 @@
 <library path="lib/libobstacle_detector_nodelets">
 
-  <class name="obstacle_detector/ScanRectifier"
-         type="obstacle_detector::ScanRectifierNodelet"
-         base_class_type="nodelet::Nodelet">
-    <description>
-      Scan Rectifier Nodelet
-    </description>
-  </class>
-
   <class name="obstacle_detector/ScansMerger" 
          type="obstacle_detector::ScansMergerNodelet" 
          base_class_type="nodelet::Nodelet">

diff --git a/package.xml b/package.xml
@@ -20,6 +20,7 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>laser_geometry</depend>
   <depend>std_srvs</depend>
 
   <export>

diff --git a/src/nodelets/scan_rectifier_nodelet.cpp b/src/nodelets/scan_rectifier_nodelet.cpp
diff --git a/src/nodelets/scans_merger_nodelet.cpp b/src/nodelets/scans_merger_nodelet.cpp
@@ -52,7 +52,7 @@ class ScansMergerNodelet : public nodelet::Nodelet
     try {
       scans_merger_ = std::shared_ptr<ScansMerger>(new ScansMerger(nh, nh_local));
     }
-    catch (std::string s) {
+    catch (const char* s) {
       NODELET_FATAL_STREAM(s);
     }
   }

diff --git a/src/nodes/scan_rectifier_node.cpp b/src/nodes/scan_rectifier_node.cpp
diff --git a/src/nodes/scans_merger_node.cpp b/src/nodes/scans_merger_node.cpp
@@ -47,7 +47,7 @@ int main(int argc, char** argv) {
     ScansMerger sm(nh, nh_local);
     ros::spin();
   }
-  catch (std::string s) {
+  catch (const char* s) {
     ROS_FATAL_STREAM(s);
   }