Added the topic pubkish in the image callback.

The stream is initialized in the dynamic reconfigure callback.

src/srt_to_ros_alg_node.cpp

@@ -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();
 }