diff --git a/CMakeLists.txt b/CMakeLists.txt
index cc1d54f40442c629394a0312e60a3333131b5d8b..53f54681664ae8a3d89df4f33f8805ff8245b1ff 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,35 +1,20 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-set(PROJECT_NAME firewire_camera_driver_node)
-set(NODELET_PROJECT_NAME firewire_camera_driver_nodelet)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-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)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-# added to include support for dynamic reconfiguration
-rosbuild_find_ros_package(dynamic_reconfigure)
-include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
-gencfg()
-# end dynamic reconfiguration
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_firewire_camera)
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS sensor_msgs image_transport camera_info_manager nodelet iri_base_driver)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(firewire REQUIRED)
@@ -44,14 +29,103 @@ IF (raw1394_INCLUDE_DIR AND raw1394_LIBRARY AND dc1394_INCLUDE_DIR AND dc1394_LI
SET(firewire_ready TRUE)
ENDIF (raw1394_INCLUDE_DIR AND raw1394_LIBRARY AND dc1394_INCLUDE_DIR AND dc1394_LIBRARY)
-INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${dc1394_INCLUDE_DIR} ${raw1394_INCLUDE_DIR} ${firewire_INCLUDE_DIR} ./include)
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/FirewireCamera.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+ INCLUDE_DIRS include
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS sensor_msgs image_transport camera_info_manager nodelet iri_base_driver
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+ DEPENDS iriutils raw1394 dc1394
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${firewire_INCLUDE_DIR})
+include_directories(${raw1394_INCLUDE_DIR})
+include_directories(${dc1394_INCLUDE_DIR})
+
+## Declare a cpp library
+add_library(${PROJECT_NAME}_nodelet src/firewire_camera_driver_node.cpp src/firewire_camera_driver.cpp)
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/firewire_camera_driver.cpp src/firewire_camera_driver_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${firewire_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${raw1394_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${dc1394_LIBRARY})
+
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${firewire_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${raw1394_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${dc1394_LIBRARY})
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
-#common commands for building c++ executables and libraries
-rosbuild_add_library(${NODELET_PROJECT_NAME} src/firewire_camera_driver_nodelet.cpp src/firewire_camera_driver.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-rosbuild_add_executable(${PROJECT_NAME} src/firewire_camera_driver.cpp src/firewire_camera_driver_node.cpp)
+## Specify libraries to link a library or executable target against
+# target_link_libraries(foo_node
+# ${catkin_LIBRARIES}
+# )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dc1394_LIBRARY} ${raw1394_LIBRARY} ${firewire_LIBRARY} ${iriutils_LIBRARY})
-TARGET_LINK_LIBRARIES(${NODELET_PROJECT_NAME} ${dc1394_LIBRARY} ${raw1394_LIBRARY} ${firewire_LIBRARY} ${iriutils_LIBRARY})
diff --git a/calibration/dabo_right_camera.yaml b/calibration/dabo_right_camera.yaml
index d5cae776dd38b00244f30738f739812436c85a6f..7935691979876fac323516739adb4146938b72eb 100644
--- a/calibration/dabo_right_camera.yaml
+++ b/calibration/dabo_right_camera.yaml
@@ -1,6 +1,6 @@
image_width: 1024
image_height: 768
-camera_name: 00b09d01007d6d85_right
+camera_name: 00b09d01006b6fb5_right
camera_matrix:
rows: 3
cols: 3
@@ -17,4 +17,4 @@ rectification_matrix:
projection_matrix:
rows: 3
cols: 4
- data: [713.326843261719, 0, 518.830156137115, 0, 0, 768.828002929688, 391.548470335729, 0, 0, 0, 1, 0]
+ data: [713.326843261719, 0, 518.830156137115, 0, 0, 768.828002929688, 391.548470335729, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/calibration/flea_6mm.yaml b/calibration/flea_6mm.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0c9dee4fb1606cb2c6bed7c5cb2d0bf21bd46703
--- /dev/null
+++ b/calibration/flea_6mm.yaml
@@ -0,0 +1,20 @@
+image_width: 1024
+image_height: 768
+camera_name: firewire_camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1296.554904, 0, 683.998137, 0, 1300.230368, 533.151633, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.24349, 0.227189, -0.000748, -4.6e-05, 0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1241.253662, 0, 692.043757, 0, 0, 1262.634277, 536.985341, 0, 0, 0, 1, 0]
diff --git a/calibration/tibi_left_camera.yaml b/calibration/tibi_left_camera.yaml
index d64c82816d1c0e0b1974b8cf2424f5dd050d29a1..621f134d7c78d94a34e6909161d7f37d43e648af 100644
--- a/calibration/tibi_left_camera.yaml
+++ b/calibration/tibi_left_camera.yaml
@@ -17,4 +17,4 @@ rectification_matrix:
projection_matrix:
rows: 3
cols: 4
- data: [702.534545898438, 0, 518.245361432979, 0, 0, 760.346069335938, 382.076798518574, 0, 0, 0, 1, 0]
\ No newline at end of file
+ data: [702.534545898438, 0, 518.245361432979, 0, 0, 760.346069335938, 382.076798518574, 0, 0, 0, 1, 0]
diff --git a/calibration/tibi_right_camera.yaml b/calibration/tibi_right_camera.yaml
index 7935691979876fac323516739adb4146938b72eb..0566710aa866465605e37657eed52612704d1892 100644
--- a/calibration/tibi_right_camera.yaml
+++ b/calibration/tibi_right_camera.yaml
@@ -17,4 +17,4 @@ rectification_matrix:
projection_matrix:
rows: 3
cols: 4
- data: [713.326843261719, 0, 518.830156137115, 0, 0, 768.828002929688, 391.548470335729, 0, 0, 0, 1, 0]
\ No newline at end of file
+ data: [713.326843261719, 0, 518.830156137115, 0, 0, 768.828002929688, 391.548470335729, 0, 0, 0, 1, 0]
diff --git a/cfg/FirewireCamera.cfg b/cfg/FirewireCamera.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..3a19b37a35eeba63bfe5356a65f5228ca4238196
--- /dev/null
+++ b/cfg/FirewireCamera.cfg
@@ -0,0 +1,76 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='iri_firewire_camera'
+
+from driver_base.msg import SensorLevels
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+enum_mode = gen.enum([ gen.const("Auto", int_t, 0, "Auto mode"),
+gen.const("Manual", int_t, 1, "Manual mode")],"Available feature modes")
+
+enum_color_coding = gen.enum([ gen.const("MONO8", int_t, 0, "Mono coding with 8 bits per pixel"),
+gen.const("YUV8", int_t, 1, "YUV coding with 4,2,2 pixels per channel"),
+gen.const("YUV16", int_t, 2, "YUV coding with 4,4,4 pixels per channel"),
+gen.const("RGB24", int_t, 3, "RGB coding with 8 bits per channel"),
+gen.const("MONO16", int_t, 4, "MONO coding with 16 bits per pixel"),
+gen.const("RGB48", int_t, 5, "RGB coding with 16 bits per channel"),
+gen.const("RAW8", int_t, 6, "RAW coding with 8 bits per pixel"),
+gen.const("RAW16", int_t, 7, "RAW coding with 16 bits per pixel")],"Available color modes")
+
+# Name Type Reconfiguration level Description Default Min Max
+gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_STOP, "Camera frame identifier", "camera_frame")
+gen.add("Camera_node", int_t, SensorLevels.RECONFIGURE_STOP, "Desired camera id", -1, -1 , 100)
+gen.add("Camera_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Desired camera serial number", "")
+gen.add("ISO_speed", int_t, SensorLevels.RECONFIGURE_STOP, "Desired ISO speed", 800, 100, 800)
+gen.add("Image_width", int_t, SensorLevels.RECONFIGURE_STOP, "Desired image width in pixels", 640, 160 , 2448)
+gen.add("Image_height", int_t, SensorLevels.RECONFIGURE_STOP, "Desired image height in pixels", 480, 120 , 2048)
+gen.add("Left_offset", int_t, SensorLevels.RECONFIGURE_STOP, "Desired left offset in pixels", 0, 0, 2448)
+gen.add("Top_offset", int_t, SensorLevels.RECONFIGURE_STOP, "Desired top offset in pixels", 0, 0, 2048)
+gen.add("Framerate", double_t, SensorLevels.RECONFIGURE_STOP, "Desired framerate in frames per second", 50,1.875 ,200)
+gen.add("Color_coding", int_t, SensorLevels.RECONFIGURE_STOP, "Desired color coding", 0, 0, 7, edit_method=enum_color_coding)
+gen.add("White_balance_enabled", bool_t, SensorLevels.RECONFIGURE_STOP, "Enable white balance feature", True)
+gen.add("White_balance_mode", int_t, SensorLevels.RECONFIGURE_STOP, "White balance mode", 0, 0, 1, edit_method=enum_mode)
+gen.add("White_balance_u_b_value", int_t, SensorLevels.RECONFIGURE_STOP, "White balance U/B value", 0, 0, 1023)
+gen.add("White_balance_v_r_value", int_t, SensorLevels.RECONFIGURE_STOP, "White balance V/R value", 0, 0, 1023)
+gen.add("Shutter_enabled", bool_t, SensorLevels.RECONFIGURE_STOP, "Enable shutter feature", True)
+gen.add("Shutter_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Shutter mode", 0, 0, 1, edit_method=enum_mode)
+gen.add("Shutter_value", int_t, SensorLevels.RECONFIGURE_STOP, "Shutter value", 0, 28, 4095)
+gen.add("Gain_enabled", bool_t, SensorLevels.RECONFIGURE_STOP, "Enable gain feature", True)
+gen.add("Gain_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Gain mode", 0, 0, 1, edit_method=enum_mode)
+gen.add("Gain_value", int_t, SensorLevels.RECONFIGURE_STOP, "Gain value", 0, 48, 730)
+import roslib; roslib.load_manifest(PACKAGE)
+
+exit(gen.generate(PACKAGE, "FirewireCameraDriver", "FirewireCamera"))
diff --git a/firewire_nodelet_plugin.xml b/firewire_nodelet_plugin.xml
index 78eb24b71833f00a395acb92b9ed1d01e40492ad..90cb46ea380d61741b704a7976a64f07cf26b48f 100755
--- a/firewire_nodelet_plugin.xml
+++ b/firewire_nodelet_plugin.xml
@@ -1,5 +1,5 @@
-<library path="lib/libfirewire_camera_driver_nodelet">
- <class name="iri_firewire_camera/iri_firewire_nodelet" type="FirewireCameraNodelet" base_class_type="nodelet::Nodelet">
+<library path="lib/libiri_firewire_camera_nodelet">
+ <class name="iri_firewire_camera/FirewireCameraNodelet" type="FirewireCameraNodelet" base_class_type="nodelet::Nodelet">
<description>
Nodelete for the iri firewire camera module
</description>
diff --git a/include/camera_common.h b/include/camera_common.h
index aa0e6c1deb5442e16a58d4f64317f1649ea840fc..26415dff2740e403727f7456b06efd2b28b95378 100755
--- a/include/camera_common.h
+++ b/include/camera_common.h
@@ -27,4 +27,12 @@ typedef struct
sensor_msgs::CameraInfo CameraInfo_msg_;
}TROSCamera;
+typedef struct
+{
+ boost::shared_ptr<image_transport::ImageTransport> it;
+ image_transport::CameraPublisher camera_image_publisher_;
+ sensor_msgs::ImagePtr Image_msg_;
+ sensor_msgs::CameraInfoPtr CameraInfo_msg_;
+}TROSCameraPtr;
+
#endif
diff --git a/include/firewire_camera_driver.h b/include/firewire_camera_driver.h
index 18fa4e95fb5c933276a56d86fd7b897740d2b6be..72be8e30e6f9beac868aaab460cb866c193fccfd 100644
--- a/include/firewire_camera_driver.h
+++ b/include/firewire_camera_driver.h
@@ -58,7 +58,8 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
// private attributes and methods
CFirewireCamera *camera;
CFirewireServer *server;
- long int camera_id;
+ int camera_id;
+ std::string camera_serial;
int ISO_speed;
// desired configuration
TCameraConfig desired_conf;
@@ -66,10 +67,10 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
TCameraConfig current_conf;
// default_configuration
TCameraConfig default_conf;
- // calibration file
- std::string calibration_file;
// frame identifier
std::string frame_id;
+ unsigned int cal_width;
+ unsigned int cal_height;
protected:
/**
* \brief
@@ -200,7 +201,7 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
* \brief
*
*/
- void get_image(char **image_data);
+ void get_image(char *image_data);
/**
* \brief
*
@@ -236,6 +237,16 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
*
*/
std::string get_frame_id(void);
+ /**
+ * \brief
+ *
+ */
+ void set_calibration_params(unsigned int width, unsigned int height);
+ /**
+ * \brief
+ *
+ */
+ std::string get_bayer_pattern(void);
/**
* \brief Destructor
*
diff --git a/include/firewire_camera_driver_node.h b/include/firewire_camera_driver_node.h
index c3b799d7bdb6b799a98e80c44ebc89930f31ceff..976ce6040fdba4231954a9175968f280ae7b6224 100644
--- a/include/firewire_camera_driver_node.h
+++ b/include/firewire_camera_driver_node.h
@@ -44,6 +44,7 @@
// [action server client headers]
#include "eventserver.h"
+#include "camera_common.h"
/**
* \brief IRI ROS Specific Driver Class
@@ -66,9 +67,7 @@ class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Firew
{
private:
// [publisher attributes]
- image_transport::ImageTransport *it;
- image_transport::CameraPublisher camera_image_publisher_;
- sensor_msgs::Image Image_msg_;
+ TROSCameraPtr camera;
camera_info_manager::CameraInfoManager camera_manager;
diagnostic_updater::HeaderlessTopicDiagnostic *diagnosed_camera_image;
@@ -201,4 +200,29 @@ class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Firew
};
+#include "nodelet/nodelet.h"
+
+class FirewireCameraNodelet : public nodelet::Nodelet
+{
+ private:
+ FirewireCameraDriverNode *node;
+ virtual void onInit();// initialization function
+ // thread attributes
+ CThreadServer *thread_server;
+ std::string spin_thread_id;
+ protected:
+ static void *spin_thread(void *param);
+ public:
+ FirewireCameraNodelet();
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~FirewireCameraNodelet();
+};
+
+
#endif
diff --git a/launch/camera0rgb.launch b/launch/camera0rgb.launch
index 6a34be44019ca2baf71994d6201c511dc7a05e9c..748bc50609aedee3e659546d11a372e14d25e9b5 100644
--- a/launch/camera0rgb.launch
+++ b/launch/camera0rgb.launch
@@ -1,3 +1,4 @@
+<!-- -->
<launch>
<node name="iri_firewire_camera" pkg="iri_firewire_camera" type="iri_firewire_camera" output="screen">
diff --git a/launch/camera_flea.launch b/launch/camera_flea.launch
index d7679355b1f47a66728b3aa2f9a5be8ed1ef98aa..9a200220d6f465ba01b0653338b6b92f3ece91dc 100644
--- a/launch/camera_flea.launch
+++ b/launch/camera_flea.launch
@@ -1,22 +1,24 @@
+<!-- -->
<launch>
<node pkg="iri_firewire_camera"
name="iri_firewire_camera"
type="iri_firewire_camera"
output="screen">
+ <!--<param name="Camera_serial" value="00b09d01006cf72a" />-->
<param name="Camera_node" value="0" />
<param name="Color_coding" value="3" />
<param name="ISO_speed" value="400" />
- <param name="Framerate" value="7.5" />
- <param name="Image_width" value="1280" />
- <param name="Image_height" value="960" />
+ <param name="Framerate" value="2.5" />
+ <param name="Image_width" value="1024" />
+ <param name="Image_height" value="768" />
</node>
- <node pkg="image_view"
+<!-- <node pkg="image_view"
type="image_view"
name="image_view" >
<remap from="/image" to="/iri_firewire_camera/camera_image"/>
- </node>
+ </node>-->
</launch>
diff --git a/launch/crop_by_2_rect.launch b/launch/crop_by_2_rect.launch
index b4c5bae9aed8525e2eb1230cbf060965c03f568f..7f11c67ede58d37fe484a6b8fb41db2c14f443a0 100644
--- a/launch/crop_by_2_rect.launch
+++ b/launch/crop_by_2_rect.launch
@@ -1,3 +1,4 @@
+<!-- -->
<launch>
<!-- published topics: /processed_image -->
diff --git a/launch/firewire.launch b/launch/firewire.launch
new file mode 100755
index 0000000000000000000000000000000000000000..50aad10f92efcd6bedd9119907242b1f03e17f1b
--- /dev/null
+++ b/launch/firewire.launch
@@ -0,0 +1,72 @@
+<!-- -->
+<launch>
+
+ <!-- bumblebee -->
+ <!-- published topics: /sensors/head_right/image_raw -->
+ <!-- subscribed topics: -->
+ <!-- service clients: -->
+ <!-- service servers: -->
+ <!-- action clients: -->
+ <!-- action servers: -->
+ <node pkg ="iri_firewire_camera"
+ type="iri_firewire_camera"
+ name="bumblebee"
+ output="screen">
+ <remap from="/bumblebee/camera_image"
+ to="/sensors/head_right/image_raw"/>
+ <!--<param name="Camera_node" value="0" />-->
+ <!--<param name="Camera_serial" value="00b09d01006b6fb5" />-->
+ <param name="Camera_serial" value="00b09d0100a96262" />
+ <param name="frame_id" value="bumblebee_right" type="string" />
+ <!-- set calibration file -->
+ <param name="cal_file" value="file://$(find iri_firewire_camera)/calibration/tibi_right_camera.yaml" type="string" />
+ <param name="ISO_speed" value="400" />
+ <param name="Framerate" value="1.875" />
+ <param name="Color_coding" value="6" />
+ <param name="Image_width" value="1024" />
+ <param name="Image_height" value="768" />
+ <param name="Shutter_enabled" value="True"/>
+ <param name="Shutter_mode" value="0" />
+ <param name="Shutter_value" value="10"/>
+ <param name="~tf_prefix" value="/bumblebee_frame" type="str"/>
+ </node>
+
+ <!-- published topics: /processed_image -->
+ <!-- subscribed topics: /sensors/head_right/image_raw -->
+ <!-- /sensors/head_right/camera_info -->
+ <!-- service clients: -->
+ <!-- service servers: -->
+ <!-- action clients: -->
+ <!-- action servers: -->
+ <!-- image crop -->
+ <node pkg ="nodelet"
+ type="nodelet"
+ name="image_proc_dec"
+ args="standalone image_proc/crop_decimate">
+ <param name="decimation_x" value="2" />
+ <param name="decimation_y" value="2" />
+ <remap from="/camera/image_raw"
+ to="/sensors/head_right/image_raw"/>
+ <remap from="/camera/camera_info"
+ to="/sensors/head_right/camera_info"/>
+ </node>
+
+ <!-- image rectification -->
+ <node pkg ="image_proc"
+ type="image_proc"
+ name="image_proc_rect">
+ <remap from="/image_raw"
+ to="/camera_out/image_raw" />
+ <remap from="/camera_info"
+ to="/camera_out/camera_info" />
+ <remap from="/image_rect_color"
+ to="/processed_image" />
+ </node>
+
+ <node pkg="image_view"
+ type="image_view"
+ name="image_view" >
+ <remap from="/image" to="/processed_image"/>
+ </node>
+
+</launch>
diff --git a/launch/firewire_arpose_nodelet.launch b/launch/firewire_arpose_nodelet.launch
new file mode 100755
index 0000000000000000000000000000000000000000..4dcb332524da0dd21955925e29b8611572564c9a
--- /dev/null
+++ b/launch/firewire_arpose_nodelet.launch
@@ -0,0 +1,70 @@
+<!-- -->
+<launch>
+ <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen">
+ <!--<param name="Camera_serial" value="00b09d01006cf72a" />--> <!-- tibi rear left -->
+ <param name="Camera_serial" value="00b09d0100a96262" /> <!-- tibi rear right -->
+ <param name="frame_id" value="bumblebee_frame" type="string" />
+ <param name="cal_file" value="file://$(find iri_firewire_camera)/calibration/tibi_right_camera.yaml" type="string" />
+ <param name="ISO_speed" value="800" />
+ <param name="Framerate" value="2.5" />
+ <param name="Color_coding" value="6" />
+ <param name="Image_width" value="1024" />
+ <param name="Image_height" value="768" />
+ <param name="Shutter_enabled" value="True"/>
+ <param name="Shutter_mode" value="1" />
+ <param name="Shutter_value" value="10"/>
+ <param name="autosize" value="true"/>
+ <remap from="/standalone_nodelet/camera_image"
+ to="/camera/image_raw" />
+ <remap from="/standalone_nodelet/camera_info"
+ to="/camera/camera_info" />
+ </node>
+
+ <!-- firewire camera driver -->
+ <node pkg="nodelet" type="nodelet" name="iri_firewire_nodelet" args="load iri_firewire_camera/FirewireCameraNodelet standalone_nodelet" output="screen">
+ </node>
+
+ <node pkg ="nodelet" type="nodelet" name="image_proc_debayer" args="load image_proc/debayer standalone_nodelet" output="screen">
+ <remap from="/image_raw"
+ to="/camera/image_raw"/>
+ <remap from="/camera_info"
+ to="/camera/camera_info"/>
+ <remap from="/image_mono"
+ to="/camera/image_mono"/>
+ </node>
+ <!-- image rectification -->
+ <node pkg ="nodelet" type="nodelet" name="image_proc_rect" args="load image_proc/rectify standalone_nodelet" output="screen">
+ <remap from="/image_mono"
+ to="/camera/image_mono"/>
+ <remap from="/camera_info"
+ to="/camera/camera_info"/>
+ <remap from="/image_rect"
+ to="/camera/image_rect"/>
+ </node>
+
+ <node pkg ="nodelet" type="nodelet" name="local_th" args="load iri_image_local_binarization/ImageLocalBinNodelet standalone_nodelet" output="screen">
+ <remap from="local_th/image_in/image_raw"
+ to="/camera/image_rect"/>
+ <remap from="local_th/image_in/camera_info"
+ to="/camera/camera_info"/>
+ <remap from="local_th/image_out/image_raw"
+ to="/ar_pose/image_raw"/>
+ <remap from="local_th/image_out/camera_info"
+ to="/ar_pose/camera_info"/>
+ </node>
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find iri_ladybug2)/data/object_4x4_front"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/camera/image_raw"
+ to="/ar_pose/image_raw"/>
+ <remap from="/camera/camera_info"
+ to="/ar_pose/camera_info"/>
+ </node>
+
+</launch>
diff --git a/launch/firewire_nodelet.launch b/launch/firewire_nodelet.launch
index 94dc81c257c764b2ad7f0fc059c94e290939f8da..fbd7abe8fe4db4201b404a37e796a4e5cbfca49b 100755
--- a/launch/firewire_nodelet.launch
+++ b/launch/firewire_nodelet.launch
@@ -1,14 +1,16 @@
+<!-- -->
<launch>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen">
- <param name="Camera_node" value="0" />
- <param name="frame_id" value="bumblebee_frame" type="string" />
- <param name="cal_file" value="file://$(find iri_firewire_camera)/calibration/bumblebee_right.yml" type="string" />
- <param name="ISO_speed" value="400" />
- <param name="Framerate" value="7.5" />
- <param name="Color_coding" value="3" />
- <param name="Image_width" value="1024" />
- <param name="Image_height" value="768" />
- <param name="autosize" value="true"/>
+ <param name="Camera_node" value="0" />
+ <!--<param name="Camera_serial" value="00b09d01006b6fb5" />-->
+ <param name="frame_id" value="bumblebee_frame" type="string" />
+ <param name="cal_file" value="file://$(find iri_firewire_camera)/calibration/tibi_right_camera.yaml" type="string" />
+ <param name="ISO_speed" value="800" />
+ <param name="Framerate" value="2.5" />
+ <param name="Color_coding" value="3" />
+ <param name="Image_width" value="1024" />
+ <param name="Image_height" value="768" />
+ <param name="autosize" value="true"/>
<remap from="/camera_out/image_raw"
to="/image_mono" />
<remap from="/camera_out/camera_info"
@@ -22,7 +24,7 @@
</node>
<!-- firewire camera driver -->
- <node pkg="nodelet" type="nodelet" name="iri_firewire_nodelet" args="load iri_firewire_camera/iri_firewire_nodelet standalone_nodelet" output="screen">
+ <node pkg="nodelet" type="nodelet" name="iri_firewire_nodelet" args="load iri_firewire_camera/FirewireCameraNodelet standalone_nodelet" output="screen">
</node>
<!-- image decimation -->
diff --git a/launch/tibi_dabo_crop_by_2_rect.launch b/launch/tibi_dabo_crop_by_2_rect.launch
index b38ee60cdae74a4e2d82e3ee3d699e468a7f57cb..1eab31a45baaf4e126eca9fdc62ef687d971f9fd 100644
--- a/launch/tibi_dabo_crop_by_2_rect.launch
+++ b/launch/tibi_dabo_crop_by_2_rect.launch
@@ -1,8 +1,8 @@
<launch>
<!-- load robot defined machines -->
- <include file="$(find tibi_dabo_base)/machines/$(env ROBOT).machines" />
-
+ <include file="$(find tibi_dabo_base)/machines/$(env ROBOT)_$(env ROS_MODE).machines" />
+
<group ns="$(env ROBOT)">
<!-- published topics: /$(env ROBOT)/processed_image -->
<!-- subscribed topics: /$(env ROBOT)/sensors/head_right/image_raw -->
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..99c438411984c5328fdb2eb52ccb8bbbd376e77f
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+ <name>iri_firewire_camera</name>
+ <version>1.0.0</version>
+ <description>Firewire camera driver</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <maintainer email="shernand@iri.upc.edu">Sergi Hernandez</maintainer>
+ <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer>
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <url type="website">http://wiki.ros.org/iri_firewire_camera</url>
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <author email="shernand@iri.upc.edu">Sergi Hernandez</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- ****************************************************************** -->
+ <!-- Place build dependencies here -->
+ <!-- ****************************************************************** -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <build_depend>iriutils</build_depend>
+ <build_depend>firewire</build_depend>
+ <build_depend>raw1394</build_depend>
+ <build_depend>dc1394</build_depend>
+ <build_depend>iri_base_driver</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>camera_info_manager</build_depend>
+ <build_depend>nodelet</build_depend>
+
+ <!-- ****************************************************************** -->
+ <!-- Place run dependencies here -->
+ <!-- ****************************************************************** -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <run_depend>iriutils</run_depend>
+ <run_depend>firewire</run_depend>
+ <run_depend>raw1394</run_depend>
+ <run_depend>dc1394</run_depend>
+ <run_depend>iri_base_driver</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>camera_info_manager</run_depend>
+ <run_depend>nodelet</run_depend>
+
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <nodelet plugin="${prefix}/firewire_nodelet_plugin.xml" />
+ </export>
+</package>
diff --git a/src/firewire_camera_driver.cpp b/src/firewire_camera_driver.cpp
index 0db011172d277203b9b78b3d4f6fb1a4dac9ced6..254df2a33001663a8f0823a885ca89c531dc8b40 100644
--- a/src/firewire_camera_driver.cpp
+++ b/src/firewire_camera_driver.cpp
@@ -32,26 +32,53 @@ FirewireCameraDriver::FirewireCameraDriver()
this->default_conf.depth=(depths_t)-1;
this->default_conf.coding=(codings_t)-1;
this->default_conf.framerate=0.0;
- // default calibration file
- this->calibration_file="";
// default frame id
this->frame_id="";
+ // default calibration parameters
+ this->cal_width=-1;
+ this->cal_height=-1;
}
bool FirewireCameraDriver::openDriver(void)
{
+ std::stringstream buffer;
+ unsigned long long serial;
+
//setDriverId(driver string id);
- if(this->camera_id!=-1)
+ if(this->camera_id!=-1 || this->camera_serial.size()!=0)
{
this->server->init();
if(this->server->get_num_cameras()>0)
{
- this->server->get_camera(this->camera_id,&this->camera);
+ if(this->camera_serial.size()==0)
+ this->server->get_camera(this->camera_id,&this->camera);
+ else
+ {
+ buffer << this->camera_serial;
+ buffer >> std::hex >> serial;
+ this->server->get_camera_guid(serial,&this->camera);
+ }
this->camera->set_ISO_speed(this->ISO_speed);
this->camera->get_config(&this->default_conf.left_offset,&this->default_conf.top_offset,&this->default_conf.width,
&this->default_conf.height,&this->default_conf.framerate,&this->default_conf.depth,
&this->default_conf.coding);
+ // set the desired configuration
this->change_config(&this->desired_conf);
+ if((this->cal_width!=-1 && this->desired_conf.width!=this->cal_width) ||
+ (this->cal_height!=-1 && this->desired_conf.height!=this->cal_height))
+ {
+ ROS_ERROR("The calibration file does not coincide with the current configuration ");
+ delete this->camera;
+ this->camera=NULL;
+ return false;
+ }
+ // white balance feature
+ this->set_white_balance(&this->config_.White_balance_enabled,&this->config_.White_balance_mode,&this->config_.White_balance_u_b_value,
+ &this->config_.White_balance_v_r_value);
+ // shutter feature
+ this->set_shutter(&this->config_.Shutter_enabled,&this->config_.Shutter_mode,&this->config_.Shutter_value);
+ // gain feature
+ this->set_gain(&this->config_.Gain_enabled,&this->config_.Gain_mode,&this->config_.Gain_value);
ROS_INFO("Driver opened");
return true;
}
@@ -131,10 +158,9 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
case FirewireCameraDriver::CLOSED:
this->lock();
this->camera_id=new_cfg.Camera_node;
+ this->camera_serial=new_cfg.Camera_serial;
this->ISO_speed=new_cfg.ISO_speed;
this->dyn_rec_to_camera(new_cfg,this->desired_conf);
- // change the calibration file
- this->calibration_file=new_cfg.cal_file;
// update the frame identifier
this->frame_id=new_cfg.frame_id;
this->unlock();
@@ -153,6 +179,12 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
this->camera_id=new_cfg.Camera_node;
this->open();
}
+ if(this->camera_serial!=new_cfg.Camera_serial)
+ {
+ this->close();
+ this->camera_serial=new_cfg.Camera_serial;
+ this->open();
+ }
this->dyn_rec_to_camera(new_cfg,conf);
this->change_config(&conf);
// write the configuration back to the dynamic reconfigure window
@@ -164,8 +196,6 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
this->set_shutter(&new_cfg.Shutter_enabled,&new_cfg.Shutter_mode,&new_cfg.Shutter_value);
// gain feature
this->set_gain(&new_cfg.Gain_enabled,&new_cfg.Gain_mode,&new_cfg.Gain_value);
- // change the calibration file
- this->calibration_file=new_cfg.cal_file;
// update the frame identifier
this->frame_id=new_cfg.frame_id;
this->unlock();
@@ -492,12 +522,10 @@ void FirewireCameraDriver::set_gain(bool *enable, int *mode, int *value)
}
}
-void FirewireCameraDriver::get_image(char **image_data)
+void FirewireCameraDriver::get_image(char *image_data)
{
if(this->camera!=NULL)
- this->camera->get_image(image_data);
- else
- *image_data=NULL;
+ this->camera->get_image_shared(image_data);
}
void FirewireCameraDriver::set_ISO_speed(int iso_speed)
@@ -560,14 +588,35 @@ std::string FirewireCameraDriver::get_new_frame_event(void)
return event;
}
-std::string FirewireCameraDriver::get_calibration_file(void)
+std::string FirewireCameraDriver::get_frame_id(void)
+{
+ return this->frame_id;
+}
+
+void FirewireCameraDriver::set_calibration_params(unsigned int width, unsigned int height)
{
- return this->calibration_file;
+ this->cal_width=width;
+ this->cal_height=height;
}
-std::string FirewireCameraDriver::get_frame_id(void)
+std::string FirewireCameraDriver::get_bayer_pattern(void)
{
- return this->frame_id;
+ dc1394color_filter_t bayer;
+
+ if(this->camera!=NULL)
+ {
+ this->camera->get_bayer_pattern(&bayer);
+ if((int)bayer==512)
+ return std::string("bayer_rggb8");
+ else if((int)bayer==513)
+ return std::string("bayer_gbrg8");
+ else if((int)bayer==514)
+ return std::string("bayer_grbg8");
+ else
+ return std::string("bayer_bggr8");
+ }
+ else
+ return std::string("");
}
FirewireCameraDriver::~FirewireCameraDriver()
diff --git a/src/firewire_camera_driver_node.cpp b/src/firewire_camera_driver_node.cpp
index 467ee0c7faa77202b9a61854810eefcb3211a95d..d52b3ecd8993dbdc7b8c2f2bef3ff0da2a40a839 100644
--- a/src/firewire_camera_driver_node.cpp
+++ b/src/firewire_camera_driver_node.cpp
@@ -1,33 +1,53 @@
#include "firewire_camera_driver_node.h"
-FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_))
+FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh),
+ camera_manager(ros::NodeHandle(this->public_node_handle_))
{
std::string cal_file;
+ bool cal_ok=false;
//init class attributes if necessary
this->loop_rate_ = 1000;//in [Hz]
this->desired_framerate=30.0;
this->diagnosed_camera_image=NULL;
- this->it=NULL;
- this->it=new image_transport::ImageTransport(this->public_node_handle_);
+ this->camera.it.reset(new image_transport::ImageTransport(this->public_node_handle_));
+ this->camera.Image_msg_=sensor_msgs::ImagePtr(new sensor_msgs::Image);
this->diagnosed_camera_image = new diagnostic_updater::HeaderlessTopicDiagnostic("camera_image",this->diagnostic_,
diagnostic_updater::FrequencyStatusParam(&this->desired_framerate,&this->desired_framerate,0.01,5));
this->event_server=CEventServer::instance();
// [init publishers]
- this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
+ this->camera.camera_image_publisher_ = this->camera.it->advertiseCamera("camera_image", 1);
// try to load the calibration file
- public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
+ public_node_handle_.param<std::string>("cal_file", cal_file, "");
+ this->camera_manager.setCameraName("firewire_camera");
if(this->camera_manager.validateURL(cal_file))
{
- if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
- ROS_INFO("Invalid calibration file");
- }
+ if(this->camera_manager.loadCameraInfo(cal_file))
+ {
+ ROS_INFO("Found calibration file for the camera: %s",cal_file.c_str());
+ cal_ok=true;
+ }
+ else
+ {
+ ROS_INFO("Invalid calibration file for the camera");
+ cal_ok=false;
+ }
+ this->camera.CameraInfo_msg_.reset(new sensor_msgs::CameraInfo(this->camera_manager.getCameraInfo()));
+ }
+ else
+ {
+ ROS_INFO("Calibration file does not exist");
+ cal_ok=false;
+ }
+
+ if(!cal_ok)
+ this->driver_.set_calibration_params(-1,-1);
else
- ROS_INFO("Invalid calibration file");
+ this->driver_.set_calibration_params(this->camera.CameraInfo_msg_->width,this->camera.CameraInfo_msg_->height);
public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
@@ -44,7 +64,6 @@ FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_ba
void FirewireCameraDriverNode::mainNodeThread(void)
{
- sensor_msgs::CameraInfo camera_info=this->camera_manager.getCameraInfo();
std::list<std::string> events;
char *image_data=NULL;
TCameraConfig config;
@@ -59,48 +78,50 @@ void FirewireCameraDriverNode::mainNodeThread(void)
{
events.push_back(this->new_frame_event);
this->event_server->wait_all(events,1000);
- this->driver_.get_image(&image_data);
- if(image_data!=NULL && this->camera_image_publisher_.getNumSubscribers()>0)
+ if(this->camera.camera_image_publisher_.getNumSubscribers()>0)
{
this->driver_.get_current_config(&config);
// update the desired framerate for the diagnostics
this->desired_framerate=config.framerate;
//fill msg structures
- this->Image_msg_.width=config.width;
- this->Image_msg_.height=config.height;
- this->Image_msg_.step=config.width*config.depth/8;
- this->Image_msg_.data=std::vector<unsigned char>(image_data,image_data+config.width*config.height*config.depth/8);
- if(config.coding==MONO || config.coding==RAW)
+ this->camera.Image_msg_->width=config.width;
+ this->camera.Image_msg_->height=config.height;
+ this->camera.Image_msg_->step=config.width*config.depth/8;
+ this->camera.Image_msg_->data.resize(config.width*config.height*config.depth/8);
+ this->driver_.get_image((char *)this->camera.Image_msg_->data.data());
+ if(config.coding==MONO)
{
if(config.depth==DEPTH8)
- this->Image_msg_.encoding="8UC1";
+ this->camera.Image_msg_->encoding="mono8";
else if(config.depth==DEPTH16)
- this->Image_msg_.encoding="16UC1";
+ this->camera.Image_msg_->encoding="16UC1";
else
- this->Image_msg_.encoding="16UC1";
+ this->camera.Image_msg_->encoding="16UC1";
+ }
+ else if(config.coding==RAW)
+ {
+ this->camera.Image_msg_->encoding=this->driver_.get_bayer_pattern();
}
else if(config.coding==YUV)
{
- this->Image_msg_.encoding="yuv422";
+ this->camera.Image_msg_->encoding="yuv422";
}
else
{
if(config.depth==DEPTH24)
- this->Image_msg_.encoding="rgb8";
+ this->camera.Image_msg_->encoding="rgb8";
else if(config.depth==DEPTH48)
- this->Image_msg_.encoding="16UC3";
+ this->camera.Image_msg_->encoding="16UC3";
else
- this->Image_msg_.encoding="16UC3";
+ this->camera.Image_msg_->encoding="16UC3";
}
}
- delete[] image_data;
//publish messages
- this->Image_msg_.header.stamp = ros::Time::now();
- this->Image_msg_.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
- camera_info.header.stamp = this->Image_msg_.header.stamp;
- camera_info.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
- //this->Image_msg_.header.frame_id = "<publisher_topic_name>";
- this->camera_image_publisher_.publish(this->Image_msg_,camera_info);
+ this->camera.Image_msg_->header.stamp = ros::Time::now();
+ this->camera.Image_msg_->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
+ this->camera.CameraInfo_msg_->header.stamp = this->camera.Image_msg_->header.stamp;
+ this->camera.CameraInfo_msg_->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
+ this->camera.camera_image_publisher_.publish(this->camera.Image_msg_,this->camera.CameraInfo_msg_);
this->diagnosed_camera_image->tick();
}
}
@@ -176,23 +197,11 @@ void FirewireCameraDriverNode::addNodeRunningTests(void)
void FirewireCameraDriverNode::reconfigureNodeHook(int level)
{
- // try to load the calibration file
- if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
- {
- this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
- }
- else
- ROS_INFO("Invalid calibration file");
}
FirewireCameraDriverNode::~FirewireCameraDriverNode()
{
// [free dynamic memory]
- if(this->it!=NULL)
- {
- delete this->it;
- this->it=NULL;
- }
if(this->diagnosed_camera_image!=NULL)
{
delete this->diagnosed_camera_image;
@@ -205,3 +214,48 @@ int main(int argc,char *argv[])
{
return driver_base::main<FirewireCameraDriverNode>(argc,argv,"firewire_camera_driver_node");
}
+
+#include <pluginlib/class_list_macros.h>
+
+FirewireCameraNodelet::FirewireCameraNodelet()
+{
+ this->node=NULL;
+}
+
+FirewireCameraNodelet::~FirewireCameraNodelet(void)
+{
+ // kill the thread
+ this->thread_server->kill_thread(this->spin_thread_id);
+ this->thread_server->detach_thread(this->spin_thread_id);
+ this->thread_server->delete_thread(this->spin_thread_id);
+ this->spin_thread_id="";
+ // [free dynamic memory]
+ if(this->node!=NULL)
+ delete this->node;
+}
+
+void FirewireCameraNodelet::onInit()
+{
+ // initialize the driver node
+ this->node=new FirewireCameraDriverNode(getPrivateNodeHandle());
+ // initialize the thread
+ this->thread_server=CThreadServer::instance();
+ this->spin_thread_id=getName() + "_firewire_nodelet_spin";
+ this->thread_server->create_thread(this->spin_thread_id);
+ this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+ // start the spin thread
+ this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *FirewireCameraNodelet::spin_thread(void *param)
+{
+ FirewireCameraNodelet *nodelet=(FirewireCameraNodelet *)param;
+
+ nodelet->node->spin();
+
+ pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(iri_firewire_camera, FirewireCameraNodelet, FirewireCameraNodelet, nodelet::Nodelet);
+