add complementary_filter_nodelet (#181)

CCNYRoboticsLab · Nov 14, 2023 · f55d47e · f55d47e
1 parent 472666b
commit f55d47e
Show file tree

Hide file tree

Showing 5 changed files with 60 additions and 1 deletion.
diff --git a/imu_complementary_filter/CMakeLists.txt b/imu_complementary_filter/CMakeLists.txt
@@ -7,6 +7,8 @@ find_package(catkin REQUIRED COMPONENTS
   cmake_modules
   geometry_msgs
   message_filters
+  nodelet
+  pluginlib
   roscpp
   sensor_msgs
   std_msgs
@@ -19,6 +21,8 @@ catkin_package(
   CATKIN_DEPENDS
     geometry_msgs
     message_filters
+    nodelet
+    pluginlib
     roscpp
     sensor_msgs
     std_msgs
@@ -40,13 +44,16 @@ add_library(complementary_filter
 )
 target_link_libraries(complementary_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
+# create complementary_filter_nodelet library
+add_library (complementary_filter_nodelet src/complementary_filter_nodelet.cpp)
+target_link_libraries(complementary_filter_nodelet complementary_filter ${catkin_LIBRARIES})
 
 # create complementary_filter_node executable
 add_executable(complementary_filter_node
   src/complementary_filter_node.cpp)
 target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES})
 
-install(TARGETS complementary_filter
+install(TARGETS complementary_filter complementary_filter_nodelet
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
@@ -64,3 +71,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
+
+install(FILES complementary_filter_nodelet.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/imu_complementary_filter/complementary_filter_nodelet.xml b/imu_complementary_filter/complementary_filter_nodelet.xml
@@ -0,0 +1,9 @@
+<!-- Complementary Filter nodelet publisher -->
+<library path="lib/libcomplementary_filter_nodelet">
+  <class name="imu_complementary_filter/ComplementaryFilterNodelet" type="ComplementaryFilterNodelet"
+    base_class_type="nodelet::Nodelet">
+    <description>
+      Complementary Filter nodelet publisher.
+    </description>
+  </class>
+</library>
diff --git a/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_nodelet.h b/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_nodelet.h
@@ -0,0 +1,17 @@
+#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_NODELET_H
+#define IMU_TOOLS_COMPLEMENTARY_FILTER_NODELET_H
+
+#include <nodelet/nodelet.h>
+
+#include "imu_complementary_filter/complementary_filter_ros.h"
+
+class ComplementaryFilterNodelet : public nodelet::Nodelet
+{
+  public:
+    virtual void onInit();
+
+  private:
+    boost::shared_ptr<imu_tools::ComplementaryFilterROS> filter_;
+};
+
+#endif  // IMU_TOOLS_COMPLEMENTARY_FILTER_NODELET_H
diff --git a/imu_complementary_filter/package.xml b/imu_complementary_filter/package.xml
@@ -14,15 +14,23 @@
   <build_depend>cmake_modules</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>message_filters</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pluginlib</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>tf</build_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>message_filters</run_depend>
   <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>tf</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/complementary_filter_nodelet.xml" />
+  </export>
+
 </package>
diff --git a/imu_complementary_filter/src/complementary_filter_nodelet.cpp b/imu_complementary_filter/src/complementary_filter_nodelet.cpp
@@ -0,0 +1,14 @@
+#include "imu_complementary_filter/complementary_filter_nodelet.h"
+#include <pluginlib/class_list_macros.h>
+
+void ComplementaryFilterNodelet::onInit()
+{
+    NODELET_INFO("Initializing Complementary Filter Nodelet");
+
+    ros::NodeHandle nh = getMTNodeHandle();
+    ros::NodeHandle nh_private = getMTPrivateNodeHandle();
+
+    filter_.reset(new imu_tools::ComplementaryFilterROS(nh, nh_private));
+}
+
+PLUGINLIB_EXPORT_CLASS(ComplementaryFilterNodelet, nodelet::Nodelet)