Quantcast
Channel: ROS Answers: Open Source Q&A Forum - RSS feed
Viewing all articles
Browse latest Browse all 475

Can't read bagfile containing custom messages

$
0
0
I have a bag file with video and imu data defined with custom messages that I want to read (especially topics /capture and /imu_raw). I know from documentation what the structure of the custom messages is. How can I read the data?

Doing **rosbag info** on the file shows: types: libks/ImageSet [b3ef08c9cff052c83f5039d098add069] pximu/AttitudeData [bc609da5ddac9016c096c67da33d8b9c] pximu/RawIMUData [0380911195329d307d8f033bc714dbbf] [...] topics: /attitude 2174 msgs : pximu/AttitudeData /capture 3282 msgs : libks/ImageSet /imu_raw 10873 msgs : pximu/RawIMUData [...] I cannot play the file with 'rosbag play', probably because rosbag doesn't know the message definitions. Therefore, I thought I needed to use migration(see http_wiki_ros_org/rosbag_migration) and provide custom message definitions (see http_wiki_ros_org/msg) and use them in a rule file. I made a new package 'bag_analyser", and added the custom messages to the 'msg' folder and compiled. E.g. for ImageSet I made a file ImageSet.msg:
Header header sensor_msgs/Image[] images This seems to work, e.g. for **' rosmsg show bag_analyser/ImageSet'** I get:
std_msgs/Header header uint32 seq time stamp string frame_id sensor_msgs/Image[] images std_msgs/Header header uint32 seq time stamp string frame_id uint32 height uint32 width string encoding uint8 is_bigendian uint32 step uint8[] data
I also made messages for RawIMUData and AttitudeData. Then I generated a rules file (rules.bmr, see full output at end), adjusted the rules that needed to be adjusted and executed: rosbag fix original_bag.bag converted_bag.bag rules.bmr which told me "bag migrated successfully". Doing **rosbag info** on the converted file gives me:
types: bag_analyser/AttitudeData [bc609da5ddac9016c096c67da33d8b9c] bag_analyser/ImageSet [b3ef08c9cff052c83f5039d098add069] bag_analyser/RawIMUData [0380911195329d307d8f033bc714dbbf] [...] topics: /attitude 2174 msgs : bag_analyser/AttitudeData /capture 3282 msgs : bag_analyser/ImageSet /imu_raw 10873 msgs : bag_analyser/RawIMUData My problem is that I still cannot read the file. I tried using **'rosbag play'** but it simply gives me: [ INFO] [1402332944.127314834]: Opening converted_bag.bag [FATAL] [1402332944.127848761]: Error opening file: converted_bag.bag I also tried reading in the file using the rosbag API, which also fails opening the file (code in image_converter.cpp below). I changed the bagfile's permissions and set it to allow read/write/execute just to make sure it wasn't that. **What am I doing wrong? How can I read the data from the file?** I am fairly new to ROS, so any help or tips are greatly appreciated!


**File listings:**
---------------------------------------- folder bag/analyser/msg

