├── CMakeLists.txt ├── README.md ├── config ├── basler_params.yaml ├── daA1280-54uc_1280x960.yaml ├── dart_sync_master.yaml ├── dart_sync_slave.yaml ├── left.yaml ├── left_unrectified.yaml ├── right.yaml └── right_unrectified.yaml ├── include └── image_publisher.h ├── launch ├── basler_camera.launch ├── stereo_rectified.launch └── stereo_unrectified.launch ├── package.xml └── src └── basler_camera_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(basler_camera) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp image_transport camera_info_manager cv_bridge) 5 | find_package(OpenCV REQUIRED) 6 | 7 | catkin_package( 8 | INCLUDE_DIRS include 9 | # LIBRARIES basler_camera 10 | # CATKIN_DEPENDS roscpp 11 | # DEPENDS system_lib 12 | ) 13 | 14 | ########### 15 | ## Build ## 16 | ########### 17 | 18 | ## Additional Linker flags for pylon 19 | SET( CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-E") 20 | 21 | ## Specify additional locations of header files 22 | ## Your package locations should be listed before other locations 23 | include_directories(include ${catkin_INCLUDE_DIRS} $ENV{PYLON_ROOT}/include $ENV{GENICAM_ROOT_V2_3}/library/CPP/include) 24 | link_directories( 25 | $ENV{PYLON_ROOT}/lib64 26 | $ENV{GENICAM_ROOT_V2_3}/bin/Linux64_x64 27 | $ENV{GENICAM_ROOT_V2_3}/bin/Linux64_x64/GenApi/Generic 28 | $ENV{GENICAM_ROOT_V2_3}/library/CPP/lib/Linux64_x64 29 | ) 30 | 31 | ## Declare a C++ library 32 | #add_library(basler_camera src/basler_camera_node.cpp) 33 | #add_dependencies(basler_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 34 | 35 | ## Declare a C++ executable 36 | add_executable(basler_camera_node src/basler_camera_node.cpp) 37 | add_dependencies(basler_camera_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 38 | 39 | ## Specify libraries to link a library or executable target against 40 | target_link_libraries(basler_camera_node 41 | ${OpenCV_LIBS} 42 | ${catkin_LIBRARIES} 43 | pylonbase 44 | pylonutility 45 | GCBase_gcc_v3_0_Basler_pylon_v5_0 46 | GenApi_gcc_v3_0_Basler_pylon_v5_0 47 | ) 48 | 49 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Basler camera interface with ROS 2 | ======================== 3 | 4 | These are the steps needed to publish Basler camera images to ROS: 5 | 6 | 1. Get the Pylon 5 Camera Software Suite from the Basler website: 7 | 8 | http://www.baslerweb.com/en/support/downloads/software-downloads?type=28&series=0&model=0 9 | 10 | 2. Follow these instructions (also available in the install file): 11 | 12 | a. Change to the directory which contains this INSTALL file, e.g.: 13 | ```bash 14 | cd ~/pylon-5.0.1.6388-x86_64 15 | ``` 16 | b. Extract the corresponding SDK into /opt 17 | ```bash 18 | sudo tar -C /opt -xzf pylonSDK*.tar.gz 19 | ``` 20 | c. Install udev-rules to set up permissions for basler USB cameras 21 | ```bash 22 | ./setup-usb.sh 23 | ``` 24 | 25 | d. Unplug and replug all USB cameras to get the udev rules applied. 26 | 27 | e. Execute /opt/pylon5/bin/PylonViewerApp to test your cameras. 28 | 29 | 3. Setup your environment for pylon to find the necessary dependencies: 30 | 31 | ```bash 32 | source /opt/pylon5/bin/pylon-setup-env.sh /opt/pylon5 33 | ``` 34 | If using it more than once, you probably also want to add this to your ~/.bashrc 35 | 36 | 4. Build the package using catkin_make 37 | 38 | 5. Launch the basler_Camera node with: 39 | 40 | ```bash 41 | roslaunch basler_camera basler_camera.launch 42 | ``` 43 | 44 | The frame rate could be modified using the `frame_rate` parameter. 45 | 46 | The camera can optionally be selected with the `serial_number` parameter. 47 | 48 | 6. The image can be visualized using image_view: 49 | 50 | ```bash 51 | rosrun image_view image_view image:=/camera/image_raw 52 | ``` 53 | 54 | 7. If you have a stereo system then launch the cameras as: 55 | ```bash 56 | roslaunch basler_camera stereo_rectified.launch 57 | ``` 58 | or: 59 | ```bash 60 | roslaunch basler_camera stereo_unrectified.launch 61 | ``` -------------------------------------------------------------------------------- /config/basler_params.yaml: -------------------------------------------------------------------------------- 1 | basler_params: 2 | - 3 | name: AcquisitionFrameRate 4 | type: float 5 | value: 30.0 6 | - 7 | name: AcquisitionFrameRateEnable 8 | type: boolean 9 | value: true 10 | - 11 | name: AcquisitionMode 12 | type: enum 13 | value: Continuous 14 | - 15 | name: ExposureAuto 16 | type: enum 17 | value: Continuous 18 | -------------------------------------------------------------------------------- /config/daA1280-54uc_1280x960.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 960 3 | camera_name: camera 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [521.305668, 0, 632.190486, 0, 518.581771, 437.368475, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.224668, 0.031387, 0.002094, 0.001785, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [315.864044, 0, 642.374059, 0, 0, 311.883514, 441.329364, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /config/dart_sync_master.yaml: -------------------------------------------------------------------------------- 1 | basler_params: 2 | - 3 | name: AcquisitionFrameRate 4 | type: float 5 | value: 10.0 6 | - 7 | name: ExposureTime 8 | type: float 9 | value: 15000.0 10 | - 11 | name: AcquisitionMode 12 | type: enum 13 | value: Continuous 14 | - 15 | name: OverlapMode 16 | type: enum 17 | value: 'Off' 18 | - 19 | name: ExposureAuto 20 | type: enum 21 | value: 'Off' 22 | - 23 | name: GainAuto 24 | type: enum 25 | value: 'Off' 26 | - 27 | name: LightSourcePreset 28 | type: enum 29 | value: 'Off' 30 | - 31 | name: BalanceWhiteAuto 32 | type: enum 33 | value: 'Off' 34 | - 35 | name: BalanceRatioSelector 36 | type: enum 37 | value: 'Blue' 38 | - 39 | name: BalanceRatio 40 | type: float 41 | value: 2.56 42 | - 43 | name: Gain 44 | type: float 45 | value: 1.0 46 | - 47 | name: TriggerMode 48 | type: enum 49 | value: 'Off' 50 | - 51 | name: LineSelector 52 | type: enum 53 | value: Line1 54 | - 55 | name: LineSource 56 | type: enum 57 | value: ExposureActive 58 | - 59 | name: LineMode 60 | type: enum 61 | value: Output 62 | -------------------------------------------------------------------------------- /config/dart_sync_slave.yaml: -------------------------------------------------------------------------------- 1 | basler_params: 2 | - 3 | name: AcquisitionFrameRate 4 | type: float 5 | value: 10.0 6 | - 7 | name: ExposureTime 8 | type: float 9 | value: 15000.0 10 | - 11 | name: AcquisitionMode 12 | type: enum 13 | value: Continuous 14 | - 15 | name: OverlapMode 16 | type: enum 17 | value: 'Off' 18 | - 19 | name: ExposureAuto 20 | type: enum 21 | value: 'Off' 22 | - 23 | name: GainAuto 24 | type: enum 25 | value: 'Off' 26 | - 27 | name: LightSourcePreset 28 | type: enum 29 | value: 'Off' 30 | - 31 | name: BalanceWhiteAuto 32 | type: enum 33 | value: 'Off' 34 | - 35 | name: BalanceRatioSelector 36 | type: enum 37 | value: 'Blue' 38 | - 39 | name: BalanceRatio 40 | type: float 41 | value: 2.56 42 | - 43 | name: Gain 44 | type: float 45 | value: 1.0 46 | - 47 | name: TriggerMode 48 | type: enum 49 | value: 'On' 50 | - 51 | name: TriggerSource 52 | type: enum 53 | value: Line2 54 | - 55 | name: LineSelector 56 | type: enum 57 | value: Line2 58 | - 59 | name: LineMode 60 | type: enum 61 | value: Input 62 | -------------------------------------------------------------------------------- /config/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 960 3 | camera_name: /stereo/left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1540.862843, 0.000000, 585.239644, 0.000000, 1535.828105, 471.615346, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.440305, 0.254103, 0.001757, 0.002867, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.997847, 0.006990, -0.065219, -0.006770, 0.999971, 0.003586, 0.065242, -0.003137, 0.997865] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1479.941538, 0.000000, 687.118614, 0.000000, 0.000000, 1479.941538, 466.843243, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /config/left_unrectified.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 960 3 | camera_name: /stereo/left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1540.862843, 0.000000, 585.239644, 0.000000, 1535.828105, 471.615346, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.440305, 0.254103, 0.001757, 0.002867, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1540.862843, 0.000000, 585.239644, 0.000000, 0.000000, 1535.828105, 471.615346, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /config/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 960 3 | camera_name: /stereo/right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1548.601464, 0.000000, 579.749613, 0.000000, 1544.428999, 464.543600, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.428837, 0.248585, -0.002261, 0.000450, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.997600, 0.000726, -0.069244, -0.000959, 0.999994, -0.003332, 0.069241, 0.003391, 0.997594] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1479.941538, 0.000000, 687.118614, -121.462250, 0.000000, 1479.941538, 466.843243, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /config/right_unrectified.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 960 3 | camera_name: /stereo/right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1548.601464, 0.000000, 579.749613, 0.000000, 1544.428999, 464.543600, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.428837, 0.248585, -0.002261, 0.000450, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1546.251376, 4.950007, 585.967820, -123.497519, -10.847694, 1541.139201, 475.219927, 2.547986, -0.003987, -0.006946, 0.999968, 0.005683] 21 | -------------------------------------------------------------------------------- /include/image_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDED_IMAGEPUBLISHER 2 | #define INCLUDED_IMAGEPUBLISHER 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace Pylon 16 | { 17 | using namespace GenApi; 18 | using namespace std; 19 | 20 | class ImagePublisher : public CImageEventHandler 21 | { 22 | public: 23 | 24 | ImagePublisher(ros::NodeHandle nh, sensor_msgs::CameraInfo::Ptr cinfo, string frame_id) 25 | : nh_(nh), it_(nh_) { 26 | cam_pub_ = it_.advertiseCamera("image_raw", 1); 27 | converter_.OutputPixelFormat = PixelType_RGB8packed; 28 | cinfo_ = cinfo; 29 | frame_id_ = frame_id; 30 | } 31 | 32 | virtual void OnImagesSkipped( CInstantCamera& camera, size_t countOfSkippedImages) 33 | { 34 | ROS_ERROR_STREAM (countOfSkippedImages << " images have been skipped."); 35 | } 36 | 37 | virtual void OnImageGrabbed( CInstantCamera& camera, const CGrabResultPtr& ptrGrabResult) 38 | { 39 | ROS_DEBUG_STREAM("*OnImageGrabbed event for device " << camera.GetDeviceInfo().GetModelName()); 40 | 41 | ros::Time timestamp = ros::Time::now(); 42 | GenApi::CIntegerPtr width(camera.GetNodeMap().GetNode("Width")); 43 | GenApi::CIntegerPtr height(camera.GetNodeMap().GetNode("Height")); 44 | cv::Mat cv_img_rgb(width->GetValue(), height->GetValue(), CV_8UC3); 45 | 46 | if (ptrGrabResult->GrabSucceeded()) 47 | { 48 | converter_.Convert(pylon_image_, ptrGrabResult); 49 | 50 | cv_img_rgb = cv::Mat(ptrGrabResult->GetHeight(), ptrGrabResult->GetWidth(), CV_8UC3,(uint8_t*)pylon_image_.GetBuffer()); 51 | sensor_msgs::ImagePtr image = cv_bridge::CvImage(std_msgs::Header(), "rgb8", cv_img_rgb).toImageMsg(); 52 | sensor_msgs::CameraInfo::Ptr cinfo = cinfo_; 53 | 54 | image->header.frame_id = frame_id_; 55 | image->header.stamp = timestamp; 56 | cinfo->header.frame_id = frame_id_; 57 | cinfo->header.stamp = timestamp; 58 | 59 | cam_pub_.publish(image, cinfo); 60 | } 61 | else 62 | { 63 | ROS_ERROR_STREAM("Error: " << ptrGrabResult->GetErrorCode() << " " << ptrGrabResult->GetErrorDescription()); 64 | } 65 | } 66 | private: 67 | ros::NodeHandle nh_; 68 | image_transport::ImageTransport it_; 69 | image_transport::CameraPublisher cam_pub_; 70 | sensor_msgs::CameraInfo::Ptr cinfo_; 71 | string frame_id_; 72 | 73 | CImageFormatConverter converter_; 74 | CPylonImage pylon_image_; 75 | EPixelType pixel_type_; 76 | }; 77 | } 78 | 79 | #endif /* INCLUDED_IMAGEPUBLISHER */ 80 | -------------------------------------------------------------------------------- /launch/basler_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/stereo_rectified.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/stereo_unrectified.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | basler_camera 3 | 0.0.0 4 | Basler cameras ROS interface package 5 | Shadow Robot's software team 6 | 7 | GPL 8 | 9 | 10 | catkin 11 | 12 | sensor_msgs 13 | roscpp 14 | image_transport 15 | sensor_msgs 16 | roscpp 17 | image_transport 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/basler_camera_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "std_msgs/String.h" 6 | 7 | // Include files to use the PYLON API. 8 | #include 9 | 10 | #include "image_publisher.h" 11 | #include 12 | 13 | using namespace Pylon; 14 | using namespace GenApi; 15 | using std::string; 16 | 17 | void handle_basler_boolean_parameter(CInstantCamera& camera, string name, bool value) 18 | { 19 | INodeMap& nodemap = camera.GetNodeMap(); 20 | try 21 | { 22 | ROS_INFO_STREAM("Setting boolean param " << name << " to " << value << "."); 23 | CBooleanPtr this_node(nodemap.GetNode(name.c_str())); 24 | if (!IsWritable(this_node)) 25 | { 26 | ROS_ERROR_STREAM("Basler parameter '" << name << "' isn't writable or doesn't exist."); 27 | return; 28 | } 29 | this_node->SetValue(value); 30 | } 31 | catch (const GenericException& e) 32 | { 33 | ROS_ERROR_STREAM(e.GetDescription()); 34 | } 35 | } 36 | 37 | void handle_basler_int_parameter(CInstantCamera& camera, string name, int value) 38 | { 39 | INodeMap& nodemap = camera.GetNodeMap(); 40 | try 41 | { 42 | ROS_INFO_STREAM("Setting int param " << name << " to " << value << "."); 43 | CIntegerPtr this_node(nodemap.GetNode(name.c_str())); 44 | if (!IsWritable(this_node)) 45 | { 46 | ROS_ERROR_STREAM("Basler parameter '" << name << "' isn't writable or doesn't exist."); 47 | return; 48 | } 49 | this_node->SetValue(value); 50 | } 51 | catch (const GenericException& e) 52 | { 53 | ROS_ERROR_STREAM(e.GetDescription()); 54 | } 55 | } 56 | 57 | void handle_basler_float_parameter(CInstantCamera& camera, string name, double value) 58 | { 59 | INodeMap& nodemap = camera.GetNodeMap(); 60 | try 61 | { 62 | ROS_INFO_STREAM("Setting float param " << name << " to " << value << "."); 63 | CFloatPtr this_node(nodemap.GetNode(name.c_str())); 64 | if (!IsWritable(this_node)) 65 | { 66 | ROS_ERROR_STREAM("Basler parameter '" << name << "' isn't writable or doesn't exist."); 67 | return; 68 | } 69 | this_node->SetValue(value); 70 | } 71 | catch (const GenericException& e) 72 | { 73 | ROS_ERROR_STREAM(e.GetDescription()); 74 | } 75 | } 76 | 77 | void handle_basler_enum_parameter(CInstantCamera& camera, string name, string value) 78 | { 79 | INodeMap& nodemap = camera.GetNodeMap(); 80 | try 81 | { 82 | ROS_INFO_STREAM("Setting enum param " << name << " to " << value << "."); 83 | CEnumerationPtr this_node(nodemap.GetNode(name.c_str())); 84 | if (!IsWritable(this_node)) 85 | { 86 | ROS_ERROR_STREAM("Basler parameter '" << name << "' isn't writable or doesn't exist."); 87 | return; 88 | } 89 | if (!IsAvailable(this_node->GetEntryByName(value.c_str()))) 90 | { 91 | ROS_ERROR_STREAM("Valuer '" << value << "' isn't available for basler param '" << name << "'."); 92 | return; 93 | } 94 | this_node->FromString(value.c_str()); 95 | } 96 | catch (const GenericException& e) 97 | { 98 | ROS_ERROR_STREAM(e.GetDescription()); 99 | } 100 | } 101 | 102 | void handle_basler_parameter(CInstantCamera& camera, XmlRpc::XmlRpcValue& param) 103 | { 104 | string type = param["type"]; 105 | if ("boolean" == type) 106 | { 107 | ROS_ASSERT_MSG(param["value"].getType() == XmlRpc::XmlRpcValue::TypeBoolean, 108 | "Type of value for %s must be boolean", string(param["name"]).c_str()); 109 | handle_basler_boolean_parameter(camera, param["name"], param["value"]); 110 | } 111 | else if ("int" == type) 112 | { 113 | ROS_ASSERT_MSG(param["value"].getType() == XmlRpc::XmlRpcValue::TypeInt, 114 | "Type of value for %s must be int", string(param["name"]).c_str()); 115 | handle_basler_int_parameter(camera, param["name"], param["value"]); 116 | } 117 | else if ("float" == type) 118 | { 119 | ROS_ASSERT_MSG(param["value"].getType() == XmlRpc::XmlRpcValue::TypeDouble, 120 | "Type of value for %s must be float", string(param["name"]).c_str()); 121 | handle_basler_float_parameter(camera, param["name"], param["value"]); 122 | } 123 | else if ("enum" == type) 124 | { 125 | ROS_ASSERT_MSG(param["value"].getType() == XmlRpc::XmlRpcValue::TypeString, 126 | "Type of value for %s must be string", string(param["name"]).c_str()); 127 | handle_basler_enum_parameter(camera, param["name"], param["value"]); 128 | } 129 | else 130 | { 131 | ROS_FATAL_STREAM("Unknown param type for parameter " << param["name"] << ": " << type); 132 | } 133 | } 134 | 135 | void handle_basler_parameters(CInstantCamera& camera) 136 | { 137 | ros::NodeHandle private_handle("~"); 138 | XmlRpc::XmlRpcValue params; 139 | private_handle.getParam("basler_params", params); 140 | ROS_ASSERT_MSG(params.getType() == XmlRpc::XmlRpcValue::TypeArray, "Badly formed basler param yaml"); 141 | for (size_t index = 0; index < params.size(); ++index) 142 | { 143 | ROS_ASSERT_MSG(params[index].getType() == XmlRpc::XmlRpcValue::TypeStruct, "Badly formed basler param yaml"); 144 | ROS_ASSERT_MSG(params[index].hasMember("name"), "Param needs name"); 145 | ROS_ASSERT_MSG(params[index].hasMember("type"), "Param needs type"); 146 | ROS_ASSERT_MSG(params[index].hasMember("value"), "Param needs value"); 147 | 148 | handle_basler_parameter(camera, params[index]); 149 | } 150 | } 151 | 152 | int main(int argc, char* argv[]) 153 | { 154 | ros::init(argc, argv, "basler_camera"); 155 | ros::NodeHandle nh("~"); 156 | 157 | int frame_rate; 158 | if(!nh.getParam("frame_rate", frame_rate)) 159 | frame_rate = 20; 160 | 161 | string camera_info_url; 162 | if(!nh.getParam("camera_info_url", camera_info_url)) 163 | camera_info_url = ""; 164 | 165 | string frame_id; 166 | if(!nh.getParam("frame_id", frame_id)) 167 | frame_id = ""; 168 | 169 | std::string serial_number; 170 | if(!nh.getParam("serial_number", serial_number)) 171 | serial_number = ""; 172 | 173 | std::string camera_name = nh.getNamespace(); 174 | 175 | int exitCode = 0; 176 | 177 | camera_info_manager::CameraInfoManager cinfo_manager_(nh, camera_name); 178 | 179 | // Automatically call PylonInitialize and PylonTerminate to ensure the pylon runtime system 180 | // is initialized during the lifetime of this object. 181 | Pylon::PylonAutoInitTerm autoInitTerm; 182 | CGrabResultPtr ptrGrabResult; 183 | 184 | try 185 | { 186 | CTlFactory& tlFactory = CTlFactory::GetInstance(); 187 | DeviceInfoList_t devices; 188 | if (tlFactory.EnumerateDevices(devices) == 0) 189 | { 190 | throw RUNTIME_EXCEPTION("No camera present."); 191 | } 192 | 193 | CInstantCamera camera; 194 | if (serial_number == "") { 195 | // Create an instant camera object for the camera device found first. 196 | camera.Attach(CTlFactory::GetInstance().CreateFirstDevice()); 197 | } else { 198 | // Look up the camera by its serial number 199 | for (size_t i=0; iToString(); 232 | if (IsAvailable(pixelFormat->GetEntryByName("RGB8"))) 233 | { 234 | pixelFormat->FromString("RGB8"); 235 | } 236 | 237 | camera.StartGrabbing(); 238 | 239 | while (camera.IsGrabbing() && ros::ok()) 240 | { 241 | camera.RetrieveResult(1, ptrGrabResult, TimeoutHandling_Return); 242 | ros::spinOnce(); 243 | } 244 | } 245 | catch (GenICam::GenericException &e) 246 | { 247 | ROS_ERROR_STREAM ("An exception occurred." << e.GetDescription()); 248 | exitCode = 1; 249 | } 250 | return exitCode; 251 | } 252 | --------------------------------------------------------------------------------