image_pub.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <mavros/mavros_plugin.h>
00018 #include <pluginlib/class_list_macros.h>
00019 
00020 #include <sensor_msgs/image_encodings.h>
00021 #include <image_transport/image_transport.h>
00022 #include <cv_bridge/cv_bridge.h>
00023 #include <opencv2/opencv.hpp>
00024 
00025 namespace mavplugin {
00026 namespace enc = sensor_msgs::image_encodings;
00027 
00033 class ImagePubPlugin : public MavRosPlugin {
00034 public:
00035         ImagePubPlugin() :
00036                 im_nh("~image"),
00037                 im_width(0), im_height(0),
00038                 im_size(0), im_packets(0), im_payload(0),
00039                 im_seqnr(0), im_type(255),
00040                 im_buffer {}
00041         { };
00042 
00043         void initialize(UAS &uas_)
00044         {
00045                 im_nh.param<std::string>("frame_id", frame_id, "px4flow");
00046 
00047                 itp = boost::make_shared<image_transport::ImageTransport>(im_nh);
00048                 image_pub = itp->advertise("camera_image", 1);
00049         }
00050 
00051         const message_map get_rx_handlers() {
00052                 return {
00053                                MESSAGE_HANDLER(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, &ImagePubPlugin::handle_data_transmission_handshake),
00054                                MESSAGE_HANDLER(MAVLINK_MSG_ID_ENCAPSULATED_DATA, &ImagePubPlugin::handle_encapsulated_data)
00055                 };
00056         }
00057 
00058 private:
00059         ros::NodeHandle im_nh;
00060 
00061         boost::shared_ptr<image_transport::ImageTransport> itp;
00062         image_transport::Publisher image_pub;
00063 
00064         std::string frame_id;
00065 
00066         size_t im_width, im_height;
00067         size_t im_size, im_packets, im_payload;
00068         size_t im_seqnr;
00069         uint8_t im_type;
00070         std::vector<uint8_t> im_buffer;
00071 
00073         static constexpr size_t MAX_BUFFER_RESERVE_DIFF = 0x20000;
00074 
00075         bool check_supported_type(uint8_t type) const {
00076                 return (type == MAVLINK_DATA_STREAM_IMG_JPEG ||
00077                                 type == MAVLINK_DATA_STREAM_IMG_BMP ||
00078                                 type == MAVLINK_DATA_STREAM_IMG_RAW8U ||
00079                                 /*type == MAVLINK_DATA_STREAM_IMG_RAW32U ||*/
00080                                 type == MAVLINK_DATA_STREAM_IMG_PGM ||
00081                                 type == MAVLINK_DATA_STREAM_IMG_PNG);
00082         }
00083 
00084         void publish_raw8u_image() {
00085                 auto image = boost::make_shared<sensor_msgs::Image>();
00086 
00087                 image->header.frame_id = frame_id;
00088                 image->header.stamp = ros::Time::now();
00089                 image->height = im_height;
00090                 image->width = im_width;
00091                 image->encoding = enc::MONO8;
00092                 image->is_bigendian = false;
00093                 image->step = im_width;
00094                 image->data = im_buffer;
00095 
00096                 image_pub.publish(image);
00097         }
00098 
00099         void publish_compressed_image() {
00100                 cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
00101 
00102                 cv_ptr->header.frame_id = frame_id;
00103                 cv_ptr->header.stamp = ros::Time::now();
00104 
00105                 try {
00106                         cv_ptr->image = cv::imdecode(cv::Mat(im_buffer), CV_LOAD_IMAGE_UNCHANGED);
00107                         if (cv_ptr->image.channels() == 1)
00108                                 cv_ptr->encoding = enc::MONO8;
00109                         else
00110                                 cv_ptr->encoding = enc::BGR8;
00111                 }
00112                 catch (cv::Exception &ex) {
00113                         ROS_ERROR_NAMED("image", "IMG: %s", ex.what());
00114                         return;
00115                 }
00116 
00117                 image_pub.publish(cv_ptr->toImageMsg());
00118         }
00119 
00120         void publish_image() {
00121                 switch (im_type) {
00122                 case MAVLINK_DATA_STREAM_IMG_RAW8U:
00123                         publish_raw8u_image();
00124                         break;
00125                 case MAVLINK_DATA_STREAM_IMG_JPEG:
00126                 case MAVLINK_DATA_STREAM_IMG_BMP:
00127                 case MAVLINK_DATA_STREAM_IMG_PGM:
00128                 case MAVLINK_DATA_STREAM_IMG_PNG:
00129                         publish_compressed_image();
00130                         break;
00131                 default:
00132                         ROS_ERROR_NAMED("image", "IMG: Unsupported image type: %d", im_type);
00133                 }
00134         }
00135 
00136         void handle_data_transmission_handshake(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00137                 mavlink_data_transmission_handshake_t img_header;
00138                 mavlink_msg_data_transmission_handshake_decode(msg, &img_header);
00139 
00140                 if (!check_supported_type(img_header.type)) {
00141                         ROS_WARN_NAMED("image", "IMG: Unknown stream type: %d", img_header.type);
00142                         im_packets = 0;
00143                         return;
00144                 }
00145 
00146                 // Note: no mutex, because all work done by one thread (reader)
00147                 im_seqnr = 0;
00148                 im_type = img_header.type;
00149                 im_size = img_header.size;
00150                 im_width = img_header.width;
00151                 im_height = img_header.height;
00152                 im_packets = img_header.packets;
00153                 im_payload = img_header.payload;
00154 
00155                 ROS_DEBUG_NAMED("image", "IMG: header: %zu x %zu t:%d, %zu bytes in %zu packets",
00156                                 im_width, im_height, im_type,
00157                                 im_size, im_packets);
00158 
00159                 // prepare image buffer
00160                 im_buffer.clear();
00161                 if (im_buffer.capacity() < im_size ||
00162                                 im_buffer.capacity() > im_size + MAX_BUFFER_RESERVE_DIFF) {
00163                         im_buffer.reserve(im_size);
00164                 }
00165         }
00166 
00167         void handle_encapsulated_data(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00168                 if (im_packets == 0)
00169                         return;
00170 
00171                 mavlink_encapsulated_data_t img_data;
00172                 mavlink_msg_encapsulated_data_decode(msg, &img_data);
00173 
00174                 size_t seqnr = img_data.seqnr;
00175                 //ROS_DEBUG_NAMED("image", "IMG: chunk %2zu, waiting %2zu", seqnr, im_seqnr);
00176 
00177                 if (seqnr + 1 > im_packets) {
00178                         ROS_ERROR_NAMED("image", "IMG: More data packets, than specified in handshake, seqnr: %zu, packets: %zu",
00179                                         seqnr, im_packets);
00180                         im_packets = 0;
00181                         return;
00182                 }
00183 
00184                 if (seqnr > im_seqnr) {
00185                         ROS_WARN_NAMED("image", "IMG: %zu data packets probably lost", seqnr - im_seqnr);
00186                         // we lost some packets (or packet ordering changed by underlining transport (UDP)
00187                         im_buffer.resize(std::min(im_size, (seqnr - 1) * im_payload), 0);
00188                         im_seqnr = seqnr;
00189                 }
00190 
00191                 size_t bytes_to_copy = im_payload;
00192                 if (seqnr * im_payload + bytes_to_copy >= im_size)
00193                         bytes_to_copy = im_size - seqnr * im_payload;
00194 
00195                 if (seqnr == im_seqnr) {
00196                         // insert waiting packet
00197                         im_seqnr++;
00198                         im_buffer.insert(im_buffer.end(), img_data.data, img_data.data + bytes_to_copy);
00199                 }
00200                 else {
00201                         // reordered packet arrives (seqnr < im_seqnr)
00202                         ROS_DEBUG_NAMED("image", "IMG: reordered data message, seqnr: %zu, waiting: %zu",
00203                                         seqnr, im_seqnr);
00204                         memcpy(im_buffer.data() + (seqnr * im_payload), img_data.data, bytes_to_copy);
00205                 }
00206 
00207                 if (seqnr + 1 == im_packets) {
00208                         im_packets = 0;
00209                         publish_image();
00210                 }
00211         }
00212 };
00213 };      // namespace mavplugin
00214 
00215 PLUGINLIB_EXPORT_CLASS(mavplugin::ImagePubPlugin, mavplugin::MavRosPlugin)


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:23