mono_camera.cpp
Go to the documentation of this file.
1 
34 
35 #define DEBUG_PRINTS 1
36 
37 namespace avt_vimba_camera
38 {
40  : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName())
41 {
42  // Start Vimba & list all available cameras
43  api_.start();
44 
45  // Set the image publisher before the streaming
46  pub_ = it_.advertiseCamera("image_raw", 1);
47 
48  // Set the frame callback
49  cam_.setCallback(std::bind(&avt_vimba_camera::MonoCamera::frameCallback, this, std::placeholders::_1));
50 
51  // Set the params
52  nhp_.param("ip", ip_, std::string(""));
53  nhp_.param("guid", guid_, std::string(""));
54  nhp_.param("camera_info_url", camera_info_url_, std::string(""));
55  nhp_.param("frame_id", frame_id_, std::string(""));
56  nhp_.param("print_all_features", print_all_features_, false);
57  nhp_.param("use_measurement_time", use_measurement_time_, false);
58  nhp_.param("ptp_offset", ptp_offset_, 0);
59 
60  // Set camera info manager
61  info_man_ = std::shared_ptr<camera_info_manager::CameraInfoManager>(
63 
64  // Start dynamic_reconfigure & run configure()
65  reconfigure_server_.setCallback(
66  std::bind(&avt_vimba_camera::MonoCamera::configure, this, std::placeholders::_1, std::placeholders::_2));
67 }
68 
70 {
71  cam_.stop();
72  pub_.shutdown();
73 }
74 
75 void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr)
76 {
77  ros::Time ros_time = ros::Time::now();
78  if (pub_.getNumSubscribers() > 0)
79  {
80  sensor_msgs::Image img;
81  if (api_.frameToImage(vimba_frame_ptr, img))
82  {
83  sensor_msgs::CameraInfo ci = info_man_->getCameraInfo();
84  // Note: getCameraInfo() doesn't fill in header frame_id or stamp
85  ci.header.frame_id = frame_id_;
87  {
88  VmbUint64_t frame_timestamp;
89  vimba_frame_ptr->GetTimestamp(frame_timestamp);
90  ci.header.stamp = ros::Time(cam_.getTimestampRealTime(frame_timestamp)) + ros::Duration(ptp_offset_, 0);
91  }
92  else
93  {
94  ci.header.stamp = ros_time;
95  }
96  img.header.frame_id = ci.header.frame_id;
97  img.header.stamp = ci.header.stamp;
98  pub_.publish(img, ci);
99  }
100  else
101  {
102  ROS_WARN_STREAM("Function frameToImage returned 0. No image published.");
103  }
104  }
105 }
106 
116 void MonoCamera::configure(Config& newconfig, uint32_t level)
117 {
118  try
119  {
120  // The camera already stops & starts acquisition
121  // so there's no problem on changing any feature.
122  if (!cam_.isOpened())
123  {
125  }
126 
127  Config config = newconfig;
128  cam_.updateConfig(config);
129  updateCameraInfo(config);
130  }
131  catch (const std::exception& e)
132  {
133  ROS_ERROR_STREAM("Error reconfiguring mono_camera node : " << e.what());
134  }
135 }
136 
137 // See REP-104 for details regarding CameraInfo parameters
138 void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig& config)
139 {
140  sensor_msgs::CameraInfo ci = info_man_->getCameraInfo();
141 
142  // Set the operational parameters in CameraInfo (binning, ROI)
143  int binning_or_decimation_x = std::max(config.binning_x, config.decimation_x);
144  int binning_or_decimation_y = std::max(config.binning_y, config.decimation_y);
145 
146  // Set the operational parameters in CameraInfo (binning, ROI)
147  int sensor_width = cam_.getSensorWidth();
148  int sensor_height = cam_.getSensorHeight();
149 
150  if (sensor_width == -1 || sensor_height == -1)
151  {
152  ROS_ERROR("Could not determine sensor pixel dimensions, camera_info will be wrong");
153  }
154 
155  ci.width = sensor_width;
156  ci.height = sensor_height;
157  ci.binning_x = binning_or_decimation_x;
158  ci.binning_y = binning_or_decimation_y;
159 
160  // ROI is in unbinned coordinates, need to scale up
161  ci.roi.width = config.width * binning_or_decimation_x;
162  ci.roi.height = config.height * binning_or_decimation_y;
163  ci.roi.x_offset = config.offset_x * binning_or_decimation_x;
164  ci.roi.y_offset = config.offset_y * binning_or_decimation_y;
165 
166  bool roi_is_full_image = (ci.roi.width == ci.width && ci.roi.height == ci.height);
167  ci.roi.do_rectify = !roi_is_full_image;
168 
169  // push the changes to manager
170  info_man_->setCameraInfo(ci);
171 }
172 
173 }; // namespace avt_vimba_camera
avt_vimba_camera::AvtVimbaCamera::getSensorHeight
int getSensorHeight()
Definition: avt_vimba_camera.cpp:342
avt_vimba_camera::AvtVimbaCamera::start
void start(const std::string &ip_str, const std::string &guid_str, const std::string &frame_id, bool print_all_features=false)
Definition: avt_vimba_camera.cpp:85
avt_vimba_camera::MonoCamera::updateCameraInfo
void updateCameraInfo(const Config &config)
Definition: mono_camera.cpp:138
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
image_transport::CameraPublisher::getNumSubscribers
uint32_t getNumSubscribers() const
avt_vimba_camera::MonoCamera::print_all_features_
bool print_all_features_
Definition: mono_camera.h:68
avt_vimba_camera::MonoCamera::frame_id_
std::string frame_id_
Definition: mono_camera.h:67
ros
avt_vimba_camera::MonoCamera::configure
void configure(Config &newconfig, uint32_t level)
Definition: mono_camera.cpp:116
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
avt_vimba_camera::MonoCamera::reconfigure_server_
ReconfigureServer reconfigure_server_
Definition: mono_camera.h:80
avt_vimba_camera::MonoCamera::ptp_offset_
int32_t ptp_offset_
Definition: mono_camera.h:70
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
VmbUint64_t
unsigned long long VmbUint64_t
Definition: VmbCommonTypes.h:77
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
avt_vimba_camera::AvtVimbaCamera::isOpened
bool isOpened()
Definition: avt_vimba_camera.h:84
mono_camera.h
avt_vimba_camera::MonoCamera::nhp_
ros::NodeHandle nhp_
Definition: mono_camera.h:62
avt_vimba_camera::MonoCamera::~MonoCamera
~MonoCamera(void)
Definition: mono_camera.cpp:69
avt_vimba_camera::MonoCamera::cam_
AvtVimbaCamera cam_
Definition: mono_camera.h:59
avt_vimba_camera::MonoCamera::camera_info_url_
std::string camera_info_url_
Definition: mono_camera.h:66
avt_vimba_camera::MonoCamera::guid_
std::string guid_
Definition: mono_camera.h:65
avt_vimba_camera::MonoCamera::ip_
std::string ip_
Definition: mono_camera.h:64
camera_info_manager::CameraInfoManager
avt_vimba_camera::MonoCamera::info_man_
std::shared_ptr< camera_info_manager::CameraInfoManager > info_man_
Definition: mono_camera.h:75
avt_vimba_camera::MonoCamera::frameCallback
void frameCallback(const FramePtr &vimba_frame_ptr)
Definition: mono_camera.cpp:75
avt_vimba_camera::AvtVimbaCamera::setCallback
void setCallback(frameCallbackFunc callback)
Definition: avt_vimba_camera.h:96
avt_vimba_camera::AvtVimbaCamera::stop
void stop()
Definition: avt_vimba_camera.cpp:162
ros::Time
avt_vimba_camera::AvtVimbaCamera::getSensorWidth
int getSensorWidth()
Definition: avt_vimba_camera.cpp:329
avt_vimba_camera::MonoCamera::it_
image_transport::ImageTransport it_
Definition: mono_camera.h:72
ROS_ERROR
#define ROS_ERROR(...)
avt_vimba_camera::AvtVimbaApi::frameToImage
bool frameToImage(const FramePtr vimba_frame_ptr, sensor_msgs::Image &image)
Definition: avt_vimba_api.h:144
avt_vimba_camera
Definition: avt_vimba_api.h:50
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
avt_vimba_camera::AvtVimbaCamera::getTimestampRealTime
double getTimestampRealTime(VmbUint64_t timestamp_ticks)
Definition: avt_vimba_camera.cpp:355
avt_vimba_camera::AvtVimbaCamera::updateConfig
void updateConfig(Config &config)
Definition: avt_vimba_camera.cpp:788
avt_vimba_camera::MonoCamera::Config
avt_vimba_camera::AvtVimbaCameraConfig Config
Definition: mono_camera.h:78
image_transport::CameraPublisher::shutdown
void shutdown()
avt_vimba_camera::AvtVimbaApi::start
void start()
Definition: avt_vimba_api.h:59
avt_vimba_camera::MonoCamera::api_
AvtVimbaApi api_
Definition: mono_camera.h:58
ros::Duration
avt_vimba_camera::MonoCamera::use_measurement_time_
bool use_measurement_time_
Definition: mono_camera.h:69
ros::NodeHandle
avt_vimba_camera::MonoCamera::pub_
image_transport::CameraPublisher pub_
Definition: mono_camera.h:73
ros::Time::now
static Time now()
avt_vimba_camera::MonoCamera::MonoCamera
MonoCamera(ros::NodeHandle &nh, ros::NodeHandle &nhp)
Definition: mono_camera.cpp:39


avt_vimba_camera
Author(s): Allied Vision Technologies, Miquel Massot
autogenerated on Sat Jun 3 2023 02:14:12