gcs_image_bridge.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014 Vladimir Ermakov.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <ros/ros.h>
00015 #include <ros/console.h>
00016 
00017 #include <mavros/utils.h>
00018 #include <mavros/mavlink_diag.h>
00019 #include <mavconn/interface.h>
00020 
00021 #include <sensor_msgs/image_encodings.h>
00022 #include <image_transport/image_transport.h>
00023 #include <cv_bridge/cv_bridge.h>
00024 #include <opencv2/opencv.hpp>
00025 
00026 using namespace mavros;
00027 using namespace mavconn;
00028 namespace enc = sensor_msgs::image_encodings;
00029 
00030 int jpeg_quality;
00031 MAVConnInterface::Ptr gcs_link;
00032 ros::Publisher mavlink_pub;
00033 
00034 void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid)
00035 {
00036         auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
00037 
00038         rmsg->header.stamp = ros::Time::now();
00039         mavros_msgs::mavlink::convert(*mmsg, *rmsg);
00040         mavlink_pub.publish(rmsg);
00041 }
00042 
00043 void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg)
00044 {
00045         mavlink_message_t mmsg;
00046 
00047         if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
00048                 gcs_link->send_message(&mmsg, rmsg->sysid, rmsg->compid);
00049         else
00050                 ROS_ERROR("Packet drop: illegal payload64 size");
00051 }
00052 
00053 void send_jpeg_image(std::vector<uint8_t> &jpeg_buffer, int jpeg_quality,
00054                 int height, int width)
00055 {
00056         constexpr size_t PAYLOAD_SIZE = sizeof(mavlink_encapsulated_data_t::data);
00057         mavlink_message_t msg;
00058 
00059         if (jpeg_buffer.empty()) {
00060                 ROS_ERROR("IMG: Empty JPEG buffer!");
00061                 return;
00062         }
00063 
00064         size_t packet_count = jpeg_buffer.size() / PAYLOAD_SIZE + 1;
00065         if (jpeg_buffer.capacity() < packet_count * PAYLOAD_SIZE) {
00066                 // preventing copying unowned data in next step
00067                 ROS_DEBUG("IMG: Reserved: %zu -> %zu bytes", jpeg_buffer.capacity(),
00068                                 packet_count * PAYLOAD_SIZE);
00069                 jpeg_buffer.reserve(packet_count * PAYLOAD_SIZE);
00070         }
00071 
00072         ROS_DEBUG("IMG: Send image %d x %d, %zu bytes in %zu packets",
00073                         width, height, jpeg_buffer.size(), packet_count);
00074 
00075         mavlink_msg_data_transmission_handshake_pack_chan(
00076                 gcs_link->get_system_id(),
00077                 gcs_link->get_component_id(),
00078                 gcs_link->get_channel(),
00079                 &msg,
00080                 MAVLINK_DATA_STREAM_IMG_JPEG,
00081                 jpeg_buffer.size(),
00082                 width,
00083                 height,
00084                 packet_count,
00085                 PAYLOAD_SIZE,
00086                 jpeg_quality);
00087         gcs_link->send_message(&msg);
00088 
00089         for (size_t seqnr = 0; seqnr < packet_count; seqnr++) {
00090                 mavlink_msg_encapsulated_data_pack_chan(
00091                                 gcs_link->get_system_id(),
00092                                 gcs_link->get_component_id(),
00093                                 gcs_link->get_channel(),
00094                                 &msg,
00095                                 seqnr,
00096                                 jpeg_buffer.data() + (PAYLOAD_SIZE * seqnr));
00097                 gcs_link->send_message(&msg);
00098 
00099                 //ROS_DEBUG("IMG: chunk %2zu, %p->%p", seqnr, jpeg_buffer.data(),
00100                 //              jpeg_buffer.data() + (PAYLOAD_SIZE * seqnr));
00101         }
00102 }
00103 
00104 void image_cb(const sensor_msgs::Image::ConstPtr &img_msg)
00105 {
00106         constexpr size_t PAYLOAD_SIZE = sizeof(mavlink_encapsulated_data_t::data);
00107         cv_bridge::CvImageConstPtr cv_ptr;
00108 
00109         try {
00110                 if (enc::isColor(img_msg->encoding))
00111                         cv_ptr = cv_bridge::toCvShare(img_msg, "bgr8");
00112                 else
00113                         cv_ptr = cv_bridge::toCvShare(img_msg);
00114 
00115                 // code from image_transport_plugins compressed_publisher.cpp
00116                 std::vector<int> params;
00117                 std::vector<uint8_t> jpeg_buffer;
00118                 params.resize(3, 0);
00119                 // typical image size 60-70 packet
00120                 jpeg_buffer.reserve(80 * PAYLOAD_SIZE);
00121 
00122                 params[0] = CV_IMWRITE_JPEG_QUALITY;
00123                 params[1] = jpeg_quality;
00124 
00125                 if (cv::imencode(".jpg", cv_ptr->image, jpeg_buffer, params)) {
00126                         float comp_ratio = (float)(cv_ptr->image.rows *
00127                                         cv_ptr->image.cols *
00128                                         cv_ptr->image.elemSize())
00129                                 / jpeg_buffer.size();
00130 
00131                         ROS_DEBUG("IMG: JPEG quality %d, ratio %f", jpeg_quality, comp_ratio);
00132 
00133                         send_jpeg_image(jpeg_buffer, jpeg_quality,
00134                                         cv_ptr->image.rows,
00135                                         cv_ptr->image.cols);
00136                 }
00137                 else {
00138                         ROS_ERROR("IMG: cv::imencode (jpeg) failed");
00139                         return;
00140                 }
00141         }
00142         catch (cv_bridge::Exception &ex) {
00143                 ROS_ERROR("IMG: %s", ex.what());
00144         }
00145         catch (cv::Exception &ex) {
00146                 ROS_ERROR("IMG: %s", ex.what());
00147         }
00148 }
00149 
00150 int main(int argc, char *argv[])
00151 {
00152         ros::init(argc, argv, "gcs_image_bridge");
00153         ros::NodeHandle priv_nh("~");
00154         ros::NodeHandle mavlink_nh("mavlink");
00155         ros::Subscriber mavlink_sub;
00156         diagnostic_updater::Updater updater;
00157         mavros::MavlinkDiag gcs_link_diag("GCS bridge");
00158         image_transport::ImageTransport it(mavlink_nh);
00159         image_transport::Subscriber image_sub;
00160 
00161         std::string gcs_url;
00162         priv_nh.param<std::string>("gcs_url", gcs_url, "udp://@");
00163         priv_nh.param("jpeg_quality", jpeg_quality, 60);
00164 
00165         try {
00166                 gcs_link = MAVConnInterface::open_url(gcs_url);
00167                 gcs_link_diag.set_mavconn(gcs_link);
00168                 gcs_link_diag.set_connection_status(true);
00169         }
00170         catch (mavconn::DeviceError &ex) {
00171                 ROS_FATAL("GCS: %s", ex.what());
00172                 return 0;
00173         }
00174 
00175         mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("to", 10);
00176         gcs_link->message_received.connect(mavlink_pub_cb);
00177 
00178         mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb,
00179                         ros::TransportHints()
00180                                 .unreliable().maxDatagramSize(1024)
00181                                 .reliable());
00182 
00183         image_sub = it.subscribe("gcs_image", 1, image_cb);
00184 
00185         // setup updater
00186         updater.setHardwareID(gcs_url);
00187         updater.add(gcs_link_diag);
00188 
00189         // updater spinner
00190         auto diag_timer = priv_nh.createTimer(ros::Duration(1.0),
00191                         [&](const ros::TimerEvent &evt) {
00192                                 updater.update();
00193                         });
00194         diag_timer.start();
00195 
00196         ros::spin();
00197         return 0;
00198 }
00199 


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:27