37 #include <rc_genicam_api/config.h> 42 std::function<
void()>& sub_changed)
51 pub = nh.
advertise<rc_common_msgs::CameraParam>(
"left/camera_param", 1,
57 pub = nh.
advertise<rc_common_msgs::CameraParam>(
"right/camera_param", 1,
79 template <
class T>
inline rc_common_msgs::KeyValue getKeyValue(
const char *key, T value)
81 rc_common_msgs::KeyValue ret;
84 ret.value=std::to_string(value);
97 rc_common_msgs::CameraParam
param;
100 param.header.stamp.sec = time / 1000000000ul;
101 param.header.stamp.nsec = time % 1000000000ul;
106 std::vector<std::string> lines;
108 for (
auto&& line : lines)
112 rc_common_msgs::KeyValue line_source;
113 line_source.key = line;
116 param.line_source.push_back(line_source);
128 float out1_reduction=0;
134 catch (
const std::exception &)
142 param.extra_data.clear();
143 param.extra_data.push_back(getKeyValue(
"noise", noise));
144 param.extra_data.push_back(getKeyValue(
"test", test));
145 param.extra_data.push_back(getKeyValue(
"out1_reduction", out1_reduction));
146 param.extra_data.push_back(getKeyValue(
"brightness", brightness));
Interface for all publishers relating to images, point clouds or other stereo-camera data...
void requiresComponents(int &components, bool &color) override
Adds components and if color images are required to the given values.
bool getBoolean(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool exception=false, bool igncache=false)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
bool used() override
Returns true if there are subscribers to the topic.
void publish(const boost::shared_ptr< M > &message) const
std::function< void()> sub_callback
int64_t getInteger(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, int64_t *vmin=0, int64_t *vmax=0, bool exception=false, bool igncache=false)
bool setEnum(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, const char *value, bool exception=false)
static const int ComponentIntensity
void subChanged(const ros::SingleSubscriberPublisher &pub)
Called by publisher if subscription changed.
std::string getEnum(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool exception=false)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double getFloat(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, double *vmin=0, double *vmax=0, bool exception=false, bool igncache=false)
uint64_t getTimestampNS() const
uint32_t getNumSubscribers() const
void publish(const rcg::Buffer *buffer, uint32_t part, uint64_t pixelformat) override
Offers a buffer for publication.
CameraParamPublisher(ros::NodeHandle &nh, const std::string &frame_id, bool left, std::function< void()> &sub_changed)
std::shared_ptr< GenApi::CNodeMapRef > nodemap