From 934c803c2566c44651fa4cc3cd225a204bf594eb Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Sun, 15 Apr 2018 10:36:59 +0800
Subject: [PATCH] Publish temp in ros wrapper

---
 .../src/mynt_eye_ros_wrapper/CMakeLists.txt   | 14 ++++++++++-
 .../launch/mynteye.launch                     |  4 ++++
 .../ros/src/mynt_eye_ros_wrapper/msg/Temp.msg |  2 ++
 .../ros/src/mynt_eye_ros_wrapper/package.xml  |  2 ++
 .../src/wrapper_nodelet.cc                    | 23 +++++++++++++++++++
 5 files changed, 44 insertions(+), 1 deletion(-)
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
index e925dc6..79548f1 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
@@ -1,7 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(mynt_eye_ros_wrapper)
 
-
 get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE)
 
 if(NOT CMAKE_BUILD_TYPE)
@@ -26,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   geometry_msgs
   image_transport
+  message_runtime
   nodelet
   roscpp
   sensor_msgs
@@ -42,6 +42,18 @@ checkPackage("sensor_msgs" "")
 checkPackage("std_msgs" "")
 checkPackage("tf" "")
 
+## messages
+
+add_message_files(
+  FILES
+  Temp.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
 catkin_package(
   CATKIN_DEPENDS cv_bridge geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs tf
 )
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
index ec071a1..f601210 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -7,11 +7,13 @@
   <arg name="left_topic" default="left" />
   <arg name="right_topic" default="right" />
   <arg name="imu_topic" default="imu" />
+  <arg name="temp_topic" default="temp" />
 
   <arg name="base_frame_id" default="$(arg mynteye)_link" />
   <arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
   <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
   <arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
+  <arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
 
   <arg name="gravity" default="9.8" />
 
@@ -72,11 +74,13 @@
       <param name="left_topic" value="$(arg left_topic)" />
       <param name="right_topic" value="$(arg right_topic)" />
       <param name="imu_topic" value="$(arg imu_topic)" />
+      <param name="temp_topic" value="$(arg temp_topic)" />
 
       <param name="base_frame_id" value="$(arg base_frame_id)" />
       <param name="left_frame_id" value="$(arg left_frame_id)" />
       <param name="right_frame_id" value="$(arg right_frame_id)" />
       <param name="imu_frame_id" value="$(arg imu_frame_id)" />
+      <param name="temp_frame_id" value="$(arg temp_frame_id)" />
 
       <param name="gravity" value="$(arg gravity)" />
 
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg b/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg
new file mode 100644
index 0000000..2072419
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg
@@ -0,0 +1,2 @@
+std_msgs/Header header
+float32 data
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
index 68c3326..4c7779d 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
@@ -16,6 +16,7 @@
   <build_depend>cv_bridge</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -34,6 +35,7 @@
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>image_transport</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
   <exec_depend>nodelet</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 60ceacf..04d9f63 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -7,6 +7,8 @@
 #include <sensor_msgs/image_encodings.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
+#include <mynt_eye_ros_wrapper/Temp.h>
+
 #include <glog/logging.h>
 
 #define _USE_MATH_DEFINES
@@ -63,18 +65,22 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     std::string left_topic = "left";
     std::string right_topic = "right";
     std::string imu_topic = "imu";
+    std::string temp_topic = "temp";
     private_nh_.getParam("left_topic", left_topic);
     private_nh_.getParam("right_topic", right_topic);
     private_nh_.getParam("imu_topic", imu_topic);
+    private_nh_.getParam("temp_topic", temp_topic);
 
     base_frame_id_ = "camera_link";
     left_frame_id_ = "camera_left_frame";
     right_frame_id_ = "camera_right_frame";
     imu_frame_id_ = "camera_imu_frame";
+    temp_frame_id_ = "camera_temp_frame";
     private_nh_.getParam("base_frame_id", base_frame_id_);
     private_nh_.getParam("left_frame_id", left_frame_id_);
     private_nh_.getParam("right_frame_id", right_frame_id_);
     private_nh_.getParam("imu_frame_id", imu_frame_id_);
+    private_nh_.getParam("temp_frame_id", temp_frame_id_);
 
     gravity_ = 9.8;
     private_nh_.getParam("gravity", gravity_);
@@ -120,6 +126,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
     NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
 
+    // temp publisher
+
+    pub_temp_ = nh_.advertise<mynt_eye_ros_wrapper::Temp>(temp_topic, 1);
+    NODELET_INFO_STREAM("Advertized on topic " << temp_topic);
+
     publishStaticTransforms();
     publishTopics();
   }
@@ -230,6 +241,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
       ++imu_count_;
       publishImu(data, imu_count_, stamp);
+      publishTemp(data.imu->temperature, imu_count_, stamp);
       NODELET_DEBUG_STREAM(
           "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
                         << ", timestamp: " << data.imu->timestamp
@@ -314,6 +326,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     pub_imu_.publish(msg);
   }
 
+  void publishTemp(float temperature, std::uint32_t seq, ros::Time stamp) {
+    mynt_eye_ros_wrapper::Temp msg;
+    msg.header.seq = seq;
+    msg.header.stamp = stamp;
+    msg.header.frame_id = temp_frame_id_;
+    msg.data = temperature;
+    pub_temp_.publish(msg);
+  }
+
  private:
   void initDevice() {
     NODELET_INFO_STREAM("Detecting MYNT EYE devices");
@@ -360,6 +381,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   image_transport::Publisher pub_right_;
 
   ros::Publisher pub_imu_;
+  ros::Publisher pub_temp_;
 
   tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 
@@ -369,6 +391,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   std::string left_frame_id_;
   std::string right_frame_id_;
   std::string imu_frame_id_;
+  std::string temp_frame_id_;
 
   double gravity_;