Fixing nodelet

Author: Stuart Alldritt

include/azure-kinect-ros-driver/k4a_ros_bridge_nodelet.h

@@ -16,22 +16,21 @@
 //
 #include "azure-kinect-ros-driver/k4a_ros_device.h"
 
-class K4AROSBridgeNodelet : public nodelet::Nodelet
+namespace Azure_Kinect_ROS_Driver
 {
-public:
+    class K4AROSBridgeNodelet : public nodelet::Nodelet
+    {
+    public:
 
-    K4AROSBridgeNodelet();
-    ~K4AROSBridgeNodelet();
+        K4AROSBridgeNodelet();
+        ~K4AROSBridgeNodelet();
 
-    virtual void onInit();
+        virtual void onInit();
 
-private:
+    private:
 
-    std::unique_ptr<K4AROSDevice> k4a_device;
-};
-
-#include <pluginlib/class_list_macros.h>
-
-PLUGINLIB_EXPORT_CLASS(K4AROSBridgeNodelet, nodelet::Nodelet)
+        std::unique_ptr<K4AROSDevice> k4a_device;
+    };
+}
 
 #endif // K4A_ROS_BRIDGE_NODELET_H

nodelet_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libazure-kinect-ros-driver_nodelet">
+  <class name="Azure-Kinect-ROS-Driver/K4AROSBridgeNodelet" type="Azure_Kinect_ROS_Driver::K4AROSBridgeNodelet" base_class_type="nodelet::Nodelet">
+  <description>
+    A nodelet version of the Azure Kinect ROS Driver.
+  </description>
+  </class>
+</library>

package.xml

@@ -22,5 +22,10 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>nodelet</depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
 
 </package>

src/k4a_ros_bridge_nodelet.cpp

@@ -10,26 +10,46 @@
 
 // Library headers
 //
+#include <pluginlib/class_list_macros.h>
 
 // Project headers
 //
 
-K4AROSBridgeNodelet::K4AROSBridgeNodelet() : Nodelet(),
-                                             k4a_device(nullptr)
-{
-}
+PLUGINLIB_EXPORT_CLASS(Azure_Kinect_ROS_Driver::K4AROSBridgeNodelet, nodelet::Nodelet)
 
-K4AROSBridgeNodelet::~K4AROSBridgeNodelet()
+namespace Azure_Kinect_ROS_Driver
 {
-}
+    K4AROSBridgeNodelet::K4AROSBridgeNodelet() : Nodelet(),
+                                                k4a_device(nullptr)
+    {
+    }
 
-void K4AROSBridgeNodelet::onInit()
-{
-    k4a_device = std::unique_ptr<K4AROSDevice>(new K4AROSDevice(getNodeHandle(), getPrivateNodeHandle()));
+    K4AROSBridgeNodelet::~K4AROSBridgeNodelet()
+    {
+        k4a_device->stopCameras();
+        k4a_device->stopImu();
+    }
 
-    if (!k4a_device->startCameras())
+    void K4AROSBridgeNodelet::onInit()
     {
-        k4a_device.reset(nullptr);
-        throw nodelet::Exception("Could not start K4A_ROS_Device!");
+        NODELET_INFO("K4A ROS Nodelet Start");
+
+        /*k4a_device = std::unique_ptr<K4AROSDevice>(new K4AROSDevice(getNodeHandle(), getPrivateNodeHandle()));
+
+        if (k4a_device->startCameras() !=  K4A_RESULT_SUCCEEDED)
+        {
+            k4a_device.reset(nullptr);
+            throw nodelet::Exception("Could not start K4A_ROS_Device!");
+        }
+
+        NODELET_INFO("Cameras started");
+
+        if (k4a_device->startImu() != K4A_RESULT_SUCCEEDED)
+        {
+            k4a_device.reset(nullptr);
+            throw nodelet::Exception("Could not start K4A_ROS_Device!");
+        }
+
+        NODELET_INFO("IMU started");*/
     }
-}
+}