From f862fbb33abd6fa4f257a94d0551cdd7e2b38277 Mon Sep 17 00:00:00 2001 From: George Brindeiro Date: Thu, 24 Apr 2014 00:34:16 -0300 Subject: [PATCH] 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 | 6 +-- include/state_estimator/state_estimator.h | 37 +++++++++++++++++++ .../state_estimator/state_estimator_node.h | 23 ++++++++++++ .../sensor_processor_node.cpp | 3 +- src/state_estimator/state_estimator.cpp | 16 ++++++++ src/state_estimator/state_estimator_node.cpp | 37 +++++++++++++++++++ 6 files changed, 118 insertions(+), 4 deletions(-) create mode 100644 include/state_estimator/state_estimator.h create mode 100644 include/state_estimator/state_estimator_node.h create mode 100644 src/state_estimator/state_estimator.cpp create mode 100644 src/state_estimator/state_estimator_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a86d8f8..a71defd 100755 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/state_estimator/state_estimator.h b/include/state_estimator/state_estimator.h new file mode 100644 index 0000000..477c112 --- /dev/null +++ b/include/state_estimator/state_estimator.h @@ -0,0 +1,37 @@ +/** + * @file state_estimator.h + * @author George Andrew Brindeiro (georgebrindeiro@lara.unb.br) + * @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_ diff --git a/include/state_estimator/state_estimator_node.h b/include/state_estimator/state_estimator_node.h new file mode 100644 index 0000000..5c9c340 --- /dev/null +++ b/include/state_estimator/state_estimator_node.h @@ -0,0 +1,23 @@ +/** + * @file state_estimator_node.h + * @author George Andrew Brindeiro (georgebrindeiro@lara.unb.br) + * @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 + +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_ diff --git a/src/sensor_processor/sensor_processor_node.cpp b/src/sensor_processor/sensor_processor_node.cpp index c66709b..d2bf3ba 100644 --- a/src/sensor_processor/sensor_processor_node.cpp +++ b/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 diff --git a/src/state_estimator/state_estimator.cpp b/src/state_estimator/state_estimator.cpp new file mode 100644 index 0000000..af87213 --- /dev/null +++ b/src/state_estimator/state_estimator.cpp @@ -0,0 +1,16 @@ +/** + * @file state_estimator.cpp + * @author George Andrew Brindeiro (georgebrindeiro@lara.unb.br) + * @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 + diff --git a/src/state_estimator/state_estimator_node.cpp b/src/state_estimator/state_estimator_node.cpp new file mode 100644 index 0000000..510eddf --- /dev/null +++ b/src/state_estimator/state_estimator_node.cpp @@ -0,0 +1,37 @@ +/** + * @file state_estimator_node.cpp + * @author George Andrew Brindeiro (georgebrindeiro@lara.unb.br) + * @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 + +#include + +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("estimated_pose", 1); + + // Create SensorProcessor + state_estimator = new StateEstimator(); + + // Spin + ros::spin (); +}