px4flow.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014 M.H.Kabir.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <mavros/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 
00021 #include <mavros_msgs/OpticalFlowRad.h>
00022 #include <sensor_msgs/Temperature.h>
00023 #include <sensor_msgs/Range.h>
00024 
00025 namespace mavplugin {
00031 class PX4FlowPlugin : public MavRosPlugin {
00032 public:
00033         PX4FlowPlugin() :
00034                 flow_nh("~px4flow"),
00035                 uas(nullptr),
00036                 ranger_fov(0.0),
00037                 ranger_min_range(0.3),
00038                 ranger_max_range(5.0)
00039         { };
00040 
00041         void initialize(UAS &uas_)
00042         {
00043                 uas = &uas_;
00044 
00045                 flow_nh.param<std::string>("frame_id", frame_id, "px4flow");
00046 
00048                 flow_nh.param("ranger_fov", ranger_fov, 0.0);   
00049                 flow_nh.param("ranger_min_range", ranger_min_range, 0.3);
00050                 flow_nh.param("ranger_max_range", ranger_max_range, 5.0);
00051 
00052                 flow_rad_pub = flow_nh.advertise<mavros_msgs::OpticalFlowRad>("raw/optical_flow_rad", 10);
00053                 range_pub = flow_nh.advertise<sensor_msgs::Range>("ground_distance", 10);
00054                 temp_pub = flow_nh.advertise<sensor_msgs::Temperature>("temperature", 10);
00055         }
00056 
00057         const message_map get_rx_handlers() {
00058                 return {
00059                                MESSAGE_HANDLER(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &PX4FlowPlugin::handle_optical_flow_rad)
00060                 };
00061         }
00062 
00063 private:
00064         ros::NodeHandle flow_nh;
00065         UAS *uas;
00066 
00067         std::string frame_id;
00068 
00069         double ranger_fov;
00070         double ranger_min_range;
00071         double ranger_max_range;
00072 
00073         ros::Publisher flow_rad_pub;
00074         ros::Publisher range_pub;
00075         ros::Publisher temp_pub;
00076 
00077         void handle_optical_flow_rad(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00078                 mavlink_optical_flow_rad_t flow_rad;
00079                 mavlink_msg_optical_flow_rad_decode(msg, &flow_rad);
00080 
00081                 auto header = uas->synchronized_header(frame_id, flow_rad.time_usec);
00082 
00089                 auto int_xy = UAS::transform_frame_aircraft_baselink(
00090                                 Eigen::Vector3d(
00091                                         flow_rad.integrated_x,
00092                                         flow_rad.integrated_y,
00093                                         0.0));
00094                 auto int_gyro = UAS::transform_frame_aircraft_baselink(
00095                                 Eigen::Vector3d(
00096                                         flow_rad.integrated_xgyro,
00097                                         flow_rad.integrated_ygyro,
00098                                         flow_rad.integrated_zgyro));
00099 
00100                 auto flow_rad_msg = boost::make_shared<mavros_msgs::OpticalFlowRad>();
00101 
00102                 flow_rad_msg->header = header;
00103                 flow_rad_msg->integration_time_us = flow_rad.integration_time_us;
00104 
00105                 flow_rad_msg->integrated_x = int_xy.x();
00106                 flow_rad_msg->integrated_y = int_xy.y();
00107 
00108                 flow_rad_msg->integrated_xgyro = int_gyro.x();
00109                 flow_rad_msg->integrated_ygyro = int_gyro.y();
00110                 flow_rad_msg->integrated_zgyro = int_gyro.z();
00111 
00112                 flow_rad_msg->temperature = flow_rad.temperature / 100.0f;      // in degrees celsius
00113                 flow_rad_msg->time_delta_distance_us = flow_rad.time_delta_distance_us;
00114                 flow_rad_msg->distance = flow_rad.distance;
00115                 flow_rad_msg->quality = flow_rad.quality;
00116 
00117                 flow_rad_pub.publish(flow_rad_msg);
00118 
00119                 // Temperature
00120                 auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
00121 
00122                 temp_msg->header = header;
00123                 temp_msg->temperature = flow_rad_msg->temperature;
00124 
00125                 temp_pub.publish(temp_msg);
00126 
00127                 // Rangefinder
00136                 auto range_msg = boost::make_shared<sensor_msgs::Range>();
00137 
00138                 range_msg->header = header;
00139 
00140                 range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND;
00141                 range_msg->field_of_view = ranger_fov;
00142                 range_msg->min_range = ranger_min_range;
00143                 range_msg->max_range = ranger_max_range;
00144                 range_msg->range = flow_rad.distance;
00145 
00146                 range_pub.publish(range_msg);
00147         }
00148 };
00149 };      // namespace mavplugin
00150 
00151 PLUGINLIB_EXPORT_CLASS(mavplugin::PX4FlowPlugin, mavplugin::MavRosPlugin)


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