Merge pull request #2579 from sktometometo/PR/add-panorama-info

jsk-ros-pkg · Jul 11, 2021 · 30f8b7a · 30f8b7a
2 parents 7a86706 + 8fd2d65
commit 30f8b7a
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 0 deletions.
diff --git a/jsk_perception/include/jsk_perception/dual_fisheye_to_panorama.h b/jsk_perception/include/jsk_perception/dual_fisheye_to_panorama.h
@@ -40,6 +40,7 @@
 #include <jsk_topic_tools/diagnostic_nodelet.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <jsk_recognition_msgs/PanoramaInfo.h>
 
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -77,6 +78,9 @@ namespace jsk_perception
     boost::shared_ptr<stitcher::FisheyeStitcher> stitcher_;
     ros::Subscriber sub_image_;
     ros::Publisher pub_panorama_image_;
+    ros::Publisher pub_panorama_info_;
+
+    jsk_recognition_msgs::PanoramaInfo msg_panorama_info_;
 
     bool   enb_lc_;
     bool   enb_ra_;

diff --git a/jsk_perception/src/dual_fisheye_to_panorama.cpp b/jsk_perception/src/dual_fisheye_to_panorama.cpp
@@ -38,6 +38,7 @@
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
+#include <jsk_recognition_msgs/PanoramaInfo.h>
 #include <algorithm>
 #include <math.h>
 #include <boost/assign.hpp>
@@ -60,13 +61,20 @@ namespace jsk_perception
     ROS_INFO("save_unwarped: %7.3f", save_unwarped_?"true":"false");
     ROS_INFO("mls_map_path : %s", mls_map_path_.c_str());
     pub_panorama_image_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
+    pub_panorama_info_ = advertise<jsk_recognition_msgs::PanoramaInfo>(*pnh_, "panorama_info", 1);
 
     sticher_initialized_ = false;
     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
     dynamic_reconfigure::Server<Config>::CallbackType f =
       boost::bind (&DualFisheyeToPanorama::configCallback, this, _1, _2);
     srv_->setCallback (f);
 
+    msg_panorama_info_.projection_model = "equirectangular";
+    msg_panorama_info_.theta_min = 0;
+    msg_panorama_info_.theta_max = M_PI;
+    msg_panorama_info_.phi_min = -M_PI;
+    msg_panorama_info_.phi_max = M_PI;
+
     onInitPostProcess();
   }
 
@@ -118,6 +126,8 @@ namespace jsk_perception
     pub_panorama_image_.publish(cv_bridge::CvImage(image_msg->header,
                                                    image_msg->encoding,
                                                    pano).toImageMsg());
+    msg_panorama_info_.header = image_msg->header;
+    pub_panorama_info_.publish(msg_panorama_info_);
   }
 }
 

diff --git a/jsk_recognition_msgs/CMakeLists.txt b/jsk_recognition_msgs/CMakeLists.txt
@@ -34,6 +34,7 @@ add_message_files(
   ModelCoefficientsArray.msg
   Object.msg
   ObjectArray.msg
+  PanoramaInfo.msg  #  added on jsk_recognition_msgs until https://github.com/ros/common_msgs/pull/171 merged
   ParallelEdgeArray.msg
   ParallelEdge.msg
   PlotData.msg

diff --git a/jsk_recognition_msgs/msg/PanoramaInfo.msg b/jsk_recognition_msgs/msg/PanoramaInfo.msg
@@ -0,0 +1,19 @@
+# This message defines meta information for a panorama image.
+
+#######################################################################
+#                     Image acquisition info                          #
+#######################################################################
+std_msgs/Header header
+
+#######################################################################
+#                      Image projection info                          #
+#######################################################################
+string projection_model
+
+#######################################################################
+#                     Field of View Parameters                         #
+#######################################################################
+float64 theta_min
+float64 theta_max
+float64 phi_min
+float64 phi_max