prosilica_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *
00005 * Redistribution and use in source and binary forms, with or without
00006 * modification, are permitted provided that the following conditions
00007 * are met:
00008 *
00009 * * Redistributions of source code must retain the above copyright
00010 * notice, this list of conditions and the following disclaimer.
00011 * * Redistributions in binary form must reproduce the above
00012 * copyright notice, this list of conditions and the following
00013 * disclaimer in the documentation and/or other materials provided
00014 * with the distribution.
00015 * * Neither the name of the Willow Garage nor the names of its
00016 * contributors may be used to endorse or promote products derived
00017 * from this software without specific prior written permission.
00018 *
00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00020 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00021 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00022 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00023 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00024 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00025 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00026 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00027 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00028 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00029 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030 * POSSIBILITY OF SUCH DAMAGE.
00031 *********************************************************************/
00032 
00033 #include <string>
00034 #include <ros/ros.h>
00035 #include <ros/console.h>
00036 #include <nodelet/nodelet.h>
00037 #include <image_transport/image_transport.h>
00038 #include <dynamic_reconfigure/server.h>
00039 #include <driver_base/SensorLevels.h>
00040 #include <diagnostic_updater/diagnostic_updater.h>
00041 #include <diagnostic_updater/publisher.h>
00042 #include <camera_calibration_parsers/parse_ini.h>
00043 #include <polled_camera/publication_server.h>
00044 #include <sensor_msgs/CameraInfo.h>
00045 
00046 #include <sensor_msgs/Image.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 #include <sensor_msgs/fill_image.h>
00049 #include <sensor_msgs/SetCameraInfo.h>
00050 
00051 #include <boost/thread.hpp>
00052 
00053 #include <prosilica_camera/ProsilicaCameraConfig.h>
00054 #include "prosilica/prosilica.h"
00055 #include "prosilica/rolling_sum.h"
00056 
00057 bool prosilica_inited = false;//for if the nodelet is loaded multiple times in the same manager
00058 int num_cameras = 0;
00059 
00060 namespace prosilica_camera
00061 {
00062 
00063 class ProsilicaNodelet : public nodelet::Nodelet
00064 {
00065 
00066 public:
00067 
00068     virtual ~ProsilicaNodelet()
00069     {
00071         init_thread_.interrupt();
00072         init_thread_.join();
00073 
00074         if(camera_)
00075         {
00076             camera_->stop();
00077             camera_.reset(); // must destroy Camera before calling prosilica::fini
00078         }
00079 
00080         trigger_sub_.shutdown();
00081         poll_srv_.shutdown();
00082         image_publisher_.shutdown();
00083 
00084         --num_cameras;
00085         if(num_cameras<=0)
00086         {
00087             prosilica::fini();
00088             prosilica_inited = false;
00089             num_cameras = 0;
00090         }
00091 
00092         NODELET_WARN("Unloaded prosilica camera with guid %s", hw_id_.c_str());
00093     }
00094 
00095     ProsilicaNodelet()
00096       : auto_adjust_stream_bytes_per_second_(true),
00097         count_(0),
00098         frames_dropped_total_(0), frames_completed_total_(0),
00099         frames_dropped_acc_(WINDOW_SIZE),
00100         frames_completed_acc_(WINDOW_SIZE),
00101         packets_missed_total_(0), packets_received_total_(0),
00102         packets_missed_acc_(WINDOW_SIZE),
00103         packets_received_acc_(WINDOW_SIZE)
00104     {
00105         ++num_cameras;
00106     }
00107 
00108 private:
00109 
00110     boost::shared_ptr<prosilica::Camera> camera_;
00111     boost::thread init_thread_;
00112     ros::Timer update_timer_;
00113 
00114     image_transport::CameraPublisher                           image_publisher_;
00115     polled_camera::PublicationServer                           poll_srv_;
00116     ros::ServiceServer                                         set_camera_info_srv_;
00117     ros::Subscriber                                            trigger_sub_;
00118 
00119     sensor_msgs::Image img_;
00120     sensor_msgs::CameraInfo cam_info_;
00121 
00122     std::string   frame_id_;
00123     unsigned long guid_;
00124     std::string   hw_id_;
00125     std::string   ip_address_;
00126     double        open_camera_retry_period_;
00127     std::string   trig_timestamp_topic_;
00128     ros::Time     trig_time_;
00129 
00130     // Dynamic reconfigure parameters
00131     double        update_rate_;
00132     int           trigger_mode_;
00133     bool          auto_adjust_stream_bytes_per_second_;
00134 
00135     tPvUint32 sensor_width_, sensor_height_;
00136     tPvUint32 max_binning_x, max_binning_y, dummy;
00137     int count_;
00138 
00139     // Dynamic Reconfigure
00140     prosilica_camera::ProsilicaCameraConfig last_config_;
00141     boost::recursive_mutex config_mutex_;
00142     typedef dynamic_reconfigure::Server<prosilica_camera::ProsilicaCameraConfig> ReconfigureServer;
00143     boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00144 
00145     // State updater
00146     enum CameraState
00147     {
00148         OPENING,
00149         CAMERA_NOT_FOUND,
00150         FORMAT_ERROR,
00151         ERROR,
00152         OK
00153     }camera_state_;
00154     std::string state_info_;
00155     std::string intrinsics_;
00156     static const int WINDOW_SIZE = 100; // remember previous 5s
00157     unsigned long frames_dropped_total_, frames_completed_total_;
00158     RollingSum<unsigned long> frames_dropped_acc_, frames_completed_acc_;
00159     unsigned long packets_missed_total_, packets_received_total_;
00160     RollingSum<unsigned long> packets_missed_acc_, packets_received_acc_;
00161 
00162     diagnostic_updater::Updater updater;
00163 
00164 
00165     virtual void onInit()
00166     {
00170         init_thread_ = boost::thread(boost::bind(&ProsilicaNodelet::onInitImpl, this));
00171 
00172     }
00173 
00174     void onInitImpl()
00175     {
00176         ros::NodeHandle& nh = getNodeHandle();
00177         ros::NodeHandle& pn = getPrivateNodeHandle();
00178 
00180         if(!prosilica_inited)
00181         {
00182             NODELET_INFO("Initializing prosilica GIGE API");
00183             prosilica::init();
00184             prosilica_inited = true;
00185         }
00186 
00188         count_ = 0;
00189         update_rate_=30;
00190         NODELET_INFO("namespace: %s", pn.getNamespace().c_str());
00191         pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame");
00192         NODELET_INFO("Loaded param frame_id: %s", frame_id_.c_str());
00193 
00194         pn.param<std::string>("guid", hw_id_, "");
00195         if(hw_id_ == "")
00196         {
00197             guid_ = 0;
00198         }
00199         else
00200         {
00201             guid_ = boost::lexical_cast<int>(hw_id_);
00202             NODELET_INFO("Loaded param guid: %lu", guid_);
00203         }
00204 
00205         pn.param<std::string>("ip_address", ip_address_, "");
00206         NODELET_INFO("Loaded ip address: %s", ip_address_.c_str());
00207 
00208         pn.param<double>("open_camera_retry_period", open_camera_retry_period_, 3.);
00209         NODELET_INFO("Retry period: %f", open_camera_retry_period_);
00210 
00211         // Setup updater
00212         updater.add(getName().c_str(), this, &ProsilicaNodelet::getCurrentState);
00213         NODELET_INFO("updated state");
00214         // Setup periodic callback to get new data from the camera
00215         update_timer_ = nh.createTimer(ros::Rate(update_rate_).expectedCycleTime(), &ProsilicaNodelet::updateCallback, this, false ,false);
00216         update_timer_.stop();
00217         NODELET_INFO("created update timer");
00218         // Open camera
00219         openCamera();
00220 
00221         // Advertise topics
00222         ros::NodeHandle image_nh(nh);
00223         image_transport::ImageTransport image_it(image_nh);
00224         image_publisher_ = image_it.advertiseCamera("image_raw", 1);
00225         poll_srv_ = polled_camera::advertise(nh, "request_image", &ProsilicaNodelet::pollCallback, this);
00226         set_camera_info_srv_ = pn.advertiseService("set_camera_info", &ProsilicaNodelet::setCameraInfo, this);
00227         trigger_sub_ = pn.subscribe(trig_timestamp_topic_, 1, &ProsilicaNodelet::syncInCallback, this);
00228 
00229         // Setup dynamic reconfigure server
00230         reconfigure_server_.reset(new ReconfigureServer(config_mutex_, pn));
00231         ReconfigureServer::CallbackType f = boost::bind(&ProsilicaNodelet::reconfigureCallback, this, _1, _2);
00232         reconfigure_server_->setCallback(f);
00233     }
00234 
00235     void openCamera()
00236     {
00237         while (!camera_)
00238         {
00239             boost::lock_guard<boost::recursive_mutex> scoped_lock(config_mutex_);
00240             camera_state_ = OPENING;
00241             try
00242             {
00243                 if(guid_ != 0)
00244                 {
00245                     state_info_ = "Trying to load camera with guid " + hw_id_;
00246                     NODELET_INFO("%s", state_info_.c_str());
00247                     camera_ = boost::make_shared<prosilica::Camera>((unsigned long)guid_);
00248                     updater.setHardwareIDf("%d", guid_);
00249                     NODELET_INFO("Started Prosilica camera with guid \"%lu\"", guid_);
00250 
00251                 }
00252                 else if(!ip_address_.empty())
00253                 {
00254                     state_info_ = "Trying to load camera with ipaddress: " + ip_address_;
00255                     NODELET_INFO("%s", state_info_.c_str());
00256                     camera_ = boost::make_shared<prosilica::Camera>(ip_address_.c_str());
00257                     guid_ = camera_->guid();
00258                     hw_id_ = boost::lexical_cast<std::string>(guid_);
00259                     updater.setHardwareIDf("%d", guid_);
00260 
00261                     NODELET_INFO("Started Prosilica camera with guid \"%d\"", (int)camera_->guid());
00262                 }
00263                 else
00264                 {
00265                     updater.setHardwareID("unknown");
00266                     if(prosilica::numCameras()>0)
00267                     {
00268                         state_info_ = "Trying to load first camera found";
00269                         NODELET_INFO("%s", state_info_.c_str());
00270                         guid_ = prosilica::getGuid(0);
00271                         camera_ = boost::make_shared<prosilica::Camera>((unsigned long)guid_);
00272                         hw_id_ = boost::lexical_cast<std::string>(guid_);
00273                         updater.setHardwareIDf("%d", guid_);
00274                         NODELET_INFO("Started Prosilica camera with guid \"%d\"", (int)guid_);
00275                     }
00276                     else
00277                     {
00278                         throw std::runtime_error("Found no cameras on local subnet");
00279                     }
00280                 }
00281 
00282             }
00283             catch (std::exception& e)
00284             {
00285                 camera_state_ = CAMERA_NOT_FOUND;
00286                 std::stringstream err;
00287                 if (prosilica::numCameras() == 0)
00288                 {
00289                     err << "Found no cameras on local subnet";
00290                 }
00291                 else if (guid_ != 0)
00292                 {
00293                     err << "Unable to open prosilica camera with guid " << guid_ <<": "<<e.what();
00294                 }
00295                 else if (ip_address_ != "")
00296                 {
00297                     err << "Unable to open prosilica camera with ip address " << ip_address_ <<": "<<e.what();
00298                 }
00299 
00300                 state_info_ = err.str();
00301                 NODELET_WARN("%s", state_info_.c_str());
00302                 if(prosilica::numCameras() > 0)
00303                 {
00304                     NODELET_INFO("Available cameras:\n%s", getAvailableCameras().c_str());
00305                 }
00306                 camera_.reset();
00307 
00308             }
00309             updater.update();
00310             boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period_));
00311         }
00312         loadIntrinsics();
00313         start();
00314     }
00315 
00316     std::string getAvailableCameras()
00317     {
00318         std::vector<prosilica::CameraInfo> cameras = prosilica::listCameras();
00319         std::stringstream list;
00320         for (unsigned int i = 0; i < cameras.size(); ++i)
00321         {
00322             list << cameras[i].serial << " - " <<cameras[i].name<< " - Unique ID = "<<cameras[i].guid<<" IP = "<<cameras[i].ip_address<<std::endl;
00323         }
00324         return list.str();
00325     }
00326 
00327     void loadIntrinsics()
00328     {
00329         try
00330         {
00331             camera_->setKillCallback(boost::bind(&ProsilicaNodelet::kill, this, _1));
00332 
00333             if(auto_adjust_stream_bytes_per_second_ && camera_->hasAttribute("StreamBytesPerSecond"))
00334                 camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(115000000/num_cameras));
00335 
00336             // Retrieve contents of user memory
00337             std::string buffer(prosilica::Camera::USER_MEMORY_SIZE, '\0');
00338             camera_->readUserMemory(&buffer[0], prosilica::Camera::USER_MEMORY_SIZE);
00339 
00340             PvAttrRangeUint32(camera_->handle(), "BinningX", &dummy, &max_binning_x);
00341             PvAttrRangeUint32(camera_->handle(), "BinningY", &dummy, &max_binning_y);
00342             PvAttrRangeUint32(camera_->handle(), "Width",    &dummy, &sensor_width_);
00343             PvAttrRangeUint32(camera_->handle(), "Height",   &dummy, &sensor_height_);
00344 
00345 
00346             // Parse calibration file
00347             std::string camera_name;
00348             if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, cam_info_))
00349             {
00350                 intrinsics_ = "Loaded calibration";
00351                 NODELET_INFO("Loaded calibration for camera '%s'", camera_name.c_str());
00352             }
00353             else
00354             {
00355                 intrinsics_ = "Failed to load intrinsics from camera";
00356                 NODELET_WARN("Failed to load intrinsics from camera");
00357             }
00358         }
00359         catch(std::exception &e)
00360         {
00361             camera_state_ = CAMERA_NOT_FOUND;
00362             state_info_ = e.what();
00363         }
00364     }
00365 
00366     void start()
00367     {
00368         try
00369         {
00370             switch(trigger_mode_)
00371             {
00372                 case prosilica::Software:
00373                     NODELET_INFO("starting camera %s in software trigger mode", hw_id_.c_str());
00374                     camera_->start(prosilica::Software, 1., prosilica::Continuous);
00375                     if(update_rate_ > 0)
00376                     {
00377                         update_timer_.setPeriod(ros::Rate(update_rate_).expectedCycleTime());
00378                         update_timer_.start();
00379                     }
00380                     break;
00381                 case prosilica::Freerun:
00382                     NODELET_INFO("starting camera %s in freerun trigger mode", hw_id_.c_str());
00383                     camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1));
00384                     camera_->start(prosilica::Freerun, 1., prosilica::Continuous);
00385                     break;
00386                 case prosilica::FixedRate:
00387                     NODELET_INFO("starting camera %s in fixedrate trigger mode", hw_id_.c_str());
00388                     camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1));
00389                     camera_->start(prosilica::FixedRate, update_rate_, prosilica::Continuous);
00390                     break;
00391                 case prosilica::SyncIn1:
00392                     NODELET_INFO("starting camera %s in sync1 trigger mode", hw_id_.c_str());
00393                     camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1));
00394                     camera_->start(prosilica::SyncIn1, update_rate_, prosilica::Continuous);
00395                     break;
00396                 case prosilica::SyncIn2:
00397                     NODELET_INFO("starting camera %s in sync2 trigger mode", hw_id_.c_str());
00398                     camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1));
00399                     camera_->start(prosilica::SyncIn2, update_rate_, prosilica::Continuous);
00400                     break;
00401                 default:
00402                     break;
00403             }
00404         }
00405         catch(std::exception &e)
00406         {
00407             camera_state_ = CAMERA_NOT_FOUND;
00408             state_info_ = e.what();
00409         }
00410     }
00411 
00412     void stop()
00413     {
00414         update_timer_.stop();
00415         if(!camera_)
00416             return;
00417         camera_->removeEvents();
00418         camera_->stop();
00419 
00420     }
00421 
00422     void kill(unsigned long guid)
00423     {
00424         if(guid == guid_)
00425         {
00426             NODELET_WARN("[%s] got kill request for prosilica camera %lu",getName().c_str(), guid);
00428             init_thread_.interrupt();
00429             init_thread_.join();
00430 
00431             camera_state_ = CAMERA_NOT_FOUND;
00432             state_info_ = "Prosilica camera " + hw_id_ + " disconnected";
00433             NODELET_ERROR("%s", state_info_.c_str());
00434             updater.update();
00435             boost::lock_guard<boost::recursive_mutex> scoped_lock(config_mutex_);
00436             stop();
00437             camera_.reset();
00438             init_thread_ = boost::thread(boost::bind(&ProsilicaNodelet::openCamera, this));
00439             return;
00440         }
00441     }
00442 
00443     void publishImage(tPvFrame* frame)
00444     {
00445         publishImage(frame, ros::Time::now());
00446     }
00447 
00448     void publishImage(tPvFrame* frame, ros::Time time)
00449     {
00450         camera_state_ = OK;
00451         state_info_ = "Camera operating normally";
00452         if (image_publisher_.getNumSubscribers() > 0)
00453         {
00454 
00455             if (processFrame(frame, img_, cam_info_))
00456             {
00457                 image_publisher_.publish(img_, cam_info_, time);
00458                 frames_dropped_acc_.add(0);
00459             }
00460             else
00461             {
00462                 camera_state_ = FORMAT_ERROR;
00463                 state_info_ = "Unable to process frame";
00464                 ++frames_dropped_total_;
00465                 frames_dropped_acc_.add(1);
00466             }
00467             ++frames_completed_total_;
00468             frames_completed_acc_.add(1);
00469         }
00470         updater.update();
00471     }
00472 
00473     void updateCallback(const ros::TimerEvent &event)
00474     {
00475         // Download the most recent data from the device
00476         camera_state_ = OK;
00477         state_info_ = "Camera operating normally";
00478         if(image_publisher_.getNumSubscribers() > 0)
00479         {
00480             boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00481             try
00482             {
00483                 tPvFrame* frame = NULL;
00484                 frame = camera_->grab(1000);
00485                 publishImage(frame, event.current_real);
00486             }
00487             catch(std::exception &e)
00488             {
00489                 camera_state_ = ERROR;
00490                 state_info_ = e.what();
00491                 NODELET_ERROR("Unable to read from camera: %s", e.what());
00492                 ++frames_dropped_total_;
00493                 frames_dropped_acc_.add(1);
00494                 updater.update();
00495                 return;
00496             }
00497         }
00498     }
00499 
00500     void pollCallback(polled_camera::GetPolledImage::Request& req,
00501                       polled_camera::GetPolledImage::Response& rsp,
00502                       sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
00503     {
00504         if (trigger_mode_ != prosilica::Software)
00505         {
00506             rsp.success = false;
00507             rsp.status_message = "Camera is not in software triggered mode";
00508             return;
00509         }
00510 
00511         last_config_.binning_x = req.binning_x;
00512         last_config_.binning_y = req.binning_y;
00513         last_config_.x_offset = req.roi.x_offset;
00514         last_config_.y_offset = req.roi.y_offset;
00515         last_config_.height   = req.roi.height;
00516         last_config_.width    = req.roi.width;
00517 
00518         reconfigureCallback(last_config_, driver_base::SensorLevels::RECONFIGURE_RUNNING);
00519 
00520         try
00521         {
00522             tPvFrame* frame = NULL;
00523             frame = camera_->grab(req.timeout.toSec()*100);
00524             if (processFrame(frame, image, info))
00525             {
00526                 image.header.stamp = info.header.stamp =rsp.stamp = ros::Time::now();
00527                 rsp.status_message = "Success";
00528                 rsp.success = true;
00529             }
00530             else
00531             {
00532                 rsp.success = false;
00533                 rsp.status_message = "Failed to process image";
00534                 return;
00535             }
00536         }
00537         catch(std::exception &e)
00538         {
00539             rsp.success = false;
00540             std::stringstream err;
00541             err<< "Failed to grab frame: "<<e.what();
00542             rsp.status_message = err.str();
00543             return;
00544         }
00545     }
00546 
00547     void syncInCallback (const std_msgs::HeaderConstPtr& msg)
00548     {
00549         if (trigger_mode_ != prosilica::Software)
00550         {
00551             camera_state_ = ERROR;
00552             state_info_ = "Can not sync from topic trigger unless in Software Trigger mode";
00553             NODELET_ERROR_ONCE("%s", state_info_.c_str());
00554             return;
00555         }
00556         ros::TimerEvent e;
00557         e.current_real = msg->stamp;
00558         updateCallback(e);
00559     }
00560 
00561     bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info)
00562     {
00564         if (frame==NULL || frame->Status != ePvErrSuccess)
00565             return false;
00566 
00567         try
00568         {
00572             tPvUint32 binning_x = 1, binning_y = 1;
00573             if (camera_->hasAttribute("BinningX"))
00574             {
00575                 camera_->getAttribute("BinningX", binning_x);
00576                 camera_->getAttribute("BinningY", binning_y);
00577             }
00578 
00579             // Binning averages bayer samples, so just call it mono8 in that case
00580             if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1))
00581                 frame->Format = ePvFmtMono8;
00582 
00583             if (!frameToImage(frame, img))
00584                 return false;
00585 
00586             // Set the operational parameters in CameraInfo (binning, ROI)
00587             cam_info.binning_x = binning_x;
00588             cam_info.binning_y = binning_y;
00589             // ROI in CameraInfo is in unbinned coordinates, need to scale up
00590             cam_info.roi.x_offset = frame->RegionX * binning_x;
00591             cam_info.roi.y_offset = frame->RegionY * binning_y;
00592             cam_info.roi.height = frame->Height * binning_y;
00593             cam_info.roi.width = frame->Width * binning_x;
00594             cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) ||
00595                                        (frame->Width  != sensor_width_  / binning_x);
00596 
00597             if(auto_adjust_stream_bytes_per_second_ && camera_->hasAttribute("StreamBytesPerSecond"))
00598                 camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(115000000/num_cameras));
00599         }
00600         catch(std::exception &e)
00601         {
00602             return false;
00603         }
00604 
00605         count_++;
00606         return true;
00607     }
00608 
00609     bool frameToImage(tPvFrame* frame, sensor_msgs::Image &image)
00610     {
00611         // NOTE: 16-bit and Yuv formats not supported
00612         static const char* BAYER_ENCODINGS[] = { "bayer_rggb8", "bayer_gbrg8", "bayer_grbg8", "bayer_bggr8" };
00613 
00614         std::string encoding;
00615         if (frame->Format == ePvFmtMono8)       encoding = sensor_msgs::image_encodings::MONO8;
00616         else if (frame->Format == ePvFmtBayer8) encoding = BAYER_ENCODINGS[frame->BayerPattern];
00617         else if (frame->Format == ePvFmtRgb24)  encoding = sensor_msgs::image_encodings::RGB8;
00618         else if (frame->Format == ePvFmtBgr24)  encoding = sensor_msgs::image_encodings::BGR8;
00619         else if (frame->Format == ePvFmtRgba32) encoding = sensor_msgs::image_encodings::RGBA8;
00620         else if (frame->Format == ePvFmtBgra32) encoding = sensor_msgs::image_encodings::BGRA8;
00621         else {
00622         NODELET_WARN("Received frame with unsupported pixel format %d", frame->Format);
00623         return false;
00624       }
00625 
00626       if(frame->ImageSize == 0 || frame->Height == 0)
00627           return false;
00628       uint32_t step = frame->ImageSize / frame->Height;
00629       return sensor_msgs::fillImage(image, encoding, frame->Height, frame->Width, step, frame->ImageBuffer);
00630     }
00631 
00632     bool setCameraInfo(sensor_msgs::SetCameraInfoRequest &req, sensor_msgs::SetCameraInfoResponse &rsp)
00633     {
00634         NODELET_INFO("New camera info received");
00635         sensor_msgs::CameraInfo &info = req.camera_info;
00636 
00637         // Sanity check: the image dimensions should match the max resolution of the sensor.
00638         if (info.width != sensor_width_ || info.height != sensor_height_)
00639         {
00640             rsp.success = false;
00641             rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00642                                                 "setting, camera running at resolution %ix%i.")
00643                                                  % info.width % info.height % sensor_width_ % sensor_height_).str();
00644             NODELET_ERROR("%s", rsp.status_message.c_str());
00645             return true;
00646         }
00647 
00648         stop();
00649 
00650         std::string cam_name = "prosilica";
00651         cam_name += hw_id_;
00652         std::stringstream ini_stream;
00653         if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info))
00654         {
00655             rsp.status_message = "Error formatting camera_info for storage.";
00656             rsp.success = false;
00657         }
00658         else
00659         {
00660             std::string ini = ini_stream.str();
00661             if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE)
00662             {
00663                 rsp.success = false;
00664                 rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity.";
00665             }
00666             else
00667             {
00668                 try
00669                 {
00670                     camera_->writeUserMemory(ini.c_str(), ini.size());
00671                     cam_info_ = info;
00672                     rsp.success = true;
00673                 }
00674                 catch (prosilica::ProsilicaException &e)
00675                 {
00676                     rsp.success = false;
00677                     rsp.status_message = e.what();
00678                 }
00679             }
00680         }
00681         if (!rsp.success)
00682         NODELET_ERROR("%s", rsp.status_message.c_str());
00683 
00684         start();
00685 
00686         return true;
00687     }
00688 
00689     void reconfigureCallback(prosilica_camera::ProsilicaCameraConfig &config, uint32_t level)
00690     {
00691         NODELET_DEBUG("Reconfigure request received");
00692 
00693         if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00694             stop();
00695 
00697         if (config.trigger_mode == "streaming")
00698         {
00699             trigger_mode_ = prosilica::Freerun;
00700             update_rate_ = 1.; // make sure we get _something_
00701         }
00702         else if (config.trigger_mode == "syncin1")
00703         {
00704             trigger_mode_ = prosilica::SyncIn1;
00705             update_rate_ = config.trig_rate;
00706         }
00707         else if (config.trigger_mode == "syncin2")
00708         {
00709             trigger_mode_ = prosilica::SyncIn2;
00710             update_rate_ = config.trig_rate;
00711         }
00712         else if (config.trigger_mode == "fixedrate")
00713         {
00714             trigger_mode_ = prosilica::FixedRate;
00715             update_rate_ = config.trig_rate;
00716         }
00717         else if (config.trigger_mode == "software")
00718         {
00719             trigger_mode_ = prosilica::Software;
00720             update_rate_ = config.trig_rate;
00721         }
00722 
00723         else if (config.trigger_mode == "polled")
00724         {
00725             trigger_mode_ = prosilica::Software;
00726             update_rate_ = 0;
00727         }
00728         else if (config.trigger_mode == "triggered")
00729         {
00730             trigger_mode_ = prosilica::Software;
00731             update_rate_ = 0;
00732         }
00733         else
00734         {
00735             NODELET_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str());
00736         }
00737 
00738         if(config.trig_timestamp_topic != last_config_.trig_timestamp_topic)
00739         {
00740             trigger_sub_.shutdown();
00741             trig_timestamp_topic_ = config.trig_timestamp_topic;
00742         }
00743 
00744         if(!trigger_sub_ && config.trigger_mode == "triggered")
00745         {
00746             trigger_sub_ = ros::NodeHandle().subscribe(trig_timestamp_topic_, 1, &ProsilicaNodelet::syncInCallback, this);
00747         }
00748 
00749 
00750         // Exposure
00751         if (config.auto_exposure)
00752         {
00753             camera_->setExposure(0, prosilica::Auto);
00754             if (camera_->hasAttribute("ExposureAutoMax"))
00755             {
00756                 tPvUint32 us = config.exposure_auto_max*1000000. + 0.5;
00757                 camera_->setAttribute("ExposureAutoMax", us);
00758             }
00759             if (camera_->hasAttribute("ExposureAutoTarget"))
00760                 camera_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target);
00761         }
00762         else
00763         {
00764             unsigned us = config.exposure*1000000. + 0.5;
00765             camera_->setExposure(us, prosilica::Manual);
00766             camera_->setAttribute("ExposureValue", (tPvUint32)us);
00767         }
00768 
00769         // Gain
00770         if (config.auto_gain)
00771         {
00772             if (camera_->hasAttribute("GainAutoMax"))
00773             {
00774                 camera_->setGain(0, prosilica::Auto);
00775                 camera_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max);
00776                 camera_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target);
00777             }
00778             else
00779             {
00780                 tPvUint32 major, minor;
00781                 camera_->getAttribute("FirmwareVerMajor", major);
00782                 camera_->getAttribute("FirmwareVerMinor", minor);
00783                 NODELET_WARN("Auto gain not available for this camera. Auto gain is available "
00784                 "on firmware versions 1.36 and above. You are running version %u.%u.",
00785                 (unsigned)major, (unsigned)minor);
00786                 config.auto_gain = false;
00787             }
00788         }
00789         else
00790         {
00791             camera_->setGain(config.gain, prosilica::Manual);
00792             camera_->setAttribute("GainValue", (tPvUint32)config.gain);
00793         }
00794 
00795         // White balance
00796         if (config.auto_whitebalance)
00797         {
00798             if (camera_->hasAttribute("WhitebalMode"))
00799                 camera_->setWhiteBalance(0, 0, prosilica::Auto);
00800             else
00801             {
00802                 NODELET_WARN("Auto white balance not available for this camera.");
00803                 config.auto_whitebalance = false;
00804             }
00805         }
00806         else
00807         {
00808             camera_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual);
00809             if (camera_->hasAttribute("WhitebalValueRed"))
00810                 camera_->setAttribute("WhitebalValueRed", (tPvUint32)config.whitebalance_red);
00811             if (camera_->hasAttribute("WhitebalValueBlue"))
00812                 camera_->setAttribute("WhitebalValueBlue", (tPvUint32)config.whitebalance_blue);
00813         }
00814 
00815         // Binning configuration
00816         if (camera_->hasAttribute("BinningX"))
00817         {
00818             config.binning_x = std::min(config.binning_x, (int)max_binning_x);
00819             config.binning_y = std::min(config.binning_y, (int)max_binning_y);
00820 
00821             camera_->setBinning(config.binning_x, config.binning_y);
00822         }
00823         else if (config.binning_x > 1 || config.binning_y > 1)
00824         {
00825             NODELET_WARN("Binning not available for this camera.");
00826             config.binning_x = config.binning_y = 1;
00827         }
00828 
00829         // Region of interest configuration
00830         // Make sure ROI fits in image
00831         config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1);
00832         config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1);
00833         config.width  = std::min(config.width, (int)sensor_width_ - config.x_offset);
00834         config.height = std::min(config.height, (int)sensor_height_ - config.y_offset);
00835         // If width or height is 0, set it as large as possible
00836         int width  = config.width  ? config.width  : sensor_width_  - config.x_offset;
00837         int height = config.height ? config.height : sensor_height_ - config.y_offset;
00838 
00839         // Adjust full-res ROI to binning ROI
00841         int x_offset = config.x_offset / config.binning_x;
00842         int y_offset = config.y_offset / config.binning_y;
00843         unsigned int right_x  = (config.x_offset + width  + config.binning_x - 1) / config.binning_x;
00844         unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y;
00845         // Rounding up is bad when at max resolution which is not divisible by the amount of binning
00846         right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x));
00847         bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y));
00848         width = right_x - x_offset;
00849         height = bottom_y - y_offset;
00850 
00851         camera_->setRoi(x_offset, y_offset, width, height);
00852 
00853       // TF frame
00854       img_.header.frame_id = cam_info_.header.frame_id = config.frame_id;
00855 
00856         // Normally the node adjusts the bandwidth used by the camera during diagnostics, to use as
00857         // much as possible without dropping packets. But this can create interference if two
00858         // cameras are on the same switch, e.g. for stereo. So we allow the user to set the bandwidth
00859         // directly.
00860         auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second;
00861         if (!auto_adjust_stream_bytes_per_second_)
00862             camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second);
00863         else
00864             camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(115000000/num_cameras));
00865 
00868         if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00869         {
00870             try
00871             {
00872                 start();
00873             }
00874             catch(std::exception &e)
00875             {
00876                 NODELET_ERROR("Invalid settings: %s", e.what());
00877                 config = last_config_;
00878             }
00879         }
00880 
00881         last_config_ = config;
00882     }
00883 
00884     void getCurrentState(diagnostic_updater::DiagnosticStatusWrapper &stat)
00885     {
00886         stat.add("Serial", guid_);
00887         stat.add("Info", state_info_);
00888         stat.add("Intrinsics", intrinsics_);
00889         stat.add("Total frames dropped", frames_dropped_total_);
00890         stat.add("Total frames", frames_completed_total_);
00891 
00892         if(frames_completed_total_>0)
00893         {
00894             stat.add("Total % frames dropped", 100.*(double)frames_dropped_total_/frames_completed_total_);
00895         }
00896         if(frames_completed_acc_.sum()>0)
00897         {
00898             stat.add("Recent % frames dropped", 100.*frames_dropped_acc_.sum()/frames_completed_acc_.sum());
00899         }
00900 
00901         switch (camera_state_)
00902         {
00903             case OPENING:
00904                 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Opening camera");
00905                 break;
00906             case OK:
00907                 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera operating normally");
00908                 break;
00909             case CAMERA_NOT_FOUND:
00910                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Can not find camera %d", guid_ );
00911                 stat.add("Available Cameras", getAvailableCameras());
00912                 break;
00913             case FORMAT_ERROR:
00914                 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Problem retrieving frame");
00915                 break;
00916             case ERROR:
00917                 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera has encountered an error");
00918                 break;
00919             default:
00920                 break;
00921         }
00922     }
00923 };
00924 }
00925 
00926 #include <pluginlib/class_list_macros.h>
00927 PLUGINLIB_EXPORT_CLASS(prosilica_camera::ProsilicaNodelet, nodelet::Nodelet);
00928 


prosilica_camera
Author(s): Maintained by William Woodall - wwoodall@willowgarage.com, Contributions by Allison Thackston - allison.thackston@nasa.gov
autogenerated on Fri Aug 28 2015 12:15:29