Add get_info service to ros wrapper

wrappers/ros/README.md

@@ -32,6 +32,15 @@ source wrappers/ros/devel/setup.bash
 roslaunch mynt_eye_ros_wrapper display.launch
 ```
 
+## Test
+
+Test `get_info` service,
+
+```bash
+source wrappers/ros/devel/setup.bash
+rosrun mynt_eye_ros_wrapper get_device_info.py
+```
+
 ## ROS Indigo
 
 How to install ROS Indigo (Ubuntu 14.04),

wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt

@@ -66,6 +66,11 @@ add_message_files(
   Temp.msg
 )
 
+add_service_files(
+  FILES
+  GetInfo.srv
+)
+
 generate_messages(
   DEPENDENCIES
   std_msgs
@@ -113,6 +118,11 @@ endif()
 
 # install
 
+#install(PROGRAMS
+#  scripts/get_device_info.py
+#  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#)
+
 install(TARGETS mynteye_wrapper mynteye_wrapper_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Copyright 2018 Slightech Co., Ltd. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# pylint: disable=missing-docstring
+
+from __future__ import print_function
+
+import sys
+
+import rospy
+
+# pylint: disable=wildcard-import, unused-wildcard-import
+from mynt_eye_ros_wrapper.srv import *
+
+
+def get_device_info(**keys):
+  if not keys:
+    sys.exit('get_device_info without keys :(')
+
+  rospy.wait_for_service('mynteye/get_info', 1)
+
+  try:
+    get_info = rospy.ServiceProxy('mynteye/get_info', GetInfo)
+
+    result = {}
+    for key_name, key_value in keys.items():
+      result[key_name] = get_info(key_value).value
+
+    return result
+  except rospy.ServiceException as error:
+    sys.exit('Failed to call service: {}'.format(error))
+
+
+def main():
+  keys = {
+      'DEVICE_NAME': GetInfoRequest.DEVICE_NAME,
+      'SERIAL_NUMBER': GetInfoRequest.SERIAL_NUMBER,
+      'FIRMWARE_VERSION': GetInfoRequest.FIRMWARE_VERSION,
+      'HARDWARE_VERSION': GetInfoRequest.HARDWARE_VERSION,
+      'SPEC_VERSION': GetInfoRequest.SPEC_VERSION,
+      'LENS_TYPE': GetInfoRequest.LENS_TYPE,
+      'IMU_TYPE': GetInfoRequest.IMU_TYPE,
+      'NOMINAL_BASELINE': GetInfoRequest.NOMINAL_BASELINE,
+  }
+  for k, v in get_device_info(**keys).items():
+    print('{}: {}'.format(k, v))
+
+
+if __name__ == '__main__':
+  main()

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

@@ -23,6 +23,7 @@
 #include <tf/tf.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
+#include <mynt_eye_ros_wrapper/GetInfo.h>
 #include <mynt_eye_ros_wrapper/Temp.h>
 
 #include <glog/logging.h>
@@ -56,7 +57,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       double time_end = ros::Time::now().toSec();
       double time_elapsed = time_end - time_beg_;
 
-      LOG(INFO) << "Time elapsed: " << time_elapsed << "s";
+      LOG(INFO) << "Time elapsed: " << time_elapsed << " s";
       LOG(INFO) << "Left count: " << left_count_
                 << ", fps: " << (left_count_ / time_elapsed);
       LOG(INFO) << "Right count: " << right_count_
@@ -191,10 +192,53 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
     }
 
+    // services
+
+    const std::string DEVICE_INFO_SERVICE = "get_info";
+    get_info_service_ = nh_.advertiseService(
+        DEVICE_INFO_SERVICE, &ROSWrapperNodelet::getInfo, this);
+    NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
+
     publishStaticTransforms();
     publishTopics();
   }
 
+  bool getInfo(
+      mynt_eye_ros_wrapper::GetInfo::Request &req,     // NOLINT
+      mynt_eye_ros_wrapper::GetInfo::Response &res) {  // NOLINT
+    using Request = mynt_eye_ros_wrapper::GetInfo::Request;
+    switch (req.key) {
+      case Request::DEVICE_NAME:
+        res.value = api_->GetInfo(Info::DEVICE_NAME);
+        break;
+      case Request::SERIAL_NUMBER:
+        res.value = api_->GetInfo(Info::SERIAL_NUMBER);
+        break;
+      case Request::FIRMWARE_VERSION:
+        res.value = api_->GetInfo(Info::FIRMWARE_VERSION);
+        break;
+      case Request::HARDWARE_VERSION:
+        res.value = api_->GetInfo(Info::HARDWARE_VERSION);
+        break;
+      case Request::SPEC_VERSION:
+        res.value = api_->GetInfo(Info::SPEC_VERSION);
+        break;
+      case Request::LENS_TYPE:
+        res.value = api_->GetInfo(Info::LENS_TYPE);
+        break;
+      case Request::IMU_TYPE:
+        res.value = api_->GetInfo(Info::IMU_TYPE);
+        break;
+      case Request::NOMINAL_BASELINE:
+        res.value = api_->GetInfo(Info::NOMINAL_BASELINE);
+        break;
+      default:
+        NODELET_WARN_STREAM("Info of key " << req.key << " not exist");
+        return false;
+    }
+    return true;
+  }
+
   void publishTopics() {
     api_->SetStreamCallback(
         Stream::LEFT, [this](const api::StreamData &data) {
@@ -728,6 +772,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
   tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 
+  ros::ServiceServer get_info_service_;
+
   // node params
 
   std::string base_frame_id_;

wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv

@@ -0,0 +1,12 @@
+uint32 DEVICE_NAME=0
+uint32 SERIAL_NUMBER=1
+uint32 FIRMWARE_VERSION=2
+uint32 HARDWARE_VERSION=3
+uint32 SPEC_VERSION=4
+uint32 LENS_TYPE=5
+uint32 IMU_TYPE=6
+uint32 NOMINAL_BASELINE=7
+
+uint32 key
+---
+string value