42 #include "libirimager/ImageBuilder.h" 45 #include <sensor_msgs/CameraInfo.h> 47 #include <optris_drivers/Palette.h> 68 unsigned short* data = (
unsigned short*)&image->data[0];
69 _iBuilder.setData(image->width, image->height, data);
72 _bufferThermal =
new unsigned char[image->width * image->height * 3];
76 sensor_msgs::Image img;
77 img.header.frame_id =
"thermal_image_view";
78 img.height = image->height;
79 img.width = image->width;
80 img.encoding =
"rgb8";
81 img.step = image->width*3;
86 img.data.resize(img.height*img.step);
103 _bufferVisible =
new unsigned char[image->width * image->height * 3];
105 const unsigned char* data = &image->data[0];
108 sensor_msgs::Image img;
109 img.header.frame_id =
"visible_image_view";
110 img.height = image->height;
111 img.width = image->width;
112 img.encoding =
"rgb8";
113 img.step = image->width*3;
114 img.data.resize(img.height*img.step);
119 for(
unsigned int i=0; i<image->width*image->height*3; i++) {
126 bool onPalette(optris_drivers::Palette::Request &req, optris_drivers::Palette::Response &res)
130 if(req.palette > 0 && req.palette < 12)
132 _iBuilder.setPalette((evo::EnumOptrisColoringPalette)req.palette);
136 if(req.paletteScaling >=1 && req.paletteScaling <= 4)
138 _iBuilder.setPaletteScalingMethod((evo::EnumOptrisPaletteScalingMethod) req.paletteScaling);
142 if(
_iBuilder.getPaletteScalingMethod() == evo::eManual && req.temperatureMin < req.temperatureMax)
144 _iBuilder.setManualTemperatureRange(req.temperatureMin, req.temperatureMax);
151 int main (
int argc,
char* argv[])
153 ros::init (argc, argv,
"optris_colorconvert_node");
161 evo::EnumOptrisPaletteScalingMethod scalingMethod = evo::eMinMax;
164 if(sm>=1 && sm <=4) scalingMethod = (evo::EnumOptrisPaletteScalingMethod) sm;
166 _iBuilder.setPaletteScalingMethod(scalingMethod);
167 _iBuilder.setPalette((evo::EnumOptrisColoringPalette)palette);
171 double looprate = 30.;
173 n_.
getParam(
"temperatureMin", tMin);
174 n_.
getParam(
"temperatureMax", tMax);
177 _iBuilder.setManualTemperatureRange((
float)tMin, (
float)tMax);
192 std::string camera_name;
193 std::string camera_info_url;
194 n_.
getParam(
"camera_name", camera_name);
195 n_.
getParam(
"camera_info_url", camera_info_url);
200 _camera_info_manager = &cinfo_manager;
206 ROS_WARN_STREAM(
"[" << camera_name <<
"] name not valid" <<
" for camera_info_manger");
209 if (_camera_info_manager->
validateURL(camera_info_url))
213 ROS_WARN(
"camera_info_url does not contain calibration data." );
217 ROS_WARN(
"Camera is not calibrated. Using default values." );
227 _camera_info_pub = &cinfo_pub;
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
sensor_msgs::CameraInfo getCameraInfo(void)
image_transport::Publisher * _pubThermal
unsigned char * _bufferThermal
bool loadCameraInfo(const std::string &url)
uint32_t getNumSubscribers() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
uint32_t getNumSubscribers() const
camera_info_manager::CameraInfoManager * _camera_info_manager
bool validateURL(const std::string &url)
ROSCPP_DECL void spin(Spinner &spinner)
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
unsigned char * _bufferVisible
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
image_transport::CameraPublisher * _camera_info_pub
evo::ImageBuilder _iBuilder
bool onPalette(optris_drivers::Palette::Request &req, optris_drivers::Palette::Response &res)
void publish(const sensor_msgs::Image &message) const
image_transport::Publisher * _pubVisible
int main(int argc, char *argv[])
sensor_msgs::CameraInfo _camera_info
#define ROS_WARN_STREAM(args)
ros::ServiceServer _sPalette
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM_ONCE(args)
void onVisibleDataReceive(const sensor_msgs::ImageConstPtr &image)
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
void onThermalDataReceive(const sensor_msgs::ImageConstPtr &image)
bool setCameraName(const std::string &cname)