Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added conversion from temporary bag type to StampedMessage type #85

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Added conversion from temporary bag type to StampedMessage type #85

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ca73ead
added conversion from temporary bag type to StampedMessage type
Kaju-Bubanja Sep 12, 2018
e07a84a
added more accuracy in reporting timestamps, addded mono8 conversion
Kaju-Bubanja Jan 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 18 additions & 2 deletions hand_eye_calibration/bin/tf_to_csv.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,28 @@

import numpy as np

import geometry_msgs
import rosbag
import rospy
import tf
from tf2_msgs.msg import TFMessage
import warnings


def bag_type_to_geometry_msgs(msg_tf):
casted_msg = geometry_msgs.msg.TransformStamped()
casted_msg.header = msg_tf.header
casted_msg.child_frame_id = msg_tf.child_frame_id
casted_msg.transform.translation.x = msg_tf.transform.translation.x
casted_msg.transform.translation.y = msg_tf.transform.translation.y
casted_msg.transform.translation.z = msg_tf.transform.translation.z
casted_msg.transform.rotation.x = msg_tf.transform.rotation.x
casted_msg.transform.rotation.y = msg_tf.transform.rotation.y
casted_msg.transform.rotation.z = msg_tf.transform.rotation.z
casted_msg.transform.rotation.w = msg_tf.transform.rotation.w
return casted_msg


def write_transformation_to_csv_file(bag_file, target_frame, source_frame,
csv_file_name):
print("Loading tfs into Transformer...")
Expand All @@ -21,7 +36,8 @@ def write_transformation_to_csv_file(bag_file, target_frame, source_frame,

for topic, msg, t in bag.read_messages(topics=['/tf']):
for msg_tf in msg.transforms:
tf_tree.setTransform(msg_tf)
casted_msg = bag_type_to_geometry_msgs(msg_tf)
tf_tree.setTransform(casted_msg)
bag.close()

print("Listening to tf transformation from ", source_frame, " to ",
Expand Down Expand Up @@ -66,7 +82,7 @@ def write_transformation_to_csv_file(bag_file, target_frame, source_frame,
# Write to csv file.
quaternion = np.array(hamilton_quaternion)
csv_file.write(
str(single_tf.header.stamp.to_sec()) + ', ' +
str("{:.9f}".format(single_tf.header.stamp.to_nsec()/1000000000.0)) + ', ' +
str(translation[0]) + ', ' + str(translation[1]) + ', ' +
str(translation[2]) + ', ' + str(quaternion[0]) + ', ' +
str(quaternion[1]) + ', ' + str(quaternion[2]) + ', ' +
Expand Down
11 changes: 6 additions & 5 deletions hand_eye_calibration_target_extractor/src/target_extractor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,9 +116,7 @@ int main(int argc, char** argv) {
sensor_msgs::ImageConstPtr image_message =
message.instantiate<sensor_msgs::Image>();
LOG_IF(FATAL, !image_message) << "Can only process image messages.";

timestamps.push_back(image_message->header.stamp.toSec());

// Convert image to cv::Mat.
cv::Mat image;
if ((image_message->encoding == "16UC1") ||
Expand All @@ -131,7 +129,6 @@ int main(int argc, char** argv) {
img.step = image_message->step;
img.data = image_message->data;
img.encoding = "mono16";

// Scale up the image to take use of the full 16bit range before encoding
// it to 8bit range.
cv_bridge::CvImagePtr cv_ptr_tmp;
Expand All @@ -152,8 +149,12 @@ int main(int argc, char** argv) {
} else {
cv_bridge::CvImageConstPtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvShare(image_message,
sensor_msgs::image_encodings::MONO8);
if(image_message->encoding == "8UC1"){
cv_ptr = cv_bridge::toCvShare(image_message);
}
else{
cv_ptr = cv_bridge::toCvShare(image_message, sensor_msgs::image_encodings::MONO8);
}
} catch (const cv_bridge::Exception& e) {
LOG(FATAL) << "cv_bridge exception: " << e.what();
}
Expand Down