Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039 #include <self_test/self_test.h>
00040
00041
00042 #include <sensor_msgs/fill_image.h>
00043
00044
00045 #include <photo/GetConfig.h>
00046 #include <photo/SetConfig.h>
00047 #include <photo/Capture.h>
00048
00049
00050 #include "photo/photo_camera_list.hpp"
00051 #include "photo/photo_camera.hpp"
00052 #include "photo/photo_image.hpp"
00053
00054
00055 class PhotoNode
00056 {
00057 public:
00058 photo_camera_list camera_list_;
00059 photo_camera camera_;
00060 photo_image image_;
00061
00062 boost::mutex photo_mutex_ ;
00063
00064 ros::ServiceServer set_config_srv_;
00065 ros::ServiceServer get_config_srv_;
00066 ros::ServiceServer capture_srv_;
00067
00068 PhotoNode() :
00069 camera_list_(),
00070 camera_(),
00071 image_()
00072 {
00073
00074 ros::NodeHandle private_nh("~");
00075 GPContext* private_context;
00076
00077
00078
00079
00080 private_context = camera_.photo_camera_create_context();
00081
00082
00083 if( camera_list_.autodetect( private_context ) == false )
00084 {
00085 ROS_FATAL( "photo_node: Autodetection of cameras failed." );
00086 gp_context_unref( private_context );
00087 private_nh.shutdown();
00088 return;
00089 }
00090
00091
00092 if( camera_.photo_camera_open( &camera_list_, 0 ) == false )
00093 {
00094 ROS_FATAL( "photo_node: Could not open camera %d.", 0 );
00095 gp_context_unref( private_context );
00096 private_nh.shutdown();
00097 return;
00098 }
00099
00100
00101 set_config_srv_ = private_nh.advertiseService("set_config", &PhotoNode::setConfig, this);
00102 get_config_srv_ = private_nh.advertiseService("get_config", &PhotoNode::getConfig, this);
00103 capture_srv_ = private_nh.advertiseService("capture", &PhotoNode::capture, this);
00104 }
00105
00106 ~PhotoNode()
00107 {
00108
00109 camera_.photo_camera_close();
00110 }
00111
00112 bool setConfig( photo::SetConfig::Request& req, photo::SetConfig::Response& resp )
00113 {
00114 photo_mutex_.lock();
00115 bool error_code = camera_.photo_camera_set_config( req.param, req.value );
00116 photo_mutex_.unlock();
00117 return error_code;
00118 }
00119
00120 bool getConfig( photo::GetConfig::Request& req, photo::GetConfig::Response& resp )
00121 {
00122 char* value = new char[255];
00123 photo_mutex_.lock();
00124 bool error_code = camera_.photo_camera_get_config( req.param, &value );
00125 if( error_code )
00126 {
00127 resp.value = value;
00128 }
00129 photo_mutex_.unlock();
00130 delete value;
00131 return error_code;
00132 }
00133
00134 bool capture( photo::Capture::Request& req, photo::Capture::Response& resp )
00135 {
00136
00137 photo_mutex_.lock();
00138 bool error_code = camera_.photo_camera_capture( &image_ );
00139 if( error_code )
00140 {
00141
00142 fillImage( resp.image, "rgb8", image_.getHeight(), image_.getWidth(), image_.getBytesPerPixel() * image_.getWidth(), image_.getDataAddress() );
00143 }
00144 photo_mutex_.unlock();
00145 return error_code;
00146 }
00147 };
00148
00149 int main(int argc, char **argv)
00150 {
00151 ros::init(argc, argv, "photo");
00152 PhotoNode a;
00153 ros::spin();
00154
00155 return 0;
00156 }