Added skeleton for state_estimator and state_estimator_node

Added state_estimator to CMakeLists.txt
Minor change in doxygen comment in sensor_processor_node

CMakeLists.txt

@@ -90,12 +90,12 @@ include_directories(
 add_executable(sensor_processor src/sensor_processor/sensor_processor_node.cpp src/sensor_processor/sensor_processor.cpp)
 target_link_libraries(sensor_processor ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
 
+add_executable(state_estimator src/state_estimator/state_estimator_node.cpp src/state_estimator/state_estimator.cpp)
+target_link_libraries(state_estimator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
 #add_executable(hs_map_manager src/MapManager/MapManager.cpp)
 #target_link_libraries(hs_map_manager ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
 
-#add_executable(hs_state_estimator src/StateEstimator/StateEstimator.cpp src/Filter/Filter.cpp src/Model/Motion/MotionModel.cpp src/Model/Measurement/MeasurementModel.cpp)
-#target_link_libraries(hs_state_estimator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
-
 #add_executable(hs_cloud_saver src/Utilities/CloudSaver.cpp)
 #target_link_libraries(hs_cloud_saver ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
 

include/state_estimator/state_estimator.h

@@ -0,0 +1,37 @@
+/**
+ * @file   state_estimator.h
+ * @author George Andrew Brindeiro ([email protected])
+ * @date   Apr 24, 2014
+ *
+ * @attention Copyright (C) 2014
+ * @attention Laboratório de Automação e Robótica (LARA)
+ * @attention Universidade de Brasília (UnB)
+ */
+
+#ifndef LARA_RGBD_STATE_ESTIMATOR_H_
+#define LARA_RGBD_STATE_ESTIMATOR_H_
+
+class StateEstimator
+{
+    public:
+        StateEstimator()
+        {
+
+        }
+
+        ~SensorProcessor() {}
+
+        /*!
+        * @brief
+        *
+        * This does
+        *
+        * @param bar                    ef
+        */
+        void foo(int bar);
+
+    private:
+        int var_;      /**< Indicates which feature detector we are using to process point clouds  */
+};
+
+#endif  // LARA_RGBD_STATE_ESTIMATOR_H_

include/state_estimator/state_estimator_node.h

@@ -0,0 +1,23 @@
+/**
+ * @file   state_estimator_node.h
+ * @author George Andrew Brindeiro ([email protected])
+ * @date   Apr 24, 2014
+ *
+ * @attention Copyright (C) 2014
+ * @attention Laboratório de Automação e Robótica (LARA)
+ * @attention Universidade de Brasília (UnB)
+ */
+
+#ifndef LARA_RGBD_STATE_ESTIMATOR_NODE_H_
+#define LARA_RGBD_STATE_ESTIMATOR_NODE_H_
+
+#include <state_estimator/state_estimator.h>
+
+void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
+void odom_cb(const nav_msgs::Odometry::ConstPtr& odom_msg);
+
+ros::Publisher pub_pose;
+
+StateEstimator* state_estimator;
+
+#endif  // LARA_RGBD_STATE_ESTIMATOR_NODE_H_

src/sensor_processor/sensor_processor_node.cpp

@@ -9,7 +9,8 @@
  *
  * @brief  ROS node used to process raw sensor data
  *
- * This ROS node receives both the RGB-D point cloud from the Kinect and the odometry info from the Pioneer
+ * This ROS node receives both the RGB-D point cloud from the Kinect and the odometry info from the Pioneer and does
+ * some processing using the SensorProcessor class to transform it to the format expected by LARA RGB-D SLAM.
  */
 
 #include <ros/ros.h>

src/state_estimator/state_estimator.cpp

@@ -0,0 +1,16 @@
+/**
+ * @file   state_estimator.cpp
+ * @author George Andrew Brindeiro ([email protected])
+ * @date   Apr 24, 2014
+ *
+ * @attention Copyright (C) 2014
+ * @attention Laboratório de Automação e Robótica (LARA)
+ * @attention Universidade de Brasília (UnB)
+ *
+ * @brief
+ *
+ * This class is responsible for
+ */
+
+#include <state_estimator/state_estimator.h>
+

src/state_estimator/state_estimator_node.cpp

@@ -0,0 +1,37 @@
+/**
+ * @file   state_estimator_node.cpp
+ * @author George Andrew Brindeiro ([email protected])
+ * @date   Apr 24, 2014
+ *
+ * @attention Copyright (C) 2014
+ * @attention Laboratório de Automação e Robótica (LARA)
+ * @attention Universidade de Brasília (UnB)
+ *
+ * @brief  ROS node used to p
+ *
+ * This ROS node receives
+ */
+
+#include <ros/ros.h>
+
+#include <state_estimator/state_estimator_node.h>
+
+int main (int argc, char** argv)
+{
+    // Initialize ROS
+    ros::init(argc, argv, "lara_rgbd_state_estimator");
+    ros::NodeHandle nh;
+
+    // Create a ROS subscriber for sensor data
+    ros::Subscriber sub_cloud = nh.subscribe("feature_cloud", 1, cloud_cb);
+    ros::Subscriber sub_odom  = nh.subscribe("odometry", 1, odom_cb);
+
+    // Create a ROS publisher for estimated pose
+    pub_pose = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("estimated_pose", 1);
+
+    // Create SensorProcessor
+    state_estimator = new StateEstimator();
+
+    // Spin
+    ros::spin ();
+}