├── .gitignore ├── README.md ├── bag_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── bag_tools │ │ ├── camera_bag_processor.h │ │ ├── image_bag_processor.h │ │ ├── stereo_bag_processor.h │ │ └── tum_bag_processor.h ├── launch │ ├── extract_images.launch │ └── extract_stereo_images.launch ├── mainpage.dox ├── package.xml ├── scripts │ ├── add_header_time_offset.py │ ├── bag_add_time_offset.py │ ├── batch_process.py │ ├── camera_info_parser.py │ ├── change_camera_info.py │ ├── change_camera_info_folder.py │ ├── change_frame_id.py │ ├── change_frame_id_folder.py │ ├── change_topics.py │ ├── check_delay.py │ ├── check_drop.py │ ├── cut.py │ ├── extract_topics.py │ ├── gps_to_std_gt.py │ ├── image_sequence_publisher.py │ ├── make_video.py │ ├── merge.py │ ├── plot.py │ ├── remove_tf.py │ ├── replace_msg_time_with_hdr.py │ ├── stereo_sequence_publisher.py │ └── transform_tf.py ├── shell_scripts │ └── change_calib.sh └── src │ ├── extract_images.cpp │ ├── extract_images_from_realsense.cpp │ ├── extract_stereo_images.cpp │ ├── extract_tum_format.cpp │ └── process_stereo.cpp ├── launch_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── mainpage.dox ├── package.xml ├── scripts │ └── launch_tools │ │ ├── launchViz.py │ │ └── services_timer.py ├── setup.py └── src │ └── executer.py ├── plot_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── mainpage.dox ├── package.xml ├── scripts │ └── plot_tools │ │ └── real_time_odometry_plot.py └── setup.py ├── pointcloud_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── mainpage.dox ├── package.xml └── src │ ├── pcd_publisher.cpp │ ├── pointcloud_filtering.cpp │ ├── pointcloud_mapper.cpp │ ├── pointcloud_mapper_for_slam.cpp │ ├── pointcloud_to_webgl.cpp │ └── pointcloud_viewer.cpp ├── srv_tools ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── tf_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── mainpage.dox ├── package.xml └── src ├── apply_tf_to_odom_msg.cpp ├── tf_filter.cpp └── tf_logger.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # directories created by ros 2 | *.pyc 3 | */lib/ 4 | */bin/ 5 | */build/ 6 | */cfg/cpp/ 7 | */cfg/*.cfgc 8 | */CMakeFiles/ 9 | */msg_gen/ 10 | */src/*_msgs/msg/_*.py 11 | */src/*_msgs/_*.py 12 | */msg/lisp/ 13 | */srv_gen/ 14 | */src/*/srv/_*.py 15 | */src/*/_*_.py 16 | */src/*/srv 17 | *~ 18 | 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # srv_tools 2 | 3 | ROS stack with some tools. For more information, see http://www.ros.org/wiki/srv_tools 4 | 5 | -------------------------------------------------------------------------------- /bag_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package bag_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Cleaned scripts. 8 | * Updated to Noetic. 9 | 10 | 0.0.3 (2017-02-24) 11 | ------------------ 12 | * Fix release 13 | * Add missing changes 14 | * Forgot to remove extract_image_positions target 15 | * Fix `#12 `_: Remove old field from camera_info msg 16 | * Remove extract_image_positions and auv_msgs depencency 17 | * add auv_msgs deps in CMake 18 | * Fix `#10 `_ 19 | * add extract_image_positions 20 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 21 | * Minnor changes 22 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 23 | * add missing rospy node init 24 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 25 | * Fix camera info tool 26 | * Fix bag_tools scripts install 27 | * Added stereo_sequence_publisher.py 28 | * fix seq publisher for indigo 29 | * Addapt the pointlcoud_to_webgl script to the new format of the website 30 | * Contributors: Enrique Fernandez, Miquel Massot, matlabbe, plnegre 31 | 32 | 0.0.2 (2017-02-23) 33 | ------------------ 34 | * Add missing changes 35 | * Forgot to remove extract_image_positions target 36 | * Fix `#12 `_: Remove old field from camera_info msg 37 | * Remove extract_image_positions and auv_msgs depencency 38 | * add auv_msgs deps in CMake 39 | * Fix `#10 `_ 40 | * add extract_image_positions 41 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 42 | * Minnor changes 43 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 44 | * add missing rospy node init 45 | * Merge branch 'indigo' of github.com:srv/srv_tools into indigo 46 | * Fix camera info tool 47 | * Fix bag_tools scripts install 48 | * Added stereo_sequence_publisher.py 49 | * fix seq publisher for indigo 50 | * Addapt the pointlcoud_to_webgl script to the new format of the website 51 | * Contributors: Enrique Fernandez, Miquel Massot, matlabbe, plnegre 52 | 53 | * typo on console_bridge dependency 54 | * added -this- to reference the same class 55 | * Of course, we must clear the tmp folder... 56 | * This is the "intersting" patch I did : Now extract_image works also with rosbag containing compressed images, if these images are decodable by opencv (this is the case for JPG and PNG compression). 57 | * In this python script, I added "import rospy" to be able to run this script in a Catkin environment. 58 | Then, I removed the option "sameq" of the ffmpeg process. In ubuntu 14.04, ffmpeg is now called avconv, on my computer I have a symlink from avconv to ffmpeg, so calling ffmpeg still work. But with this version of FFMPEG (avconv 9.16), there is nolonger the sameq argument. Maybe it could be interesting to call avconc and rather than ffmpeg is ffmpeg executable is not found... 59 | * Syntax error fixed in this script. 60 | * Changes for indigo 61 | * preparing for indigo. changed prints for ros logs 62 | * added gps_to_std_gt and services_timer from fuerte 63 | * added missing dependency 64 | * hydro catkinization changes 65 | * added python setup files and wet'ed plot tools 66 | * wet repo 67 | * new script for topic extraction 68 | * remove_tf now removes child tfs as well 69 | * new script for changing a topic in a bagfile 70 | * added bag processor for single cameras 71 | * added export flags to manifest 72 | * made camera info optional 73 | * added batch processing script 74 | * new script for tf manipulation 75 | * improved help text 76 | * added script for making videos from bagfiles 77 | * removed unused method 78 | * new binary for image extraction 79 | * changed logging output 80 | * added param for file pattern 81 | * added link to boost signals 82 | * added some more processing 83 | * new executable for stereo processing inbag->outbag, some refactoring 84 | * added license for cut script 85 | * Merge branch 'master' of github.com:srv/srv_tools 86 | * changed manifests 87 | * added BSD license 88 | * new script for cutting bagfiles 89 | * removed unused import 90 | * changed parameter handling for image sequence publisher node to use rosparams instead of command line arguments 91 | * added parsers to the python scripts 92 | * added main README and bag_tools README 93 | * fixed boost dependency for fuerte 94 | * removed extract_images script as now there is a better c++ code for that 95 | * new binary for extraction of stereo images from a bagfile 96 | * new image sequence publisher 97 | * added camera info parser as seperate script 98 | * fixed launch, added cv_bridge to manifest 99 | * added launch files for image extraction 100 | * fixed wrong package name 101 | * new script for header time modification 102 | * added median calc and support for /tf topic 103 | * added support for multiple bagfile input in check_delay 104 | * new script for delay check 105 | * new script for camera info changing 106 | * bugfix, output bagfile contained only changed topics before 107 | * bugfixes 108 | * added script: change frame id of topics in bagfile 109 | * added script: replaces bagfile message time with header timestamp 110 | * first working version of bag_add_time_offset.py 111 | * added missing packages in manifest 112 | * initial commit 113 | * Contributors: Miquel Massot, Stephan Wirth, aba 114 | -------------------------------------------------------------------------------- /bag_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(bag_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rospy rosbag sensor_msgs cv_bridge message_filters image_proc stereo_image_proc image_geometry camera_calibration_parsers) 5 | find_package(Boost REQUIRED COMPONENTS thread) 6 | find_package(OpenCV REQUIRED) 7 | find_package(console_bridge REQUIRED) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS include 11 | CATKIN_DEPENDS rosbag 12 | ) 13 | 14 | include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) 15 | 16 | add_executable(extract_images src/extract_images.cpp) 17 | add_executable(extract_images_from_realsense src/extract_images_from_realsense.cpp) 18 | 19 | add_executable(extract_tum_format src/extract_tum_format.cpp) 20 | add_executable(extract_stereo_images src/extract_stereo_images.cpp) 21 | add_executable(process_stereo src/process_stereo.cpp) 22 | 23 | target_link_libraries(extract_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) 24 | target_link_libraries(extract_images_from_realsense ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) 25 | target_link_libraries(extract_stereo_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) 26 | target_link_libraries(extract_tum_format ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) 27 | target_link_libraries(process_stereo ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) 28 | 29 | install(TARGETS 30 | extract_images 31 | extract_images_from_realsense 32 | extract_tum_format 33 | extract_stereo_images 34 | process_stereo 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(PROGRAMS 41 | scripts/add_header_time_offset.py 42 | scripts/bag_add_time_offset.py 43 | scripts/batch_process.py 44 | scripts/camera_info_parser.py 45 | scripts/change_camera_info_folder.py 46 | scripts/change_camera_info.py 47 | scripts/change_frame_id.py 48 | scripts/change_topics.py 49 | scripts/check_delay.py 50 | scripts/check_drop.py 51 | scripts/cut.py 52 | scripts/extract_topics.py 53 | scripts/gps_to_std_gt.py 54 | scripts/image_sequence_publisher.py 55 | scripts/make_video.py 56 | scripts/merge.py 57 | scripts/plot.py 58 | scripts/remove_tf.py 59 | scripts/replace_msg_time_with_hdr.py 60 | scripts/stereo_sequence_publisher.py 61 | scripts/transform_tf.py 62 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | PATTERN ".svn" EXCLUDE) 68 | -------------------------------------------------------------------------------- /bag_tools/include/bag_tools/camera_bag_processor.h: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | 43 | namespace bag_tools 44 | { 45 | 46 | /** 47 | * Inherits from message_filters::SimpleFilter 48 | * to use protected signalMessage function 49 | */ 50 | template 51 | class BagSubscriber : public message_filters::SimpleFilter 52 | { 53 | public: 54 | void newMessage(const boost::shared_ptr &msg) 55 | { 56 | this->signalMessage(msg); 57 | } 58 | }; 59 | 60 | class CameraBagProcessor 61 | { 62 | 63 | public: 64 | CameraBagProcessor(const std::string& camera_base_topic) : 65 | camera_base_topic_(camera_base_topic), 66 | sync_(img_sub_, info_sub_, 25) 67 | { 68 | ros::Time::init(); 69 | } 70 | 71 | template 72 | void registerCallback(const C& callback) 73 | { 74 | sync_.registerCallback(callback); 75 | } 76 | 77 | /** 78 | * Processes given bagfile, calls registered callback function when 79 | * a synchronized stereo pair with camera infos is found. 80 | */ 81 | void processBag(const std::string &filename) 82 | { 83 | 84 | // Image topics to load 85 | std::string cam_image = camera_base_topic_ + "/image_raw"; 86 | std::string cam_info = camera_base_topic_ + "/camera_info"; 87 | 88 | std::vector topics; 89 | topics.push_back(cam_image); 90 | topics.push_back(cam_info); 91 | 92 | std::cout << "Starting processing " << filename << ", "; 93 | rosbag::Bag bag; 94 | bag.open(filename, rosbag::bagmode::Read); 95 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 96 | 97 | int num_messages = view.size(); 98 | std::cout << num_messages << " messages to process." << std::endl; 99 | 100 | // Load all messages 101 | boost::progress_display show_progress(num_messages); 102 | BOOST_FOREACH(rosbag::MessageInstance const m, view) 103 | { 104 | if (m.getTopic() == cam_image || ("/" + m.getTopic() == cam_image)) 105 | { 106 | sensor_msgs::Image::ConstPtr img = m.instantiate(); 107 | if (img != NULL) 108 | img_sub_.newMessage(img); 109 | } 110 | 111 | if (m.getTopic() == cam_info || ("/" + m.getTopic() == cam_info)) 112 | { 113 | sensor_msgs::CameraInfo::ConstPtr info = m.instantiate(); 114 | if (info != NULL) 115 | info_sub_.newMessage(info); 116 | } 117 | 118 | ++show_progress; 119 | } 120 | bag.close(); 121 | std::cout << "Finished processing " << filename << std::endl; 122 | } 123 | 124 | private: 125 | 126 | // Fake subscribers to capture images 127 | BagSubscriber img_sub_; 128 | BagSubscriber info_sub_; 129 | 130 | std::string camera_base_topic_; 131 | 132 | message_filters::TimeSynchronizer sync_; 133 | 134 | }; 135 | 136 | } // end of namespace 137 | 138 | -------------------------------------------------------------------------------- /bag_tools/include/bag_tools/image_bag_processor.h: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | namespace bag_tools 40 | { 41 | 42 | class ImageBagProcessor 43 | { 44 | 45 | public: 46 | 47 | typedef boost::function CallbackType; 48 | typedef boost::function CallbackCompressedType; 49 | 50 | ImageBagProcessor(const std::string& image_topic) : 51 | image_topic_(image_topic) 52 | { 53 | ros::Time::init(); 54 | } 55 | 56 | void registerCallback(const CallbackType& callback) 57 | { 58 | callback_ = callback; 59 | } 60 | 61 | void registerCompressedCallback(const CallbackCompressedType& callback) 62 | { 63 | callback_compressed_ = callback; 64 | } 65 | 66 | /** 67 | * Processes given bagfile, calls registered callback function when 68 | * a message is found. 69 | */ 70 | void processBag(const std::string &filename) 71 | { 72 | // Image topics to load 73 | std::vector topics; 74 | topics.push_back(image_topic_); 75 | 76 | std::cout << "Starting processing " << filename << ", "; 77 | rosbag::Bag bag; 78 | bag.open(filename, rosbag::bagmode::Read); 79 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 80 | 81 | int num_messages = view.size(); 82 | std::cout << num_messages << " messages to process." << std::endl; 83 | 84 | // Load all messages 85 | boost::progress_display show_progress(num_messages); 86 | BOOST_FOREACH(rosbag::MessageInstance const m, view) 87 | { 88 | if (m.getTopic() == image_topic_) 89 | { 90 | sensor_msgs::Image::ConstPtr img_msg = m.instantiate(); 91 | if (img_msg != NULL && callback_.empty() == false) 92 | { 93 | callback_(img_msg); 94 | }else 95 | { 96 | sensor_msgs::CompressedImageConstPtr compressed = m.instantiate(); 97 | if(compressed != NULL && callback_compressed_.empty() == false) 98 | { 99 | callback_compressed_(compressed); 100 | } 101 | } 102 | } 103 | ++show_progress; 104 | } 105 | bag.close(); 106 | std::cout << "Finished processing " << filename << std::endl; 107 | } 108 | 109 | private: 110 | 111 | std::string image_topic_; 112 | CallbackType callback_; 113 | CallbackCompressedType callback_compressed_; 114 | 115 | }; 116 | 117 | } // end of namespace 118 | 119 | -------------------------------------------------------------------------------- /bag_tools/include/bag_tools/stereo_bag_processor.h: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | 43 | namespace bag_tools 44 | { 45 | 46 | /** 47 | * Inherits from message_filters::SimpleFilter 48 | * to use protected signalMessage function 49 | */ 50 | template 51 | class BagSubscriber : public message_filters::SimpleFilter 52 | { 53 | public: 54 | void newMessage(const boost::shared_ptr &msg) 55 | { 56 | this->signalMessage(msg); 57 | } 58 | }; 59 | 60 | class StereoBagProcessor 61 | { 62 | 63 | public: 64 | StereoBagProcessor(const std::string& stereo_base_topic) : 65 | stereo_base_topic_(stereo_base_topic), 66 | sync_(l_img_sub_, r_img_sub_, l_info_sub_, r_info_sub_, 25) 67 | { 68 | ros::Time::init(); 69 | } 70 | 71 | template 72 | void registerCallback(const C& callback) 73 | { 74 | sync_.registerCallback(callback); 75 | } 76 | 77 | /** 78 | * Processes given bagfile, calls registered callback function when 79 | * a synchronized stereo pair with camera infos is found. 80 | */ 81 | void processBag(const std::string &filename) 82 | { 83 | 84 | // Image topics to load 85 | std::string l_cam = stereo_base_topic_ + "/left"; 86 | std::string r_cam = stereo_base_topic_ + "/right"; 87 | std::string l_cam_image = l_cam + "/image_raw"; 88 | std::string r_cam_image = r_cam + "/image_raw"; 89 | std::string l_cam_info = l_cam + "/camera_info"; 90 | std::string r_cam_info = r_cam + "/camera_info"; 91 | 92 | std::vector topics; 93 | topics.push_back(l_cam_image); 94 | topics.push_back(r_cam_image); 95 | topics.push_back(l_cam_info); 96 | topics.push_back(r_cam_info); 97 | 98 | std::cout << "Starting processing " << filename << ", "; 99 | rosbag::Bag bag; 100 | bag.open(filename, rosbag::bagmode::Read); 101 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 102 | 103 | int num_messages = view.size(); 104 | std::cout << num_messages << " messages to process." << std::endl; 105 | 106 | // Load all messages 107 | boost::progress_display show_progress(num_messages); 108 | BOOST_FOREACH(rosbag::MessageInstance const m, view) 109 | { 110 | if (m.getTopic() == l_cam_image || ("/" + m.getTopic() == l_cam_image)) 111 | { 112 | sensor_msgs::Image::ConstPtr l_img = m.instantiate(); 113 | if (l_img != NULL) 114 | l_img_sub_.newMessage(l_img); 115 | } 116 | 117 | if (m.getTopic() == r_cam_image || ("/" + m.getTopic() == r_cam_image)) 118 | { 119 | sensor_msgs::Image::ConstPtr r_img = m.instantiate(); 120 | if (r_img != NULL) 121 | r_img_sub_.newMessage(r_img); 122 | } 123 | 124 | if (m.getTopic() == l_cam_info || ("/" + m.getTopic() == l_cam_info)) 125 | { 126 | sensor_msgs::CameraInfo::ConstPtr l_info = m.instantiate(); 127 | if (l_info != NULL) 128 | l_info_sub_.newMessage(l_info); 129 | } 130 | 131 | if (m.getTopic() == r_cam_info || ("/" + m.getTopic() == r_cam_info)) 132 | { 133 | sensor_msgs::CameraInfo::ConstPtr r_info = m.instantiate(); 134 | if (r_info != NULL) 135 | r_info_sub_.newMessage(r_info); 136 | } 137 | ++show_progress; 138 | } 139 | bag.close(); 140 | std::cout << "Finished processing " << filename << std::endl; 141 | } 142 | 143 | private: 144 | 145 | // Fake subscribers to capture images 146 | BagSubscriber l_img_sub_, r_img_sub_; 147 | BagSubscriber l_info_sub_, r_info_sub_; 148 | 149 | std::string stereo_base_topic_; 150 | 151 | message_filters::TimeSynchronizer sync_; 152 | 153 | }; 154 | 155 | } // end of namespace 156 | 157 | -------------------------------------------------------------------------------- /bag_tools/include/bag_tools/tum_bag_processor.h: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | 43 | namespace bag_tools 44 | { 45 | 46 | /** 47 | * Inherits from message_filters::SimpleFilter 48 | * to use protected signalMessage function 49 | */ 50 | template 51 | class BagSubscriber : public message_filters::SimpleFilter 52 | { 53 | public: 54 | void newMessage(const boost::shared_ptr &msg) 55 | { 56 | this->signalMessage(msg); 57 | } 58 | }; 59 | 60 | class TUMBagProcessor 61 | { 62 | 63 | public: 64 | TUMBagProcessor(const std::string& tum_base_topic) : 65 | tum_base_topic_(tum_base_topic), 66 | sync_(depth_img_sub_, color_img_sub_, _info_sub_, color_info_sub_, 25) 67 | { 68 | ros::Time::init(); 69 | } 70 | 71 | template 72 | void registerCallback(const C& callback) 73 | { 74 | sync_.registerCallback(callback); 75 | } 76 | 77 | /** 78 | * Processes given bagfile, calls registered callback function when 79 | * a synchronized color + depth with camera infos is found. 80 | */ 81 | void processBag(const std::string &filename) 82 | { 83 | 84 | // Image topics to load 85 | std::string depth_cam_image = tum_base_topic_+ "/depth/image_rect_raw"; 86 | std::string color_cam_image = tum_base_topic_ + "/color/image_rect_color"; 87 | std::string depth_cam_info = tum_base_topic_ + "/depth/camera_info"; 88 | std::string color_cam_info = tum_base_topic_ + "/color/camera_info"; 89 | 90 | std::vector topics; 91 | topics.push_back(depth_cam_image); 92 | topics.push_back(color_cam_image); 93 | topics.push_back(depth_cam_info); 94 | topics.push_back(color_cam_info); 95 | 96 | std::cout << "Starting processing " << filename << ", "; 97 | rosbag::Bag bag; 98 | bag.open(filename, rosbag::bagmode::Read); 99 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 100 | 101 | int num_messages = view.size(); 102 | std::cout << num_messages << " messages to process." << std::endl; 103 | 104 | // Load all messages 105 | boost::progress_display show_progress(num_messages); 106 | BOOST_FOREACH(rosbag::MessageInstance const m, view) 107 | { 108 | if (m.getTopic() == depth_cam_image || ("/" + m.getTopic() == depth_cam_image)) 109 | { 110 | sensor_msgs::Image::ConstPtr depth_img = m.instantiate(); 111 | if (depth_img != NULL) 112 | depth_img_sub_.newMessage(depth_img); 113 | } 114 | 115 | if (m.getTopic() == color_cam_image || ("/" + m.getTopic() == color_cam_image)) 116 | { 117 | sensor_msgs::Image::ConstPtr color_img = m.instantiate(); 118 | if (color_img != NULL) 119 | color_img_sub_.newMessage(color_img); 120 | } 121 | 122 | if (m.getTopic() == depth_cam_info || ("/" + m.getTopic() == depth_cam_info)) 123 | { 124 | sensor_msgs::CameraInfo::ConstPtr depth_info = m.instantiate(); 125 | if (depth_info != NULL) 126 | _info_sub_.newMessage(depth_info); 127 | } 128 | 129 | if (m.getTopic() == color_cam_info || ("/" + m.getTopic() == color_cam_info)) 130 | { 131 | sensor_msgs::CameraInfo::ConstPtr color_info = m.instantiate(); 132 | if (color_info != NULL) 133 | color_info_sub_.newMessage(color_info); 134 | } 135 | ++show_progress; 136 | } 137 | bag.close(); 138 | std::cout << "Finished processing " << filename << std::endl; 139 | } 140 | 141 | private: 142 | 143 | // Fake subscribers to capture images 144 | BagSubscriber depth_img_sub_, color_img_sub_; 145 | BagSubscriber _info_sub_, color_info_sub_; 146 | 147 | std::string tum_base_topic_; 148 | 149 | message_filters::TimeSynchronizer sync_; 150 | 151 | }; 152 | 153 | } // end of namespace 154 | 155 | -------------------------------------------------------------------------------- /bag_tools/launch/extract_images.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /bag_tools/launch/extract_stereo_images.launch: -------------------------------------------------------------------------------- 1 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 43 | 45 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /bag_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b bag_tools is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /bag_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bag_tools 5 | 0.0.4 6 | ROS tools and scripts related to bagfiles 7 | Bo Miquel Nordfeldt-Fiol 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/bag_tools 12 | 13 | 14 | Stephan Wirth 15 | Miquel Massot 16 | 17 | catkin 18 | rospy 19 | rosbag 20 | sensor_msgs 21 | cv_bridge 22 | message_filters 23 | image_proc 24 | stereo_image_proc 25 | image_geometry 26 | camera_calibration_parsers 27 | libconsole-bridge-dev 28 | 29 | rospy 30 | rosbag 31 | sensor_msgs 32 | cv_bridge 33 | message_filters 34 | image_proc 35 | stereo_image_proc 36 | image_geometry 37 | camera_calibration_parsers 38 | libconsole-bridge-dev 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /bag_tools/scripts/add_header_time_offset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def add_offset(inbags, topics, offset): 38 | for inbag in inbags: 39 | outbag = "timefixed-" + inbag 40 | rospy.loginfo(' Processing input bagfile: %s', inbag) 41 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 42 | rospy.loginfo('Changing header time of topics: %s', topics) 43 | rospy.loginfo(' Adding offset: %s', offset) 44 | outbag = rosbag.Bag(outbag,'w') 45 | time_offset = rospy.Duration.from_sec(offset) 46 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 47 | if topic in topics: 48 | if topic == "/tf": 49 | for transform in msg.transforms: 50 | transform.header.stamp += time_offset 51 | elif msg._has_header: 52 | msg.header.stamp += time_offset 53 | outbag.write(topic, msg, t) 54 | outbag.close() 55 | 56 | 57 | if __name__ == "__main__": 58 | rospy.init_node('add_header_time_offset') 59 | parser = argparse.ArgumentParser( 60 | description='Changes header timestamps using given offset, can change' 61 | '/tf as well.') 62 | parser.add_argument('-o', metavar='OFFSET', required=True, type=float, help='time offset to add in seconds') 63 | parser.add_argument('-i', metavar='BAGFILE', required=True, help='input bagfile(s)', nargs='+') 64 | parser.add_argument('-t', metavar='TOPIC', required=True, help='topics to change', nargs='+') 65 | args = parser.parse_args() 66 | try: 67 | add_offset(args.i, args.t, args.o) 68 | except Exception as e: 69 | import traceback 70 | traceback.print_exc() 71 | -------------------------------------------------------------------------------- /bag_tools/scripts/bag_add_time_offset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def fix_bagfile(inbag, outbag, topics, offset): 38 | rospy.loginfo(' Processing input bagfile: %s', inbag) 39 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 40 | rospy.loginfo(' Changing topics: %s', topics) 41 | rospy.loginfo(' Changing publishing time by: %s', offset) 42 | 43 | outbag = rosbag.Bag(outbag, 'w') 44 | 45 | time_offset = rospy.Duration.from_sec(offset) 46 | 47 | count = {} 48 | for topic in topics: 49 | count[topic] = 0 50 | 51 | for topic, msg, t in rosbag.Bag(inbag).read_messages(): 52 | if topic in topics: 53 | outbag.write(topic, msg, t + time_offset) 54 | count[topic] = count[topic] + 1 55 | else: 56 | outbag.write(topic, msg, t) 57 | rospy.loginfo('Closing output bagfile.') 58 | outbag.close() 59 | rospy.loginfo('Changed the following:') 60 | for k, v in count.items(): 61 | rospy.loginfo( '%s:%s messages.',k,v) 62 | 63 | 64 | if __name__ == "__main__": 65 | rospy.init_node('bag_add_time_offset') 66 | parser = argparse.ArgumentParser( 67 | description='Shift the publishing time of given topics in input bagfile.') 68 | parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile') 69 | parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile(s)', nargs='+') 70 | parser.add_argument('-of', metavar='OFFSET', required=True, type=float, help='time offset to add in seconds') 71 | parser.add_argument('-t', metavar='TOPIC', required=True, help='topic(s) to change', nargs='+') 72 | args = parser.parse_args() 73 | try: 74 | for bagfile in args.i: 75 | fix_bagfile(bagfile, args.o, args.t, args.of) 76 | except Exception as e: 77 | import traceback 78 | traceback.print_exc() 79 | -------------------------------------------------------------------------------- /bag_tools/scripts/batch_process.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import os 33 | import glob 34 | import rospy 35 | import argparse 36 | import subprocess 37 | 38 | 39 | def process(in_dir,out_dir,command): 40 | bagfiles = glob.glob(in_dir + "/*.bag") 41 | for bagfile in bagfiles: 42 | outbag = out_dir + "/" + os.path.basename(bagfile) 43 | if os.path.exists(outbag): 44 | rospy.loginfo('%s exists, skipping.', outbag) 45 | else: 46 | cmd = command.split() 47 | cmd.append("-i") 48 | cmd.append(bagfile) 49 | cmd.append("-o") 50 | cmd.append(outbag) 51 | subprocess.check_call(cmd) 52 | 53 | 54 | if __name__ == "__main__": 55 | rospy.init_node('batch_process') 56 | parser = argparse.ArgumentParser( 57 | description='batch processes all bagfiles in INPUT_DIR, writing output to OUTPUT_DIR by calling given command with -i and -o arguments.') 58 | parser.add_argument('-i', metavar='INPUT_DIR', required=True, help='input directory with input bagfiles') 59 | parser.add_argument('-o', metavar='OUTPUT_DIR', required=True, help='output directory for bagfiles') 60 | parser.add_argument('-c', metavar='COMMAND', required=True, help='command to execute with each bagfile as input and with same name in output OUTPUT_DIR') 61 | args = parser.parse_args() 62 | 63 | try: 64 | process(args.i,args.o,args.c) 65 | except Exception as e: 66 | import traceback 67 | traceback.print_exc() 68 | -------------------------------------------------------------------------------- /bag_tools/scripts/camera_info_parser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import yaml 33 | import rospy 34 | import argparse 35 | import sensor_msgs.msg 36 | 37 | 38 | def parse_yaml(filename): 39 | with open(filename, 'r') as stream: 40 | try: 41 | calib_data = yaml.safe_load(stream) 42 | except yaml.YAMLError as e: 43 | rospy.logerr(e) 44 | return None 45 | cam_info = sensor_msgs.msg.CameraInfo() 46 | cam_info.width = calib_data['image_width'] 47 | cam_info.height = calib_data['image_height'] 48 | cam_info.header.frame_id = calib_data['camera_name'] 49 | cam_info.K = calib_data['camera_matrix']['data'] 50 | cam_info.D = calib_data['distortion_coefficients']['data'] 51 | cam_info.R = calib_data['rectification_matrix']['data'] 52 | cam_info.P = calib_data['projection_matrix']['data'] 53 | cam_info.distortion_model = calib_data['distortion_model'] 54 | return cam_info 55 | 56 | 57 | if __name__ == "__main__": 58 | rospy.init_node('camera_info_parser') 59 | parser = argparse.ArgumentParser(description='Parses camera info yaml files and returns them as sensor_msgs.msg.CameraInfo.') 60 | parser.add_argument('filename', help='input yaml file') 61 | args = parser.parse_args() 62 | info = parse_yaml(args.filename) 63 | if info is not None: 64 | rospy.loginfo('Read the following info from %s\n%s', args.filename, info) -------------------------------------------------------------------------------- /bag_tools/scripts/change_camera_info.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import yaml 33 | import rospy 34 | import rosbag 35 | import argparse 36 | import sensor_msgs.msg 37 | 38 | 39 | def change_camera_info(inbag,outbag,replacements): 40 | rospy.loginfo(' Processing input bagfile: %s', inbag) 41 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 42 | # parse the replacements 43 | maps = {} 44 | for k, v in replacements.items(): 45 | rospy.loginfo('Changing topic %s to contain following info (header will not be changed):\n%s',k,v) 46 | 47 | outbag = rosbag.Bag(outbag,'w') 48 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 49 | if topic in replacements: 50 | new_msg = replacements[topic] 51 | new_msg.header = msg.header 52 | msg = new_msg 53 | outbag.write(topic, msg, t) 54 | rospy.loginfo('Closing output bagfile and exit...') 55 | outbag.close() 56 | 57 | 58 | def replacement(replace_string): 59 | pair = replace_string.split('=', 1) 60 | if len(pair) != 2: 61 | raise argparse.ArgumentTypeError("Replace string must have the form /topic=calib_file.yaml") 62 | if pair[0][0] != '/': 63 | pair[0] = '/'+pair[0] 64 | stream = open(pair[1], 'r') 65 | calib_data = yaml.load(stream, Loader=yaml.FullLoader) 66 | cam_info = sensor_msgs.msg.CameraInfo() 67 | cam_info.width = calib_data['image_width'] 68 | cam_info.height = calib_data['image_height'] 69 | cam_info.K = calib_data['camera_matrix']['data'] 70 | cam_info.D = calib_data['distortion_coefficients']['data'] 71 | cam_info.R = calib_data['rectification_matrix']['data'] 72 | cam_info.P = calib_data['projection_matrix']['data'] 73 | cam_info.distortion_model = calib_data['distortion_model'] 74 | return pair[0], cam_info 75 | 76 | 77 | if __name__ == "__main__": 78 | 79 | ''' 80 | CALL : python3 change_camera_info.py --inbag in.bag --outbag out.bag --replacement /stereo_down/left/camera_info=left.yaml 81 | 82 | troubleshooting: 83 | - pip3 install rospkg 84 | - pip3 install pycryptodomex 85 | - pip3 install gnupg 86 | 87 | ''' 88 | rospy.init_node('change_camera_info') 89 | parser = argparse.ArgumentParser(description='Change camera info messages in a bagfile.') 90 | parser.add_argument('--inbag', help='input bagfile') 91 | parser.add_argument('--outbag', help='output bagfile') 92 | parser.add_argument('--replacement', type=replacement, nargs='+', help='replacement in form "TOPIC=CAMERA_INFO_FILE", e.g. /stereo/left/camera_info=my_new_info.yaml') 93 | args = parser.parse_args() 94 | replacements = {} 95 | 96 | for topic, calib_file in args.replacement: 97 | replacements[topic] = calib_file 98 | try: 99 | change_camera_info(args.inbag, args.outbag, replacements) 100 | except Exception as e: 101 | import traceback 102 | traceback.print_exc() 103 | -------------------------------------------------------------------------------- /bag_tools/scripts/change_camera_info_folder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import os 33 | import yaml 34 | import rospy 35 | import rosbag 36 | import argparse 37 | import sensor_msgs.msg 38 | 39 | 40 | def change_camera_info(inbag,outbag,replacements): 41 | rospy.loginfo(' Processing input bagfile: %s', inbag) 42 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 43 | # parse the replacements 44 | maps = {} 45 | for k, v in replacements.items(): 46 | rospy.loginfo('Changing topic %s to contain following info (header will not be changed):\n%s',k,v) 47 | 48 | outbag = rosbag.Bag(outbag,'w') 49 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 50 | if topic in replacements: 51 | new_msg = replacements[topic] 52 | new_msg.header = msg.header 53 | msg = new_msg 54 | outbag.write(topic, msg, t) 55 | rospy.loginfo('Closing output bagfile and exit...') 56 | outbag.close() 57 | 58 | 59 | def replacement(replace_string): 60 | pair = replace_string.split('=', 1) 61 | if len(pair) != 2: 62 | raise argparse.ArgumentTypeError("Replace string must have the form /topic=calib_file.yaml") 63 | if pair[0][0] != '/': 64 | pair[0] = '/'+pair[0] 65 | stream = open(pair[1], 'r') 66 | calib_data = yaml.load(stream, Loader=yaml.FullLoader) 67 | cam_info = sensor_msgs.msg.CameraInfo() 68 | cam_info.width = calib_data['image_width'] 69 | cam_info.height = calib_data['image_height'] 70 | cam_info.K = calib_data['camera_matrix']['data'] 71 | cam_info.D = calib_data['distortion_coefficients']['data'] 72 | cam_info.R = calib_data['rectification_matrix']['data'] 73 | cam_info.P = calib_data['projection_matrix']['data'] 74 | cam_info.distortion_model = calib_data['distortion_model'] 75 | return pair[0], cam_info 76 | 77 | 78 | if __name__ == "__main__": 79 | 80 | ''' 81 | CALL : python3 change_camera_info_folder.py --path_in path/to/in/folder --path_out path/to/out/folder --replacement /stereo_down/left/camera_info=path/to/left.yaml /stereo_down/right/camera_info=path/to/right.yaml 82 | 83 | troubleshooting: 84 | - pip3 install rospkg 85 | - pip3 install pycryptodomex 86 | - pip3 install gnupg 87 | 88 | ''' 89 | rospy.init_node('change_camera_info') 90 | parser = argparse.ArgumentParser(description='Change camera info messages in a bagfile.') 91 | parser.add_argument('--path_in', help='input folder') 92 | parser.add_argument('--path_out', help='output folder') 93 | parser.add_argument('--replacement', type=replacement, nargs='+', help='replacement in form "TOPIC=CAMERA_INFO_FILE", e.g. /stereo/left/camera_info=my_new_info.yaml') 94 | args = parser.parse_args() 95 | replacements = {} 96 | 97 | path_in = args.path_in # get in folder 98 | path_out = args.path_out # get out folder 99 | 100 | for file in sorted(os.listdir(path_in)): 101 | print("working on file:" + str(file)) 102 | 103 | file_path_in = os.path.join(path_in, file) 104 | file_path_out = os.path.join(path_out, file) 105 | 106 | for topic, calib_file in args.replacement: 107 | replacements[topic] = calib_file 108 | try: 109 | change_camera_info(file_path_in, file_path_out, replacements) 110 | except Exception as e: 111 | import traceback 112 | traceback.print_exc() 113 | -------------------------------------------------------------------------------- /bag_tools/scripts/change_frame_id.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def change_frame_id(inbag_file, outbag_file, frame_id, topics): 38 | rospy.loginfo(' Processing input bagfile: %s', inbag_file) 39 | rospy.loginfo(' Writing to output bagfile: %s', outbag_file) 40 | rospy.loginfo(' Changing topics: %s', topics) 41 | rospy.loginfo(' Writing frame_id: %s', frame_id) 42 | 43 | outbag = rosbag.Bag(outbag_file, 'w') 44 | 45 | for topic, msg, t in rosbag.Bag(inbag_file,'r').read_messages(): 46 | if topic in topics: 47 | if msg._has_header: 48 | msg.header.frame_id = frame_id 49 | outbag.write(topic, msg, t) 50 | rospy.loginfo('Closing output bagfile and exit...') 51 | outbag.close() 52 | 53 | 54 | if __name__ == "__main__": 55 | 56 | ''' 57 | CALL : python change_frame_id.py -i path/to/in/bagfile -o path/to/out/bagfile -f desired_frame_id -t topic_to_change1 topic_to_change2 topic_to_change3 ... 58 | 59 | ''' 60 | 61 | rospy.init_node('change_frame_id') 62 | parser = argparse.ArgumentParser( 63 | description='Create a new bagfile from an existing one replacing the frame id of requested topics.') 64 | parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile') 65 | parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile') 66 | parser.add_argument('-f', metavar='FRAME_ID', required=True, help='desired frame_id name in the topics') 67 | parser.add_argument('-t', metavar='TOPIC', required=True, help='topic(s) to change', nargs='+') 68 | args = parser.parse_args() 69 | 70 | try: 71 | change_frame_id(args.i,args.o,args.f,args.t) 72 | except Exception as e: 73 | import traceback 74 | traceback.print_exc() 75 | -------------------------------------------------------------------------------- /bag_tools/scripts/change_frame_id_folder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import os 33 | import rospy 34 | import argparse 35 | from change_frame_id import change_frame_id 36 | 37 | 38 | if __name__ == "__main__": 39 | 40 | ''' 41 | CALL : python change_frame_id.py -i path/to/in/folder -o path/to/out/folder -f desired_frame_id -t topic_to_change1 topic_to_change2 topic_to_change3 ... 42 | 43 | ''' 44 | rospy.init_node('change_frame_id') 45 | parser = argparse.ArgumentParser( 46 | description='Create a new bagfile from an existing one replacing the frame id of requested topics.') 47 | parser.add_argument('-i', metavar = 'INPUT_PATH', required = True, help = 'input folder') 48 | parser.add_argument('-o', metavar = 'OUTPUT_PATH', required = True, help = 'output folder') 49 | parser.add_argument('-f', metavar = 'FRAME_ID', required = True, help = 'desired frame_id name in the topics') 50 | parser.add_argument('-t', metavar = 'TOPIC', required = True, nargs='+', help='topic(s) to change') 51 | args = parser.parse_args() 52 | 53 | if not os.path.exists(args.o): 54 | os.makedirs(args.o) 55 | 56 | for item in sorted(os.listdir(args.i)): 57 | 58 | file_path_in = os.path.join(args.i, item) 59 | file_path_out = os.path.join(args.o, item) 60 | 61 | if os.path.isfile(file_path_in): 62 | if ".bag" in item: 63 | print("working on file: " + str(item)) 64 | try: 65 | change_frame_id(file_path_in, file_path_out, args.f, args.t) 66 | except Exception: 67 | import traceback 68 | traceback.print_exc() -------------------------------------------------------------------------------- /bag_tools/scripts/change_topics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def change_topics(inbag,outbag,replacements): 38 | rospy.loginfo(' Processing input bagfile: %s', inbag) 39 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 40 | rospy.loginfo(' Replacements: %s', replacements) 41 | 42 | outbag = rosbag.Bag(outbag,'w') 43 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 44 | if topic in replacements: 45 | topic = replacements[topic] 46 | outbag.write(topic, msg, t) 47 | rospy.loginfo('Closing output bagfile and exit...') 48 | outbag.close() 49 | 50 | 51 | def replacement(replace_string): 52 | pair = replace_string.split('=', 1) 53 | if len(pair) != 2: 54 | raise argparse.ArgumentTypeError("Replace string must have the form /topic=/new_topic") 55 | if pair[0][0] != '/': 56 | pair[0] = '/'+pair[0] 57 | if pair[1][0] != '/': 58 | pair[1] = '/'+pair[1] 59 | return pair[0], pair[1] 60 | 61 | 62 | if __name__ == "__main__": 63 | rospy.init_node('change_topics') 64 | parser = argparse.ArgumentParser( 65 | description='changes topic names inside a bagfile.') 66 | parser.add_argument('inbag', help='input bagfile') 67 | parser.add_argument('outbag', help='output bagfile') 68 | parser.add_argument('replacement', type=replacement, nargs='+', help='replacement in form "TOPIC=NEW_TOPIC", e.g. /laser1/scan=/laser2/scan') 69 | args = parser.parse_args() 70 | replacements = {} 71 | 72 | for topic, new_topic in args.replacement: 73 | replacements[topic] = new_topic 74 | 75 | try: 76 | change_topics(args.inbag,args.outbag,replacements) 77 | except Exception as e: 78 | import traceback 79 | traceback.print_exc() 80 | 81 | -------------------------------------------------------------------------------- /bag_tools/scripts/check_delay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | import statistics 36 | 37 | 38 | def check_delay(inbags): 39 | delays = {} 40 | for inbag in inbags: 41 | rospy.loginfo(' Processing input bagfile: %s', inbag) 42 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 43 | if topic == "/tf": 44 | for transform in msg.transforms: 45 | delay = abs((t - transform.header.stamp).to_sec()) 46 | key = "/tf: " + transform.header.frame_id + " -> " + transform.child_frame_id 47 | if key not in delays: 48 | delays[key] = [] 49 | delays[key].append(delay) 50 | elif msg._has_header: 51 | key = topic 52 | if key not in delays: 53 | delays[key] = [] 54 | delay = abs((t - msg.header.stamp).to_sec()) 55 | delays[key].append(delay) 56 | max_len = max(len(topic) for topic in delays.keys()) 57 | topics = delays.keys() 58 | list(topics).sort() 59 | for topic in topics: 60 | delay_list = delays[topic] 61 | delay_list.sort() 62 | dmin, dmax, dmean = min(delay_list), max(delay_list), sum(delay_list)/len(delay_list) 63 | dmedian = statistics.median(delay_list) 64 | rospy.loginfo('%s : mean = %s, min = %s, max = %s, median = %s', topic.ljust(max_len + 2), dmean, dmin, dmax, dmedian) 65 | 66 | 67 | if __name__ == "__main__": 68 | rospy.init_node('check_delay') 69 | parser = argparse.ArgumentParser( 70 | description='Checks the delay in a bagfile between publishing (recording) ' 71 | 'time and the time stamp in the header (if exists). Prints ' 72 | 'out min, max and mean delays.') 73 | parser.add_argument('inbag', help='input bagfile(s)', nargs='+') 74 | args = parser.parse_args() 75 | try: 76 | check_delay(args.inbag) 77 | except Exception as e: 78 | import traceback 79 | traceback.print_exc() 80 | -------------------------------------------------------------------------------- /bag_tools/scripts/check_drop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2015, 4 | Enrique Fernandez Perdomo 5 | Clearpath Robotics, Inc. 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import numpy 33 | import argparse 34 | import matplotlib as mpl # Workaround to avoid issues with X11 rendering when running on background: 35 | mpl.use('Agg') 36 | import matplotlib.pyplot as plt 37 | 38 | import rospy 39 | import rosbag 40 | 41 | 42 | def check_drop(inbags, plot_format='png'): 43 | # Retrieve msg time, bag time and sequence number for all topics and messages: 44 | msg_time = {} 45 | bag_time = {} 46 | seq = {} 47 | 48 | for inbag in inbags: 49 | rospy.loginfo(' Processing input bagfile: %s', inbag) 50 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 51 | if topic == "/tf": 52 | for transform in msg.transforms: 53 | key = "/tf: " + transform.header.frame_id + " -> " + transform.child_frame_id 54 | if key not in msg_time: 55 | msg_time[key] = [] 56 | bag_time[key] = [] 57 | seq[key] = [] 58 | else: 59 | msg_time[key].append(transform.header.stamp.to_sec()) 60 | bag_time[key].append(t.to_sec()) 61 | seq[key].append(transform.header.seq) 62 | elif msg._has_header: 63 | key = topic 64 | if key not in msg_time: 65 | msg_time[key] = [] 66 | bag_time[key] = [] 67 | seq[key] = [] 68 | else: 69 | msg_time[key].append(msg.header.stamp.to_sec()) 70 | bag_time[key].append(t.to_sec()) 71 | seq[key].append(msg.header.seq) 72 | 73 | # Convert lists to numpy.arrays: 74 | for key in msg_time.iterkeys(): 75 | msg_time[key] = numpy.array(msg_time[key]) 76 | bag_time[key] = numpy.array(bag_time[key]) 77 | seq[key] = numpy.array(seq[key]) 78 | 79 | # Compute differences: 80 | msg_time_diff = {} 81 | bag_time_diff = {} 82 | seq_diff = {} 83 | 84 | min_time = [] 85 | for key in msg_time.iterkeys(): 86 | msg_time_diff[key] = numpy.diff(msg_time[key]) 87 | min_time.append(msg_time[key][0]) 88 | bag_time_diff[key] = numpy.diff(bag_time[key]) 89 | seq_diff[key] = numpy.diff(seq[key]) 90 | 91 | min_time = numpy.min(min_time) 92 | 93 | # Compute min, max and mean differences: 94 | basename = inbags[0].replace('.bag', '') 95 | 96 | max_len = max(len(topic) for topic in msg_time_diff.keys()) 97 | topics = msg_time_diff.keys() 98 | topics.sort() 99 | 100 | 101 | for topic in topics: 102 | msg_time_diff_topic = msg_time_diff[topic] 103 | bag_time_diff_topic = bag_time_diff[topic] 104 | seq_diff_topic = seq_diff[topic] 105 | 106 | if len(msg_time_diff_topic) == 0: 107 | rospy.logwarn('%s has no messages', topic.ljust(max_len + 2)) 108 | continue 109 | 110 | msg_time_diff_min = numpy.min(msg_time_diff_topic) 111 | msg_time_diff_max = numpy.max(msg_time_diff_topic) 112 | msg_time_diff_mean = numpy.mean(msg_time_diff_topic) 113 | msg_time_diff_median = numpy.median(msg_time_diff_topic) 114 | 115 | bag_time_diff_min = numpy.min(bag_time_diff_topic) 116 | bag_time_diff_max = numpy.max(bag_time_diff_topic) 117 | bag_time_diff_mean = numpy.mean(bag_time_diff_topic) 118 | bag_time_diff_median = numpy.median(bag_time_diff_topic) 119 | 120 | seq_diff_min = numpy.min(seq_diff_topic) 121 | seq_diff_max = numpy.max(seq_diff_topic) 122 | seq_diff_mean = numpy.mean(seq_diff_topic) 123 | seq_diff_median = numpy.median(seq_diff_topic) 124 | 125 | rospy.loginfo('%s message time: mean = %s, min = %s, max = %s, median = %s', topic.ljust(max_len + 2), msg_time_diff_mean, msg_time_diff_min, msg_time_diff_max, msg_time_diff_median) 126 | rospy.loginfo('%s bag time: mean = %s, min = %s, max = %s, median = %s', topic.ljust(max_len + 2), bag_time_diff_mean, bag_time_diff_min, bag_time_diff_max, bag_time_diff_median) 127 | rospy.loginfo('%s seq: mean = %s, min = %s, max = %s, median = %s', topic.ljust(max_len + 2), seq_diff_mean, seq_diff_min, seq_diff_max, seq_diff_median) 128 | 129 | # Create and save plots: 130 | mt = msg_time[topic][1:] - min_time 131 | 132 | try: 133 | fig = plt.figure() 134 | fig.set_size_inches(20, 15) 135 | 136 | plt.subplot(311) 137 | plt.title(topic + ' - Message time difference [s]') 138 | plt.plot(mt, msg_time_diff_topic,'b') 139 | plt.plot([mt[0], mt[-1]], [msg_time_diff_min, msg_time_diff_min], 'r--') 140 | plt.plot([mt[0], mt[-1]], [msg_time_diff_max, msg_time_diff_max], 'r--') 141 | plt.plot([mt[0], mt[-1]], [msg_time_diff_mean, msg_time_diff_mean], 'g') 142 | 143 | plt.subplot(312) 144 | plt.title(topic + ' - Bag time difference [s]') 145 | plt.plot(mt, bag_time_diff_topic,'b') 146 | plt.plot([mt[0], mt[-1]], [bag_time_diff_min, bag_time_diff_min], 'r--') 147 | plt.plot([mt[0], mt[-1]], [bag_time_diff_max, bag_time_diff_max], 'r--') 148 | plt.plot([mt[0], mt[-1]], [bag_time_diff_mean, bag_time_diff_mean], 'g') 149 | 150 | plt.subplot(313) 151 | plt.title(topic + ' - Sequence number difference') 152 | plt.plot(mt, seq_diff_topic, 'b') 153 | plt.plot([mt[0], mt[-1]], [seq_diff_min, seq_diff_min], 'r--') 154 | plt.plot([mt[0], mt[-1]], [seq_diff_max, seq_diff_max], 'r--') 155 | plt.plot([mt[0], mt[-1]], [seq_diff_mean, seq_diff_mean], 'g') 156 | 157 | 158 | plt.savefig(basename + topic.replace('/', '_').replace(' ', '_').replace(':', '_') + '.' + plot_format) 159 | plt.close(fig) 160 | except OverflowError as e: 161 | rospy.logerr('%s: Failed to save plots as %s image files (try other format, e.g. svg): %s', topic.ljust(max_len + 2), plot_format, e.message) 162 | 163 | if __name__ == "__main__": 164 | rospy.init_node('check_drop', anonymous=True) 165 | 166 | parser = argparse.ArgumentParser( 167 | description='Checks the header.seq and header.stamp difference to detect ' 168 | 'message dropping in a bagfile. Prints out min, max and mean ' 169 | 'differences and all values are shown on a plot.') 170 | parser.add_argument('inbag', help='input bagfile(s)', nargs='+') 171 | parser.add_argument('--plot_format', help='output plot format', default='png') 172 | args = parser.parse_args() 173 | try: 174 | check_drop(args.inbag, args.plot_format) 175 | except Exception as e: 176 | import traceback 177 | traceback.print_exc() 178 | -------------------------------------------------------------------------------- /bag_tools/scripts/cut.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python33 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | def cut(inbags, outbagfile, start, duration): 37 | start_time = rospy.Time.from_sec(999999999999) 38 | for inbag in inbags: 39 | rospy.loginfo(' Looking for smallest time in: %s', inbag) 40 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 41 | if t < start_time: 42 | start_time = t 43 | break 44 | rospy.loginfo(' Bagfiles start at %s', start_time) 45 | start_time = start_time + rospy.Duration.from_sec(start) 46 | end_time = start_time + rospy.Duration.from_sec(duration) 47 | rospy.loginfo(' Cutting out from %s to %s',start_time, end_time) 48 | outbag = rosbag.Bag(outbagfile, 'w') 49 | num_messages = 0 50 | for inbag in inbags: 51 | rospy.loginfo(' Extracting messages from: %s', inbag) 52 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(start_time=start_time, end_time=end_time): 53 | outbag.write(topic, msg, t) 54 | num_messages = num_messages + 1 55 | outbag.close() 56 | rospy.loginfo(' New output bagfile has %s messages', num_messages) 57 | 58 | 59 | if __name__ == "__main__": 60 | rospy.init_node('cut') 61 | parser = argparse.ArgumentParser( 62 | description='Cuts out a section from an input bagfile and writes it to an output bagfile') 63 | parser.add_argument('--inbag', help='input bagfile(s)', nargs='+', required=True) 64 | parser.add_argument('--outbag', help='output bagfile', required=True) 65 | parser.add_argument('--start', help='start time', type=float, required=True) 66 | parser.add_argument('--duration', help='duration of the resulting part', type=float, required=True) 67 | args = parser.parse_args() 68 | try: 69 | cut(args.inbag, args.outbag, args.start, args.duration) 70 | except Exception as e: 71 | import traceback 72 | traceback.print_exc() 73 | -------------------------------------------------------------------------------- /bag_tools/scripts/extract_topics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def extract_topics(inbag,outbag,topics): 38 | rospy.loginfo(' Processing input bagfile: %s', inbag) 39 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 40 | rospy.loginfo(' Extracting topics: %s', topics) 41 | 42 | outbag = rosbag.Bag(outbag,'w') 43 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 44 | if topic in topics: 45 | outbag.write(topic, msg, t) 46 | rospy.loginfo('Closing output bagfile and exit...') 47 | outbag.close() 48 | 49 | 50 | if __name__ == "__main__": 51 | rospy.init_node('extract_topics') 52 | parser = argparse.ArgumentParser( 53 | description='Extracts topics from a bagfile into another bagfile.') 54 | parser.add_argument('inbag', help='input bagfile') 55 | parser.add_argument('outbag', help='output bagfile') 56 | parser.add_argument('topics', nargs='+', help='topics to extract') 57 | args = parser.parse_args() 58 | 59 | try: 60 | extract_topics(args.inbag,args.outbag,args.topics) 61 | except Exception as e: 62 | import traceback 63 | traceback.print_exc() 64 | 65 | -------------------------------------------------------------------------------- /bag_tools/scripts/gps_to_std_gt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import rospy 4 | import argparse 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | 8 | 9 | class Error(Exception): 10 | """ Base class for exceptions in this module. """ 11 | pass 12 | 13 | 14 | def get_xyz(gps_point): 15 | lat = gps_point[3]*np.pi/180 #converting to radians. 16 | lon = gps_point[4]*np.pi/180 #converting to radians. 17 | alt = gps_point[5]*np.pi/180 #converting to radians. 18 | a = 6378137.0 # earth semimajor axis in meters. 19 | f = 1/298.257223563 # reciprocal flattening. 20 | e2 = 2*f - np.power(f,2) # eccentricity squared. 21 | 22 | chi = np.sqrt(1-e2 * np.power(np.sin(lat),2)) 23 | x = (a/chi +alt) * np.cos(lat) * np.cos(lon) 24 | y = (a/chi +alt) * np.cos(lat) * np.sin(lon) 25 | z = (a*(1-e2)/chi + alt) * np.sin(lat) 26 | return x, y, z 27 | 28 | 29 | if __name__ == "__main__": 30 | rospy.init_node('gps_to_std_gt') 31 | 32 | parser = argparse.ArgumentParser( 33 | description='Convert gps/fix topic to standard ground truth file', 34 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 35 | parser.add_argument('gps_topic_file', 36 | help='file with the gps/fix topic. Saved with "rostopic echo -p /gps/fix > data/anselm_turmeda/gt_gps.txt"') 37 | parser.add_argument('output_file', 38 | help='output file where the standard ground truth values will be saved.') 39 | args = parser.parse_args() 40 | 41 | # Read the 42 | gps_topic = np.loadtxt(args.gps_topic_file, delimiter=',', skiprows=1, usecols=(0,1,2,6,7,8)) 43 | 44 | # Write the x, y, z data to the output file 45 | with open(args.output_file, 'w') as outfile: 46 | outfile.write("%time,field.header.seq,field.header.stamp,field.header.frame_id,field.child_frame_id,x,y,z,qx,qy,qz,qw\n") 47 | x0, y0, z0 = get_xyz(gps_topic[0]) 48 | gt = [] 49 | gt.append([0.0, 0.0, 0.0]) 50 | for i in range(len(gps_topic)-1): 51 | x, y, z = get_xyz(gps_topic[i+1]) 52 | x = x-x0 53 | y = y-y0 54 | z = z-z0 55 | gt.append([x, y, z]) 56 | outfile.write("%.9F," % gps_topic[i+1, 0]) 57 | outfile.write(str(gps_topic[i+1, 1]) + "," + str(gps_topic[i+1, 2]) + ",/gps,,") 58 | outfile.write("%.9F," % x) 59 | outfile.write("%.9F," % y) 60 | outfile.write("%.9F," % z) 61 | outfile.write("0.0,0.0,0.0,0.0") 62 | outfile.write("\n") 63 | gt = np.array(gt) 64 | 65 | # Init figure 66 | fig = plt.figure(1) 67 | ax = fig.add_subplot(111, projection = '3d') 68 | ax.grid() 69 | ax.set_title("GT Viewer") 70 | ax.set_xlabel("X") 71 | ax.set_ylabel("Y") 72 | ax.set_zlabel("Z") 73 | 74 | ax.plot(gt[:,0], gt[:,1], gt[:,2], 'g', label='Ground Truth') 75 | ax.legend() 76 | 77 | plt.draw() 78 | plt.show() 79 | -------------------------------------------------------------------------------- /bag_tools/scripts/image_sequence_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import os 33 | import math 34 | import glob 35 | import numpy as np 36 | from numpy import genfromtxt 37 | 38 | import cv2 39 | 40 | import rospy 41 | import tf 42 | import cv_bridge 43 | import sensor_msgs.msg 44 | import camera_info_parser 45 | 46 | fake_green = False 47 | 48 | def collect_image_files(image_dir,file_pattern): 49 | images = glob.glob(image_dir + '/' + file_pattern) 50 | images.sort() 51 | #images = images[:1101] 52 | #images = images[1102:2202] 53 | # images = images[2203:] 54 | return images 55 | 56 | def collect_poses(file): 57 | poses = genfromtxt(file, delimiter=',') 58 | return poses 59 | 60 | def playback_images(image_dir,file_pattern,camera_info_file,pose_file,publish_rate): 61 | if camera_info_file != "": 62 | cam_info = camera_info_parser.parse_yaml(camera_info_file) 63 | publish_cam_info = True 64 | else: 65 | publish_cam_info = False 66 | if pose_file != "": 67 | poses = collect_poses(pose_file) 68 | publish_poses = True 69 | else: 70 | publish_poses = False 71 | image_files = collect_image_files(image_dir,file_pattern) 72 | rospy.loginfo('Found %i images.',len(image_files)) 73 | bridge = cv_bridge.CvBridge() 74 | rate = rospy.Rate(publish_rate) 75 | image_publisher = rospy.Publisher('camera/image_color', sensor_msgs.msg.Image, queue_size = 5) 76 | if publish_cam_info: 77 | cam_info_publisher = rospy.Publisher('camera/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5) 78 | if publish_poses: 79 | tf_pose_publisher = tf.TransformBroadcaster() 80 | rospy.loginfo('Starting playback.') 81 | for image_file in image_files: 82 | if rospy.is_shutdown(): 83 | break 84 | now = rospy.Time.now() 85 | image = cv2.imread(image_file) 86 | if fake_green: 87 | image[:,:,0] = 0 88 | image[:,:,2] = 0 89 | image_msg = bridge.cv2_to_imgmsg(np.asarray(image[:,:]), encoding='bgr8') 90 | image_msg.header.stamp = now 91 | image_msg.header.frame_id = "/camera" 92 | image_publisher.publish(image_msg) 93 | if publish_cam_info: 94 | cam_info.header.stamp = now 95 | cam_info.header.frame_id = "/camera" 96 | cam_info_publisher.publish(cam_info) 97 | if publish_poses: 98 | img_name = os.path.basename(image_file) 99 | idx = int(''.join(x for x in img_name if x.isdigit())) + 1 100 | tf_pose_publisher.sendTransform((poses[idx, 1], poses[idx, 2], poses[idx, 3]), 101 | tf.transformations.quaternion_from_euler(math.radians(poses[idx, 4]), math.radians(poses[idx, 5]), math.radians(poses[idx, 6])), 102 | now, 103 | 'dvl', 104 | 'world') 105 | # print img_name + ' =? ' + str(poses[idx, 0]) + " pose (" + str(poses[idx, 1]) + ", " + str(poses[idx, 2]) + ", " + str(poses[idx, 3]) + ") orientation (" + str(poses[idx, 4]) + ", " + str(poses[idx, 5]) + ", " + str(poses[idx, 6]) + ")" 106 | rate.sleep() 107 | rospy.loginfo('No more images left. Stopping.') 108 | 109 | if __name__ == "__main__": 110 | rospy.init_node('image_sequence_publisher') 111 | try: 112 | image_dir = rospy.get_param("~image_dir") 113 | file_pattern = rospy.get_param("~file_pattern") 114 | camera_info_file = rospy.get_param("~camera_info_file", "") 115 | pose_file = rospy.get_param("~pose_file", "") 116 | frequency = rospy.get_param("~frequency", 10) 117 | fake_green = rospy.get_param("~fake_green", False) 118 | playback_images(image_dir, file_pattern, camera_info_file, pose_file, frequency) 119 | except KeyError as e: 120 | rospy.logerr('Required parameter missing: %s', e) 121 | except Exception as e: 122 | import traceback 123 | traceback.print_exc() 124 | -------------------------------------------------------------------------------- /bag_tools/scripts/make_video.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import glob 33 | import shutil 34 | import tempfile 35 | import subprocess 36 | 37 | import rospy 38 | 39 | 40 | def create_video(tmp_dir, args): 41 | rospy.loginfo('Using {} as working directory.'.format(tmp_dir)) 42 | rospy.loginfo('Extracting images...') 43 | 44 | cmd = ["rosrun", "bag_tools", "extract_images" , tmp_dir, "jpg", args.topic] + args.inbag 45 | rospy.loginfo(' {}'.format(' '.join(cmd))) 46 | subprocess.call(cmd) 47 | 48 | rospy.loginfo("Renaming...") 49 | images = glob.glob(tmp_dir + '/*.jpg') 50 | images.sort() 51 | i = 1 52 | for image in images: 53 | shutil.move(image, tmp_dir + '/img-' + str(i) + '.jpg') 54 | i = i + 1 55 | 56 | rospy.loginfo('Creating video...') 57 | cmd = ["ffmpeg", "-f", "image2", "-r", str(args.fps), "-i", tmp_dir + "/img-%d.jpg", args.output] 58 | rospy.loginfo(' {}'.format(' '.join(cmd))) 59 | subprocess.call(cmd) 60 | 61 | 62 | if __name__ == "__main__": 63 | rospy.init_node('make_video') 64 | import argparse 65 | parser = argparse.ArgumentParser( 66 | description= 67 | 'Creates a video from sensor_msgs/Image messages from a bagfile. ' 68 | 'This script uses the extract_images binary to extract color images ' 69 | 'from bagfiles and calls ffmpeg afterwards to combine them together ' 70 | 'to form a video. Note that ffmpeg must be installed on your system.') 71 | parser.add_argument('topic', help='topic of the images to use') 72 | parser.add_argument('--output', help='name of the output video. Note that the file ending defines the codec to use.', default='video.mp4') 73 | parser.add_argument('--fps', help='frames per second in the output video, as long as codec supports this', type=int, default=20) 74 | parser.add_argument('inbag', help='input bagfile(s)', nargs='+') 75 | args = parser.parse_args() 76 | tmp_dir = tempfile.mkdtemp() 77 | try: 78 | create_video(tmp_dir, args) 79 | except Exception as e: 80 | import traceback 81 | traceback.print_exc() 82 | rospy.loginfo('Cleaning up temp files...') 83 | shutil.rmtree(tmp_dir) 84 | -------------------------------------------------------------------------------- /bag_tools/scripts/merge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2015, 4 | Enrique Fernandez Perdomo 5 | Clearpath Robotics, Inc. 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import os 33 | import sys 34 | import argparse 35 | 36 | import rosbag 37 | 38 | 39 | def merge(inbags, outbag='output.bag', topics=None, exclude_topics=[], raw=True): 40 | # Open output bag file: 41 | try: 42 | out = rosbag.Bag(outbag, 'a' if os.path.exists(outbag) else 'w') 43 | except IOError as e: 44 | print('Failed to open output bag file %s!: %s' % (outbag, e.message), file=sys.stderr) 45 | return 127 46 | 47 | # Write the messages from the input bag files into the output one: 48 | for inbag in inbags: 49 | try: 50 | print(' Processing input bagfile: %s' % inbag) 51 | for topic, msg, t in rosbag.Bag(inbag, 'r').read_messages(topics=topics, raw=raw): 52 | if topic not in args.exclude_topics: 53 | out.write(topic, msg, t, raw=raw) 54 | except IOError as e: 55 | print('Failed to open input bag file %s!: %s' % (inbag, e.message), file=sys.stderr) 56 | return 127 57 | 58 | out.close() 59 | 60 | return 0 61 | 62 | 63 | if __name__ == "__main__": 64 | parser = argparse.ArgumentParser( 65 | description='Merge multiple bag files into a single one.') 66 | parser.add_argument('inbag', help='input bagfile(s)', nargs='+') 67 | parser.add_argument('--output', help='output bag file', default='output.bag') 68 | parser.add_argument('--topics', help='topics to merge from the input bag files', nargs='+', default=None) 69 | parser.add_argument('--exclude_topics', help='topics not to merge from the input bag files', nargs='+', default=[]) 70 | args = parser.parse_args() 71 | 72 | try: 73 | sys.exit(merge(args.inbag, args.output, args.topics, args.exclude_topics)) 74 | except Exception as e: 75 | import traceback 76 | traceback.print_exc() 77 | -------------------------------------------------------------------------------- /bag_tools/scripts/remove_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def remove_tf(inbag,outbag,frame_ids): 38 | rospy.loginfo(' Processing input bagfile: %s', inbag) 39 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 40 | rospy.loginfo(' Removing frame_ids: %s', ' '.join(frame_ids)) 41 | 42 | outbag = rosbag.Bag(outbag,'w') 43 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 44 | if topic == "/tf": 45 | new_transforms = [] 46 | for transform in msg.transforms: 47 | if transform.header.frame_id not in frame_ids and transform.child_frame_id not in frame_ids: 48 | new_transforms.append(transform) 49 | msg.transforms = new_transforms 50 | outbag.write(topic, msg, t) 51 | rospy.loginfo('Closing output bagfile and exit...') 52 | outbag.close(); 53 | 54 | 55 | if __name__ == "__main__": 56 | rospy.init_node('remove_tf') 57 | parser = argparse.ArgumentParser( 58 | description='removes all transforms from the /tf topic that contain one of the given frame_ids in the header as parent or child.') 59 | parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile') 60 | parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile') 61 | parser.add_argument('-f', metavar='FRAME_ID', required=True, help='frame_id(s) of the transforms to remove from the /tf topic', nargs='+') 62 | args = parser.parse_args() 63 | 64 | try: 65 | remove_tf(args.i,args.o,args.f) 66 | except Exception as e: 67 | import traceback 68 | traceback.print_exc() 69 | -------------------------------------------------------------------------------- /bag_tools/scripts/replace_msg_time_with_hdr.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import rosbag 34 | import argparse 35 | 36 | 37 | def rpl_msg_time_with_hdr(inbag,outbag): 38 | rospy.loginfo('Processing input bagfile : %s', inbag) 39 | rospy.loginfo('Writing to output bagfile : %s', outbag) 40 | outbag = rosbag.Bag(outbag,'w') 41 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 42 | # This also replaces tf timestamps under the assumption 43 | # that all transforms in the message share the same timestamp 44 | if topic == "/tf" and msg.transforms: 45 | outbag.write(topic, msg, msg.transforms[0].header.stamp) 46 | else: 47 | outbag.write(topic, msg, msg.header.stamp if msg._has_header else t) 48 | rospy.loginfo('Closing output bagfile and exit...') 49 | outbag.close() 50 | 51 | 52 | if __name__ == "__main__": 53 | rospy.init_node('rpl_msg_time_with_hdr') 54 | parser = argparse.ArgumentParser( 55 | description='Create a new bagfile from an existing one replacing the message time for the header time.') 56 | parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile') 57 | parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile') 58 | args = parser.parse_args() 59 | try: 60 | rpl_msg_time_with_hdr(args.i, args.o) 61 | except Exception as e: 62 | import traceback 63 | traceback.print_exc() 64 | -------------------------------------------------------------------------------- /bag_tools/scripts/stereo_sequence_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Created from image_sequence_publisher.py 4 | Mathieu Labbe 5 | 6 | Copyright (c) 2012, 7 | Systems, Robotics and Vision Group 8 | University of the Balearican Islands 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | * Redistributions of source code must retain the above copyright 14 | notice, this list of conditions and the following disclaimer. 15 | * Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | * Neither the name of Systems, Robotics and Vision Group, University of 19 | the Balearican Islands nor the names of its contributors may be used to 20 | endorse or promote products derived from this software without specific 21 | prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 24 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 25 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 27 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 28 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 30 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 31 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 32 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | """ 34 | 35 | import re 36 | import glob 37 | import numpy as np 38 | 39 | import cv2 40 | 41 | import rospy 42 | import cv_bridge 43 | import sensor_msgs.msg 44 | 45 | import camera_info_parser 46 | 47 | 48 | def natural_sort(l): 49 | convert = lambda text: int(text) if text.isdigit() else text.lower() 50 | alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ] 51 | return sorted(l, key = alphanum_key) 52 | 53 | 54 | def collect_image_files(image_dir,file_pattern): 55 | images = glob.glob(image_dir + '/' + str(file_pattern)) 56 | images = natural_sort(images) 57 | return images 58 | 59 | 60 | def playback_images(image_dir_l, image_dir_r, file_pattern, camera_info_file_l, camera_info_file_r, publish_rate): 61 | frame_id = "/camera" 62 | if camera_info_file_l != "": 63 | cam_info_l = camera_info_parser.parse_yaml(camera_info_file_l) 64 | frame_id = cam_info_l.header.frame_id 65 | publish_cam_info_l = True 66 | else: 67 | publish_cam_info_l = False 68 | if camera_info_file_r != "": 69 | cam_info_r = camera_info_parser.parse_yaml(camera_info_file_r) 70 | publish_cam_info_r = True 71 | else: 72 | publish_cam_info_r = False 73 | 74 | image_files_l = collect_image_files(image_dir_l, file_pattern) 75 | image_files_r = collect_image_files(image_dir_r, file_pattern) 76 | rospy.loginfo('Found %i left images.',len(image_files_l)) 77 | rospy.loginfo('Found %i right images.',len(image_files_r)) 78 | 79 | bridge = cv_bridge.CvBridge() 80 | rate = rospy.Rate(publish_rate) 81 | image_publisher_l = rospy.Publisher('stereo_camera/left/image_raw', sensor_msgs.msg.Image, queue_size = 5) 82 | image_publisher_r = rospy.Publisher('stereo_camera/right/image_raw', sensor_msgs.msg.Image, queue_size = 5) 83 | if publish_cam_info_l: 84 | cam_info_publisher_l = rospy.Publisher('stereo_camera/left/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5) 85 | if publish_cam_info_r: 86 | cam_info_publisher_r = rospy.Publisher('stereo_camera/right/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5) 87 | 88 | rospy.loginfo('Starting playback.') 89 | for image_file_l, image_file_r in zip(image_files_l, image_files_r): 90 | if rospy.is_shutdown(): 91 | break 92 | now = rospy.Time.now() 93 | image_l = cv2.LoadImage(image_file_l) 94 | image_r = cv2.LoadImage(image_file_r) 95 | image_msg = bridge.cv2_to_imgmsg(np.asarray(image_l[:,:]), encoding='bgr8') 96 | image_msg.header.stamp = now 97 | image_msg.header.frame_id = frame_id 98 | image_publisher_l.publish(image_msg) 99 | image_msg = bridge.cv2_to_imgmsg(np.asarray(image_r[:,:]), encoding='bgr8') 100 | image_msg.header.stamp = now 101 | image_msg.header.frame_id = frame_id 102 | image_publisher_r.publish(image_msg) 103 | if publish_cam_info_l: 104 | cam_info_l.header.stamp = now 105 | cam_info_publisher_l.publish(cam_info_l) 106 | if publish_cam_info_r: 107 | cam_info_r.header.stamp = now 108 | cam_info_r.header.frame_id = frame_id 109 | cam_info_publisher_r.publish(cam_info_r) 110 | rate.sleep() 111 | rospy.loginfo('No more images left. Stopping.') 112 | 113 | 114 | if __name__ == "__main__": 115 | rospy.init_node('image_sequence_publisher') 116 | try: 117 | image_dir_l = rospy.get_param("~image_dir_left") 118 | image_dir_r = rospy.get_param("~image_dir_right") 119 | file_pattern = rospy.get_param("~file_pattern") 120 | camera_info_file_l = rospy.get_param("~camera_info_file_left", "") 121 | camera_info_file_r = rospy.get_param("~camera_info_file_right", "") 122 | frequency = rospy.get_param("~frequency", 10) 123 | playback_images(image_dir_l, image_dir_r, file_pattern, camera_info_file_l, camera_info_file_r, frequency) 124 | except KeyError as e: 125 | rospy.logerr('Required parameter missing: %s', e) 126 | except Exception as e: 127 | import traceback 128 | traceback.print_exc() 129 | -------------------------------------------------------------------------------- /bag_tools/scripts/transform_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | Copyright (c) 2015, 4 | Clearpath Robotics, Inc. 5 | Enrique Fernandez Perdomo 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import numpy 33 | import argparse 34 | 35 | 36 | import rospy 37 | import rosbag 38 | import tf.transformations as tft 39 | from geometry_msgs.msg import Transform, Vector3, Quaternion 40 | 41 | 42 | def transform_vector3_msg_to_tf(msg): 43 | return tft.translation_matrix([msg.x, msg.y, msg.z]) 44 | 45 | 46 | def transform_tf_to_vector3_msg(tf): 47 | return Vector3(*tf) 48 | 49 | 50 | def transform_quaternion_msg_to_tf(msg): 51 | return tft.quaternion_matrix([msg.x, msg.y, msg.z, msg.w]) 52 | 53 | 54 | def transform_tf_to_quaternion_msg(tf): 55 | q = tft.quaternion_from_euler(*rotation) 56 | return Quaternion(*q) 57 | 58 | 59 | def transform_msg_to_tf(msg): 60 | translation = transform_vector3_msg_to_tf(msg.translation) 61 | rotation = transform_quaternion_msg_to_tf(msg.rotation) 62 | 63 | return numpy.dot(rotation, translation) 64 | 65 | 66 | def transform_tf_to_msg(tf): 67 | _, _, rotation, translation, _ = tft.decompose_matrix(tf) 68 | 69 | translation = transform_tf_to_vector3_msg(translation) 70 | rotation = transform_tf_to_quaternion_msg(rotation) 71 | 72 | return Transform(translation, rotation) 73 | 74 | 75 | def transform_tf(inbag, outbag, transform, frame_id, child_frame_id): 76 | rospy.loginfo(' Processing input bagfile: %s', inbag) 77 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 78 | rospy.loginfo(' Transforming TF: %s -> %s' % (frame_id, child_frame_id)) 79 | 80 | outbag = rosbag.Bag(outbag,'w') 81 | for topic, msg, t in rosbag.Bag(inbag, 'r').read_messages(): 82 | if topic == "/tf": 83 | new_transforms = [] 84 | for tf in msg.transforms: 85 | if tf.header.frame_id == frame_id and tf.child_frame_id == child_frame_id: 86 | tf_transform = transform_msg_to_tf(tf.transform) 87 | tf_transform = numpy.dot(tf_transform, transform) 88 | 89 | tf.transform = transform_tf_to_msg(tf_transform) 90 | new_transforms.append(tf) 91 | msg.transforms = new_transforms 92 | outbag.write(topic, msg, t) 93 | 94 | rospy.loginfo('Closing output bagfile and exit...') 95 | outbag.close() 96 | 97 | 98 | if __name__ == "__main__": 99 | parser = argparse.ArgumentParser( 100 | description='transform all transforms from the /tf topic that contain one of the given frame_ids in the header as parent.') 101 | 102 | parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile') 103 | parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile') 104 | parser.add_argument('-x', metavar='TRANSFORM_X', required=False, type=float, default=0.0, help='transform translation on x-axis [m]') 105 | parser.add_argument('-y', metavar='TRANSFORM_Y', required=False, type=float, default=0.0, help='transform translation on y-axis [m]') 106 | parser.add_argument('-z', metavar='TRANSFORM_Z', required=False, type=float, default=0.0, help='transform translation on z-axis [m]') 107 | parser.add_argument('-R', metavar='TRANSFORM_ROLL', required=False, type=float, default=0.0, help='transform rotation roll [deg]') 108 | parser.add_argument('-P', metavar='TRANSFORM_PITCH', required=False, type=float, default=0.0, help='transform rotation pitch [deg]') 109 | parser.add_argument('-Y', metavar='TRANSFORM_YAW', required=False, type=float, default=0.0, help='transform rotation yaw [deg]') 110 | parser.add_argument('-f', metavar='FRAME_ID', required=True, help='frame_id of the transform to change/transform from the /tf topic') 111 | parser.add_argument('-c', metavar='CHILD_FRAME_ID', required=True, help='child_frame_id of the transform to change/transform from the /tf topic') 112 | 113 | args = parser.parse_args() 114 | 115 | # Create transform: 116 | translation = numpy.array([args.x, args.y, args.z]) 117 | rotation = numpy.deg2rad([args.R, args.P, args.Y]) 118 | 119 | transform = tft.compose_matrix(angles=rotation, translate=translation) 120 | 121 | try: 122 | transform_tf(args.i, args.o, transform, args.f, args.c) 123 | except Exception as e: 124 | import traceback 125 | traceback.print_exc() 126 | -------------------------------------------------------------------------------- /bag_tools/shell_scripts/change_calib.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Set your camera calibration files 4 | L_CALIB="/data/HD3/bagfiles/turbot/2016_06_13_sant_feliu/l_calibration_water_bags.yaml" 5 | R_CALIB="/data/HD3/bagfiles/turbot/2016_06_13_sant_feliu/r_calibration_water_bags.yaml" 6 | 7 | # Set the directories where the bagfiles are 8 | DIRS="13 14 15 16 17" 9 | 10 | 11 | 12 | for d in $DIRS 13 | do 14 | echo "---- DIRECTORY $d ----" 15 | 16 | mkdir corrected 17 | FILES=*.bag 18 | for f in $FILES 19 | do 20 | echo "Changing bag: $f" 21 | rosrun bag_tools change_camera_info.py $f corrected/$f /stereo_down/left/camera_info=$L_CALIB /stereo_down/right/camera_info=$R_CALIB 22 | done 23 | 24 | done -------------------------------------------------------------------------------- /bag_tools/src/extract_images.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include // for cv::imwrite 41 | 42 | #include "bag_tools/image_bag_processor.h" 43 | #include 44 | /** 45 | * Saves color images from raw input 46 | */ 47 | class ImageSaver 48 | { 49 | public: 50 | ImageSaver(const std::string& save_dir, const std::string& filetype, 51 | const std::string& prefix) : 52 | save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0) 53 | {} 54 | 55 | void save(const sensor_msgs::ImageConstPtr &img) 56 | { 57 | image_proc::ImageSet output; 58 | image_geometry::PinholeCameraModel camera_model; // empty, we don't need it here 59 | if (!processor_.process(img, camera_model, output, image_proc::Processor::COLOR)) 60 | { 61 | std::cerr << "ERROR Processing image" << std::endl; 62 | return; 63 | } 64 | save_image(img->header.stamp.toNSec(), output.color); 65 | } 66 | void save_compressed(const sensor_msgs::CompressedImageConstPtr& _compressed) 67 | { 68 | cv::Mat image_uncomrpessed = cv::imdecode(cv::Mat(_compressed->data),1); 69 | save_image(_compressed->header.stamp.toNSec(), image_uncomrpessed); 70 | } 71 | 72 | private: 73 | void save_image(uint64_t _time_stamp, const cv::Mat& _image) 74 | { 75 | std::string filename = 76 | boost::str(boost::format("%s/%s%06lu_%lu.%s") 77 | % save_dir_ 78 | % prefix_ 79 | % num_saved_ 80 | % _time_stamp 81 | % filetype_); 82 | if (!cv::imwrite(filename, _image)) 83 | { 84 | ROS_ERROR_STREAM("ERROR Saving " << filename); 85 | } 86 | else 87 | { 88 | ROS_DEBUG_STREAM("Saved " << filename); 89 | num_saved_++; 90 | } 91 | } 92 | 93 | image_proc::Processor processor_; 94 | std::string save_dir_; 95 | std::string filetype_; 96 | std::string prefix_; 97 | int num_saved_; 98 | }; 99 | 100 | int main(int argc, char** argv) 101 | { 102 | if (argc < 4) 103 | { 104 | std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE IMAGE_TOPIC BAGFILE [BAGFILE...]" << std::endl; 105 | std::cout << " Example: " << argv[0] << " /tmp jpg /stereo_down/left/image_raw bag1.bag bag2.bag" << std::endl; 106 | return 0; 107 | } 108 | 109 | std::string out_dir(argv[1]); 110 | std::string filetype(argv[2]); 111 | std::string image_topic(argv[3]); 112 | 113 | ros::Time::init(); 114 | 115 | std::string prefix("image"); 116 | 117 | ImageSaver saver(out_dir, filetype, prefix); 118 | bag_tools::ImageBagProcessor processor(image_topic); 119 | processor.registerCallback(boost::bind(&ImageSaver::save, saver, _1)); 120 | processor.registerCompressedCallback(boost::bind(&ImageSaver::save_compressed, saver, _1)); 121 | 122 | for (int i = 4; i < argc; ++i) 123 | processor.processBag(argv[i]); 124 | 125 | return 0; 126 | } 127 | 128 | -------------------------------------------------------------------------------- /bag_tools/src/extract_images_from_realsense.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include // for cv::imwrite 41 | 42 | #include 43 | 44 | #include "bag_tools/image_bag_processor.h" 45 | #include 46 | /** 47 | * Saves color images from raw input 48 | */ 49 | class ImageSaver 50 | { 51 | public: 52 | ImageSaver(const std::string& save_dir, const std::string& filetype, 53 | const std::string& prefix) : 54 | save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0) 55 | {} 56 | 57 | void save(const sensor_msgs::ImageConstPtr &img) 58 | { 59 | 60 | cv_bridge::CvImagePtr cv_ptr; 61 | 62 | if (img->encoding == "16UC1") 63 | { 64 | try 65 | { 66 | cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_16UC1); 67 | } 68 | catch (cv_bridge::Exception& e) 69 | { 70 | ROS_ERROR("cv_bridge exception: %s", e.what()); 71 | return; 72 | } 73 | cv_ptr->image.convertTo(cv_ptr->image, CV_8U, 255.0 / 4096.0); 74 | save_image(img->header.stamp.toNSec(), cv_ptr->image); 75 | } 76 | 77 | else if (img->encoding == "rgb8") 78 | { 79 | try 80 | { 81 | cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_8UC3); 82 | } 83 | catch (cv_bridge::Exception& e) 84 | { 85 | ROS_ERROR("cv_bridge exception: %s", e.what()); 86 | return; 87 | } 88 | cv::cvtColor( cv_ptr->image, cv_ptr->image, cv::COLOR_BGR2RGB ); 89 | save_image(img->header.stamp.toNSec(), cv_ptr->image); 90 | } 91 | 92 | else 93 | { 94 | image_proc::ImageSet output; 95 | image_geometry::PinholeCameraModel camera_model; // empty, we don't need it here 96 | if (!processor_.process(img, camera_model, output, image_proc::Processor::COLOR)) 97 | { 98 | std::cerr << "ERROR Processing image" << std::endl; 99 | return; 100 | } 101 | save_image(img->header.stamp.toNSec(), output.color); 102 | 103 | } 104 | 105 | } 106 | void save_compressed(const sensor_msgs::CompressedImageConstPtr& _compressed) 107 | { 108 | cv::Mat image_uncomrpessed = cv::imdecode(cv::Mat(_compressed->data),1); 109 | save_image(_compressed->header.stamp.toNSec(), image_uncomrpessed); 110 | } 111 | 112 | private: 113 | void save_image(uint64_t _time_stamp, const cv::Mat& _image) 114 | { 115 | std::string filename = 116 | boost::str(boost::format("%s/%lu.%s") 117 | % save_dir_ 118 | // % prefix_ 119 | // % num_saved_ 120 | % _time_stamp 121 | % filetype_); 122 | if (!cv::imwrite(filename, _image)) 123 | { 124 | ROS_ERROR_STREAM("ERROR Saving " << filename); 125 | } 126 | else 127 | { 128 | ROS_DEBUG_STREAM("Saved " << filename); 129 | num_saved_++; 130 | } 131 | } 132 | 133 | image_proc::Processor processor_; 134 | std::string save_dir_; 135 | std::string filetype_; 136 | std::string prefix_; 137 | int num_saved_; 138 | }; 139 | 140 | int main(int argc, char** argv) 141 | { 142 | if (argc < 4) 143 | { 144 | std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE IMAGE_TOPIC BAGFILE [BAGFILE...]" << std::endl; 145 | std::cout << " Example: " << argv[0] << " /tmp jpg /stereo_down/left/image_raw bag1.bag bag2.bag" << std::endl; 146 | return 0; 147 | } 148 | 149 | std::string out_dir(argv[1]); 150 | std::string filetype(argv[2]); 151 | std::string image_topic(argv[3]); 152 | 153 | ros::Time::init(); 154 | 155 | std::string prefix("image"); 156 | 157 | ImageSaver saver(out_dir, filetype, prefix); 158 | bag_tools::ImageBagProcessor processor(image_topic); 159 | processor.registerCallback(boost::bind(&ImageSaver::save, saver, _1)); 160 | processor.registerCompressedCallback(boost::bind(&ImageSaver::save_compressed, saver, _1)); 161 | 162 | for (int i = 4; i < argc; ++i) 163 | processor.processBag(argv[i]); 164 | 165 | return 0; 166 | } 167 | 168 | -------------------------------------------------------------------------------- /bag_tools/src/extract_stereo_images.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include // for cv::imwrite 40 | 41 | #include 42 | 43 | /** 44 | * Saves rectified color images from raw and camera info input 45 | */ 46 | class ImageSaver 47 | { 48 | public: 49 | ImageSaver(const std::string& save_dir, const std::string& filetype, 50 | const std::string& prefix) : 51 | save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0) 52 | {} 53 | 54 | void save(const sensor_msgs::Image::ConstPtr &img, 55 | const sensor_msgs::CameraInfo::ConstPtr &info) 56 | { 57 | camera_model_.fromCameraInfo(info); 58 | std::string calib_filename_ini = 59 | boost::str(boost::format("%s/calibration_%scamera.ini") 60 | % save_dir_ % prefix_ ); 61 | std::string calib_filename_yaml = 62 | boost::str(boost::format("%s/calibration_%scamera.yaml") 63 | % save_dir_ % prefix_ ); 64 | camera_calibration_parsers::writeCalibration(calib_filename_yaml, prefix_ + "camera", *info); 65 | image_proc::ImageSet output; 66 | if (!processor_.process(img, camera_model_, output, image_proc::Processor::RECT_COLOR)) 67 | { 68 | std::cerr << "ERROR Processing image" << std::endl; 69 | return; 70 | } 71 | std::string filename = 72 | boost::str(boost::format("%s/%s%lu.%s") 73 | % save_dir_ 74 | % prefix_ 75 | % img->header.stamp.toNSec() 76 | % filetype_); 77 | if (!cv::imwrite(filename, output.rect_color)) 78 | ROS_ERROR_STREAM("ERROR Saving " << filename); 79 | else 80 | { 81 | ROS_DEBUG_STREAM("Saved " << filename); 82 | num_saved_++; 83 | } 84 | } 85 | 86 | private: 87 | 88 | image_proc::Processor processor_; 89 | image_geometry::PinholeCameraModel camera_model_; 90 | std::string save_dir_; 91 | std::string filetype_; 92 | std::string prefix_; 93 | int num_saved_; 94 | 95 | }; 96 | 97 | class StereoImageSaver 98 | { 99 | public: 100 | StereoImageSaver(const std::string& save_dir, const std::string& filetype) : 101 | l_saver_(save_dir, filetype, "left_"), 102 | r_saver_(save_dir, filetype, "right_") 103 | {} 104 | 105 | void save(const sensor_msgs::Image::ConstPtr &l_img, 106 | const sensor_msgs::Image::ConstPtr &r_img, 107 | const sensor_msgs::CameraInfo::ConstPtr &l_info, 108 | const sensor_msgs::CameraInfo::ConstPtr &r_info) 109 | { 110 | l_saver_.save(l_img, l_info); 111 | r_saver_.save(r_img, r_info); 112 | } 113 | 114 | private: 115 | 116 | ImageSaver l_saver_, r_saver_; 117 | 118 | }; 119 | 120 | int main(int argc, char** argv) 121 | { 122 | if (argc < 4) 123 | { 124 | std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE STEREO_BASE_TOPIC BAGFILE [BAGFILE...]" << std::endl; 125 | std::cout << " Example: " << argv[0] << " /tmp jpg /stereo_down bag1.bag bag2.bag" << std::endl; 126 | return 0; 127 | } 128 | 129 | std::string out_dir(argv[1]); 130 | std::string filetype(argv[2]); 131 | std::string base_topic(argv[3]); 132 | 133 | ros::Time::init(); 134 | 135 | StereoImageSaver saver(out_dir, filetype); 136 | bag_tools::StereoBagProcessor processor(base_topic); 137 | processor.registerCallback(boost::bind(&StereoImageSaver::save, saver, _1, _2, _3, _4)); 138 | 139 | for (int i = 4; i < argc; ++i) 140 | processor.processBag(argv[i]); 141 | 142 | return 0; 143 | } 144 | 145 | -------------------------------------------------------------------------------- /bag_tools/src/extract_tum_format.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include // for cv::imwrite 42 | 43 | #include 44 | 45 | #include 46 | #include 47 | using namespace std; 48 | /** 49 | * Saves rectified color images from raw and camera info input 50 | */ 51 | class ImageSaver 52 | { 53 | public: 54 | 55 | ImageSaver(const std::string& save_dir, const std::string& filetype, 56 | const std::string& prefix) : 57 | save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0) 58 | {} 59 | 60 | void save(const sensor_msgs::Image::ConstPtr &img, 61 | const sensor_msgs::CameraInfo::ConstPtr &info) 62 | { 63 | 64 | cv_bridge::CvImagePtr cv_ptr; 65 | std::string save_dir; 66 | if (img->encoding == "16UC1") 67 | { 68 | try 69 | { 70 | cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_16UC1); 71 | } 72 | catch (cv_bridge::Exception& e) 73 | { 74 | ROS_ERROR("cv_bridge exception: %s", e.what()); 75 | return; 76 | } 77 | cv_ptr->image.convertTo(cv_ptr->image, CV_8U, 255.0 / 4096.0); 78 | 79 | save_dir = save_dir_ +"/depth"; 80 | } 81 | 82 | else if (img->encoding == "rgb8") 83 | { 84 | try 85 | { 86 | cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_8UC3); 87 | } 88 | catch (cv_bridge::Exception& e) 89 | { 90 | ROS_ERROR("cv_bridge exception: %s", e.what()); 91 | return; 92 | } 93 | cv::cvtColor( cv_ptr->image, cv_ptr->image, cv::COLOR_BGR2RGB ); 94 | save_dir = save_dir_ + "/rgb"; 95 | } 96 | 97 | else 98 | { 99 | image_proc::ImageSet output; 100 | image_geometry::PinholeCameraModel camera_model; // empty, we don't need it here 101 | if (!processor_.process(img, camera_model, output, image_proc::Processor::COLOR)) 102 | { 103 | std::cerr << "ERROR Processing image" << std::endl; 104 | return; 105 | } 106 | } 107 | 108 | std::string filename = 109 | boost::str(boost::format("%s/%lu.%s") 110 | % save_dir 111 | % img->header.stamp.toNSec() 112 | // % prefix_ 113 | % filetype_); 114 | if (!cv::imwrite(filename, cv_ptr->image)) 115 | ROS_ERROR_STREAM("ERROR Saving " << filename); 116 | else 117 | { 118 | ROS_DEBUG_STREAM("Saved " << filename); 119 | num_saved_++; 120 | } 121 | } 122 | 123 | private: 124 | 125 | image_proc::Processor processor_; 126 | image_geometry::PinholeCameraModel camera_model_; 127 | std::string save_dir_; 128 | std::string filetype_; 129 | std::string prefix_; 130 | int num_saved_; 131 | }; 132 | 133 | class TUMImageSaver 134 | { 135 | public: 136 | TUMImageSaver(const std::string& save_dir, const std::string& filetype) : 137 | depth_saver_(save_dir, filetype, "depth_"), 138 | color_saver_(save_dir, filetype, "color_"), num_img_(1), save_dir_(save_dir + "/associate.txt" ) 139 | {} 140 | 141 | void save(const sensor_msgs::Image::ConstPtr &depth_img, 142 | const sensor_msgs::Image::ConstPtr &color_img, 143 | const sensor_msgs::CameraInfo::ConstPtr &depth_info, 144 | const sensor_msgs::CameraInfo::ConstPtr &color_info) 145 | { 146 | depth_saver_.save(depth_img, depth_info); 147 | color_saver_.save(color_img, color_info); 148 | 149 | std::ofstream outputFile_; 150 | outputFile_.open(save_dir_.c_str(), std::ios::app); 151 | outputFile_ << num_img_ << "\t"; 152 | outputFile_ << "rgb/" << color_img->header.stamp.toNSec() << ".png"<<"\t"; 153 | outputFile_ << num_img_ << "\t"; 154 | outputFile_ << "depth/" << depth_img->header.stamp.toNSec() << ".png"<< "\n"; 155 | 156 | 157 | outputFile_.close(); 158 | num_img_++; 159 | } 160 | 161 | private: 162 | 163 | ImageSaver depth_saver_, color_saver_; 164 | int num_img_; 165 | std::string save_dir_; 166 | 167 | }; 168 | 169 | int main(int argc, char** argv) 170 | { 171 | if (argc < 4) 172 | { 173 | std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE STEREO_BASE_TOPIC BAGFILE [BAGFILE...]" << std::endl; 174 | std::cout << " Example: " << argv[0] << " /tmp png /camera bag1.bag bag2.bag" << std::endl; 175 | return 0; 176 | } 177 | 178 | std::string out_dir(argv[1]); 179 | std::string filetype(argv[2]); 180 | std::string base_topic(argv[3]); 181 | 182 | std::string out_txt_file = out_dir + "/associate.txt" ; 183 | 184 | // Create a folder if doesn't exist 185 | boost::filesystem::path dir(out_dir); 186 | 187 | if (!(boost::filesystem::exists(dir))) { 188 | std::cout << dir << " Doesn't Exists" << std::endl; 189 | 190 | if (boost::filesystem::create_directory(dir)) 191 | std::cout << ".... Folder Successfully Created !" << std::endl; 192 | } 193 | 194 | //-- If associate file exist, clear the file data 195 | std::ifstream outputFile_(out_txt_file.c_str()); 196 | if (outputFile_.good()) { 197 | std::cout << " Clearing data of Associations" << std::endl; 198 | std::ofstream outputFile_(out_txt_file.c_str()); 199 | outputFile_.close( ); 200 | } 201 | 202 | // Create two folders 203 | boost::filesystem::path dir_rgb(out_dir + "/rgb"); 204 | boost::filesystem::path dir_depth(out_dir + "/depth"); 205 | 206 | 207 | if (!(boost::filesystem::exists(dir_rgb))) { 208 | std::cout << "Doesn't Exists" << std::endl; 209 | 210 | if (boost::filesystem::create_directory(dir_rgb)) 211 | if (boost::filesystem::create_directory(dir_depth)) 212 | std::cout << "....Successfully Created RGB and Depth folders !" << std::endl; 213 | } 214 | else 215 | { 216 | std::cerr << " ERROR: This file has already data inside, please select another folder." << std::endl; 217 | return 0; 218 | } 219 | 220 | ros::Time::init(); 221 | 222 | TUMImageSaver saver(out_dir, filetype); 223 | bag_tools::TUMBagProcessor processor(base_topic); 224 | processor.registerCallback(boost::bind(&TUMImageSaver::save, saver, _1, _2, _3, _4)); 225 | 226 | for (int i = 4; i < argc; ++i) 227 | processor.processBag(argv[i]); 228 | 229 | return 0; 230 | } 231 | 232 | -------------------------------------------------------------------------------- /bag_tools/src/process_stereo.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | class StereoImageProcessor 40 | { 41 | public: 42 | StereoImageProcessor(const std::string& base_topic, const std::string& out_bag, int processing_flags) : 43 | stereo_base_topic_(base_topic), 44 | flags_(processing_flags) 45 | { 46 | std::cout << "Opening bagfile '" << out_bag << "' for writing." << std::endl; 47 | bag_.open(out_bag, rosbag::bagmode::Write); 48 | } 49 | 50 | ~StereoImageProcessor() 51 | { 52 | std::cout << "Closing bagfile." << std::endl; 53 | bag_.close(); 54 | } 55 | 56 | void process(const sensor_msgs::Image::ConstPtr &l_img, 57 | const sensor_msgs::Image::ConstPtr &r_img, 58 | const sensor_msgs::CameraInfo::ConstPtr &l_info, 59 | const sensor_msgs::CameraInfo::ConstPtr &r_info) 60 | { 61 | model_.fromCameraInfo(l_info, r_info); 62 | stereo_image_proc::StereoImageSet image_set; 63 | processor_.process(l_img, r_img, model_, image_set, flags_); 64 | 65 | bag_.write(stereo_base_topic_ + "/left/camera_info", 66 | l_info->header.stamp, l_info); 67 | bag_.write(stereo_base_topic_ + "/right/camera_info", 68 | r_info->header.stamp, r_info); 69 | 70 | if (flags_ & stereo_image_proc::StereoProcessor::LEFT_MONO) 71 | { 72 | sensor_msgs::Image::Ptr msg = createMsg( 73 | l_img->header, 74 | image_set.left.color_encoding, 75 | image_set.left.mono); 76 | bag_.write(stereo_base_topic_ + "/left/image_mono", 77 | l_img->header.stamp, msg); 78 | } 79 | if (flags_ & stereo_image_proc::StereoProcessor::LEFT_COLOR) 80 | { 81 | sensor_msgs::Image::Ptr msg = createMsg( 82 | l_img->header, 83 | image_set.left.color_encoding, 84 | image_set.left.color); 85 | bag_.write(stereo_base_topic_ + "/left/image_color", 86 | l_img->header.stamp, msg); 87 | } 88 | if (flags_ & stereo_image_proc::StereoProcessor::LEFT_RECT) 89 | { 90 | sensor_msgs::Image::Ptr msg = createMsg( 91 | l_img->header, 92 | image_set.left.color_encoding, 93 | image_set.left.rect); 94 | bag_.write(stereo_base_topic_ + "/left/image_rect", 95 | l_img->header.stamp, msg); 96 | } 97 | if (flags_ & stereo_image_proc::StereoProcessor::LEFT_RECT_COLOR) 98 | { 99 | sensor_msgs::Image::Ptr msg = createMsg( 100 | l_img->header, 101 | image_set.left.color_encoding, 102 | image_set.left.rect_color); 103 | bag_.write(stereo_base_topic_ + "/left/image_rect_color", 104 | l_img->header.stamp, msg); 105 | } 106 | if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_MONO) 107 | { 108 | sensor_msgs::Image::Ptr msg = createMsg( 109 | r_img->header, 110 | image_set.right.color_encoding, 111 | image_set.right.mono); 112 | bag_.write(stereo_base_topic_ + "/right/image_mono", 113 | r_img->header.stamp, msg); 114 | } 115 | if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_COLOR) 116 | { 117 | sensor_msgs::Image::Ptr msg = createMsg( 118 | r_img->header, 119 | image_set.right.color_encoding, 120 | image_set.right.color); 121 | bag_.write(stereo_base_topic_ + "/right/image_color", 122 | r_img->header.stamp, msg); 123 | } 124 | if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_RECT) 125 | { 126 | sensor_msgs::Image::Ptr msg = createMsg( 127 | r_img->header, 128 | image_set.right.color_encoding, 129 | image_set.right.rect); 130 | bag_.write(stereo_base_topic_ + "/right/image_rect", 131 | r_img->header.stamp, msg); 132 | } 133 | if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_RECT_COLOR) 134 | { 135 | sensor_msgs::Image::Ptr msg = createMsg( 136 | r_img->header, 137 | image_set.right.color_encoding, 138 | image_set.right.rect_color); 139 | bag_.write(stereo_base_topic_ + "/right/image_rect_color", 140 | r_img->header.stamp, msg); 141 | } 142 | 143 | if (flags_ & stereo_image_proc::StereoProcessor::POINT_CLOUD2) 144 | { 145 | bag_.write(stereo_base_topic_ + "/points2", 146 | l_img->header.stamp, image_set.points2); 147 | } 148 | 149 | } 150 | 151 | sensor_msgs::Image::Ptr createMsg( 152 | const std_msgs::Header& header, 153 | const std::string& encoding, 154 | const cv::Mat& img) 155 | { 156 | cv_bridge::CvImage cv_image; 157 | cv_image.header = header; 158 | cv_image.encoding = encoding; 159 | cv_image.image = img; 160 | return cv_image.toImageMsg(); 161 | } 162 | 163 | private: 164 | 165 | rosbag::Bag bag_; 166 | std::string stereo_base_topic_; 167 | int flags_; 168 | image_geometry::StereoCameraModel model_; 169 | stereo_image_proc::StereoProcessor processor_; 170 | 171 | }; 172 | 173 | int main(int argc, char** argv) 174 | { 175 | if (argc < 4) 176 | { 177 | std::cout << "Takes raw images from an input bagfile, processes them and writes the output to a new bagfile." << std::endl; 178 | std::cout << "Usage: " << argv[0] << " INBAG STEREO_BASE_TOPIC OUTBAG" << std::endl; 179 | std::cout << " Example: " << argv[0] << " bag.bag /stereo_forward bag_processed.bag" << std::endl; 180 | return 0; 181 | } 182 | 183 | std::string in_bag(argv[1]); 184 | std::string base_topic(argv[2]); 185 | std::string out_bag(argv[3]); 186 | 187 | int flags = 0; 188 | flags |= stereo_image_proc::StereoProcessor::LEFT_RECT_COLOR; 189 | flags |= stereo_image_proc::StereoProcessor::RIGHT_RECT_COLOR; 190 | flags |= stereo_image_proc::StereoProcessor::POINT_CLOUD2; 191 | StereoImageProcessor image_processor(base_topic, out_bag, flags); 192 | bag_tools::StereoBagProcessor bag_processor(base_topic); 193 | bag_processor.registerCallback(boost::bind(&StereoImageProcessor::process, &image_processor, _1, _2, _3, _4)); 194 | 195 | bag_processor.processBag(in_bag); 196 | 197 | return 0; 198 | } 199 | 200 | -------------------------------------------------------------------------------- /launch_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package launch_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Updated to Noetic. 8 | 9 | 0.0.3 (2017-02-24) 10 | ------------------ 11 | * Fix release 12 | * Contributors: Miquel Massot 13 | 14 | 0.0.2 (2017-02-23) 15 | ------------------ 16 | 17 | * preparing for indigo. changed prints for ros logs 18 | * added gps_to_std_gt and services_timer from fuerte 19 | * hydro catkinization changes 20 | * catched exception on xml parse error 21 | * added python setup files and wet'ed plot tools 22 | * wet repo 23 | * added params for stdout and stderr files 24 | * new node that executes a command 25 | * changed manifests 26 | * added BSD license 27 | * added launchViz and launch_tools package 28 | * Contributors: Miquel Massot, Stephan Wirth 29 | -------------------------------------------------------------------------------- /launch_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(launch_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rospy roslib) 5 | 6 | catkin_package() 7 | 8 | catkin_python_setup() 9 | 10 | install(PROGRAMS 11 | scripts/launch_tools/launchViz.py 12 | scripts/launch_tools/services_timer.py 13 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 14 | ) -------------------------------------------------------------------------------- /launch_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b launch_tools 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /launch_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | launch_tools 4 | 0.0.4 5 | ROS tools and scripts related to launchfiles 6 | Bo Miquel Nordfeldt-Fiol 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/launch_tools 11 | 12 | Stephan Wirth 13 | Miquel Massot 14 | 15 | catkin 16 | 17 | rospy 18 | roslib 19 | 20 | rospy 21 | python-xdot 22 | roslib 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch_tools/scripts/launch_tools/launchViz.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | 33 | try: 34 | import os 35 | import sys 36 | #import subprocess 37 | import argparse 38 | import gtk 39 | import xdot 40 | import StringIO 41 | import fnmatch 42 | from xml.etree import ElementTree 43 | import roslib.packages 44 | except ImportError: 45 | # Checks the installation of the necessary python modules 46 | print((os.linesep * 2).join(["An error found importing one module:", 47 | str(sys.exc_info()[1]), "You need to install it", "Stopping..."])) 48 | sys.exit(-2) 49 | 50 | def find_launchfiles(directory, pattern='*.launch'): 51 | for root, dirs, files in os.walk(directory): 52 | for basename in files: 53 | if fnmatch.fnmatch(basename, pattern): 54 | filename = os.path.join(root, basename) 55 | yield root, basename, filename 56 | 57 | def check_launchfiles(directory, pattern='*.launch'): 58 | for root, dirs, files in os.walk(directory): 59 | for basename in files: 60 | if fnmatch.fnmatch(basename, pattern): 61 | return True; 62 | return False; 63 | 64 | def prepare_file(): 65 | g = StringIO.StringIO() 66 | g.write('digraph G {\n rankdir=LR \n') 67 | return g 68 | 69 | def show_graph(g,package = 0): 70 | drawable = False 71 | g.write('}') 72 | if len(g.getvalue())>26: 73 | drawable = True 74 | window = xdot.DotWindow() 75 | window.set_dotcode(g.getvalue()) 76 | if package: 77 | window.set_title('Package {0}'.format(package)) 78 | window.connect('destroy', gtk.main_quit) 79 | g.close() 80 | return drawable 81 | 82 | def start_subgraph(g,boxname): 83 | g.write(' subgraph "{0}"'.format(boxname)) 84 | g.write(' {\n') 85 | g.write(' label = "{0}";\n'.format(boxname)) 86 | 87 | def write_information(g,node_name,node_args=0): 88 | if node_args==0: 89 | for name in node_name: 90 | g.write('"{0}_node" [shape=Mrecord,label="{0}",style="filled",fillcolor="khaki"]\n'.format(name)) 91 | else: 92 | g.write('"{0}" [shape=none, margin=0, label=<\n'.format(node_name)) 93 | g.write('\n') 94 | g.write('\n'.format(node_name)) 95 | if node_args: 96 | g.write('\n') 97 | for (arg, arg_def) in node_args: 98 | g.write('\n'.format(arg,arg_def)) 99 | g.write('
{0}
Arguments:
{0} = {1}
>];\n') 100 | 101 | def write_connection(g,origin,destination, isNode=False): 102 | if not isNode: 103 | g.write(' "{0}" -> "{1}"\n'.format(origin,destination)) 104 | elif isNode: 105 | g.write(' "{0}" -> "{1}_node"\n'.format(origin,destination)) 106 | 107 | def get_args_from_parser(): 108 | parser = argparse.ArgumentParser(description='Draws graph from launchfiles') 109 | parser.add_argument('--pkg', metavar='package', help='ROS packages you want to inspect',nargs='+') 110 | return parser.parse_args() 111 | 112 | def draw_folder(g,folder): 113 | working_directory = folder 114 | if check_launchfiles(folder): 115 | for root,filename,filepath in find_launchfiles(folder): 116 | if root != working_directory: 117 | working_directory = root 118 | f = open(filepath,'rt') 119 | try: 120 | tree = ElementTree.parse(f) 121 | node_name = os.path.splitext(filename)[0] #remove extension 122 | it = tree.iter() 123 | #Iterate 124 | node_args = [] 125 | node_includes = [] 126 | node_nodes = [] 127 | for node in it: 128 | if node.tag == "arg": 129 | #save arguments and default values 130 | arg_name = node.attrib.get('name') 131 | default_value = node.attrib.get('default') 132 | value = node.attrib.get('value') 133 | if_clause = node.attrib.get('if') 134 | if default_value: 135 | #add to the node_args list with default 136 | node_args.append((arg_name,default_value)) 137 | elif not value: 138 | #add to the node_args list with REQUIRED 139 | node_args.append((arg_name,"REQUIRED")) 140 | elif if_clause: 141 | nodelet_args = value.split()[1] 142 | if node.tag == "include": 143 | #remove extension and path to file 144 | incl_ext = os.path.basename(node.attrib.get('file')) 145 | incl = os.path.splitext(incl_ext)[0] 146 | node_includes.append(incl) 147 | if node.tag == "node": 148 | node_type = node.attrib.get('type') 149 | if node_type == "nodelet": 150 | node_type = node.attrib.get('args') 151 | if len(node_type.split())>1: 152 | node_type = node_type.split()[1] 153 | if node_type[len(node_type)-1] == ")": 154 | node_type = nodelet_args 155 | node_nodes.append(node_type) 156 | else: 157 | node_nodes.append(node_type) 158 | 159 | write_information(g,node_name,node_args) 160 | write_information(g,node_nodes) 161 | for dest in node_nodes: 162 | write_connection(g,node_name,dest,True) 163 | for dest in node_includes: 164 | write_connection(g,node_name,dest) 165 | except ElementTree.ParseError: 166 | rospy.loginfo("Error parsing {}".format(filepath)) 167 | return g 168 | else: 169 | rospy.loginfo("[W]\tNo launchfile was found in {0}".format(os.path.basename(folder))) 170 | 171 | def xml_process(packages = 0): 172 | g = prepare_file() 173 | if not packages: 174 | working_directory = os.getcwd(); 175 | draw_folder(g,working_directory) 176 | else: 177 | for pkg in packages: 178 | directory = roslib.packages.get_pkg_dir(pkg) 179 | draw_folder(g,directory) 180 | return g 181 | 182 | def main(): 183 | args = get_args_from_parser() 184 | g = xml_process(args.pkg) 185 | if show_graph(g): 186 | gtk.main() 187 | 188 | if __name__ == "__main__": 189 | main() 190 | -------------------------------------------------------------------------------- /launch_tools/scripts/launch_tools/services_timer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2013, 5 | Systems, Robotics and Vision Group 6 | University of the Balearican Islands 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | * Neither the name of Systems, Robotics and Vision Group, University of 17 | the Balearican Islands nor the names of its contributors may be used to 18 | endorse or promote products derived from this software without specific 19 | prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 22 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 23 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 25 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 28 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | """ 32 | 33 | 34 | 35 | import roslib; roslib.load_manifest('launch_tools') 36 | import sys 37 | import rospy 38 | import rosservice 39 | import threading 40 | 41 | 42 | ## Class for calling a service using a timer. 43 | class TimedService(threading.Thread): 44 | 45 | ## The constructor 46 | # @param self The object pointer. 47 | # @param name The service name this class is going to call 48 | # @param freq The desired timer period 49 | def __init__(self, name, period): 50 | threading.Thread.__init__(self) 51 | self._service_name = name 52 | self._service_period = period 53 | 54 | ## Run function required by threading library 55 | def run(self): 56 | rospy.wait_for_service(self._service_name) 57 | rospy.Timer(rospy.Duration(self._service_period), self.callback) 58 | rospy.loginfo('Initialized timer for service: \n\t* Name: %s\n\t* Period: %f ', self._service_name, self._service_period) 59 | 60 | ## Timer callback 61 | # @param event The event that has generated this callback 62 | def callback(self,event): 63 | rospy.wait_for_service(self._service_name) 64 | service_class = rosservice.get_service_class_by_name(self._service_name) 65 | try: 66 | service = rospy.ServiceProxy(self._service_name,service_class) 67 | service() 68 | rospy.loginfo('Service %s called.', self._service_name) 69 | except rospy.ServiceException, e: 70 | rospy.logwarn('Service %s call failed: %s',self._service_name,e) 71 | 72 | ## @var _service_name 73 | # The service name going to be called 74 | _service_name = "service" 75 | 76 | ## @var _service_period 77 | # The timer period to call the service 78 | _service_period = 1.0 79 | 80 | ## Print usage for people that does not deserve to use this awesome python node. 81 | def usage(): 82 | return "%s service period [service period ...]"%sys.argv[0] 83 | 84 | ## main function 85 | if __name__ == "__main__": 86 | rospy.init_node('services_timer') 87 | if len(sys.argv) >= 3: 88 | names = sys.argv[1:len(sys.argv):2] 89 | periods = sys.argv[2:len(sys.argv):2] 90 | rospy.loginfo('names : %s', names) 91 | rospy.loginfo('periods : %s', periods) 92 | ts_list = [] 93 | for name,period in zip(names,periods): 94 | ts_list.append(TimedService(str(name), float(period))) 95 | for ts in ts_list: 96 | ts.start() 97 | else: 98 | rospy.loginfo(usage()) 99 | sys.exit(1) 100 | rospy.spin() -------------------------------------------------------------------------------- /launch_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | ## don't do this unless you want a globally visible script 8 | # scripts=['bin/myscript'], 9 | packages=['launch_tools'], 10 | package_dir={'': 'scripts'} 11 | ) 12 | 13 | setup(**d) 14 | -------------------------------------------------------------------------------- /launch_tools/src/executer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | PKG = 'launch_tools' # this package name 4 | 5 | import roslib; roslib.load_manifest(PKG) 6 | import rospy 7 | import subprocess 8 | import signal 9 | import shlex 10 | import time 11 | 12 | class Executer(): 13 | """ 14 | Simple class for executing a command in a subprocess. 15 | """ 16 | def __init__(self, command, stdout, stderr): 17 | self.command = shlex.split(command) 18 | self.stdout = stdout 19 | self.stderr = stderr 20 | def execute(self): 21 | rospy.loginfo("Launching process: \"%s\"...", " ".join(self.command)) 22 | rospy.loginfo("stdout/stderr: %s/%s", str(self.stdout), str(self.stderr)) 23 | self.process = subprocess.Popen(self.command, stdout=self.stdout, stderr=self.stderr) 24 | rospy.loginfo("Process launched.") 25 | def interrupt(self): 26 | if self.process.poll() is None: # check if process is still running 27 | rospy.loginfo("Terminating process...") 28 | self.process.terminate() 29 | waiting_seconds = 0 30 | while self.process.poll() is None and waiting_seconds < 5: 31 | rospy.loginfo("Waiting for process to finish...") 32 | time.sleep(1) 33 | waiting_seconds = waiting_seconds + 1 34 | if self.process.poll() is None: 35 | rospy.loginfo("Killing process...") 36 | self.process.kill() 37 | self.process.wait() 38 | rospy.loginfo("Process has stopped with return code %i", self.process.returncode) 39 | 40 | if __name__ == "__main__": 41 | rospy.init_node('executer', anonymous=True) 42 | command = rospy.get_param("~command") 43 | stdout_file = rospy.get_param("~stdout_file", "") 44 | stderr_file = rospy.get_param("~stderr_file", "") 45 | stdout = None if len(stdout_file) == 0 else open(stdout_file, 'w') 46 | stderr = None if len(stderr_file) == 0 else open(stderr_file, 'w') 47 | executer = Executer(command, stdout, stderr) 48 | executer.execute() 49 | rospy.on_shutdown(executer.interrupt) 50 | while executer.process.poll() is None and not rospy.is_shutdown(): 51 | time.sleep(0.1) 52 | 53 | -------------------------------------------------------------------------------- /plot_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package plot_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Cleaned scripts. 8 | * Updated to Noetic. 9 | 10 | 0.0.3 (2017-02-24) 11 | ------------------ 12 | * Fix release 13 | * Contributors: Miquel Massot 14 | 15 | 0.0.2 (2017-02-23) 16 | ------------------ 17 | 18 | * hydro catkinization changes 19 | * added python setup files and wet'ed plot tools 20 | * Legend now is the filename 21 | * changed odom plot to open as many files as required 22 | * changed the private name topics to input and output. 23 | * NEW: plot_tools pkg with a python script for 3D real-time odometry visualization 24 | * Contributors: Miquel Massot, plnegre 25 | -------------------------------------------------------------------------------- /plot_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(plot_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rospy) 5 | 6 | catkin_package() 7 | 8 | catkin_python_setup() 9 | 10 | install(PROGRAMS 11 | scripts/plot_tools/real_time_odometry_plot.py 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 13 | ) -------------------------------------------------------------------------------- /plot_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b plot_tools 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /plot_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plot_tools 5 | 0.0.4 6 | plot_tools 7 | Bo Miquel Nordfeldt-Fiol 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/plot_tools 12 | 13 | 14 | Pep Lluis Negre 15 | 16 | catkin 17 | 18 | rospy 19 | 20 | rospy 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /plot_tools/scripts/plot_tools/real_time_odometry_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | 4 | import ntpath 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | 8 | 9 | # Global variables 10 | len_data = 0 11 | first_iter = True 12 | colors = ['g','r','b'] 13 | 14 | 15 | class Error(Exception): 16 | """ Base class for exceptions in this module. """ 17 | pass 18 | 19 | 20 | def real_time_plot(files): 21 | """ 22 | Function to plot the data saved into the files in real time 23 | """ 24 | global len_data, first_iter, colors 25 | 26 | for i,F in enumerate(files): 27 | 28 | # Load data 29 | data = np.loadtxt(F, delimiter=',', skiprows=1, usecols=(5,6,7)) 30 | 31 | # Check if new data 32 | if (len_data!= len(data[:,0])): 33 | 34 | # Plot 35 | label = ntpath.basename(F) 36 | label = label[0:-4] 37 | ax.plot(data[:,0], data[:,1], data[:,2], colors[i], label=label) 38 | 39 | plt.draw() 40 | 41 | # Update globals 42 | len_data = len(data[:,0]) 43 | 44 | if (first_iter == True): 45 | ax.legend() 46 | first_iter = False 47 | 48 | 49 | if __name__ == "__main__": 50 | import argparse 51 | parser = argparse.ArgumentParser( 52 | description='Plot 3D graphics of odometry data files in real time.', 53 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 54 | parser.add_argument('ground_truth_files', 55 | help='file with ground truth odometry', 56 | nargs='+') 57 | parser.add_argument('-ts','--time-step', 58 | help='update frequency (in milliseconds)', 59 | default='200') 60 | args = parser.parse_args() 61 | 62 | # Init figure 63 | fig = plt.figure(1) 64 | ax = fig.add_subplot(111, projection = '3d') 65 | ax.grid() 66 | ax.set_title("Realtime Odometry Plot") 67 | ax.set_xlabel("X") 68 | ax.set_ylabel("Y") 69 | ax.set_zlabel("Z") 70 | 71 | timer = fig.canvas.new_timer(interval=args.time_step) 72 | timer.add_callback(real_time_plot, args.ground_truth_files) 73 | timer.start() 74 | 75 | plt.show() 76 | -------------------------------------------------------------------------------- /plot_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | ## don't do this unless you want a globally visible script 8 | # scripts=['bin/myscript'], 9 | packages=['plot_tools'], 10 | package_dir={'': 'scripts'} 11 | ) 12 | 13 | setup(**d) 14 | -------------------------------------------------------------------------------- /pointcloud_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pointcloud_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Updated to Noetic. 8 | 9 | 0.0.3 (2017-02-24) 10 | ------------------ 11 | * Fix release 12 | * Minnor changes 13 | * fixed pointcloud mapper 14 | * fixed bug with XYZ clouds 15 | * Addapt the pointlcoud_to_webgl script to the new format of the website 16 | * Fix ros timers to work when bagfile finishes 17 | * Fix save problem in pointlcoud viewer 18 | * Publish the pointcloud at the same rate 19 | * Add info message when save pointcloud 20 | * added waitForTransform to mapper 21 | * changed to use system VTK paths 22 | * Contributors: Miquel Massot, Scott K Logan, plnegre 23 | 24 | 0.0.2 (2017-02-23) 25 | ------------------ 26 | * Minnor changes 27 | * fixed pointcloud mapper 28 | * fixed bug with XYZ clouds 29 | * Addapt the pointlcoud_to_webgl script to the new format of the website 30 | * Fix ros timers to work when bagfile finishes 31 | * Fix save problem in pointlcoud viewer 32 | * Publish the pointcloud at the same rate 33 | * Add info message when save pointcloud 34 | * added waitForTransform to mapper 35 | * changed to use system VTK paths 36 | * Contributors: Miquel Massot, Scott K Logan, plnegre 37 | 38 | * added pointcloud_to_webgl from fuerte branch 39 | * New node: pointcloud_mapper_for_slam 40 | * Fix dependency for hydro compliance 41 | * hydro changes 42 | * updated to master branch 43 | * wet repo 44 | * Variable renaming to accomplish standards 45 | * Grammatical change 46 | * FIX: allow enable/disable filter types 47 | * added filter to map 48 | * NEW: outlier filtering 49 | * changed the private name topics to input and output. 50 | * Changed the color handler for point clouds with no rgb field 51 | * changed to global namespace topic subscription 52 | * NEW: plot_tools pkg with a python script for 3D real-time odometry visualization 53 | * NEW: timer in pcd_publisher to control the frequency. Point cloud viewer improved. 54 | * Added new package of point clouds tools 55 | * Contributors: Miquel Massot, plnegre 56 | -------------------------------------------------------------------------------- /pointcloud_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(pointcloud_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp pcl_ros std_msgs sensor_msgs nav_msgs pcl_conversions) 5 | find_package(PCL REQUIRED) 6 | find_package(VTK REQUIRED) 7 | 8 | find_package(Boost REQUIRED COMPONENTS filesystem system) 9 | include_directories(${Boost_INCLUDE_DIRS}) 10 | 11 | catkin_package() 12 | 13 | include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${VTK_INCLUDE_DIRS}) 14 | 15 | add_executable(pointcloud_mapper src/pointcloud_mapper.cpp) 16 | add_executable(pointcloud_mapper_for_slam src/pointcloud_mapper_for_slam.cpp) 17 | add_executable(pointcloud_filtering src/pointcloud_filtering.cpp) 18 | add_executable(pcd_publisher src/pcd_publisher.cpp) 19 | add_executable(pointcloud_viewer src/pointcloud_viewer.cpp) 20 | add_executable(pointcloud_to_webgl src/pointcloud_to_webgl.cpp) 21 | 22 | target_link_libraries(pointcloud_viewer ${Boost_LIBRARIES} ${PCL_LIBRARIES} ${catkin_LIBRARIES} ${VTK_LIBRARIES}) 23 | target_link_libraries(pcd_publisher ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 24 | target_link_libraries(pointcloud_mapper ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 25 | target_link_libraries(pointcloud_mapper_for_slam ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 26 | target_link_libraries(pointcloud_filtering ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 27 | target_link_libraries(pointcloud_to_webgl ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 28 | 29 | install(TARGETS pointcloud_viewer pcd_publisher pointcloud_mapper pointcloud_mapper_for_slam pointcloud_filtering pointcloud_to_webgl 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | ) -------------------------------------------------------------------------------- /pointcloud_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pointcloud_tools 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /pointcloud_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pointcloud_tools 5 | 0.0.4 6 | pointcloud_tools 7 | Bo Miquel Nordfeldt-Fiol 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/pointcloud_tools 12 | 13 | 14 | Pep Lluis Negre 15 | 16 | catkin 17 | roscpp 18 | pcl_ros 19 | std_msgs 20 | sensor_msgs 21 | nav_msgs 22 | libvtk 23 | pcl_conversions 24 | 25 | roscpp 26 | pcl_ros 27 | std_msgs 28 | sensor_msgs 29 | nav_msgs 30 | libvtk 31 | pcl_conversions 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /pointcloud_tools/src/pcd_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | typedef pcl::PointXYZ Point; 14 | typedef pcl::PointCloud PointCloud; 15 | typedef pcl::PointXYZRGB PointRGB; 16 | typedef pcl::PointCloud PointCloudRGB; 17 | 18 | class PcdPublisher { 19 | 20 | // ROS properties 21 | ros::NodeHandle nh_; 22 | ros::NodeHandle nh_private_; 23 | 24 | // Publisher to send out the point cloud 25 | ros::Publisher point_cloud_pub_; 26 | 27 | // Timer for point cloud publication 28 | ros::WallTimer timer_; 29 | 30 | // PCD data 31 | std::string pcd_files_dir_; 32 | std::vector pcd_files_; 33 | unsigned int pcd_counter_; 34 | 35 | public: 36 | 37 | /** 38 | * Class constructor 39 | */ 40 | PcdPublisher(std::string pcd_files_dir) 41 | { 42 | int len; 43 | double timer_period; 44 | 45 | // Node handlers 46 | ros::NodeHandle nh; 47 | ros::NodeHandle nh_private_("~"); 48 | 49 | // Read parameters 50 | nh_private_.param("timer_period", timer_period, 0.1); 51 | 52 | // Init variables 53 | pcd_files_dir_ = pcd_files_dir; 54 | pcd_files_ = std::vector(); 55 | pcd_counter_ = 0; 56 | 57 | // Check if directory name finishes with "/" 58 | len = pcd_files_dir_.length(); 59 | if (strcmp("/", &(pcd_files_dir_[len - 1])) != 0) 60 | { 61 | pcd_files_dir_ = pcd_files_dir_ + "/"; 62 | } 63 | 64 | // List all pcd files into the directory 65 | listPcdFiles(pcd_files_dir_, pcd_files_); 66 | 67 | // Declare the point cloud topic 68 | point_cloud_pub_ = nh_private_.advertise("output", 1); 69 | 70 | // Init the timer 71 | timer_ = nh.createWallTimer(ros::WallDuration(timer_period), 72 | boost::bind(&PcdPublisher::readAndPublish, 73 | this)); 74 | ROS_INFO_STREAM("[PcdPublisher:] Timer started!"); 75 | } 76 | 77 | protected: 78 | 79 | /** 80 | * List PCD files into directory 81 | */ 82 | int listPcdFiles(std::string dir, std::vector &files) 83 | { 84 | int len; 85 | DIR *dp; 86 | struct dirent *dirp; 87 | 88 | // Check if directory exists 89 | if((dp = opendir(dir.c_str())) == NULL) 90 | { 91 | std::cout << "Error(" << errno << ") opening " << dir << std::endl; 92 | return errno; 93 | } 94 | 95 | // List the .pcd files 96 | while ((dirp = readdir(dp)) != NULL) 97 | { 98 | len = strlen(dirp->d_name); 99 | if (len >= 4) 100 | { 101 | if (strcmp(".pcd", &(dirp->d_name[len - 4])) == 0) 102 | { 103 | files.push_back(dirp->d_name); 104 | } 105 | } 106 | } 107 | closedir(dp); 108 | 109 | // Sort the files alphabetically 110 | std::sort(files.begin(), files.end()); 111 | return 0; 112 | } 113 | 114 | /** 115 | * Read and publish every point cloud 116 | */ 117 | void readAndPublish() 118 | { 119 | // Check if all pcd files have been published 120 | if (pcd_counter_ < pcd_files_.size()) 121 | { 122 | PointCloud::Ptr cloud(new PointCloud); 123 | 124 | if (pcl::io::loadPCDFile(pcd_files_dir_ + pcd_files_[pcd_counter_], *cloud) == -1) 125 | { 126 | ROS_ERROR_STREAM("Couldn't read file " << pcd_files_dir_ + pcd_files_[pcd_counter_]); 127 | return; 128 | } 129 | 130 | // Convert the point cloud to RGB 131 | PointCloudRGB::Ptr cloud_rgb(new PointCloudRGB); 132 | pcl::copyPointCloud(*cloud, *cloud_rgb); 133 | 134 | // Publish the point cloud 135 | point_cloud_pub_.publish(cloud_rgb); 136 | 137 | ROS_INFO_STREAM("[PcdPublisher:] Published " << cloud->width * cloud->height 138 | << " data points from " << pcd_files_dir_ + pcd_files_[pcd_counter_]); 139 | 140 | pcd_counter_++; 141 | } 142 | else 143 | { 144 | // All files processed 145 | timer_.stop(); 146 | ROS_INFO_STREAM("[PcdPublisher:] All PCD files processed. Timer stoped!"); 147 | } 148 | } 149 | }; 150 | 151 | /** 152 | * Main entry point of the code 153 | */ 154 | int main(int argc, char **argv) 155 | { 156 | ros::init(argc, argv, "pcd_publisher"); 157 | 158 | // First argument is the directory where pcd files are. 159 | std::string pcd_files_dir = argc > 1 ? argv[1] : "data"; 160 | 161 | // Init the node 162 | PcdPublisher node(pcd_files_dir); 163 | ros::spin(); 164 | return 0; 165 | } 166 | 167 | -------------------------------------------------------------------------------- /pointcloud_tools/src/pointcloud_filtering.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | typedef pcl::PointXYZRGB Point; 13 | typedef pcl::PointCloudPointCloud; 14 | 15 | class PointCloudFiltering { 16 | 17 | // ROS properties 18 | ros::NodeHandle nh_; 19 | ros::NodeHandle nh_private_; 20 | ros::Subscriber point_cloud_sub_; 21 | 22 | // Publisher to send out the filtered point cloud 23 | ros::Publisher point_cloud_filtered_; 24 | 25 | // Filter parameters 26 | double x_filter_min_; 27 | double x_filter_max_; 28 | double y_filter_min_; 29 | double y_filter_max_; 30 | double z_filter_min_; 31 | double z_filter_max_; 32 | double voxel_size_; 33 | int mean_k_; 34 | double std_dev_thresh_; 35 | 36 | bool apply_xyz_limits_; 37 | bool apply_voxel_grid_; 38 | bool apply_outlier_removal_; 39 | 40 | public: 41 | 42 | /** 43 | * Class constructor 44 | */ 45 | PointCloudFiltering() : nh_private_("~") 46 | { 47 | // Read the parameters from the parameter server (set defaults) 48 | nh_private_.param("apply_xyz_limits", apply_xyz_limits_, true); 49 | nh_private_.param("apply_voxel_grid", apply_voxel_grid_, true); 50 | nh_private_.param("apply_outlier_removal", apply_outlier_removal_, false); 51 | nh_private_.param("x_filter_min", x_filter_min_, -3.0); 52 | nh_private_.param("x_filter_max", x_filter_max_, 3.0); 53 | nh_private_.param("y_filter_min", y_filter_min_, -3.0); 54 | nh_private_.param("y_filter_max", y_filter_max_, 3.0); 55 | nh_private_.param("z_filter_min", z_filter_min_, 0.2); 56 | nh_private_.param("z_filter_max", z_filter_max_, 3.0); 57 | nh_private_.param("voxel_size", voxel_size_, 0.01); 58 | nh_private_.param("mean_k", mean_k_, 50); 59 | nh_private_.param("std_dev_thresh", std_dev_thresh_, 1.0); 60 | 61 | // Subscription to the point cloud result from stereo_image_proc 62 | point_cloud_sub_ = nh_.subscribe( 63 | "input", 64 | 1, 65 | &PointCloudFiltering:: 66 | pointCloudCb, 67 | this); 68 | 69 | // Declare the point cloud filtered topic 70 | point_cloud_filtered_ = nh_private_.advertise("output", 1); 71 | } 72 | 73 | /** 74 | * Callback executed when a point cloud is recieved from topic "points2". 75 | */ 76 | void pointCloudCb(const PointCloud::ConstPtr& point_cloud) 77 | { 78 | PointCloud cloud = *point_cloud; 79 | 80 | // Filter 81 | PointCloud::Ptr cloud_downsampled = filter(cloud.makeShared()); 82 | 83 | // Publish the filtered point cloud 84 | point_cloud_filtered_.publish(cloud_downsampled); 85 | } 86 | 87 | /** 88 | * Function to downsample the point cloud using Z-filtering (by range) and 89 | *voxel grid. 90 | */ 91 | PointCloud::Ptr filter(PointCloud::Ptr cloud) 92 | { 93 | // NAN and limit filtering 94 | PointCloud::Ptr cloud_filtered_ptr(new PointCloud); 95 | pcl::PassThrough pass; 96 | 97 | if (apply_xyz_limits_) 98 | { 99 | // X-filtering 100 | pass.setFilterFieldName("x"); 101 | pass.setFilterLimits(x_filter_min_, x_filter_max_); 102 | pass.setInputCloud(cloud); 103 | pass.filter(*cloud_filtered_ptr); 104 | 105 | // Y-filtering 106 | pass.setFilterFieldName("y"); 107 | pass.setFilterLimits(y_filter_min_, y_filter_max_); 108 | pass.setInputCloud(cloud_filtered_ptr); 109 | pass.filter(*cloud_filtered_ptr); 110 | 111 | // Z-filtering 112 | pass.setFilterFieldName("z"); 113 | pass.setFilterLimits(z_filter_min_, z_filter_max_); 114 | pass.setInputCloud(cloud_filtered_ptr); 115 | pass.filter(*cloud_filtered_ptr); 116 | } 117 | else 118 | { 119 | cloud_filtered_ptr = cloud; 120 | } 121 | 122 | // Downsampling using voxel grid 123 | 124 | PointCloud::Ptr cloud_downsampled_ptr(new PointCloud); 125 | 126 | if (apply_voxel_grid_) 127 | { 128 | pcl::VoxelGrid grid; 129 | grid.setLeafSize(voxel_size_, voxel_size_, voxel_size_); 130 | grid.setDownsampleAllData(true); 131 | grid.setInputCloud(cloud_filtered_ptr); 132 | grid.filter(*cloud_downsampled_ptr); 133 | } 134 | else 135 | { 136 | cloud_downsampled_ptr = cloud_filtered_ptr; 137 | } 138 | 139 | // Statistical outlier removal 140 | PointCloud::Ptr cloud_outlier_ptr(new PointCloud); 141 | 142 | if (apply_outlier_removal_) 143 | { 144 | pcl::StatisticalOutlierRemoval sor; 145 | sor.setInputCloud(cloud_downsampled_ptr); 146 | sor.setMeanK(mean_k_); 147 | sor.setStddevMulThresh(std_dev_thresh_); 148 | sor.filter(*cloud_outlier_ptr); 149 | } 150 | else 151 | { 152 | cloud_outlier_ptr = cloud_downsampled_ptr; 153 | } 154 | 155 | return cloud_outlier_ptr; 156 | } 157 | }; 158 | 159 | /** 160 | * Main entry point of the code 161 | */ 162 | int main(int argc, char **argv) 163 | { 164 | ros::init(argc, argv, "point_cloud_filtering"); 165 | PointCloudFiltering node; 166 | ros::spin(); 167 | return 0; 168 | } 169 | 170 | -------------------------------------------------------------------------------- /pointcloud_tools/src/pointcloud_mapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * Stores incoming point clouds in a map transforming 11 | * each cloud to a global fixed frame using tf. 12 | */ 13 | class PointCloudMapper 14 | { 15 | public: 16 | 17 | typedef pcl::PointXYZRGB Point; 18 | typedef pcl::PointCloud PointCloud; 19 | 20 | PointCloudMapper() : 21 | nh_(), nh_priv_("~") 22 | { 23 | nh_priv_.param("fixed_frame", fixed_frame_, std::string("/map")); 24 | // Read the parameters from the parameter server (set defaults) 25 | nh_priv_.param("x_filter_min", x_filter_min_, -30.0); 26 | nh_priv_.param("x_filter_max", x_filter_max_, 30.0); 27 | nh_priv_.param("y_filter_min", y_filter_min_, -30.0); 28 | nh_priv_.param("y_filter_max", y_filter_max_, 30.0); 29 | nh_priv_.param("z_filter_min", z_filter_min_, 2.0); 30 | nh_priv_.param("z_filter_max", z_filter_max_, 30.0); 31 | nh_priv_.param("voxel_size", voxel_size_, 0.1); 32 | nh_priv_.param("filter_map", filter_map_, false); 33 | 34 | cloud_sub_ = nh_.subscribe("input", 1, &PointCloudMapper::callback, this); 35 | cloud_pub_ = nh_priv_.advertise("output", 1, true); 36 | 37 | accumulated_cloud_.header.frame_id = fixed_frame_; 38 | 39 | ROS_INFO_STREAM("[PointCloudMapper params:\n" << 40 | "\t* Fixed frame: " << fixed_frame_ << "\n" << 41 | "\t* Filter map: " << filter_map_ << "\n" << 42 | "\t* Filter bounds: " << "( " << x_filter_min_ << ", " << y_filter_min_ << ", " << z_filter_min_ << ") and ( " << x_filter_max_ << ", " << y_filter_max_ << ", " << z_filter_max_ << ")" << "\n" << 43 | "\t* Voxel size: " << voxel_size_); 44 | 45 | pub_timer_ = nh_.createWallTimer(ros::WallDuration(3.0), &PointCloudMapper::publishCallback, this); 46 | } 47 | 48 | void callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) 49 | { 50 | PointCloud cloud; 51 | pcl::fromROSMsg(*cloud_msg, cloud); 52 | ROS_INFO_STREAM("Received cloud with " << cloud.points.size() << " points."); 53 | 54 | PointCloud transformed_cloud; 55 | tf::StampedTransform transform; 56 | try { 57 | tf_listener_.waitForTransform(fixed_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, ros::Duration(5.0)); 58 | tf_listener_.lookupTransform(fixed_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, transform); 59 | } catch (tf::TransformException ex){ 60 | ROS_ERROR("%s",ex.what()); 61 | ros::Duration(1.0).sleep(); 62 | } 63 | 64 | pcl_ros::transformPointCloud(cloud, transformed_cloud, tf::Transform(transform)); 65 | bool success = true; 66 | if (success) 67 | { 68 | accumulated_cloud_ += transformed_cloud; 69 | 70 | if(filter_map_) 71 | { 72 | PointCloud::Ptr cloud_downsampled = filter(accumulated_cloud_.makeShared()); 73 | accumulated_cloud_ = *cloud_downsampled; 74 | } 75 | 76 | ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points."); 77 | 78 | // Publish the cloud 79 | if (cloud_pub_.getNumSubscribers() > 0) 80 | cloud_pub_.publish(accumulated_cloud_); 81 | last_pub_time_ = ros::WallTime::now(); 82 | } 83 | else 84 | { 85 | ROS_ERROR("Could not transform point cloud to %s", fixed_frame_.c_str()); 86 | } 87 | } 88 | 89 | void publishCallback(const ros::WallTimerEvent&) 90 | { 91 | // Publish the accumulated cloud if last publication was more than 5 seconds before. 92 | ros::WallDuration elapsed_time = ros::WallTime::now() - last_pub_time_; 93 | if (cloud_pub_.getNumSubscribers() > 0 && elapsed_time.toSec() > 5.0) { 94 | 95 | sensor_msgs::PointCloud2 cloud_msg; 96 | pcl::toROSMsg(accumulated_cloud_, cloud_msg); 97 | cloud_pub_.publish(cloud_msg); 98 | } 99 | } 100 | 101 | PointCloud::Ptr filter(PointCloud::Ptr cloud) 102 | { 103 | // Copy the point cloud 104 | PointCloud::Ptr cloud_ptr(new PointCloud); 105 | 106 | // NAN and limit filtering 107 | PointCloud::Ptr cloud_filtered_ptr(new PointCloud); 108 | pcl::PassThrough pass_; 109 | 110 | // X-filtering 111 | pass_.setFilterFieldName("x"); 112 | pass_.setFilterLimits(x_filter_min_, x_filter_max_); 113 | pass_.setInputCloud(cloud); 114 | pass_.filter(*cloud_filtered_ptr); 115 | 116 | // Y-filtering 117 | pass_.setFilterFieldName("y"); 118 | pass_.setFilterLimits(y_filter_min_, y_filter_max_); 119 | pass_.setInputCloud(cloud_filtered_ptr); 120 | pass_.filter(*cloud_filtered_ptr); 121 | 122 | // Z-filtering 123 | pass_.setFilterFieldName("z"); 124 | pass_.setFilterLimits(z_filter_min_, z_filter_max_); 125 | pass_.setInputCloud(cloud); 126 | pass_.filter(*cloud_filtered_ptr); 127 | 128 | // Downsampling using voxel grid 129 | pcl::VoxelGrid grid_; 130 | PointCloud::Ptr cloud_downsampled_ptr(new PointCloud); 131 | double plane_detection_voxel_size_ = voxel_size_; 132 | 133 | grid_.setLeafSize(plane_detection_voxel_size_, 134 | plane_detection_voxel_size_, 135 | plane_detection_voxel_size_); 136 | grid_.setDownsampleAllData(true); 137 | grid_.setInputCloud(cloud_filtered_ptr); 138 | grid_.filter(*cloud_downsampled_ptr); 139 | 140 | return cloud_downsampled_ptr; 141 | } 142 | 143 | private: 144 | // Filter parameters 145 | double x_filter_min_; 146 | double x_filter_max_; 147 | double y_filter_min_; 148 | double y_filter_max_; 149 | double z_filter_min_; 150 | double z_filter_max_; 151 | double voxel_size_; 152 | bool filter_map_; 153 | 154 | ros::NodeHandle nh_; 155 | ros::NodeHandle nh_priv_; 156 | 157 | ros::WallTimer pub_timer_; 158 | 159 | ros::Subscriber cloud_sub_; 160 | ros::Publisher cloud_pub_; 161 | 162 | std::string fixed_frame_; 163 | tf::TransformListener tf_listener_; 164 | PointCloud accumulated_cloud_; 165 | ros::WallTime last_pub_time_; 166 | }; 167 | 168 | int main(int argc, char **argv) 169 | { 170 | ros::init(argc, argv, "cloud_mapper"); 171 | PointCloudMapper mapper; 172 | ros::spin(); 173 | return 0; 174 | } 175 | 176 | -------------------------------------------------------------------------------- /pointcloud_tools/src/pointcloud_to_webgl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class PointCloudToWebgl { 11 | 12 | public: 13 | 14 | /** 15 | * Class constructor 16 | */ 17 | PointCloudToWebgl(const std::string& input_cloud, const int& cloud_format, 18 | const std::string& output_cloud) : 19 | input_cloud_(input_cloud), cloud_format_(cloud_format), output_cloud_(output_cloud) 20 | {} 21 | 22 | /** 23 | * Convert the cloud 24 | */ 25 | void convert() 26 | { 27 | ROS_INFO_STREAM("[PointCloudToWebgl:] Opening file " << input_cloud_); 28 | 29 | // Init the cloud 30 | pcl::PointCloud cloud; 31 | 32 | // Read point cloud 33 | if (cloud_format_ == 0) 34 | { 35 | // NO RGB 36 | pcl::PointCloud::Ptr cloud_xyz (new pcl::PointCloud); 37 | if (pcl::io::loadPCDFile (input_cloud_, *cloud_xyz) == -1) //* load the file 38 | { 39 | ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << input_cloud_); 40 | return; 41 | } 42 | copyPointCloud(*cloud_xyz, cloud); 43 | } 44 | else if (cloud_format_ == 1) 45 | { 46 | // RGB 47 | pcl::PointCloud::Ptr cloud_xyzrgb (new pcl::PointCloud); 48 | if (pcl::io::loadPCDFile (input_cloud_, *cloud_xyzrgb) == -1) //* load the file 49 | { 50 | ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << input_cloud_); 51 | return; 52 | } 53 | copyPointCloud(*cloud_xyzrgb, cloud); 54 | } 55 | 56 | // Compute the cloud centroid 57 | Eigen::Vector4f centroid; 58 | pcl::compute3DCentroid(cloud, centroid); 59 | 60 | // Save int file 61 | ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << output_cloud_); 62 | std::fstream f_webgl(output_cloud_.c_str(), std::ios::out); 63 | for (unsigned int i=0; i(argv[2]); 108 | 109 | // Convert 110 | PointCloudToWebgl converter(input_cloud, cloud_format, output_cloud); 111 | converter.convert(); 112 | 113 | return 0; 114 | } 115 | 116 | -------------------------------------------------------------------------------- /srv_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package srv_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Updated to Noetic. 8 | 9 | 0.0.3 (2017-02-24) 10 | ------------------ 11 | * Fix release 12 | * Contributors: Miquel Massot 13 | 14 | 0.0.2 (2017-02-23) 15 | ------------------ 16 | 17 | * added python setup files and wet'ed plot tools 18 | * wet repo 19 | * Contributors: Miquel Massot 20 | -------------------------------------------------------------------------------- /srv_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(srv_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /srv_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | srv_tools 5 | 0.0.4 6 | Stack with interesting ROS tools 7 | Bo Miquel Nordfeldt-Fiol 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/srv_tools 12 | 13 | 14 | Stephan Wirth 15 | Miquel Massot 16 | 17 | catkin 18 | 19 | launch_tools 20 | tf_tools 21 | pointcloud_tools 22 | bag_tools 23 | plot_tools 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /tf_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package tf_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2024-12-16) 6 | ------------------ 7 | * Updated to Noetic. 8 | 9 | 0.0.3 (2017-02-24) 10 | ------------------ 11 | * Fix release 12 | * Minnor changes 13 | * Add new tf tool 14 | * Contributors: Miquel Massot, plnegre 15 | 16 | 0.0.2 (2017-02-23) 17 | ------------------ 18 | * Minnor changes 19 | * Add new tf tool 20 | * Contributors: plnegre 21 | 22 | * tf_filter added 23 | * hydro catkinization changes 24 | * added python setup files and wet'ed plot tools 25 | * wet repo 26 | * minor output tweak 27 | * changed manifests 28 | * added BSD license 29 | * better tf logger, more options 30 | * changed README, again 31 | * changed README to markup language 32 | * changed README to markup language 33 | * added README and cleaned up tf_logger 34 | * Added tf_logger 35 | * new package for tf related tools 36 | * Contributors: Miquel Massot, Stephan Wirth, plnegre 37 | -------------------------------------------------------------------------------- /tf_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tf_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS tf nav_msgs) 5 | 6 | catkin_package() 7 | 8 | #catkin_python_setup() 9 | 10 | include_directories(${catkin_INCLUDE_DIRS}) 11 | 12 | add_executable(tf_logger src/tf_logger.cpp) 13 | add_executable(tf_filter src/tf_filter.cpp) 14 | add_executable(apply_tf_to_odom_msg src/apply_tf_to_odom_msg.cpp) 15 | 16 | target_link_libraries(tf_logger ${catkin_LIBRARIES}) 17 | target_link_libraries(tf_filter ${catkin_LIBRARIES}) 18 | target_link_libraries(apply_tf_to_odom_msg ${catkin_LIBRARIES}) 19 | 20 | install(TARGETS tf_logger tf_filter apply_tf_to_odom_msg 21 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 22 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | ) -------------------------------------------------------------------------------- /tf_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b tf_tools is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /tf_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | tf_tools 5 | 0.0.4 6 | ROS tools and scripts relates to tf 7 | Bo Miquel Nordfeldt-Fiol 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/tf_tools 12 | 13 | 14 | Stephan Wirth 15 | Miquel Massot 16 | 17 | catkin 18 | 19 | tf 20 | nav_msgs 21 | 22 | tf 23 | nav_msgs 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /tf_tools/src/apply_tf_to_odom_msg.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | class ApplyTF2Odom 8 | { 9 | ros::NodeHandle nh_; 10 | ros::NodeHandle nh_private_; 11 | 12 | ros::Subscriber odom_sub_; 13 | ros::Publisher odom_pub_; 14 | 15 | tf::Transform trans_; 16 | 17 | public: 18 | ApplyTF2Odom() : nh_private_("~") 19 | { 20 | // Read parameters 21 | double x, y, z, qx, qy, qz, qw; 22 | nh_private_.param("x", x, 0.0); 23 | nh_private_.param("y", y, 0.0); 24 | nh_private_.param("z", z, 0.0); 25 | nh_private_.param("qx", qx, 0.0); 26 | nh_private_.param("qy", qy, 0.0); 27 | nh_private_.param("qz", qz, 0.0); 28 | nh_private_.param("qw", qw, 1.0); 29 | 30 | // Build tf 31 | tf::Vector3 t(x, y, z); 32 | tf::Quaternion q(qx, qy, qz, qw); 33 | tf::Transform tmp(q, t); 34 | trans_ = tmp; 35 | 36 | // Messages 37 | odom_sub_ = nh_.subscribe("odometry_in", 1, &ApplyTF2Odom::callback, this); 38 | odom_pub_ = nh_private_.advertise("odometry_out", 1); 39 | } 40 | 41 | void callback(const nav_msgs::Odometry::ConstPtr& odom_msg) 42 | { 43 | // Get the data 44 | double x = odom_msg->pose.pose.position.x; 45 | double y = odom_msg->pose.pose.position.y; 46 | double z = odom_msg->pose.pose.position.z; 47 | 48 | double qx = odom_msg->pose.pose.orientation.x; 49 | double qy = odom_msg->pose.pose.orientation.y; 50 | double qz = odom_msg->pose.pose.orientation.z; 51 | double qw = odom_msg->pose.pose.orientation.w; 52 | 53 | // Build the odometry pose 54 | tf::Vector3 t(x, y, z); 55 | tf::Quaternion q(qx, qy, qz, 1.0); 56 | tf::Transform pose(q, t); 57 | 58 | // Transform 59 | tf::Transform new_pose = trans_ * pose; 60 | 61 | // Publish 62 | nav_msgs::Odometry new_odom_msg = *odom_msg; 63 | tf::poseTFToMsg(new_pose, new_odom_msg.pose.pose); 64 | odom_pub_.publish(new_odom_msg); 65 | } 66 | }; 67 | 68 | int main(int argc, char** argv) 69 | { 70 | ros::init(argc, argv, "apply_tf_to_odom_msg"); 71 | ApplyTF2Odom node; 72 | ros::spin(); 73 | return 0; 74 | } 75 | 76 | -------------------------------------------------------------------------------- /tf_tools/src/tf_filter.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | std::ostream& operator<<(std::ostream& os, const tf::Transform& transform) 36 | { 37 | using std::setw; 38 | tf::Vector3 origin = transform.getOrigin(); 39 | tf::Quaternion quat = transform.getRotation(); 40 | os << setw(10) << origin.x() << " " << setw(10) << origin.y() << " " << setw(10) << origin.z() << " "; 41 | os << setw(10) << quat.x() << " " << setw(10) << quat.y() << " " << setw(10) << quat.z() << " " << setw(10) << quat.w(); 42 | return os; 43 | } 44 | 45 | void printHeader(std::ostream& os, const std::string& frame_id) 46 | { 47 | os << frame_id << ".x " << frame_id << ".y " << frame_id << ".z " 48 | << frame_id << ".qx " << frame_id << ".qy " << frame_id << ".qz " << frame_id << ".qw "; 49 | } 50 | 51 | int main(int argc, char** argv) 52 | { 53 | ros::init(argc, argv, "tf_filter"); 54 | 55 | ros::NodeHandle nh; 56 | ros::NodeHandle nhp("~"); 57 | 58 | std::string reference_frame_id; 59 | std::string child_frame_id; 60 | double frequency, ramp_wwf; 61 | int num_samples_wwf, wwf_index; 62 | bool full_arrays = false; 63 | 64 | nhp.param("reference_frame_id", reference_frame_id, std::string("/base_link")); 65 | nhp.param("child_frame_id", child_frame_id, std::string("/camera")); 66 | nhp.param("frequency", frequency, 100.0); 67 | 68 | // Use Weighted Window Filter 69 | nhp.param("ramp_wwf", ramp_wwf, 0.5); 70 | nhp.param("num_samples_wwf", num_samples_wwf, 10); 71 | 72 | ROS_INFO("TF Filter initiated.\n\tPARAMETERS: \ 73 | \n\t * Reference frame ID: %s \ 74 | \n\t * Child frame ID: %s \ 75 | \n\t * Frequency: %f \ 76 | \n\t * Ramp WWF: %f \ 77 | \n\t * Num samples WWF: %d", 78 | reference_frame_id.c_str(), 79 | child_frame_id.c_str(), 80 | frequency, 81 | ramp_wwf, 82 | num_samples_wwf); 83 | 84 | double * x_array, * y_array, * z_array; 85 | double * sR_array, * cR_array, * sP_array, * cP_array, * sY_array, * cY_array; 86 | double * t_array, * w_array; 87 | 88 | //fill circular arrays with zeros 89 | wwf_index = 0; 90 | x_array = new double[num_samples_wwf]; 91 | y_array = new double[num_samples_wwf]; 92 | z_array = new double[num_samples_wwf]; 93 | sR_array = new double[num_samples_wwf]; // sin(roll) array 94 | cR_array = new double[num_samples_wwf]; // cos(roll) array 95 | sP_array = new double[num_samples_wwf]; // sin(pitch) array 96 | cP_array = new double[num_samples_wwf]; // cos(pitch) array 97 | sY_array = new double[num_samples_wwf]; // sin(yaw) array 98 | cY_array = new double[num_samples_wwf]; // cos(yaw) array 99 | t_array = new double[num_samples_wwf]; //times used for the ramp 100 | w_array = new double[num_samples_wwf]; //weights 101 | for(int i = 0; i< num_samples_wwf; i++){ 102 | x_array[i] = 0.0; 103 | y_array[i] = 0.0; 104 | z_array[i] = 0.0; 105 | sR_array[i] = 0.0; 106 | cR_array[i] = 0.0; 107 | sP_array[i] = 0.0; 108 | cP_array[i] = 0.0; 109 | sY_array[i] = 0.0; 110 | cY_array[i] = 0.0; 111 | t_array[i] = 0.0; 112 | w_array[i] = 0.0; 113 | } 114 | 115 | tf::TransformListener listener; 116 | tf::TransformBroadcaster broadcaster; 117 | 118 | ros::Rate rate(frequency); 119 | while (ros::ok()) 120 | { 121 | rate.sleep(); //TODO: here or at the end? 122 | 123 | // Wait for clock 124 | if (!ros::Time::isValid()) continue; 125 | ros::Time requested_time = ros::Time(0); //(ros::Time::now() - ros::Duration(2 / frequency)); 126 | tf::StampedTransform transform; 127 | try 128 | { 129 | // first look up transform, this could throw exceptions 130 | ros::Duration timeout(2 / frequency); 131 | ros::Duration polling_sleep_duration(0.5 / frequency); 132 | listener.waitForTransform( 133 | reference_frame_id, 134 | child_frame_id, 135 | requested_time, 136 | timeout, 137 | polling_sleep_duration); 138 | listener.lookupTransform( 139 | reference_frame_id, 140 | child_frame_id, 141 | requested_time, 142 | transform); 143 | 144 | ROS_DEBUG_STREAM("Received transform: " << transform); 145 | ROS_DEBUG_STREAM("With frame_id: " << transform.frame_id_ << " and child_id " << transform.child_frame_id_); 146 | 147 | // get position and orientation 148 | double roll, pitch, yaw; 149 | tf::Matrix3x3 rotm(transform.getRotation()); 150 | rotm.getRPY(roll, pitch, yaw); 151 | 152 | x_array[wwf_index] = transform.getOrigin().x(); 153 | y_array[wwf_index] = transform.getOrigin().y(); 154 | z_array[wwf_index] = transform.getOrigin().z(); 155 | sR_array[wwf_index] = sin(roll); 156 | cR_array[wwf_index] = cos(roll); 157 | sP_array[wwf_index] = sin(pitch); 158 | cP_array[wwf_index] = cos(pitch); 159 | sY_array[wwf_index] = sin(yaw); 160 | cY_array[wwf_index] = cos(yaw); 161 | 162 | // save ros time into circular array 163 | ros::Time now = transform.stamp_; 164 | t_array[wwf_index] = now.toSec(); 165 | 166 | // compute weights for every element of the arrays 167 | // and leave weights of empty cells to zero 168 | int num_elem = num_samples_wwf; 169 | if(!full_arrays){ 170 | num_elem = wwf_index + 1; 171 | } 172 | double w_total = 0.0; 173 | for(int i = 0; i < num_elem; i++){ 174 | double t_aux = t_array[i] - t_array[wwf_index]; //<=0 175 | w_array[i] = std::max(0.0,ramp_wwf*t_aux + 1); 176 | w_total += w_array[i]; 177 | } 178 | 179 | 180 | double w_aux; 181 | double x, y, z, sinR, cosR, sinP, cosP, sinY, cosY; 182 | x = y = z = sinR = cosR = sinP = cosP = sinY = cosY = 0.0; 183 | 184 | // Compute final tf 185 | for(int i = 0; i < num_elem; i++){ 186 | w_aux = w_array[i]/w_total; 187 | x += w_aux * x_array[i]; 188 | y += w_aux * y_array[i]; 189 | z += w_aux * z_array[i]; 190 | sinR += w_aux * sR_array[i]; 191 | cosR += w_aux * cR_array[i]; 192 | sinP += w_aux * sP_array[i]; 193 | cosP += w_aux * cP_array[i]; 194 | sinY += w_aux * sY_array[i]; 195 | cosY += w_aux * cY_array[i]; 196 | } 197 | 198 | tf::Vector3 orig(x,y,z); 199 | 200 | roll = atan2(sinR, cosR); 201 | pitch = atan2(sinP, cosP); 202 | yaw = atan2(sinY, cosY); 203 | 204 | tf::Quaternion quat; 205 | quat.setRPY(roll, pitch, yaw); 206 | 207 | transform.setOrigin(orig); 208 | transform.setRotation(quat); 209 | 210 | // Set the output stamped transform and send it 211 | transform.child_frame_id_ = transform.child_frame_id_ + "_filtered"; 212 | broadcaster.sendTransform(transform); 213 | 214 | // Increment the counter 215 | wwf_index++; 216 | if (wwf_index==num_samples_wwf){ 217 | full_arrays = true; 218 | wwf_index = 0; 219 | } 220 | } 221 | catch (const tf::TransformException& e) 222 | { 223 | std::cerr << "TransformException caught: " << e.what() << std::endl; 224 | } 225 | } 226 | return 0; 227 | } 228 | 229 | -------------------------------------------------------------------------------- /tf_tools/src/tf_logger.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (c) 2012, 2 | /// Systems, Robotics and Vision Group 3 | /// University of the Balearican Islands 4 | /// All rights reserved. 5 | /// 6 | /// Redistribution and use in source and binary forms, with or without 7 | /// modification, are permitted provided that the following conditions are met: 8 | /// * Redistributions of source code must retain the above copyright 9 | /// notice, this list of conditions and the following disclaimer. 10 | /// * Redistributions in binary form must reproduce the above copyright 11 | /// notice, this list of conditions and the following disclaimer in the 12 | /// documentation and/or other materials provided with the distribution. 13 | /// * Neither the name of Systems, Robotics and Vision Group, University of 14 | /// the Balearican Islands nor the names of its contributors may be used 15 | /// to endorse or promote products derived from this software without 16 | /// 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 21 | /// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | std::ostream& operator<<(std::ostream& os, const tf::Transform& transform) 36 | { 37 | using std::setw; 38 | tf::Vector3 origin = transform.getOrigin(); 39 | tf::Quaternion quat = transform.getRotation(); 40 | os << setw(10) << origin.x() << " " << setw(10) << origin.y() << " " << setw(10) << origin.z() << " "; 41 | os << setw(10) << quat.x() << " " << setw(10) << quat.y() << " " << setw(10) << quat.z() << " " << setw(10) << quat.w(); 42 | return os; 43 | } 44 | 45 | void printHeader(std::ostream& os, const std::string& frame_id) 46 | { 47 | os << frame_id << ".x " << frame_id << ".y " << frame_id << ".z " 48 | << frame_id << ".qx " << frame_id << ".qy " << frame_id << ".qz " << frame_id << ".qw "; 49 | } 50 | 51 | struct FramePair 52 | { 53 | FramePair(const std::string& rframe, const std::string& lframe) : 54 | reference_frame(rframe), log_frame(lframe) 55 | {} 56 | std::string reference_frame; 57 | std::string log_frame; 58 | }; 59 | 60 | int main(int argc, char** argv) 61 | { 62 | ros::init(argc, argv, "tf_logger"); 63 | 64 | if (argc < 4 || argc % 2 != 0) 65 | { 66 | std::cerr << "Wrong number of arguments." << std::endl; 67 | std::cerr << "USAGE: " << argv[0] 68 | << " FREQUENCY REF_FRAME LOG_FRAME [REF_FRAME LOG_FRAME ...]" << std::endl; 69 | std::cerr << "The transforms should be published at a rate >= 2*FREQUENCY " << std::endl; 70 | return 1; 71 | } 72 | 73 | float frequency = atof(argv[1]); 74 | 75 | std::vector frame_pairs; 76 | 77 | std::cout << "#timestamp "; 78 | for (int i = 2; i < argc; i+=2) 79 | { 80 | std::string ref_frame(argv[i]); 81 | std::string log_frame(argv[i+1]); 82 | printHeader(std::cout, log_frame); 83 | frame_pairs.push_back(FramePair(ref_frame, log_frame)); 84 | } 85 | std::cout << std::endl; 86 | 87 | ros::Duration cache_time(10); 88 | tf::TransformListener tf_listener(cache_time); 89 | 90 | ros::Rate rate(frequency); 91 | while (ros::ok()) 92 | { 93 | rate.sleep(); 94 | if (!ros::Time::isValid()) continue; // wait for clock 95 | ros::Time requested_time(ros::Time::now() - ros::Duration(2 / frequency)); 96 | std::vector transforms(frame_pairs.size()); 97 | try 98 | { 99 | // first look up all transforms, this could throw exceptions 100 | for (size_t i = 0; i < frame_pairs.size(); ++i) 101 | { 102 | ros::Duration timeout(2 / frequency); 103 | ros::Duration polling_sleep_duration(0.5 / frequency); 104 | tf_listener.waitForTransform( 105 | frame_pairs[i].reference_frame, 106 | frame_pairs[i].log_frame, requested_time, 107 | timeout, polling_sleep_duration); 108 | tf::StampedTransform transform; 109 | tf_listener.lookupTransform( 110 | frame_pairs[i].reference_frame, 111 | frame_pairs[i].log_frame, requested_time, transforms[i]); 112 | } 113 | // no exception was thrown, print out set 114 | std::cout << requested_time.toNSec() << " "; 115 | for (size_t i = 0; i < transforms.size(); ++i) 116 | { 117 | std::cout << transforms[i] << " "; 118 | } 119 | std::cout << std::endl; 120 | } 121 | catch (const tf::TransformException& e) 122 | { 123 | std::cerr << "TransformException caught: " << e.what() << std::endl; 124 | } 125 | } 126 | return 0; 127 | } 128 | 129 | --------------------------------------------------------------------------------