├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── kinect1.launch ├── kinect2.launch └── kinect2_bridge.launch ├── msg └── Acquisition.msg ├── package.xml └── src └── sensor_data_collection └── data_collection_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sensor_data_collection) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | image_geometry 12 | cv_bridge 13 | image_transport 14 | pcl_conversions 15 | message_generation 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | find_package(Boost REQUIRED COMPONENTS filesystem) 20 | find_package(PCL 1.7 REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | 23 | #find_package(yaml-cpp REQUIRED) 24 | set(yaml-cpp_INCLUDE_DIRS "/usr/include/yaml-cpp/") 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 42 | ## pulled in transitively but can be declared for certainty nonetheless: 43 | ## * add a build_depend tag for "message_generation" 44 | ## * add a run_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | add_message_files( 57 | FILES 58 | Acquisition.msg 59 | ) 60 | 61 | ## Generate services in the 'srv' folder 62 | # add_service_files( 63 | # FILES 64 | # Service1.srv 65 | # Service2.srv 66 | # ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | generate_messages( 77 | DEPENDENCIES 78 | std_msgs # Or other packages containing msgs 79 | ) 80 | 81 | ################################### 82 | ## catkin specific configuration ## 83 | ################################### 84 | ## The catkin_package macro generates cmake config files for your package 85 | ## Declare things to be passed to dependent projects 86 | ## INCLUDE_DIRS: uncomment this if you package contains header files 87 | ## LIBRARIES: libraries you create in this project that dependent projects also need 88 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 89 | ## DEPENDS: system dependencies of this project that dependent projects also need 90 | catkin_package( 91 | # INCLUDE_DIRS include 92 | # LIBRARIES sensor_data_collection 93 | CATKIN_DEPENDS 94 | image_geometry 95 | cv_bridge 96 | image_transport 97 | pcl_conversions 98 | message_runtime 99 | # DEPENDS system_lib 100 | ) 101 | 102 | ########### 103 | ## Build ## 104 | ########### 105 | 106 | ## Specify additional locations of header files 107 | ## Your package locations should be listed before other locations 108 | # include_directories(include) 109 | include_directories( 110 | ${catkin_INCLUDE_DIRS} 111 | ${Boost_INCLUDE_DIRS} 112 | ${PCL_INCLUDE_DIRS} 113 | ${OpenCV_INCLUDE_DIRS} 114 | ${yaml-cpp_INCLUDE_DIRS} 115 | ) 116 | 117 | ## Declare a cpp library 118 | # add_library(sensor_data_collection 119 | # src/${PROJECT_NAME}/sensor_data_collection.cpp 120 | # ) 121 | 122 | ## Declare a cpp executable 123 | add_executable(data_collection_node 124 | src/sensor_data_collection/data_collection_node.cpp 125 | ) 126 | 127 | ## Add cmake target dependencies of the executable/library 128 | ## as an example, message headers may need to be generated before nodes 129 | add_dependencies(data_collection_node 130 | sensor_data_collection_generate_messages_cpp 131 | ) 132 | 133 | ## Specify libraries to link a library or executable target against 134 | target_link_libraries(data_collection_node 135 | ${catkin_LIBRARIES} 136 | ${PCL_LIBRARIES} 137 | ${OpenCV_LIBRARIES} 138 | yaml-cpp 139 | ) 140 | 141 | ############# 142 | ## Install ## 143 | ############# 144 | 145 | # all install targets should use catkin DESTINATION variables 146 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 147 | 148 | ## Mark executable scripts (Python etc.) for installation 149 | ## in contrast to setup.py, you can choose the destination 150 | # install(PROGRAMS 151 | # scripts/my_python_script 152 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark executables and/or libraries for installation 156 | # install(TARGETS sensor_data_collection sensor_data_collection_node 157 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark cpp header files for installation 163 | # install(DIRECTORY include/${PROJECT_NAME}/ 164 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # ) 168 | 169 | ## Mark other files for installation (e.g. launch and bag files, etc.) 170 | # install(FILES 171 | # # myfile1 172 | # # myfile2 173 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 174 | # ) 175 | 176 | ############# 177 | ## Testing ## 178 | ############# 179 | 180 | ## Add gtest based cpp test target and link libraries 181 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sensor_data_collection.cpp) 182 | # if(TARGET ${PROJECT_NAME}-test) 183 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 184 | # endif() 185 | 186 | ## Add folders to be run by python nosetests 187 | # catkin_add_nosetests(test) 188 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Filippo Basso 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of sensor_data_collection nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sensor Data Collection 2 | The package provides a ROS node for collecting data from one or more cameras/depth sensors. 3 | 4 | ## ROS Node 5 | 6 | ### Parameters 7 | 8 | * `device_name` [**type** `string`, **default** `"camera"`]: the name of the device from which the data is acquired. It is the name of the directory where data are saved. 9 | * `save_directory` [**type** `string`, **mandatory**, **no default**]: the root directory where all the data will be saved. Actually data are saved in `save_directory/device_name/`. 10 | * `continue` [**type** `bool`, **default** `false`]: flag that set whether to add data to a previous acquisition. 11 | * `continue_from` [**type** `int`, **default** `0`]: if `continue` is `true`, `continue_from` is the first index that data will be saved with. As a special case, if `continue_from` is set to `0`, the index is auto-computed. 12 | * `save_image` [**type** `bool`, **default** `false`]: flag that set whether to save the RGB image provided by the sensor or not. 13 | * `save_image_camera_info`, [**type** `bool`, **default** `false`]: flag that set whether to save the camera info data (relative to the RGB image) provided by the sensor or not. 14 | * `save_ir` [**type** `bool`, **default** `false`]: flag that set whether to save the IR image provided by the sensor or not. 15 | * `save_ir_camera_info` [**type** `bool`, **default** `false`]: flag that set whether to save the camera info data (relative to the IR image) provided by the sensor or not. 16 | * `save_depth_image` [**type** `bool`, **default** `false`]: flag that set whether to save the depth image provided by the sensor or not. 17 | * `save_depth_camera_info` [**type** `bool`, **default** `false`]: flag that set whether to save the camera info data (relative to the depth image) provided by the sensor or not. 18 | * `save_point_cloud` [**type** `bool`, **default** `false`]: flag that set whether to save the point cloud provided by the sensor or not. 19 | * `depth_type` [**type** `string`, **default** `"uint16"`]: the encoding format of the depth data. Two formats are supported: 20 | * `"uint16"`: depth format of data provided by a Kinect. 21 | * `"float32"`: depth format of data provided by a SwissRanger. 22 | 23 | ### Subscribed Topics 24 | 25 | * `~action` [**type**: `sensor_data_collection/Acquisition`]: used by the node to trigger the acquisition. I.e. data are acquired as soon as a new message is published on this topic. 26 | * `~image` [**type**: `sensor_msgs/Image`]: topic where the RGB images are published. 27 | * `~image_camera_info` [**type**: `sensor_msgs/CameraInfo`]: topic where the camera info data (relative to the RGB images) are published. 28 | * `~ir` [**type**: `sensor_msgs/Image`]: topic where the IR images are published. 29 | * `~image_camera_info` [**type**: `sensor_msgs/CameraInfo`]: topic where the camera info data (relative to the IR images) are published. 30 | * `~depth` [**type**: `sensor_msgs/Image`]: topic where the depth images are published. 31 | * `~image_camera_info` [**type**: `sensor_msgs/CameraInfo`]: topic where the camera info data (relative to the depth images) are published. 32 | * `~point_cloud` [**type**: `sensor_msgs/PointCloud2`]: topic where the point clouds are published. 33 | 34 | ## How-to 35 | 36 | ### Create a Launch File 37 | 38 | Create a file named `my_device_data_collection_node.launch` in `sensor_data_collection/launch`. 39 | Open it an fill it as following. 40 | Initiate the launcher. 41 | ``` 42 | 43 | 44 | ``` 45 | Ask the user to provide the directory each time the node is launched. 46 | ``` 47 | 48 | ``` 49 | Let the user the opportunity to continue a previously interrupted acquisition. 50 | ``` 51 | 52 | 53 | ``` 54 | Define the topics to save. 55 | ``` 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | ``` 64 | Define the name of the device. 65 | ``` 66 | 67 | ``` 68 | 69 | Create the node. Use the `device_name` argument to let multiple instances of the acquisition node to run at the same time. 70 | ``` 71 | 73 | ``` 74 | Use the previously defined arguments as parameters. 75 | ``` 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | ``` 91 | Set the depth type. 92 | ``` 93 | 94 | ``` 95 | First remap the `~action` topic to a global `/action` one. 96 | ``` 97 | 98 | 99 | ``` 100 | Then remap all the other topics to the correct ones. We use the `if="$(arg save_*)"` to avoid subscription to unused topics. 101 | ``` 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | ``` 110 | Close the node and the launcher. 111 | ``` 112 | 113 | 114 | ``` 115 | 116 | ### Run the Node 117 | 118 | You can start the acuisition by running: 119 | ``` 120 | roslaunch sensor_data_collection my_device_data_collection_node.launch save_directory:=$HOME/.ros/sensor_data_collection/red_ball 121 | ``` 122 | All the directories are automatically created if not exisiting, otherwise they are checked if they are not empty. 123 | 124 | You can continue a previously interrupted acquisition by running the same launcher: 125 | ``` 126 | roslaunch sensor_data_collection my_device_data_collection_node.launch save_directory:=$HOME/.ros/sensor_data_collection/red_ball continue:=true 127 | ``` 128 | The program will tell you the first available index: `[my_device] First index is 3.` 129 | 130 | ### Trigger Acquisition 131 | 132 | The acquisition is triggered by publishing an `sensor_data_collection/Acquisition` message on the defined topic: 133 | ``` 134 | rostopic pub /action sensor_data_collection/Acquisition "{}" -1 135 | ``` 136 | The `Acquisition` message has two fields: 137 | * `distance`: distance in meters of the acquired object. *[type: `float`]* 138 | * `info`: additional infos. *[type: `string`]** 139 | 140 | So, data can be acquired as following: 141 | ``` 142 | rostopic pub /action sensor_data_collection/Acquisition "{distance: 1.5, info: 'Red ball'}" -1 143 | ``` 144 | 145 | ## Get a Look at the Created Files 146 | 147 | The node creates the following files: 148 | * `info.yaml`: contains information about all the acquisitions performed. 149 | * `[image|ir|depth]_camera_info.yaml`: contains the calibration parameters of the respective sensors. 150 | * `image_.png`: an image for each acquisition with padding zeros in the filename (e.g. `n=5` -> `image_0005.png`). The image is compressed with a lossless encoding (png). 151 | * `ir_.png`: an image for each acquisition. The image is compressed with a lossless encoding (png). 152 | * `depth_.png`: an image for each acquisition. The image is compressed with a **16bit** lossless encoding (png) . 153 | * `point_cloud_.pcd`: an point cloud for each acquisition. 154 | -------------------------------------------------------------------------------- /launch/kinect1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/kinect2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/kinect2_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/Acquisition.msg: -------------------------------------------------------------------------------- 1 | float32 distance 2 | string info 3 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sensor_data_collection 4 | 2.0.0 5 | The sensor_data_collection package 6 | 7 | 8 | 9 | 10 | Filippo Basso 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | message_generation 45 | image_geometry 46 | cv_bridge 47 | image_transport 48 | pcl_conversions 49 | 50 | message_runtime 51 | image_geometry 52 | cv_bridge 53 | image_transport 54 | pcl_conversions 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/sensor_data_collection/data_collection_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | using sensor_data_collection::Acquisition; 22 | 23 | namespace unipd 24 | { 25 | 26 | namespace fs = boost::filesystem; 27 | 28 | fs::path computeFilePath(const fs::path & original, int n) 29 | { 30 | std::string new_file_path = original.string(); 31 | auto n_pos = new_file_path.rfind(""); 32 | if (n_pos != std::string::npos) 33 | { 34 | std::stringstream ss; 35 | ss << std::setw(4) << std::setfill('0') << n; 36 | new_file_path.replace(n_pos, 3, ss.str()); 37 | } 38 | 39 | return fs::path(new_file_path); 40 | } 41 | 42 | class BaseData 43 | { 44 | public: 45 | 46 | typedef boost::shared_ptr Ptr; 47 | 48 | BaseData(ros::NodeHandle & node_handle) 49 | { 50 | this->node_handle = node_handle; 51 | } 52 | 53 | virtual bool init() = 0; 54 | virtual void save(int n) = 0; 55 | virtual bool waitForMessage() = 0; 56 | virtual ros::Time timestamp() = 0; 57 | 58 | int save_flag = 0; 59 | std::string topic = ""; 60 | fs::path file_path; 61 | 62 | friend std::ostream & operator << (std::ostream & stream, const BaseData & data); 63 | 64 | protected: 65 | 66 | ros::NodeHandle node_handle; 67 | 68 | }; 69 | 70 | std::ostream & operator << (std::ostream & stream, const BaseData & data) 71 | { 72 | return stream << "{save_flag: " << data.save_flag << ", topic: " << data.topic << ", file_path: " << data.file_path << "}"; 73 | } 74 | 75 | template 76 | class BaseData_ : public BaseData 77 | { 78 | public: 79 | 80 | using BaseData::BaseData; 81 | 82 | virtual bool waitForMessage() override 83 | { 84 | return static_cast(ros::topic::waitForMessage(topic, node_handle)); 85 | } 86 | 87 | virtual ros::Time timestamp() override 88 | { 89 | return msg->header.stamp; 90 | } 91 | 92 | protected: 93 | 94 | boost::shared_ptr msg; 95 | 96 | }; 97 | 98 | class ImageData : public BaseData_ 99 | { 100 | public: 101 | 102 | typedef boost::shared_ptr Ptr; 103 | 104 | ImageData(ros::NodeHandle & node_handle) : BaseData_(node_handle), image_transport(node_handle) {} 105 | 106 | virtual bool init() override 107 | { 108 | sub = image_transport.subscribe(topic, 1, &ImageData::callback, this); 109 | } 110 | 111 | virtual void save(int n) override 112 | { 113 | std::string new_file_path = computeFilePath(file_path, n).string(); 114 | cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(msg); 115 | cv::imwrite(new_file_path, image_ptr->image); 116 | } 117 | 118 | private: 119 | 120 | image_transport::ImageTransport image_transport; 121 | image_transport::Subscriber sub; 122 | 123 | void callback(const sensor_msgs::Image::ConstPtr & msg) 124 | { 125 | this->msg = msg; 126 | } 127 | 128 | }; 129 | 130 | class DepthFloat32Data : public ImageData 131 | { 132 | public: 133 | 134 | typedef boost::shared_ptr Ptr; 135 | 136 | using ImageData::ImageData; 137 | 138 | virtual void save(int n) override 139 | { 140 | std::string new_file_path = computeFilePath(file_path, n).string(); 141 | cv_bridge::CvImage::Ptr depth_image_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); 142 | cv::Mat depth_image_16; 143 | depth_image_ptr->image.convertTo(depth_image_16, CV_16UC1, 1000); 144 | cv::imwrite(new_file_path, depth_image_16); 145 | } 146 | 147 | }; 148 | 149 | class DepthUint16Data : public ImageData 150 | { 151 | public: 152 | 153 | typedef boost::shared_ptr Ptr; 154 | 155 | using ImageData::ImageData; 156 | 157 | virtual void save(int n) override 158 | { 159 | std::string new_file_path = computeFilePath(file_path, n).string(); 160 | cv_bridge::CvImage::Ptr depth_image_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1); 161 | cv::imwrite(new_file_path, depth_image_ptr->image); 162 | } 163 | 164 | }; 165 | 166 | class PointCloudData : public BaseData 167 | { 168 | public: 169 | 170 | typedef boost::shared_ptr Ptr; 171 | typedef BaseData Base; 172 | 173 | using BaseData::BaseData; 174 | 175 | virtual bool init() override 176 | { 177 | sub = node_handle.subscribe(topic, 1, &PointCloudData::callback, this); 178 | } 179 | 180 | virtual bool waitForMessage() override 181 | { 182 | return static_cast(ros::topic::waitForMessage(topic, node_handle)); 183 | } 184 | 185 | virtual void save(int n) override 186 | { 187 | std::string new_file_path = computeFilePath(file_path, n).string(); 188 | pcl::PCDWriter pcd_writer; 189 | pcd_writer.writeBinary(new_file_path, *msg); 190 | } 191 | 192 | virtual ros::Time timestamp() override 193 | { 194 | return pcl_conversions::fromPCL(msg->header).stamp; 195 | } 196 | 197 | private: 198 | 199 | ros::Subscriber sub; 200 | pcl::PCLPointCloud2::Ptr msg; 201 | 202 | void callback(const sensor_msgs::PointCloud2::ConstPtr & msg) 203 | { 204 | this->msg = boost::make_shared(); 205 | pcl_conversions::toPCL(*msg, *this->msg); 206 | } 207 | 208 | }; 209 | 210 | class CameraInfoData : public BaseData_ 211 | { 212 | public: 213 | 214 | typedef boost::shared_ptr Ptr; 215 | 216 | using BaseData_::BaseData_; 217 | 218 | virtual bool init() override 219 | { 220 | sub = node_handle.subscribe(topic, 1, &CameraInfoData::callback, this); 221 | } 222 | 223 | virtual void save(int n) override 224 | { 225 | std::string new_file_path = computeFilePath(file_path, n).string(); 226 | image_geometry::PinholeCameraModel model; 227 | model.fromCameraInfo(msg); 228 | 229 | std::ofstream file; 230 | file.open(new_file_path.c_str()); 231 | 232 | file << "frame_id: " << msg->header.frame_id << std::endl; 233 | file << "height: " << msg->height << std::endl; 234 | file << "width: " << msg->width << std::endl; 235 | file << "distortion_model: " << msg->distortion_model << std::endl; 236 | file << "D: " << model.distortionCoeffs() << std::endl; 237 | file << "K: " << model.intrinsicMatrix().reshape<1, 9>() << std::endl; 238 | file << "R: " << model.rotationMatrix().reshape<1, 9>() << std::endl; 239 | file << "P: " << model.projectionMatrix().reshape<1, 12>() << std::endl; 240 | file << "binning_x: " << msg->binning_x << std::endl; 241 | file << "binning_y: " << msg->binning_y << std::endl; 242 | file << "roi:" << std::endl; 243 | file << " x_offset: " << msg->roi.x_offset << std::endl; 244 | file << " y_offset: " << msg->roi.y_offset << std::endl; 245 | file << " height: " << msg->roi.height << std::endl; 246 | file << " width: " << msg->roi.width << std::endl; 247 | file << " do_rectify: " << (msg->roi.do_rectify ? "True" : "False") << std::endl; 248 | 249 | file.close(); 250 | } 251 | 252 | private: 253 | 254 | ros::Subscriber sub; 255 | 256 | void callback(const sensor_msgs::CameraInfo::ConstPtr & msg) 257 | { 258 | this->msg = msg; 259 | } 260 | 261 | }; 262 | 263 | class DataCollectionNode 264 | { 265 | public: 266 | 267 | 268 | enum : int 269 | { 270 | IMAGE = 0, 271 | IR, 272 | DEPTH, 273 | POINT_CLOUD, 274 | 275 | IMAGE_CAMERA_INFO, 276 | IR_CAMERA_INFO, 277 | DEPTH_CAMERA_INFO, 278 | 279 | N, 280 | N_DATA = 4, 281 | N_CAMERA_INFO = 3, 282 | START_DATA = 0, 283 | START_CAMERA_INFO = 4 284 | 285 | }; 286 | 287 | const std::vector TOPIC{"image", "ir", "depth", "point_cloud", "image_camera_info", "ir_camera_info", "depth_camera_info"}; 288 | const std::vector EXTENSION{"png", "png", "png", "pcd", "yaml", "yaml", "yaml"}; 289 | 290 | DataCollectionNode(ros::NodeHandle & node_handle); 291 | 292 | bool initialize(); 293 | 294 | private: 295 | 296 | void actionCallback(const Acquisition::ConstPtr & msg); 297 | 298 | ros::NodeHandle node_handle_; 299 | std::vector data_; 300 | 301 | int save_flags_; 302 | 303 | ros::Subscriber action_sub_; 304 | 305 | bool continue_; 306 | int continue_from_; 307 | int file_index_; 308 | 309 | std::string device_name_; 310 | fs::path save_directory_; 311 | 312 | }; 313 | 314 | DataCollectionNode::DataCollectionNode(ros::NodeHandle & node_handle) : node_handle_(node_handle) 315 | { 316 | action_sub_ = node_handle.subscribe("action", 1, &DataCollectionNode::actionCallback, this); 317 | 318 | node_handle_.param("device_name", device_name_, std::string("camera")); 319 | 320 | std::string save_directory; 321 | if (not node_handle_.getParam("save_directory", save_directory)) 322 | ROS_FATAL_STREAM("[" << device_name_ << "] Missing \"save_directory\" parameter!!"); 323 | 324 | auto now_pos = save_directory.rfind(""); 325 | if (now_pos != std::string::npos) 326 | { 327 | time_t now; 328 | std::time(&now); 329 | tm * timeinfo = std::localtime(&now); 330 | char buffer[20]; 331 | std::strftime(buffer, 20, "%F_%H-%M-%S", timeinfo); 332 | save_directory.replace(now_pos, 5, buffer); 333 | } 334 | 335 | save_directory_ = fs::path(save_directory) / fs::path(device_name_); 336 | 337 | node_handle_.param("continue", continue_, false); 338 | if (continue_) 339 | { 340 | if (not node_handle_.getParam("continue_from", continue_from_)) 341 | ROS_FATAL_STREAM("[" << device_name_ << "] Missing \"continue_from\" parameter!!"); 342 | } 343 | 344 | data_.resize(N); 345 | 346 | data_[IMAGE] = boost::make_shared(node_handle_); 347 | data_[IMAGE_CAMERA_INFO] = boost::make_shared(node_handle_); 348 | data_[IR] = boost::make_shared(node_handle_); 349 | data_[IR_CAMERA_INFO] = boost::make_shared(node_handle_); 350 | 351 | std::string depth_type_s; 352 | node_handle_.param("depth_type", depth_type_s, std::string("uint16")); 353 | if (depth_type_s == std::string("float32")) 354 | data_[DEPTH] = boost::make_shared(node_handle_); 355 | else if (depth_type_s == std::string("uint16")) 356 | data_[DEPTH] = boost::make_shared(node_handle_); 357 | else 358 | ROS_FATAL_STREAM("[" << device_name_ << "] Wrong \"depth_type\" parameter. Use \"float32\" or \"uint16\"."); 359 | 360 | data_[DEPTH_CAMERA_INFO] = boost::make_shared(node_handle_); 361 | data_[POINT_CLOUD] = boost::make_shared(node_handle_); 362 | 363 | for (int i = 0; i < N; ++i) 364 | { 365 | BaseData::Ptr & data = data_[i]; 366 | data->topic = TOPIC[i]; 367 | data->save_flag = 1 << i; 368 | } 369 | 370 | for (int i = 0; i < N_DATA; ++i) 371 | { 372 | BaseData::Ptr & data = data_[START_DATA + i]; 373 | BaseData::Ptr & camera_info = data_[START_CAMERA_INFO + i]; 374 | 375 | std::string filename; 376 | node_handle_.param(data->topic + "_filename", filename, data->topic + "_"); 377 | 378 | data->file_path = save_directory_ / (filename + "." + EXTENSION[START_DATA + i]); 379 | if (i < N_CAMERA_INFO) 380 | { 381 | if (filename[filename.size() - 1] != '_') 382 | filename += '_'; 383 | camera_info->file_path = save_directory_ / (filename + "camera_info." + EXTENSION[START_CAMERA_INFO + i]); 384 | } 385 | } 386 | 387 | save_flags_ = 0; 388 | for (int i = 0; i < N; ++i) 389 | { 390 | BaseData::Ptr & data = data_[i]; 391 | bool flag; 392 | node_handle_.param("save_" + data->topic, flag, false); 393 | save_flags_ |= flag ? data->save_flag : 0; 394 | if (flag) 395 | data->init(); 396 | } 397 | 398 | } 399 | 400 | bool DataCollectionNode::initialize() 401 | { 402 | 403 | // 1. Wait for sensors to be connected 404 | ROS_INFO_STREAM("[" << device_name_ << "] Waiting for messages..."); 405 | bool ret = true; 406 | 407 | for (int i = 0; ret and i < N; ++i) 408 | { 409 | BaseData::Ptr & data = data_[i]; 410 | if (save_flags_ & data->save_flag) 411 | { 412 | ret = ret and data->waitForMessage(); 413 | ROS_INFO_STREAM("[" << device_name_ << "] " << data->topic << " message OK!"); 414 | } 415 | } 416 | 417 | if (not ret) 418 | { 419 | ROS_ERROR_STREAM("[" << device_name_ << "] Not all messages received!"); 420 | return false; 421 | } 422 | 423 | // 2. Check directories 424 | if (not fs::exists(save_directory_)) 425 | { 426 | if (fs::create_directories(save_directory_)) 427 | { 428 | ROS_INFO_STREAM("[" << device_name_ << "] Directory " << save_directory_ << " created."); 429 | } 430 | else 431 | { 432 | ROS_ERROR_STREAM("[" << device_name_ << "] Error while creating directory " << save_directory_ << ". Aborting."); 433 | return false; 434 | } 435 | } 436 | else 437 | { 438 | if (fs::is_directory(save_directory_) and fs::is_empty(save_directory_)) 439 | { 440 | ROS_INFO_STREAM("[" << device_name_ << "] Directory " << save_directory_ << " already exists and is empty."); 441 | } 442 | else if (fs::is_directory(save_directory_) and not fs::is_empty(save_directory_)) 443 | { 444 | if (continue_) 445 | { 446 | ROS_INFO_STREAM("[" << device_name_ << "] Directory " << save_directory_ << " exists as expected. "); 447 | } 448 | else 449 | { 450 | ROS_ERROR_STREAM("[" << device_name_ << "] Directory " << save_directory_ << " exists but is not empty. Aborting."); 451 | return false; 452 | } 453 | } 454 | else 455 | { 456 | ROS_ERROR_STREAM("[" << device_name_ << "] " << save_directory_ << " is not a directory. Aborting."); 457 | return false; 458 | } 459 | } 460 | 461 | // 3. Check last saved file indices (if in continue mode) 462 | fs::path info_yaml = save_directory_ / "info.yaml"; 463 | if (continue_ and continue_from_ == 0 and fs::exists(info_yaml) and not info_yaml.empty()) 464 | { 465 | ROS_INFO_STREAM("[" << device_name_ << "] Reading \"" << info_yaml.string() << "\"..."); 466 | YAML::Node info_node = YAML::LoadFile(info_yaml.string()); 467 | 468 | const YAML::Node & data_node = info_node["data"]; 469 | int max = 0; 470 | for (const YAML::Node & node : data_node) 471 | { 472 | max = std::max(node["index"].as(), max); 473 | } 474 | file_index_ = max + 1; 475 | ROS_INFO_STREAM("[" << device_name_ << "] First index is " << file_index_ << "."); 476 | } 477 | else 478 | { 479 | file_index_ = 1; 480 | } 481 | 482 | return true; 483 | } 484 | 485 | void DataCollectionNode::actionCallback(const Acquisition::ConstPtr & msg) 486 | { 487 | 488 | try 489 | { 490 | fs::path info_file_name = save_directory_ / "info.yaml"; 491 | 492 | std::ofstream info_file; 493 | info_file.open(info_file_name.string().c_str(), file_index_ == 1 ? std::ios_base::out : std::ios_base::out | std::ios_base::app); 494 | 495 | if (file_index_ == 1) 496 | { 497 | info_file << "camera_info:" << std::endl; 498 | for (int i = START_CAMERA_INFO; i < START_CAMERA_INFO + N_CAMERA_INFO; ++i) 499 | { 500 | BaseData::Ptr & data = data_[i]; 501 | if (save_flags_ & data->save_flag) 502 | { 503 | data->save(file_index_); 504 | info_file << " - {type: \"" << data->topic << "\", " 505 | << std::setprecision(19) << "timestamp: " << data->timestamp() << ", " 506 | << std::setprecision(6) << "filename: " << computeFilePath(data->file_path.filename(), file_index_) << "}" << std::endl; 507 | } 508 | } 509 | info_file << "data:" << std::endl; 510 | } 511 | 512 | info_file << " - index: " << file_index_ << std::endl; 513 | if (msg->distance > 0) 514 | info_file << " distance: " << msg->distance << std::endl; 515 | if (not msg->info.empty()) 516 | info_file << " info: \"" << msg->info << "\"" << std::endl; 517 | info_file << " acquired:" << std::endl; 518 | 519 | for (int i = START_DATA; i < START_DATA + N_DATA; ++i) 520 | { 521 | BaseData::Ptr & data = data_[i]; 522 | if (save_flags_ & data->save_flag) 523 | { 524 | data->save(file_index_); 525 | info_file << " - {type: \"" << data->topic << "\", " 526 | << std::setprecision(19) << "timestamp: " << data->timestamp() << ", " 527 | << std::setprecision(6) << "filename: " << computeFilePath(data->file_path.filename(), file_index_) << "}" << std::endl; 528 | } 529 | } 530 | 531 | info_file.close(); 532 | 533 | ROS_INFO_STREAM("[" << device_name_ << "] " << file_index_ << " saved"); 534 | file_index_++; 535 | 536 | } 537 | catch (cv_bridge::Exception & ex) 538 | { 539 | ROS_ERROR_STREAM("[" << device_name_ << "] cv_bridge exception: " << ex.what()); 540 | } 541 | 542 | } 543 | 544 | } // namespace unipd 545 | 546 | int main(int argc, 547 | char ** argv) 548 | { 549 | ros::init(argc, argv, "sensor_data_collection_node"); 550 | ros::NodeHandle node_handle("~"); 551 | 552 | try 553 | { 554 | unipd::DataCollectionNode collector_node(node_handle); 555 | if (not collector_node.initialize()) 556 | return 0; 557 | ros::spin(); 558 | } 559 | catch (std::runtime_error & error) 560 | { 561 | ROS_FATAL_STREAM("Error: " << error.what()); 562 | return 1; 563 | } 564 | 565 | return 0; 566 | } 567 | --------------------------------------------------------------------------------