From 515ce354af580db81b64b5a1064fce659b591f83 Mon Sep 17 00:00:00 2001
From: Andrew Straw <strawman@astraw.com>
Date: Fri, 30 Dec 2011 17:56:36 +0100
Subject: [PATCH] add ability to use dynamic_reconfigure to set frame_id

---
 CMakeLists.txt             |  5 +++++
 cfg/CameraAravisConfig.cfg | 15 +++++++++++++++
 manifest.xml               |  3 +++
 src/camnode.cpp            | 34 ++++++++++++++++++++++++++++++----
 4 files changed, 53 insertions(+), 4 deletions(-)
 create mode 100755 cfg/CameraAravisConfig.cfg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dd14e39..1e07fe5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,6 +21,11 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
+# add dynamic reconfigure api
+rosbuild_find_ros_package(dynamic_reconfigure)
+include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+gencfg()
+
 #uncomment if you have defined messages
 #rosbuild_genmsg()
 #uncomment if you have defined services
diff --git a/cfg/CameraAravisConfig.cfg b/cfg/CameraAravisConfig.cfg
new file mode 100755
index 0000000..a03cf33
--- /dev/null
+++ b/cfg/CameraAravisConfig.cfg
@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+# Configuration for virtual display
+# Based on tutorial at http://www.ros.org/wiki/dynamic_reconfigure/Tutorials/SettingUpDynamicReconfigureForANode
+
+PACKAGE='camera_aravis'
+import roslib; roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator import *
+from driver_base.msg import SensorLevels
+
+gen = ParameterGenerator()
+
+gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "The camera frame", "camera")
+
+exit(gen.generate(PACKAGE, "camera_aravis_params", "CameraAravis"))
diff --git a/manifest.xml b/manifest.xml
index 583d5fe..94e5b4f 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -13,6 +13,9 @@
   <depend package="sensor_msgs"/>
   <depend package="image_transport"/>
   <depend package="camera_info_manager"/>
+  <depend package="dynamic_reconfigure" />
+  <depend package="driver_base" />
+  <depend package="tf" />
 
   <rosdep name="glib2"/>
 
diff --git a/src/camnode.cpp b/src/camnode.cpp
index ddf3ea7..2d66c81 100644
--- a/src/camnode.cpp
+++ b/src/camnode.cpp
@@ -14,7 +14,13 @@
 #include <image_transport/image_transport.h>
 #include <camera_info_manager/camera_info_manager.h>
 
+#include <dynamic_reconfigure/server.h>
+#include <driver_base/SensorLevels.h>
+#include <tf/transform_listener.h>
+#include <camera_aravis/CameraAravisConfig.h>
+
 #define THROW_ERROR(m) throw std::string((m))
+typedef camera_aravis::CameraAravisConfig Config;
 
 // global variables -------------------
 static gboolean cancel = FALSE;
@@ -22,6 +28,8 @@ image_transport::CameraPublisher publisher;
 camera_info_manager::CameraInfoManager *cam_info_manager;
 sensor_msgs::CameraInfo cam_info;
 gint g_width, g_height; // buffer->width and buffer->height not working, so I used a global.
+Config g_config;
+ros::NodeHandle *node_handle;
 // ------------------------------------
 
 typedef struct {
@@ -35,6 +43,18 @@ set_cancel (int signal)
     cancel = TRUE;
 }
 
+void ros_reconfigure_callback(Config &newconfig, uint32_t level)
+{
+    if (newconfig.frame_id == "")
+        newconfig.frame_id = "camera";
+
+    std::string tf_prefix = tf::getPrefixParam(*node_handle);
+    ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
+    newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
+
+    g_config = newconfig;                // save new parameters
+}
+
 static void
 new_buffer_cb (ArvStream *stream, ApplicationData *data)
 {
@@ -51,7 +71,7 @@ new_buffer_cb (ArvStream *stream, ApplicationData *data)
             sensor_msgs::Image msg;
             msg.header.stamp = ros::Time::now(); // host timestamps (else buffer->timestamp_ns)
             msg.header.seq = buffer->frame_id;
-            //      msg.header.frame_id = "0";
+            msg.header.frame_id = g_config.frame_id;
             msg.height = g_height;
             msg.width = g_width;
             msg.encoding = "mono8";  // XXX fixme
@@ -111,6 +131,7 @@ periodic_task_cb (void *abstract_data)
 int main(int argc, char** argv) {
     ArvCamera *camera;
     char *arv_option_camera_name = NULL;
+    g_config.frame_id = "camera";
 
     ros::init(argc, argv, "camnode");
 
@@ -130,11 +151,16 @@ int main(int argc, char** argv) {
 
     std::cout << "opened camera" << std::endl;
 
-    ros::NodeHandle node_handle;
     std::string ros_camera_name = arv_camera_get_device_id(camera);
-    cam_info_manager = new camera_info_manager::CameraInfoManager(node_handle,
+    node_handle = new ros::NodeHandle();
+    cam_info_manager = new camera_info_manager::CameraInfoManager(*node_handle,
                                                                   ros_camera_name);
 
+    dynamic_reconfigure::Server<Config> srv;
+    dynamic_reconfigure::Server<Config>::CallbackType f;
+    f = boost::bind(&ros_reconfigure_callback, _1, _2);
+    srv.setCallback(f);
+
     {
         int arv_option_width = -1;
         int arv_option_height = -1;
@@ -207,7 +233,7 @@ int main(int argc, char** argv) {
 
 	// topic is "image_raw", with queue size of 1
 	// image transport interfaces
-	image_transport::ImageTransport *transport = new image_transport::ImageTransport(node_handle);
+	image_transport::ImageTransport *transport = new image_transport::ImageTransport(*node_handle);
 	publisher = transport->advertiseCamera("image_raw", 1);
 
 	arv_camera_set_acquisition_mode (camera, ARV_ACQUISITION_MODE_CONTINUOUS);