From 6c7b4e1fb29fec3b9f0b430dc1e12d11bd9978c7 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 30 May 2018 22:02:05 +0800 Subject: [PATCH] Add get_info service to ros wrapper --- wrappers/ros/README.md | 9 +++ .../src/mynt_eye_ros_wrapper/CMakeLists.txt | 10 +++ .../scripts/get_device_info.py | 63 +++++++++++++++++++ .../src/wrapper_nodelet.cc | 48 +++++++++++++- .../src/mynt_eye_ros_wrapper/srv/GetInfo.srv | 12 ++++ 5 files changed, 141 insertions(+), 1 deletion(-) create mode 100755 wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv diff --git a/wrappers/ros/README.md b/wrappers/ros/README.md index 20c42db..804109f 100644 --- a/wrappers/ros/README.md +++ b/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), diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt index 480f039..83ffce1 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt +++ b/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} diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py b/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py new file mode 100755 index 0000000..db4989b --- /dev/null +++ b/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() 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 e944143..cd47be3 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 @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -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_; diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv b/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv new file mode 100644 index 0000000..851c9f4 --- /dev/null +++ b/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