add set exposure service

communications/ff_msgs/srv/SetExposure.srv

@@ -0,0 +1,23 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The Astrobee platform is 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. #
+#
+# A generic service for toggling availability
+
+uint16 exposure
+---
+bool success
+string status_message

hardware/is_camera/include/is_camera/camera.h

@@ -24,6 +24,7 @@
 #include <nodelet/nodelet.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <ff_msgs/SetBool.h>
+#include <ff_msgs/SetExposure.h>
 #include <ff_util/ff_nodelet.h>
 #include <std_msgs/Int32MultiArray.h>
 
@@ -79,6 +80,7 @@ class CameraNodelet : public ff_util::FreeFlyerNodelet {
   void EnableBayer(bool enable);
   void AutoExposure();
   size_t getNumBayerSubscribers();
+  bool SetExposure(ff_msgs::SetExposure::Request  &req, ff_msgs::SetExposure::Response &res);
 
   sensor_msgs::CameraInfo info_msg_;
 
@@ -93,6 +95,7 @@ class CameraNodelet : public ff_util::FreeFlyerNodelet {
   ros::Publisher info_pub_;
   ros::Publisher bayer_pub_;
   ros::Publisher pub_exposure_;
+  ros::ServiceServer srv_exposure_;
   std::shared_ptr<V4LStruct> v4l_;
 
   config_reader::ConfigReader config_;

hardware/is_camera/src/camera.cc

@@ -225,9 +225,11 @@ namespace is_camera {
       false, true);
 
     pub_ = nh->advertise<sensor_msgs::Image>(camera_topic_, 1);
-    info_pub_ = nh->advertise<sensor_msgs::CameraInfo>(camera_topic_ + "/camera_info", 1);
+    info_pub_ = nh->advertise<sensor_msgs::CameraInfo>(camera_topic_ + TOPIC_HARDWARE_CAM_INFO, 1);
     pub_exposure_ = nh->advertise<std_msgs::Int32MultiArray>(camera_topic_ + "_ctrl", 1);
 
+    srv_exposure_ = nh->advertiseService(camera_topic_ + SERVICE_SET_EXPOSURE, &CameraNodelet::SetExposure, this);
+
     // Allocate space for our output msg buffer
     for (size_t i = 0; i < kImageMsgBuffer; i++) {
       img_msg_buffer_[i].reset(new sensor_msgs::Image());
@@ -243,6 +245,17 @@ namespace is_camera {
     thread_ = std::thread(&CameraNodelet::PublishLoop, this);
   }
 
+  // Set the exposure
+  bool CameraNodelet::SetExposure(ff_msgs::SetExposure::Request  &req,
+                          ff_msgs::SetExposure::Response &res) {
+    // Set exposure
+    camera_exposure_ = req.exposure;
+    // Success!
+    res.success = true;
+    res.status_message = "Success";
+    return true;
+  }
+
   size_t CameraNodelet::getNumBayerSubscribers(void) {
     if (bayer_enable_) {
       return bayer_pub_.getNumSubscribers();

shared/ff_common/include/ff_common/ff_names.h

@@ -384,6 +384,7 @@
 #define TOPIC_HARDWARE_DOCK_CAM                     "hw/cam_dock"
 #define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_RAW         "_bayer"
 #define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_COLOR       "_color"
+#define TOPIC_HARDWARE_CAM_INFO                     "/camera_info"
 #define TOPIC_HARDWARE_SCI_CAM                      "hw/cam_sci"
 #define TOPIC_HARDWARE_LIGHT_FRONT                  "hw/light_front"
 #define TOPIC_HARDWARE_LIGHT_AFT                    "hw/light_aft"
@@ -449,5 +450,6 @@
 #define SERVICE_HARDWARE_PMC_TIMEOUT                "hw/pmc/set_timeout"
 
 #define SERVICE_STREAMING_LIGHTS                    "hw/signal_lights/streaming"
+#define SERVICE_SET_EXPOSURE                        "/set_exposure"
 
 #endif  // FF_COMMON_FF_NAMES_H_