diff --git a/CMakeLists.txt b/CMakeLists.txt
index aa1ad1ccb3bba956d67c5f6a00f5cc7b67cfb402..53f54681664ae8a3d89df4f33f8805ff8245b1ff 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,7 @@ include_directories(${raw1394_INCLUDE_DIR})
include_directories(${dc1394_INCLUDE_DIR})
## Declare a cpp library
-add_library(${PROJECT_NAME}_nodelet src/firewire_camera_driver_nodelet.cpp src/firewire_camera_driver.cpp)
+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
diff --git a/cfg/FirewireCamera.cfg b/cfg/FirewireCamera.cfg
index 9eaebda7624e2acf54f4d3c4259ea2e9d9f86ede..7a78023d1e9897f1abb424681500e74aca2e5134 100755
--- a/cfg/FirewireCamera.cfg
+++ b/cfg/FirewireCamera.cfg
@@ -54,7 +54,6 @@ gen.const("RAW16", int_t, 7, "RAW coding with 16 bits per pixel")],"Available co
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("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)
diff --git a/include/camera_common.h b/include/camera_common.h
index 39264b08b823befdc0295b5e0a625dd275159737..26415dff2740e403727f7456b06efd2b28b95378 100755
--- a/include/camera_common.h
+++ b/include/camera_common.h
@@ -29,7 +29,7 @@ typedef struct
typedef struct
{
- image_transport::ImageTransport *it;
+ boost::shared_ptr<image_transport::ImageTransport> it;
image_transport::CameraPublisher camera_image_publisher_;
sensor_msgs::ImagePtr Image_msg_;
sensor_msgs::CameraInfoPtr CameraInfo_msg_;
diff --git a/include/firewire_camera_driver.h b/include/firewire_camera_driver.h
index 4b39431829002ff15f1e0f32aa110cde90e259fe..585e111396b728dc2cbd64acab5ad7d0d69e4a34 100644
--- a/include/firewire_camera_driver.h
+++ b/include/firewire_camera_driver.h
@@ -67,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
@@ -201,7 +201,7 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
* \brief
*
*/
- void get_image(char **image_data);
+ void get_image(char *image_data);
/**
* \brief
*
@@ -237,6 +237,12 @@ 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 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/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/src/firewire_camera_driver.cpp b/src/firewire_camera_driver.cpp
index 119860a405450608ede9cb8f1edf083ab4637715..f315e77cdb5e49853e5f7a86fff532c184686c22 100644
--- a/src/firewire_camera_driver.cpp
+++ b/src/firewire_camera_driver.cpp
@@ -32,10 +32,11 @@ 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)
@@ -61,6 +62,15 @@ bool FirewireCameraDriver::openDriver(void)
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
+ 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;
+ }
this->change_config(&this->desired_conf);
ROS_INFO("Driver opened");
return true;
@@ -144,8 +154,6 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
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();
@@ -181,8 +189,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();
@@ -509,12 +515,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)
@@ -577,14 +581,15 @@ 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->calibration_file;
+ return this->frame_id;
}
-std::string FirewireCameraDriver::get_frame_id(void)
+void FirewireCameraDriver::set_calibration_params(unsigned int width, unsigned int height)
{
- return this->frame_id;
+ this->cal_width=width;
+ this->cal_height=height;
}
FirewireCameraDriver::~FirewireCameraDriver()
diff --git a/src/firewire_camera_driver_node.cpp b/src/firewire_camera_driver_node.cpp
index e11831d1f8be244e39b9fcf739692de9c52dd538..541b9798aea17579a7514545596a1ea5b49ab5a8 100644
--- a/src/firewire_camera_driver_node.cpp
+++ b/src/firewire_camera_driver_node.cpp
@@ -1,33 +1,48 @@
#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>("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());
+ else
+ ROS_INFO("Invalid calibration file for the camera");
+ this->camera.CameraInfo_msg_.reset(new sensor_msgs::CameraInfo(this->camera_manager.getCameraInfo()));
+ cal_ok=true;
+ }
else
- ROS_INFO("Invalid calibration file");
+ {
+ ROS_INFO("Calibration file does not exist");
+ cal_ok=false;
+ }
+
+ if(!cal_ok)
+ this->driver_.set_calibration_params(-1,-1);
+ else
+ 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 +59,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 +73,46 @@ 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);
+ 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 || config.coding==RAW)
{
if(config.depth==DEPTH8)
- this->Image_msg_.encoding="mono8";
+ 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==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 +188,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 +205,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() + "_bumblebee2_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);
+
diff --git a/src/firewire_camera_driver_nodelet.cpp b/src/firewire_camera_driver_nodelet.cpp
deleted file mode 100755
index 1979648190ce60d814313647c52787fc716b18a5..0000000000000000000000000000000000000000
--- a/src/firewire_camera_driver_nodelet.cpp
+++ /dev/null
@@ -1,246 +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)
- {
- 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==RAW)
- {
- this->Image_msg_->encoding="bayer_rggb8";
- }
- 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;
-}
-
-void FirewireCameraNodelet::onInit()
-{
- this->dev=new FirewireCameraDriverNode(getPrivateNodeHandle());
- // initialize the thread
- this->thread_server=CThreadServer::instance();
- this->spin_thread_id=getName() + "_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);
- // 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, FirewireCameraNodelet, FirewireCameraNodelet, nodelet::Nodelet);
-