feat(ros): add mesh to rviz

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -33,6 +33,8 @@
 
   <arg name="gravity" default="9.8" />
 
+  <arg name="mesh_file" default="S1030-0315.STL" />
+
   <!-- stream toggles -->
 
   <!-- Request index -->
@@ -233,6 +235,7 @@
       <param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
       <param name="points_topic" value="$(arg points_topic)" />
       <param name="depth_topic" value="$(arg depth_topic)" />
+      <param name="mesh_file" value="$(arg mesh_file)" />
 
       <param name="left_mono_topic" value="$(arg left_mono_topic)" />
       <param name="right_mono_topic" value="$(arg right_mono_topic)" />

wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz

@@ -7,11 +7,11 @@ Panels:
         - /Global Options1
         - /Status1
         - /Images1
-        - /Images1/Gray1
         - /Images1/Rectified1
         - /Disparity1
+        - /Marker1
       Splitter Ratio: 0.5
-    Tree Height: 853
+    Tree Height: 824
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -30,7 +30,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Left
+    SyncSource: Depth
 Visualization Manager:
   Class: ""
   Displays:
@@ -187,6 +187,14 @@ Visualization Manager:
       Topic: /mynteye/imu/data_raw
       Unreliable: false
       Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /mynteye/camera_mesh
+      Name: Marker
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -241,14 +249,14 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1056
+  Height: 1026
   Hide Left Dock: false
   Hide Right Dock: false
   Left:
     collapsed: false
   LeftRect:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000017e00000396fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000040c000000860000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000001bc000002170000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004a60000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000026500000378fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000047a000001d90000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000002160000024c0000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003bf0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Right:
     collapsed: false
   RightRect:

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -21,6 +21,8 @@
 #include <sensor_msgs/Temperature.h>
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/distortion_models.h>
+#include <visualization_msgs/Marker.h>
 #include <tf/tf.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
@@ -346,6 +348,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                         sensor_msgs::Temperature>(temperature_topic, 100);
     NODELET_INFO_STREAM("Advertized on topic " << temperature_topic);
 
+    pub_mesh_ = nh_.advertise<visualization_msgs::Marker>("camera_mesh", 0 );
+    // where to get the mesh from
+    std::string mesh_file;
+    if (private_nh_.getParam("mesh_file", mesh_file)) {
+      mesh_msg_.mesh_resource = "package://mynt_eye_ros_wrapper/mesh/"+mesh_file;
+    } else {
+      LOG(INFO) << "no mesh found for visualisation, set ros param mesh_file, if desired";
+      mesh_msg_.mesh_resource = "";
+    }
+
     // stream toggles
 
     for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
@@ -611,6 +623,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   }
 
   void publishTopics() {
+    publishMesh();
+
     if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
         mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
         !is_published_[Stream::LEFT]) {
@@ -1144,6 +1158,49 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     return res;
   }
 
+  void publishMesh() {
+    // also do the mesh
+    /*if (parameters_.publishing.trackedBodyFrame == FrameName::S) {
+      mesh_msg_.child_frame_id = "sensor";
+    } else if (parameters_.publishing.trackedBodyFrame == FrameName::B) {
+      mesh_msg_.child_frame_id = "body";
+    } else {
+      mesh_msg_.child_frame_id = "body";
+    }*/
+    mesh_msg_.header.frame_id = base_frame_id_;
+    mesh_msg_.header.stamp = ros::Time::now();
+    mesh_msg_.type = visualization_msgs::Marker::MESH_RESOURCE;
+    // if ((ros::Time::now() - _t).toSec() > 10.0)
+    //   mesh_msg_.header.stamp = ros::Time::now();
+
+    // fill orientation
+    mesh_msg_.pose.orientation.x = 0;
+    mesh_msg_.pose.orientation.y = 0;
+    mesh_msg_.pose.orientation.z = 0;
+    mesh_msg_.pose.orientation.w = 0;
+
+    // fill position
+    mesh_msg_.pose.position.x = 0;
+    mesh_msg_.pose.position.y = 0;
+    mesh_msg_.pose.position.z = -0.3;
+
+    // scale -- needed
+    mesh_msg_.scale.x = 0.01;
+    mesh_msg_.scale.y = 0.01;
+    mesh_msg_.scale.z = 0.01;
+
+    mesh_msg_.action = visualization_msgs::Marker::ADD;
+    mesh_msg_.color.a = 1.0;  // Don't forget to set the alpha!
+    mesh_msg_.color.r = 1.0;
+    mesh_msg_.color.g = 1.0;
+    mesh_msg_.color.b = 1.0;
+
+    // embedded material / colour
+    // mesh_msg_.mesh_use_embedded_materials = true;
+    if (!mesh_msg_.mesh_resource.empty())
+      pub_mesh_.publish(mesh_msg_);  // publish stamped mesh
+  }
+
 
   void computeRectTransforms() {
     ROS_ASSERT(api_);
@@ -1246,7 +1303,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
         }
       }
 
-      camera_info->distortion_model = "plumb_bob";
+      camera_info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;  // plumb_bob
 
       // D of plumb_bob: (k1, k2, t1, t2, k3)
       for (int i = 0; i < 5; i++) {
@@ -1257,7 +1314,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       in -> ResizeIntrinsics();
       camera_info->width = in_base->width;
       camera_info->height = in_base->height;
-      camera_info->distortion_model = "kannala_brandt";
+      camera_info->distortion_model = sensor_msgs::distortion_models::EQUIDISTANT;  // kannala_brandt
 
       // coeffs: k2,k3,k4,k5,mu,mv,u0,v0
       camera_info->D.push_back(in->coeffs[0]);  // k2
@@ -1478,6 +1535,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   ros::Publisher pub_imu_;
   ros::Publisher pub_temperature_;
 
+  ros::Publisher pub_mesh_;  // < The publisher for camera mesh.
+  visualization_msgs::Marker mesh_msg_;  // < Mesh message.
+
   tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 
   ros::ServiceServer get_info_service_;