Commit c3243108 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Added the topic pubkish in the image callback.

The stream is initialized in the dynamic reconfigure callback.
parent 8fd8297d
......@@ -16,21 +16,6 @@ SrtToRosAlgNode::SrtToRosAlgNode(void) :
// [init publishers]
this->stream_image_publisher_ = this->it.advertiseCamera("stream_image/image_raw", 1);
// uncomment the following lines to load the calibration file for the camera
// Change <cal_file_param> for the correct parameter name holding the configuration filename
//std::string stream_image_cal_file;
//private_node_handle_.param<std::string>("<cal_file_param>",stream_image_cal_file,"");
//if(this->stream_image_camera_manager.validateURL(stream_image_cal_file))
//{
// if(!this->stream_image_camera_manager.loadCameraInfo(stream_image_cal_file))
// ROS_INFO("Invalid calibration file");
//}
//else
// ROS_INFO("Invalid calibration file");
this->stream.set_config(this->config_.srt_ip,this->config_.srt_port);
this->stream.set_video_callback(std::bind(&SrtToRosAlgNode::set_image,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
this->stream.start();
// [init subscribers]
......@@ -46,6 +31,8 @@ SrtToRosAlgNode::SrtToRosAlgNode(void) :
void SrtToRosAlgNode::set_image(unsigned char *data,unsigned int width ,unsigned int height)
{
this->alg_.lock();
sensor_msgs::CameraInfo stream_image_camera_info=this->stream_image_camera_manager.getCameraInfo();
ROS_INFO("new image");
if(this->stream_image_Image_msg_.data.size()==0)
{
this->stream_image_Image_msg_.header.stamp=ros::Time::now();
......@@ -69,6 +56,7 @@ void SrtToRosAlgNode::set_image(unsigned char *data,unsigned int width ,unsigned
this->stream_image_Image_msg_.data.resize(width*height*3);
}
memcpy(this->stream_image_Image_msg_.data.data(),data,width*height*3);
this->stream_image_publisher_.publish(this->stream_image_Image_msg_,stream_image_camera_info);
this->alg_.unlock();
}
......@@ -83,25 +71,17 @@ void SrtToRosAlgNode::mainNodeThread(void)
this->alg_.lock();
ROS_DEBUG("SrtToRosAlgNode::mainNodeThread");
// [fill msg structures]
// Initialize the topic message structure
//this->stream_image_Image_msg_.data = my_var;
// Uncomment the following lines two initialize the camera info structure
//sensor_msgs::CameraInfo stream_image_camera_info=this->stream_image_camera_manager.getCameraInfo();
//stream_image_camera_info.header.stamp = <time_stamp>;
//stream_image_camera_info.header.frame_id = <frame_id>;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to convert an OpenCV image to a ROS image message
//this->stream_image_Image_msg_=*this->cv_image_->toImageMsg();
// Uncomment the following line to publish the image together with the camera information
//this->stream_image_publisher_.publish(this->stream_image_Image_msg_,stream_image_camera_info);
this->alg_.unlock();
}
......@@ -120,6 +100,9 @@ void SrtToRosAlgNode::node_config_update(Config &config, uint32_t level)
if(config.rate!=this->getRate())
this->setRate(config.rate);
this->config_=config;
this->stream.set_config(this->config_.srt_ip,this->config_.srt_port);
this->stream.set_video_callback(std::bind(&SrtToRosAlgNode::set_image,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
this->stream.start();
this->alg_.unlock();
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment