Advertisement
Guest User

Untitled

a guest
Jul 25th, 2017
1,753
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.86 KB | None | 0 0
  1. // ros include
  2. #include <ros/ros.h>
  3. #include <ros/console.h>
  4. #include <image_transport/image_transport.h>
  5. // std include
  6. #include <iostream>
  7. #include <string>
  8. #include <vector>
  9. // signal catching
  10. #include <signal.h>
  11. // Vimba (gigE cams) include
  12. #include "VimbaCPP.h"
  13. // open cv include
  14. #include <opencv2/highgui.hpp>
  15. // convert cv::Mat object to ROS message
  16. #include <cv_bridge/cv_bridge.h>
  17.  
  18. using namespace AVT::VmbAPI;
  19.  
  20. // globals
  21. bool SIGINTrecieved = false;
  22.  
  23.  
  24. // GETTING NOTIFIED ABOUT CAMERA LIST CHANGES
  25. // define observer that reacts on camera list changes
  26. class CamObserver : public ICameraListObserver
  27. {
  28. public:
  29.     void CameraListChanged( CameraPtr pCam, UpdateTriggerType reason ) {
  30.         if ( UpdateTriggerPluggedIn == reason || UpdateTriggerPluggedOut == reason ) {
  31.             // Put code here to react on the changed camera list (gained or lost)
  32.             ROS_ERROR("THE NUMBER OF CONNECTED CAMERAS HAS CHANGED SINCE START OF PROGRAM");
  33.         }
  34.     }
  35. };
  36.  
  37. class VimbaData
  38. {
  39. public:
  40.     // funciton prototypes
  41.     VimbaData( );
  42.     void initVimbaCams( );
  43.     void cameraCapture( );
  44.    
  45.     // Vimba (gigE) data structures
  46.     CameraPtrVector cameras_;
  47.     VimbaSystem &system_ = VimbaSystem::GetInstance();
  48.     std::vector<int> openedCams_;
  49.    
  50.     // ros data
  51.     ros::NodeHandle node_;
  52.     std::vector<image_transport::Publisher> vectorOfPubs_;
  53.     image_transport::ImageTransport it_;
  54.  
  55.     // .launch params
  56.     std::string pub_topic_template_;
  57.     std::string cam_settings_xml_path_;
  58.     bool display_images_on_screen_;
  59.  
  60. private:
  61.  
  62. };
  63.  
  64. // Purpose: link ImageTransport with NodeHandle.  Also read .launch params
  65. VimbaData::VimbaData( ) : it_(node_) {
  66.     // in the top line of the constructor, link the ImageTransport object to the ros::NodeHandle object
  67.  
  68.     // Read .launch params
  69.     ros::NodeHandle private_node("~");
  70.                                 // ("name_of_launch_var", local_var, default value);
  71.     private_node.param<std::string>("gigE_cam_pub_topic_template", pub_topic_template_, "gigE_cam_0");
  72.     private_node.param<std::string>("path_to_cam_settings_xml", cam_settings_xml_path_, "");
  73.     private_node.param<bool>("display_images_on_screen", display_images_on_screen_, "");
  74. }
  75.  
  76. /* Purpose: 1. Initialize all cameras detected on the local network.
  77.  *          2. Load camera parameters from file.
  78.  *          3. Register a camera_list observer which will throw a (non-lethal) error
  79.  *             if a camera is added or removed from the network during runtime.
  80.  *
  81.  * Parameters:
  82.  */
  83. void VimbaData::initVimbaCams(  ) {
  84.    
  85.  
  86.     // initalize Vimba API  
  87.     if ( VmbErrorSuccess == system_.Startup()) {
  88.         ROS_INFO("Opening all detected, network-connected cameras");
  89.         // find all connected cameras
  90.         if ( VmbErrorSuccess == system_.GetCameras ( cameras_ ) ) {
  91.             std::string camName;
  92.             int numCam = 0;
  93.             for (   CameraPtrVector::iterator iter = cameras_.begin();
  94.                     cameras_.end() != iter;
  95.                     ++iter )
  96.             {
  97.                 // attempt to open (prepare for use) all detected cameras
  98.                 if ( VmbErrorSuccess  == (*iter)->Open( VmbAccessModeFull ) ) {
  99.  
  100.                     if (VmbErrorSuccess != (*iter)->Camera::GetID( camName )) {
  101.                         ROS_ERROR("Could not get camera name for cam %d", numCam);
  102.                     }
  103.  
  104.                     // load camera settings form .xml
  105.                     // NOTE: Use VimbaViewer to create profiles, found in Vimba examples dir.
  106.                     if (VmbErrorSuccess != (*iter)->Camera::LoadCameraSettings(cam_settings_xml_path_) )
  107.                     {
  108.                         ROS_ERROR("FAILED to load camera settings for cam %d.", numCam);
  109.                     }
  110.  
  111.                     ROS_INFO("Camera %d opened!", numCam);
  112.                     // add cam to vector of openedCams
  113.                     openedCams_.push_back( numCam );
  114.                     ++numCam;
  115.  
  116.                     // you could also open specific cameras by id (ip address) with OpenCameraByID("{ip_address}", access mode, CameraPtr)
  117.                 } else { // this camera FAILED to open
  118.                     ROS_ERROR("Camera %d, named %s, FAILED to open", numCam, camName.c_str());
  119.                     ++numCam;
  120.                 }
  121.             }
  122.             // check if all detected cameras were opened.
  123.             if ( openedCams_.size() != cameras_.size() ) {
  124.                 ROS_ERROR("Not all cameras detected on the network were able to be opened! Maybe they are already in use.");
  125.             }
  126.         }
  127.     }
  128.    
  129.     //// CAMERA_LIST OBSERVER ////
  130.     // Register the observer; automatic discovery for GigE is turned on
  131.     VmbErrorType res;
  132.     CamObserver *_pCamObserver = new CamObserver();
  133.     res = system_.RegisterCameraListObserver( ICameraListObserverPtr( _pCamObserver ) ); // TODO Do error checking
  134.     //// end CAMERA_LIST OBSERVER ////
  135.    
  136.     /////////// IGNORE THIS -- SPECIFIC TO MY ROS NECESSITIES //////////
  137.     // create a vector of publisher topics, one topic for each gigE camera feed
  138.     std::string pub_topic = pub_topic_template_;
  139.     for (int i = 0; i < openedCams_.size(); ++i) {
  140.         // @debug
  141.         ROS_INFO("pub_topic: %s", pub_topic.c_str() );
  142.        
  143.         // need a new ros::Publisher for EACH camera.
  144.         image_transport::Publisher cam_publisher = it_.advertise(pub_topic, 2);  /*<sensor_msgs::ImagePtr>*/
  145.         vectorOfPubs_.push_back(cam_publisher);
  146.        
  147.         // iterate "pub_topic" string.
  148.         // Usually starts at 0, ex: "gigE_cam_0" --> "gigE_cam_1"
  149.         pub_topic.pop_back();
  150.         pub_topic += std::to_string(i+1); // +1 so that the
  151.  
  152.     }
  153. }
  154.  
  155. /* Purpose:
  156.  *      1. Detects all cameras on network.
  157.  *      2. Attempts to open all of them - throws (non-lethal) errors if it cannot.
  158.  *      3. Publishes the images recieved to *seperate* rosTopics, 1 per camera. Message type: sensor_msgs::ImagePtr
  159.  *
  160.  * Parameters:
  161.  *      All data is shared by VimbaData class.
  162.  */
  163. void VimbaData::cameraCapture(  ) {
  164.     FramePtr pFrame;  // frame data
  165.     VmbUint32_t timeout = 500;
  166.     VmbFrameStatusType status;
  167.     VmbUint32_t nWidth = 0;
  168.     VmbUint32_t nHeight = 0;
  169.     VmbUchar_t *pImage = NULL; // frame data will be put here to be converted to cv::Mat
  170.     sensor_msgs::ImagePtr ROS_image_ptr; // message to be published to ROS_Topic
  171.  
  172.     // for each camera {
  173.     //    grame frame, convert it to ros::image_transport, publish it on its own topic
  174.     // }
  175.    
  176.     while ( openedCams_.size() > 0 && SIGINTrecieved == false ) { // could use a global variable.  Maybe change this to "total_currently_open_cams > 0"
  177.         // only grab frames from cams that were sucessfully opened
  178.         for (int i = 0; i < openedCams_.size(); ++i) {
  179.             if (VmbErrorSuccess == cameras_[ openedCams_[i] ]->Camera::AcquireSingleImage(pFrame, timeout) ) {
  180.                 // check if frame was recieved properly
  181.                 if (VmbErrorSuccess == pFrame->Frame::GetReceiveStatus( status )) {
  182.                     // frame recieved, now ~do everything necessary with it inside this scope.~
  183.  
  184.                     ROS_INFO("Frame recieved from cam %d ", openedCams_[i]);
  185.                        
  186.                     // grab width and height of frame
  187.                     if (VmbErrorSuccess != pFrame->GetWidth ( nWidth ) )
  188.                         ROS_ERROR("FAILED to aquire width of frame!");
  189.                     if (VmbErrorSuccess != pFrame->GetHeight( nHeight ) )
  190.                         ROS_ERROR("FAILED to aquire height of frame!");
  191.                     if (VmbErrorSuccess != pFrame->GetImage ( pImage ) )
  192.                         ROS_ERROR("FAILED to acquire image data of frame!");
  193.  
  194.                     // convert to cv::Mat
  195.                     cv::Mat cvMat = cv::Mat(nHeight, nWidth, CV_8UC1, pImage );
  196.                     cv::cvtColor(cvMat, cvMat, CV_BayerBG2RGB);
  197.                    
  198.                     if(display_images_on_screen_) {
  199.                         // @debug: display cam feeds in seperate windows
  200.                         cv::imshow("cvMat"+std::to_string( openedCams_[i] ), cvMat);
  201.                         cv::waitKey(1);
  202.                     }
  203.                    
  204.                     // convert from cv::Mat to ROS image message
  205.                     sensor_msgs::ImagePtr ROSmsg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", cvMat).toImageMsg();
  206.  
  207.                     // Publish to ROS_Topic
  208.                     // There will never be an empty, or skipped, topic. "gigE_cam_0" will always be filled before "gigE_cam_1".
  209.                     vectorOfPubs_[i].publish( ROSmsg );
  210.                 }
  211.                 else { // frame not recieved properly
  212.                     ROS_ERROR("FRAME FROM CAM %d NOT RECIEVED PROPERLY", openedCams_[i]);
  213.                 }
  214.             }
  215.             else { // frame not aquired at all
  216.                 ROS_ERROR("Frame from camera %d not aquired", openedCams_[i]);
  217.             }
  218.         }
  219.         std::cout << std::endl; // space separating each batch of captures
  220.     }
  221. }
  222.  
  223. /* Purpose: Catch SIGINT (typically generated by user).  Gracefully exit the otherwise infinate loop of camera capture.
  224.  *
  225.  * Parameters: SIGINT integer from signal.  ctrl+c has value of 2.
  226.  */
  227. void signalHandler( int signum ) {
  228.     ROS_INFO("Interrupt signal (%d) received", signum);
  229.    
  230.     SIGINTrecieved = true; // global var
  231. }
  232.  
  233. int main(int argc, char **argv) {
  234.  
  235.     ROS_INFO("Running Vimba.cpp!");
  236.  
  237.     ros::init(argc, argv, "vimba");
  238.  
  239.     ros::NodeHandle sigNode;
  240.     signal(SIGINT, signalHandler);
  241.  
  242.     VimbaData vimba_data;
  243.  
  244.     vimba_data.initVimbaCams( );
  245.  
  246.     // infinite loop of camera capture
  247.     vimba_data.cameraCapture( );
  248.  
  249.     // shutdown API & destroy all objects used
  250.     vimba_data.system_.Shutdown();
  251.     ros::shutdown();
  252.  
  253.     return 0;
  254. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement