Adding rotation to image_plugin

swri-robotics · May 10, 2020 · 13875a7 · 13875a7
1 parent df720cd
commit 13875a7
Show file tree

Hide file tree

Showing 3 changed files with 238 additions and 129 deletions.
diff --git a/mapviz_plugins/include/mapviz_plugins/image_plugin.h b/mapviz_plugins/include/mapviz_plugins/image_plugin.h
@@ -113,6 +113,7 @@ namespace mapviz_plugins
     void SetHeight(double height);
     void SetSubscription(bool visible);
     void SetTransport(const QString& transport);
+    void SetRotation(QString rotation);
     void KeepRatioChanged(bool checked);
 
   private:
@@ -127,12 +128,14 @@ namespace mapviz_plugins
     double width_;
     double height_;
     std::string transport_;
+    int rotation_;
 
     bool force_resubscribe_;
     bool has_image_;
 
     double last_width_;
     double last_height_;
+    int last_rotation_;
     double original_aspect_ratio_;
 
     ros::NodeHandle local_node_;
@@ -143,6 +146,7 @@ namespace mapviz_plugins
 
     cv_bridge::CvImagePtr cv_image_;
     cv::Mat scaled_image_;
+    cv::Mat rotated_image_;
 
     void imageCallback(const sensor_msgs::ImageConstPtr& image);
 
@@ -151,6 +155,7 @@ namespace mapviz_plugins
 
     std::string AnchorToString(Anchor anchor);
     std::string UnitsToString(Units units);
+    std::string RotationToString(int rotation);
   };
 }
 

diff --git a/mapviz_plugins/src/image_plugin.cpp b/mapviz_plugins/src/image_plugin.cpp
@@ -59,6 +59,7 @@ namespace mapviz_plugins
     width_(320),
     height_(240),
     transport_("default"),
+    rotation_(-1),
     has_image_(false),
     last_width_(0),
     last_height_(0),
@@ -88,6 +89,7 @@ namespace mapviz_plugins
     QObject::connect(ui_.keep_ratio, SIGNAL(toggled(bool)), this, SLOT(KeepRatioChanged(bool)));
     QObject::connect(ui_.transport_combo_box, SIGNAL(activated(const QString&)),
                      this, SLOT(SetTransport(const QString&)));
+    QObject::connect(ui_.rotation, SIGNAL(activated(QString)), this, SLOT(SetRotation(QString)));
 
     ui_.width->setKeyboardTracking(false);
     ui_.height->setKeyboardTracking(false);
@@ -217,6 +219,26 @@ namespace mapviz_plugins
     TopicEdited();
   }
 
+  void ImagePlugin::SetRotation(QString rotation)
+  {
+
+    if (rotation == "90°")
+    {
+      rotation_ = cv::ROTATE_90_CLOCKWISE;
+    }
+    else if (rotation == "180°")
+    {
+      rotation_ = cv::ROTATE_180;
+    }
+    else if (rotation == "270°")
+    {
+      rotation_ = cv::ROTATE_90_COUNTERCLOCKWISE;
+    }
+    else{
+      rotation_ = -1;
+    }
+  }
+
   void ImagePlugin::KeepRatioChanged(bool checked)
   {
     ui_.height->setEnabled( !checked );
@@ -446,6 +468,19 @@ namespace mapviz_plugins
       ScaleImage(width, height);
     }
 
+    // Rotate the source image if necessary
+    if(rotation_ >= cv::ROTATE_90_CLOCKWISE && rotation_ <= cv::ROTATE_90_COUNTERCLOCKWISE){
+      cv::rotate(scaled_image_, rotated_image_, rotation_);
+      //-- If we did not rotate 180, we have flipped width / height
+      if(rotation_ != cv::ROTATE_180){
+        height = width_;
+        width = height_;
+      }
+    }
+    else{
+      rotated_image_ = scaled_image_;
+    }
+
     // Calculate the correct render position
     double x_pos = 0;
     double y_pos = 0;
@@ -502,7 +537,7 @@ namespace mapviz_plugins
 
     glRasterPos2d(x_pos, y_pos);
 
-    DrawIplImage(&scaled_image_);
+    DrawIplImage(&rotated_image_);
 
     glPopMatrix();
 
@@ -546,6 +581,14 @@ namespace mapviz_plugins
       SetAnchor(anchor.c_str());
     }
 
+    if (node["rotation"])
+    {
+      std::string rotation;
+      node["rotation"] >> rotation;
+      ui_.rotation->setCurrentIndex(ui_.rotation->findText(rotation.c_str()));
+      SetRotation(rotation.c_str());
+    }
+
     if (node["units"])
     {
       std::string units;
@@ -597,6 +640,7 @@ namespace mapviz_plugins
     emitter << YAML::Key << "height" << YAML::Value << height_;
     emitter << YAML::Key << "keep_ratio" << YAML::Value << ui_.keep_ratio->isChecked();
     emitter << YAML::Key << "image_transport" << YAML::Value << transport_;
+    emitter << YAML::Key << "rotation" << YAML::Value << RotationToString(rotation_);
   }
 
   std::string ImagePlugin::AnchorToString(Anchor anchor)
@@ -659,6 +703,20 @@ namespace mapviz_plugins
     return units_string;
   }
 
+  std::string ImagePlugin::RotationToString(int rotation)
+  {
+    std::string rotation_string = "0°";
+
+    if (rotation == cv::ROTATE_90_CLOCKWISE)
+      rotation_string = "90°";
+    else if (rotation == cv::ROTATE_180)
+      rotation_string = "180°";
+    else if (rotation == cv::ROTATE_90_COUNTERCLOCKWISE)
+      rotation_string = "270°";
+
+    return rotation_string;
+  }
+
   void ImagePlugin::CreateLocalNode()
   {
     // This is the same way ROS generates anonymous node names.