diff --git a/Makefile b/Makefile
deleted file mode 100644
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/calibration/dabo_left_camera.yaml b/calibration/dabo_left_camera.yaml
deleted file mode 100644
index fa59a536b42941d58bb7db553d696aa5e1a23c45..0000000000000000000000000000000000000000
--- a/calibration/dabo_left_camera.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1024
-image_height: 768
-camera_name: 00b09d01007d6d85_left
-camera_matrix:
- rows: 3
- cols: 3
- data: [824.093446719227, 0, 516.106699619537, 0, 827.44845509146, 382.586605192696, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.348432976147095, 0.135239688845363, 0.000330433316497616, 0.000713018760447784, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [702.534545898438, 0, 518.245361432979, 0, 0, 760.346069335938, 382.076798518574, 0, 0, 0, 1, 0]
diff --git a/cfg/firewire_camera_driver_config.cfg b/cfg/firewire_camera_driver_config.cfg
deleted file mode 100755
index dcf659df7647830e95703fe72e1d2e4257ab9ebc..0000000000000000000000000000000000000000
--- a/cfg/firewire_camera_driver_config.cfg
+++ /dev/null
@@ -1,77 +0,0 @@
-#! /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'
-import roslib; roslib.load_manifest(PACKAGE)
-
-from driver_base.msg import SensorLevels
-from dynamic_reconfigure.parameter_generator 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("cal_file", str_t, SensorLevels.RECONFIGURE_STOP, "Camera calibration file", "")
-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", False)
-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", False)
-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", False)
-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/include/firewire_camera_driver_nodelet.h b/include/firewire_camera_driver_nodelet.h
deleted file mode 100755
index c11228c9ec888f3677234f033947c1d905ab8880..0000000000000000000000000000000000000000
--- a/include/firewire_camera_driver_nodelet.h
+++ /dev/null
@@ -1,222 +0,0 @@
-// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author
-// All rights reserved.
-//
-// This file is part of iri-ros-pkg
-// iri-ros-pkg is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-// IMPORTANT NOTE: This code has been generated through a script from the
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
-// of the scripts. ROS topics can be easly add by using those scripts. Please
-// refer to the IRI wiki page for more information:
-// http://wikiri.upc.es/index.php/Robotics_Lab
-
-#ifndef _firewire_camera_driver_nodelet_h_
-#define _firewire_camera_driver_nodelet_h_
-
-#include "nodelet/nodelet.h"
-
-#include <iri_base_driver/iri_base_driver_node.h>
-#include "firewire_camera_driver.h"
-
-// diagnostic headers
-#include "diagnostic_updater/diagnostic_updater.h"
-#include "diagnostic_updater/publisher.h"
-
-// image transport
-#include "image_transport/image_transport.h"
-#include <camera_info_manager/camera_info_manager.h>
-
-// [publisher subscriber headers]
-#include <sensor_msgs/Image.h>
-
-// [service client headers]
-
-// [action server client headers]
-
-#include "eventserver.h"
-#include "threadserver.h"
-
-/**
- * \brief IRI ROS Specific Driver Class
- *
- * This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>,
- * to provide an execution thread to the driver object. A complete framework
- * with utilites to test the node functionallity or to add diagnostics to
- * specific situations is also given. The inherit template design form allows
- * complete access to any IriBaseDriver object implementation.
- *
- * As mentioned, tests in the different driver states can be performed through
- * class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
- * common to all nodes may be also executed in the pattern class IriBaseNodeDriver.
- * Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
- * more details:
- * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
- * http://www.ros.org/wiki/self_test/ (Example: Self Test)
- */
-class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>
-{
- private:
- // [publisher attributes]
- camera_info_manager::CameraInfoManager camera_manager;
- boost::shared_ptr<image_transport::ImageTransport> it;
- sensor_msgs::ImagePtr Image_msg_;// this pointer is already a shared_ptr
- image_transport::CameraPublisher camera_image_publisher_;
- diagnostic_updater::HeaderlessTopicDiagnostic *diagnosed_camera_image;
-
- // [subscriber attributes]
-
- // [service attributes]
-
- // [client attributes]
-
- // [action server attributes]
-
- // [action client attributes]
-
- // node attributes
- double desired_framerate;
- CEventServer *event_server;
- std::string new_frame_event;
- std::string tf_prefix_;
-
- /**
- * \brief post open hook
- *
- * This function is called by IriBaseNodeDriver::postOpenHook(). In this function
- * specific parameters from the driver must be added so the ROS dynamic
- * reconfigure application can update them.
- */
- void postNodeOpenHook(void);
-
- public:
- /**
- * \brief constructor
- *
- * This constructor mainly creates and initializes the FirewireCameraDriverNode topics
- * through the given public_node_handle object. IriBaseNodeDriver attributes
- * may be also modified to suit node specifications.
- *
- * All kind of ROS topics (publishers, subscribers, servers or clients) can
- * be easyly generated with the scripts in the iri_ros_scripts package. Refer
- * to ROS and IRI Wiki pages for more details:
- *
- * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)
- * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++)
- * http://wikiri.upc.es/index.php/Robotics_Lab
- *
- * \param nh a reference to the node handle object to manage all ROS topics.
- */
- FirewireCameraDriverNode(ros::NodeHandle& nh);
-
- /**
- * \brief Destructor
- *
- * This destructor is called when the object is about to be destroyed.
- *
- */
- ~FirewireCameraDriverNode();
-
- protected:
- /**
- * \brief main node thread
- *
- * This is the main thread node function. Code written here will be executed
- * in every node loop while the driver is on running state. Loop frequency
- * can be tuned my modifying loop_rate attribute.
- *
- * Here data related to the process loop or to ROS topics (mainly data structs
- * related to the MSG and SRV files) must be updated. ROS publisher objects
- * must publish their data in this process. ROS client servers may also
- * request data to the corresponding server topics.
- */
- void mainNodeThread(void);
-
- // [diagnostic functions]
- void check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat);
-
- /**
- * \brief node add diagnostics
- *
- * In this function ROS diagnostics applied to this specific node may be
- * added. Common use diagnostics for all nodes are already called from
- * IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information
- * of how ROS diagnostics work can be readen here:
- * http://www.ros.org/wiki/diagnostics/
- * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html
- */
- void addNodeDiagnostics(void);
-
- // [driver test functions]
-
- /**
- * \brief open status driver tests
- *
- * In this function tests checking driver's functionallity when driver_base
- * status=open can be added. Common use tests for all nodes are already called
- * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
- * please refer to the Self Test example in:
- * http://www.ros.org/wiki/self_test/
- */
- void addNodeOpenedTests(void);
-
- /**
- * \brief stop status driver tests
- *
- * In this function tests checking driver's functionallity when driver_base
- * status=stop can be added. Common use tests for all nodes are already called
- * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
- * please refer to the Self Test example in:
- * http://www.ros.org/wiki/self_test/
- */
- void addNodeStoppedTests(void);
-
- /**
- * \brief run status driver tests
- *
- * In this function tests checking driver's functionallity when driver_base
- * status=run can be added. Common use tests for all nodes are already called
- * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
- * please refer to the Self Test example in:
- * http://www.ros.org/wiki/self_test/
- */
- void addNodeRunningTests(void);
-
- /**
- * \brief specific node dynamic reconfigure
- *
- * This function is called reconfigureHook()
- *
- * \param level integer
- */
- void reconfigureNodeHook(int level);
-
-};
-
-class FirewireCameraNodelet : public nodelet::Nodelet
-{
- private:
- virtual void onInit();// initialization function
- FirewireCameraDriverNode *dev;
- // thread attributes
- CThreadServer *thread_server;
- std::string spin_thread_id;
- protected:
- static void *spin_thread(void *param);
- public:
- FirewireCameraNodelet();
- ~FirewireCameraNodelet();
-};
-
-#endif
diff --git a/launch/tibi_dabo_bumblebee_calibration.launch b/launch/tibi_dabo_bumblebee_calibration.launch
deleted file mode 100644
index ee1c4cb0664ecfc06c4a1b02aa58b7dda8efef1f..0000000000000000000000000000000000000000
--- a/launch/tibi_dabo_bumblebee_calibration.launch
+++ /dev/null
@@ -1,84 +0,0 @@
-<launch>
-
- <!-- load robot defined machines -->
- <include file="$(find tibi_dabo_base)/machines/$(env ROBOT).machines" />
-
- <!-- tibi/dabo sensors -->
- <!-- published topics: /$(env ROBOT)/sensors/front_laser_scan -->
- <!-- /$(env ROBOT)/sensors/rear_laser_scan -->
- <!-- /$(env ROBOT)/sensors/vertical_laser_scan -->
- <!-- /$(env ROBOT)/sensors/head_right_image -->
- <!-- /$(env ROBOT)/sensors/payload_battery_status -->
- <!-- /$(env ROBOT)/sensors/joy -->
- <!-- subscribed topics: -->
- <!-- service clients: -->
- <!-- service servers: -->
- <!-- action clients: -->
- <!-- action servers: -->
- <include file="$(find tibi_dabo_base)/launch/$(env ROBOT)_sensors.launch">
- <arg name="front_laser" value="False" />
- <arg name="rear_laser" value="False" />
- <arg name="vertical_laser" value="False" />
- <arg name="wiimote" value="False" />
- <arg name="bumblebee_right" value="True" />
- <arg name="battery" value="False" />
- </include>
-
- <!-- tibi/dabo devices -->
- <!-- published topics: $(env ROBOT)/joint_states -->
- <!-- $(env ROBOT)/segway/status-->
- <!-- subscribed topics: $(env ROBOT)/head/joint_position -->
- <!-- $(env ROBOT)/head/facial_expression -->
- <!-- $(env ROBOT)/segway/cmd_vel -->
- <!-- $(env ROBOT)/joint_states -->
- <!-- service clients: -->
- <!-- service servers: -->
- <!-- action clients: -->
- <!-- action servers: $(env ROBOT)/left_arm/motion_sequence -->
- <!-- $(env ROBOT)/left_arm/joint_motion -->
- <!-- $(env ROBOT)/right_arm/motion_sequence -->
- <!-- $(env ROBOT)/right_arm/joint_motion -->
- <!-- $(env ROBOT)/head/motion_sequence -->
- <!-- $(env ROBOT)/head/lights_sequence -->
- <!-- $(env ROBOT)/head/joint_motion -->
- <include file="$(find tibi_dabo_base)/launch/$(env ROBOT)_devices.launch">
- <arg name="head" value="True" />
- <arg name="left_arm" value="False" />
- <arg name="right_arm" value="False" />
- <arg name="platform" value="True" />
- </include>
-
- <!--image processing -->
- <!-- published topics: /$(env ROBOT)/processed_image -->
- <!-- subscribed topics: /$(env ROBOT)/sensors/head_right/image_raw -->
- <!-- /$(env ROBOT)/sensors/head_right/camera_info -->
- <!-- service clients: -->
- <!-- service servers: -->
- <!-- action clients: -->
- <!-- action servers: -->
- <include file="$(find iri_firewire_camera)/launch/tibi_dabo_crop_by_2_rect.launch">
- </include>
-
- <group ns="$(env ROBOT)">
-
- <!-- Starts Chessboard Pattern Projection node -->
- <node name="chessboard_pattern_projection_rect"
- pkg ="iri_chessboard_pattern_projection"
- type="iri_chessboard_pattern_projection" >
- <remap from="/$(env ROBOT)/chessboard_pattern_projection_rect/image"
- to="/$(env ROBOT)/processed_image" />
- <param name="~tf_prefix" value="/$(env ROBOT)" />
- </node>
-
- <!--
- <node name="mono_calibration"
- pkg ="camera_calibration"
- type="cameracalibrator.py"
- args="-size 9x6 -square 0.054 image:=/$(env ROBOT)/iri_firewire_camera/camera_image camera:=/stereo/right">
- </node>
- -->
-
- </group>
-
-</launch>
-
diff --git a/launch/tibi_dabo_rect.launch b/launch/tibi_dabo_rect.launch
deleted file mode 100644
index 5e32353e6b190f90f5e68b8d992eec7b8f615d07..0000000000000000000000000000000000000000
--- a/launch/tibi_dabo_rect.launch
+++ /dev/null
@@ -1,42 +0,0 @@
-<launch>
-
- <!-- load robot defined machines -->
- <include file="$(find tibi_dabo_base)/machines/$(env ROBOT).machines" />
-
- <group ns="$(env ROBOT)">
- <!-- published topics: /$(env ROBOT)/processed_image -->
- <!-- subscribed topics: /$(env ROBOT)/sensors/head_right/image_raw -->
- <!-- /$(env ROBOT)/sensors/head_right/camera_info -->
- <!-- service clients: -->
- <!-- service servers: -->
- <!-- action clients: -->
- <!-- action servers: -->
- <!-- image crop -->
- <node pkg ="nodelet"
- type="nodelet"
- name="image_proc_dec"
- machine="visio"
- args="standalone image_proc/crop_decimate">
- <param name="decimation_x" value="1" />
- <param name="decimation_y" value="1" />
- <remap from="/$(env ROBOT)/camera/image_raw"
- to="/$(env ROBOT)/sensors/head_right/image_raw"/>
- <remap from="/$(env ROBOT)/camera/camera_info"
- to="/$(env ROBOT)/sensors/head_right/camera_info"/>
- </node>
-
- <!-- image rectification -->
- <node pkg ="image_proc"
- type="image_proc"
- name="image_proc_rect"
- machine="visio">
- <remap from="/$(env ROBOT)/image_raw"
- to="/$(env ROBOT)/camera_out/image_raw" />
- <remap from="/$(env ROBOT)/camera_info"
- to="/$(env ROBOT)/camera_out/camera_info" />
- <remap from="/$(env ROBOT)/image_rect_color"
- to="/$(env ROBOT)/processed_image" />
- </node>
- </group>
-</launch>
-
diff --git a/manifest.xml b/manifest.xml
deleted file mode 100644
index fc47a7d8c605d15d9996e5dd3d1adaf795bc6014..0000000000000000000000000000000000000000
--- a/manifest.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<package>
- <description brief="iri_firewire_camera">
-
- iri_firewire_camera
-
- </description>
- <author>Sergi Hernandez Juan</author>
- <license>LGPL</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/iri_firewire_camera</url>
- <depend package="roscpp"/>
- <depend package="iri_base_driver"/>
- <depend package="sensor_msgs"/>
- <depend package="image_transport"/>
- <depend package="camera_info_manager"/>
- <depend package="nodelet"/>
-
- <export>
- <nodelet plugin="${prefix}/firewire_nodelet_plugin.xml"/>
- <cpp cflags="-I${prefix}/include"
- lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_base_driver"/>
- </export>
-
-</package>
-
-
diff --git a/src/firewire_camera_driver_nodelet.cpp b/src/firewire_camera_driver_nodelet.cpp
deleted file mode 100755
index f031363448361a168c369fefc12796d28c270ded..0000000000000000000000000000000000000000
--- a/src/firewire_camera_driver_nodelet.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-#include "firewire_camera_driver_nodelet.h"
-#include <pluginlib/class_list_macros.h>
-
-FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_)),
- it(new image_transport::ImageTransport(this->public_node_handle_)),Image_msg_(new sensor_msgs::Image)
-{
- std::string cal_file;
-
- //init class attributes if necessary
- this->loop_rate_ = 1000;//in [Hz]
- this->desired_framerate=30.0;
- this->diagnosed_camera_image=NULL;
-
- 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);
-
- // try to load the calibration file
- public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
- if(this->camera_manager.validateURL(cal_file))
- {
- if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
- ROS_INFO("Invalid calibration file");
- }
- else
- ROS_INFO("Invalid calibration file");
-
- public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
-
- // [init subscribers]
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
-}
-
-void FirewireCameraDriverNode::mainNodeThread(void)
-{
- sensor_msgs::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo(this->camera_manager.getCameraInfo()));
- std::list<std::string> events;
- char *image_data=NULL;
- TCameraConfig config;
-
- try{
- //lock access to driver if necessary
- this->driver_.lock();
- if(this->driver_.isRunning())
- {
- this->new_frame_event=this->driver_.get_new_frame_event();
- if(this->new_frame_event!="")
- {
- 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)
- {
- 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)
- {
- if(config.depth==DEPTH8)
- this->Image_msg_->encoding="8UC1";
- else if(config.depth==DEPTH16)
- this->Image_msg_->encoding="16UC1";
- else
- this->Image_msg_->encoding="16UC1";
- }
- else if(config.coding==YUV)
- {
- this->Image_msg_->encoding="yuv422";
- }
- else
- {
- if(config.depth==DEPTH24)
- this->Image_msg_->encoding="rgb8";
- else if(config.depth==DEPTH48)
- this->Image_msg_->encoding="16UC3";
- else
- this->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->diagnosed_camera_image->tick();
- }
- }
- this->driver_.unlock();
- }catch(CException &e){
- this->driver_.unlock();
- ROS_INFO("Impossible to capture frame");
- }
- //fill srv structure and make request to the server
-}
- // [fill msg Header if necessary]
-
-void FirewireCameraDriverNode::check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat)
-{
- TCameraConfig desired_config;
- TCameraConfig current_config;
-
- this->driver_.get_current_config(¤t_config);
- this->driver_.get_desired_config(&desired_config);
-
- if(current_config.left_offset!=desired_config.left_offset)
- {
- stat.summary(1,"The current configuration differs from the desired configuration");
- stat.addf("Image left offset:","current value is %d,desired value is %d", current_config.left_offset,desired_config.left_offset);
- }
- else if(current_config.top_offset!=desired_config.top_offset)
- {
- stat.summary(1,"The current configuration differs from the desired configuration");
- stat.addf("Image top offset:","current value is %d,desired value is %d", current_config.top_offset,desired_config.top_offset);
- }
- else if(current_config.width!=desired_config.width)
- {
- stat.summary(1,"The current configuration differs from the desired configuration");
- stat.addf("Image width:","current value is %d,desired value is %d", current_config.width,desired_config.width);
- }
- else if(current_config.height!=desired_config.height)
- {
- stat.summary(1,"The current configuration differs from the desired configuration");
- stat.addf("Image height:","current value is %d,desired value is %d", current_config.height,desired_config.height);
- }
- else
- stat.summary(0,"The current configuration is okay");
-}
-
-/* [subscriber callbacks] */
-
-/* [service callbacks] */
-
-/* [action callbacks] */
-
-/* [action requests] */
-
-void FirewireCameraDriverNode::postNodeOpenHook(void)
-{
-}
-
-void FirewireCameraDriverNode::addNodeDiagnostics(void)
-{
- diagnostic_.add("Check Configuration", this, &FirewireCameraDriverNode::check_configuration);
-}
-
-void FirewireCameraDriverNode::addNodeOpenedTests(void)
-{
-}
-
-void FirewireCameraDriverNode::addNodeStoppedTests(void)
-{
-}
-
-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->diagnosed_camera_image!=NULL)
- {
- delete this->diagnosed_camera_image;
- this->diagnosed_camera_image=NULL;
- }
-}
-
-#include "firewire_camera_driver_nodelet.h"
-#include <pluginlib/class_list_macros.h>
-
-FirewireCameraNodelet::FirewireCameraNodelet()
-{
- this->dev=NULL;
- // initialize the thread
- this->thread_server=CThreadServer::instance();
- this->spin_thread_id="firewire_camera_driver_nodelet_spin";
- this->thread_server->create_thread(this->spin_thread_id);
- this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
-}
-
-void FirewireCameraNodelet::onInit()
-{
- ros::NodeHandle priv_nh(getPrivateNodeHandle());
- this->dev=new FirewireCameraDriverNode(priv_nh);
- // start the spin thread
- this->thread_server->start_thread(this->spin_thread_id);
-}
-
-void *FirewireCameraNodelet::spin_thread(void *param)
-{
- FirewireCameraNodelet *nodelet=(FirewireCameraNodelet *)param;
-
- nodelet->dev->spin();
-
- pthread_exit(NULL);
-}
-
-FirewireCameraNodelet::~FirewireCameraNodelet()
-{
- // 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="";
- if(this->dev!=NULL)
- delete this->dev;
-}
-
-// parameters are: package, class name, class type, base class type
-PLUGINLIB_DECLARE_CLASS(iri_firewire_camera, iri_firewire_nodelet, FirewireCameraNodelet, nodelet::Nodelet);
-