flightgoggles_ros_bridge/src/ROSClient/ROSClient.cpp
// New thread for republishing received images
New thread for republishing received images
New thread for republishing received images
void imageConsumer(ROSClient *self){
    while (true){
      if (self->b_have_renders_)
      {
        unity_incoming::RenderOutput_t renderOutput = self->flightGoggles.handleImageResponse();
        ros::Time imageTimestamp;
        imageTimestamp = imageTimestamp.fromNSec(
                           renderOutput.renderMetadata.ntime
                           + self->sim_parameters_.cam_time_delay_ns());
	    std_msgs::Float32 fps_;
	    if (self->timeSinceLastMeasure_.toSec() != 0) {
	    double t = (ros::WallTime::now() - self->timeSinceLastMeasure_).toSec();
	        if (t > 1) {
	            fps_.data = self->frameCount_ / t;
	            self->fpsPublisher_.publish(fps_);
                self->timeSinceLastMeasure_ = ros::WallTime::now();
                self->frameCount_ = 0;
            }
        } else {
            self->timeSinceLastMeasure_ = ros::WallTime::now();
        }
        sensor_msgs::ImagePtr msg;
        self->imageDistortColor(msg, renderOutput.images[0]);
        msg->header.stamp = imageTimestamp;
        msg->header.frame_id = "uav/camera/left";
        sensor_msgs::CameraInfoPtr cameraInfoMsgCopy(new sensor_msgs::CameraInfo(self->cameraInfoLeft));
        cameraInfoMsgCopy->header.frame_id = "uav/camera/left";
	cameraInfoMsgCopy->header.stamp = imageTimestamp;
	    self->imagePubLeft_.publish(msg, cameraInfoMsgCopy);
	    if (self->render_stereo) {
            sensor_msgs::ImagePtr msg_right;
            self->imageDistortColor(msg_right, renderOutput.images[1]);
            msg_right->header.stamp = imageTimestamp;
            msg_right->header.frame_id = "uav/camera/right";
            sensor_msgs::CameraInfoPtr cameraInfoMsgCopy_Right(new sensor_msgs::CameraInfo(self->cameraInfoRight));
            cameraInfoMsgCopy_Right->header.frame_id = "uav/camera/right";
            cameraInfoMsgCopy_Right->header.stamp = imageTimestamp;
            self->imagePubRight_.publish(msg_right, cameraInfoMsgCopy_Right);
        }
        if (renderOutput.renderMetadata.hasCameraCollision){
            std_msgs::Empty msg;
            self->collisionPub_.publish(msg);
        }
        sensor_msgs::Range lidarReturnMsg;
        lidarReturnMsg.header.stamp = imageTimestamp;
        lidarReturnMsg.header.frame_id = "uav/imu";
        lidarReturnMsg.radiation_type = lidarReturnMsg.INFRARED;
        lidarReturnMsg.field_of_view = 0;
        lidarReturnMsg.min_range = -1.0f*self->lidarMaxRange_;
        lidarReturnMsg.max_range = 0;
        lidarReturnMsg.range = static_cast<float>(-1.0f * (renderOutput.renderMetadata.lidarReturn + sqrt(self->lidarVariance_) * self->standardNormalDistribution_(self->randomNumberGenerator_)));
        self->lidarPub_.publish(lidarReturnMsg);
        flightgoggles::IRMarkerArray visiblePoints;
        visiblePoints.header.stamp = imageTimestamp;
        for (const unity_incoming::Landmark_t landmark : renderOutput.renderMetadata.landmarksInView){
            flightgoggles::IRMarker marker;
            std::vector<std::string> IDTokens;
            boost::split(IDTokens, landmark.ID, boost::is_any_of("_"));
            marker.landmarkID.data = IDTokens.at(0);
            marker.markerID.data = IDTokens.at(1);
            marker.x = landmark.position.at(0) * renderOutput.renderMetadata.camWidth;
            marker.y = (1.0f-landmark.position.at(1)) * renderOutput.renderMetadata.camHeight;
            marker.z = 0; 
            visiblePoints.markers.push_back(marker);
        }
        self->irMarkerPub_.publish(visiblePoints);
        if (SHOW_DEBUG_IMAGE_FEED){
            cv::imshow("Debug RGB", renderOutput.images[0]);
            cv::waitKey(1);
        }
        self->frameCount_ ++;
        self->bIamgeReceived_ = true;
      }
      else
      {
        ros::Duration(0.1).sleep();
      }
    }
}
void imageConsumer ( ROSClient * self ) { while ( true ) { if ( self -> b_have_renders_ ) { unity_incoming :: RenderOutput_t renderOutput = self -> flightGoggles . handleImageResponse ( ) ; ros :: Time imageTimestamp ; imageTimestamp = imageTimestamp . fromNSec ( renderOutput . renderMetadata . ntime + self -> sim_parameters_ . cam_time_delay_ns ( ) ) ; std_msgs :: Float32 fps_ ; if ( self -> timeSinceLastMeasure_ . toSec ( ) != 0 ) { double t = ( ros :: WallTime :: now ( ) - self -> timeSinceLastMeasure_ ) . toSec ( ) ; if ( t > 1 ) { fps_ . data = self -> frameCount_ / t ; self -> fpsPublisher_ . publish ( fps_ ) ; self -> timeSinceLastMeasure_ = ros :: WallTime :: now ( ) ; self -> frameCount_ = 0 ; } } else { self -> timeSinceLastMeasure_ = ros :: WallTime :: now ( ) ; } sensor_msgs :: ImagePtr msg ; self -> imageDistortColor ( msg , renderOutput . images [ 0 ] ) ; msg -> header . stamp = imageTimestamp ; msg -> header . frame_id = " " ; sensor_msgs :: CameraInfoPtr cameraInfoMsgCopy ( new sensor_msgs :: CameraInfo ( self -> cameraInfoLeft ) ) ; cameraInfoMsgCopy -> header . frame_id = " " ; cameraInfoMsgCopy -> header . stamp = imageTimestamp ; self -> imagePubLeft_ . publish ( msg , cameraInfoMsgCopy ) ; if ( self -> render_stereo ) { sensor_msgs :: ImagePtr msg_right ; self -> imageDistortColor ( msg_right , renderOutput . images [ 1 ] ) ; msg_right -> header . stamp = imageTimestamp ; msg_right -> header . frame_id = " " ; sensor_msgs :: CameraInfoPtr cameraInfoMsgCopy_Right ( new sensor_msgs :: CameraInfo ( self -> cameraInfoRight ) ) ; cameraInfoMsgCopy_Right -> header . frame_id = " " ; cameraInfoMsgCopy_Right -> header . stamp = imageTimestamp ; self -> imagePubRight_ . publish ( msg_right , cameraInfoMsgCopy_Right ) ; } if ( renderOutput . renderMetadata . hasCameraCollision ) { std_msgs :: Empty msg ; self -> collisionPub_ . publish ( msg ) ; } sensor_msgs :: Range lidarReturnMsg ; lidarReturnMsg . header . stamp = imageTimestamp ; lidarReturnMsg . header . frame_id = " " ; lidarReturnMsg . radiation_type = lidarReturnMsg . INFRARED ; lidarReturnMsg . field_of_view = 0 ; lidarReturnMsg . min_range = -1.0f * self -> lidarMaxRange_ ; lidarReturnMsg . max_range = 0 ; lidarReturnMsg . range = static_cast < float > ( -1.0f * ( renderOutput . renderMetadata . lidarReturn + sqrt ( self -> lidarVariance_ ) * self -> standardNormalDistribution_ ( self -> randomNumberGenerator_ ) ) ) ; self -> lidarPub_ . publish ( lidarReturnMsg ) ; flightgoggles :: IRMarkerArray visiblePoints ; visiblePoints . header . stamp = imageTimestamp ; for ( const unity_incoming :: Landmark_t landmark : renderOutput . renderMetadata . landmarksInView ) { flightgoggles :: IRMarker marker ; std :: vector < std :: string > IDTokens ; boost :: split ( IDTokens , landmark . ID , boost :: is_any_of ( " " ) ) ; marker . landmarkID . data = IDTokens . at ( 0 ) ; marker . markerID . data = IDTokens . at ( 1 ) ; marker . x = landmark . position . at ( 0 ) * renderOutput . renderMetadata . camWidth ; marker . y = ( 1.0f - landmark . position . at ( 1 ) ) * renderOutput . renderMetadata . camHeight ; marker . z = 0 ; visiblePoints . markers . push_back ( marker ) ; } self -> irMarkerPub_ . publish ( visiblePoints ) ; if ( SHOW_DEBUG_IMAGE_FEED ) { cv :: imshow ( " " , renderOutput . images [ 0 ] ) ; cv :: waitKey ( 1 ) ; } self -> frameCount_ ++ ; self -> bIamgeReceived_ = true ; } else { ros :: Duration ( 0.1 ) . sleep ( ) ; } } }
New thread for republishing received images
New thread for republishing received images
// Wait for render result (blocking). // Extract image timestamp //        imageTimestamp = imageTimestamp.fromNSec(renderOutput.renderMetadata.ntime); // Calculate average FPS every second. // Convert OpenCV image to image message //        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", renderOutput.images[0]).toImageMsg(); // Add Camera info message for camera //            sensor_msgs::ImagePtr msg_right = cv_bridge::CvImage(std_msgs::Header(), "bgr8", //                                                                 renderOutput.images[1]).toImageMsg(); // Add Camera info message for camera // Check for camera collision // Publish lidar range finder message // Add noise to lidar reading if reading is valid. // Make reading negative since the distance is in the -Z direction in '/uav/imu' frame. // Publish IR marker locations visible from FlightGoggles. // Create marker msg // Get name of gate and marker // Get location and convert to pixel space with origin at upper left corner (OpenCV convention) // Flip pixel convention to match openCV. Origin is upper left after conversion. // marker.z = landmark.position.at(2); // Do not publish distance. // Publish marker message // Display result //cv::imshow("Debug D", renderOutput.images[1]); // sleep for 100ms in case now params were published yet