Fix frames if it is empty to rotate image

MartinPeris · Jul 16, 2016 · 29d95f1 · 29d95f1
1 parent 6adf715
commit 29d95f1
Showing 1 changed file with 6 additions and 4 deletions.
diff --git a/image_rotate/src/nodelet/image_rotate_nodelet.cpp b/image_rotate/src/nodelet/image_rotate_nodelet.cpp
@@ -118,19 +118,21 @@ class ImageRotateNodelet : public nodelet::Nodelet
     try
     {
       std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
+      std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg);
+      std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg);
 
       // Transform the target vector into the image frame.
       target_vector_.header.stamp = msg->header.stamp;
-      target_vector_.header.frame_id = frameWithDefault(config_.target_frame_id, input_frame_id);
+      target_vector_.header.frame_id = target_frame_id;
       geometry_msgs::Vector3Stamped target_vector_transformed;
-      geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(config_.target_frame_id, input_frame_id, msg->header.stamp);
+      geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(target_frame_id, input_frame_id, msg->header.stamp);
       tf2::doTransform(target_vector_, target_vector_transformed, transform);
 
       // Transform the source vector into the image frame.
       source_vector_.header.stamp = msg->header.stamp;
-      source_vector_.header.frame_id = frameWithDefault(config_.source_frame_id, input_frame_id);
+      source_vector_.header.frame_id = source_frame_id;
       geometry_msgs::Vector3Stamped source_vector_transformed;
-      transform = tf_buffer_.lookupTransform(config_.source_frame_id, input_frame_id, msg->header.stamp);
+      transform = tf_buffer_.lookupTransform(source_frame_id, input_frame_id, msg->header.stamp);
       tf2::doTransform(source_vector_, source_vector_transformed, transform);
 
       //NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());