ImageSet.msg Header header sensor_msgs/Image[] images RawIMUData.msg Header header # Measured accelerations (in x, y, z) Vector3f acc # Measured angular speeds (around x, y, z) Vector3f gyro # Measured magnetic field (around x , y, z) Vector3f mag Vector3f.msg float32 x float32 y float32 z AttitudeData.msg Header header # roll angle [rad] float32 roll # pitch angle [rad] float32 pitch # yaw angle [rad] float32 yaw # roll angular speed [rad/s] float32 rollspeed # pitch angular speed [rad/s] float32 pitchspeed # yaw angular speed [rad/s] float32 yawspeed bag_analyser/image_converter.cpp ( includes message definition for ImageSet.h and tries to use rosbag API to read file):
#include #include #include #include #include #include #include #include #include #include #include #include #include static const std::string OPENCV_WINDOW = "Image window"; void loadBag() { rosbag::Bag bag("~/robotics/rosbook/src/bag_analyser/bag/converted_bag.bag"); // fails here std::string l_cam_image = "/capture"; std::vector topics; topics.push_back(l_cam_image); rosbag::View view(bag, rosbag::TopicQuery(topics)); BOOST_FOREACH(rosbag::MessageInstance const m, view) { if (m.getTopic() == l_cam_image || ("/" + m.getTopic() == l_cam_image)) { boost::shared_ptr image_array = m.instantiate(); bag_analyser::ImageSet* theSetPtr = image_array.get(); bag_analyser::ImageSet theSet = *theSetPtr; cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(theSetPtr->images[0], sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); cv::waitKey(3); } } bag.close(); } int main(int argc, char** argv) { ros::init(argc, argv, "image_converter"); loadBag(); ros::spin(); return 0; } bag_analyser/CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3) project(bag_analyser) ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation roscpp rospy std_msgs rosbag ) ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) find_package( OpenCV REQUIRED ) ## Generate messages in the 'msg' folder add_message_files( FILES ImageSet.msg RawIMUData.msg Vector3f.msg AttitudeData.msg ) ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES std_msgs sensor_msgs ) ## CATKIN_DEPENDS: catkin_packages dependent projects also need catkin_package( CATKIN_DEPENDS message_runtime ) # include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) ## Declare a cpp executable add_executable(image_converter src/image_converter.cpp) target_link_libraries ( image_converter ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ) rules.bmr
class update_pximu_RawIMUData_0380911195329d307d8f033bc714dbbf(MessageUpdateRule): old_type = "pximu/RawIMUData" old_full_text = """ Header header # Measured accelerations (in x, y, z) Vector3f acc # Measured angular speeds (around x, y, z) Vector3f gyro # Measured magnetic field (around x , y, z) Vector3f mag ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id ================================================================================ MSG: pximu/Vector3f float32 x float32 y float32 z """ new_type = "bag_analyser/RawIMUData" new_full_text = """ Header header # Measured accelerations (in x, y, z) Vector3f acc # Measured angular speeds (around x, y, z) Vector3f gyro # Measured magnetic field (around x , y, z) Vector3f mag ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id ================================================================================ MSG: bag_analyser/Vector3f float32 x float32 y float32 z """ order = 0 migrated_types = [ ("Header","Header"),] valid = True def update(self, old_msg, new_msg): self.migrate(old_msg.header, new_msg.header) #No migration path between [Vector3f] and [Vector3f] new_msg.acc = self.get_new_class('bag_analyser/Vector3f')(old_msg.acc) #No migration path between [Vector3f] and [Vector3f] new_msg.gyro = self.get_new_class('bag_analyser/Vector3f')(old_msg.gyro) #No migration path between [Vector3f] and [Vector3f] new_msg.mag = self.get_new_class('bag_analyser/Vector3f')(old_msg.mag) class update_libks_ImageSet_b3ef08c9cff052c83f5039d098add069(MessageUpdateRule): old_type = "libks/ImageSet" old_full_text = """ Header header sensor_msgs/Image[] images ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id ================================================================================ MSG: sensor_msgs/Image # This message contains an uncompressed image # (0, 0) is at top-left corner of image # Header header # Header timestamp should be acquisition time of image # Header frame_id should be optical frame of camera # origin of frame should be optical center of cameara # +x should point to the right in the image # +y should point down in the image # +z should point into to plane of the image # If the frame_id here and the frame_id of the CameraInfo # message associated with the image conflict # the behavior is undefined uint32 height # image height, that is, number of rows uint32 width # image width, that is, number of columns # The legal values for encoding are in file src/image_encodings.cpp # If you want to standardize a new string format, join # ros-users@lists.sourceforge.net and send an email proposing a new encoding. string encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in src/image_encodings.cpp uint8 is_bigendian # is this data bigendian? uint32 step # Full row length in bytes uint8[] data # actual matrix data, size is (step * rows) """ new_type = "bag_analyser/ImageSet" new_full_text = """ Header header sensor_msgs/Image[] images ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id ================================================================================ MSG: sensor_msgs/Image # This message contains an uncompressed image # (0, 0) is at top-left corner of image # Header header # Header timestamp should be acquisition time of image # Header frame_id should be optical frame of camera # origin of frame should be optical center of cameara # +x should point to the right in the image # +y should point down in the image # +z should point into to plane of the image # If the frame_id here and the frame_id of the CameraInfo # message associated with the image conflict # the behavior is undefined uint32 height # image height, that is, number of rows uint32 width # image width, that is, number of columns # The legal values for encoding are in file src/image_encodings.cpp # If you want to standardize a new string format, join # ros-users@lists.sourceforge.net and send an email proposing a new encoding. string encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.h uint8 is_bigendian # is this data bigendian? uint32 step # Full row length in bytes uint8[] data # actual matrix data, size is (step * rows) """ order = 0 migrated_types = [ ("Header","Header"), ("sensor_msgs/Image","sensor_msgs/Image"),] valid = True def update(self, old_msg, new_msg): self.migrate(old_msg.header, new_msg.header) self.migrate_array(old_msg.images, new_msg.images, "sensor_msgs/Image") class update_pximu_AttitudeData_bc609da5ddac9016c096c67da33d8b9c(MessageUpdateRule): old_type = "pximu/AttitudeData" old_full_text = """ Header header # roll angle [rad] float32 roll # pitch angle [rad] float32 pitch # yaw angle [rad] float32 yaw # roll angular speed [rad/s] float32 rollspeed # pitch angular speed [rad/s] float32 pitchspeed # yaw angular speed [rad/s] float32 yawspeed ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id """ new_type = "bag_analyser/AttitudeData" new_full_text = """ Header header # roll angle [rad] float32 roll # pitch angle [rad] float32 pitch # yaw angle [rad] float32 yaw # roll angular speed [rad/s] float32 rollspeed # pitch angular speed [rad/s] float32 pitchspeed # yaw angular speed [rad/s] float32 yawspeed ================================================================================ MSG: std_msgs/Header # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # # sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id """ order = 0 migrated_types = [ ("Header","Header"),] valid = True def update(self, old_msg, new_msg): self.migrate(old_msg.header, new_msg.header) new_msg.roll = old_msg.roll new_msg.pitch = old_msg.pitch new_msg.yaw = old_msg.yaw new_msg.rollspeed = old_msg.rollspeed new_msg.pitchspeed = old_msg.pitchspeed new_msg.yawspeed = old_msg.yawspeed

Viewing all articles
Browse latest Browse all 475

Trending Articles



<script src="https://jsc.adskeeper.com/r/s/rssing.com.1596347.js" async> </script>