diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index b49fcbfa..5af4fa36 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -21,6 +21,9 @@ find_package(catkin REQUIRED COMPONENTS tf tf2 tf2_geometry_msgs + tf2_ros + image_transport + cv_bridge ) @@ -121,7 +124,7 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES clever + LIBRARIES clever # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) @@ -137,7 +140,11 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library +# Declare a C++ library +add_library(clever + src/optical_flow.cpp +) + add_library(fcu_horiz src/fcu_horiz.cpp ) @@ -173,6 +180,10 @@ target_link_libraries(camera_markers ${catkin_LIBRARIES}) # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against +target_link_libraries(clever + ${catkin_LIBRARIES} +) + target_link_libraries(fcu_horiz ${catkin_LIBRARIES} "/opt/ros/kinetic/lib/libtf2_ros.so" diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch index 1bcc3013..f64ace1c 100644 --- a/clever/launch/clever.launch +++ b/clever/launch/clever.launch @@ -6,6 +6,7 @@ + @@ -28,6 +29,12 @@ + + + + + + diff --git a/clever/launch/mavros.launch b/clever/launch/mavros.launch index 9372c9f3..70d0bc62 100644 --- a/clever/launch/mavros.launch +++ b/clever/launch/mavros.launch @@ -38,13 +38,11 @@ - safety_area - image_pub - vibration - - distance_sensor - rangefinder - 3dr_radio - actuator_control - hil_controls - vfr_hud - - px4flow - vision_speed_estimate - fake_gps - cam_imu_sync diff --git a/clever/launch/sitl.launch b/clever/launch/sitl.launch index b01b8f9d..af9f08c4 100644 --- a/clever/launch/sitl.launch +++ b/clever/launch/sitl.launch @@ -7,6 +7,7 @@ + diff --git a/clever/nodelet_plugins.xml b/clever/nodelet_plugins.xml index 9e1c2c67..36e583b6 100644 --- a/clever/nodelet_plugins.xml +++ b/clever/nodelet_plugins.xml @@ -1,3 +1,8 @@ + + + + + diff --git a/clever/src/optical_flow.cpp b/clever/src/optical_flow.cpp new file mode 100644 index 00000000..248a9159 --- /dev/null +++ b/clever/src/optical_flow.cpp @@ -0,0 +1,198 @@ +/* + * Optical Flow node for PX4 + * Copyright (C) 2018 Copter Express Technologies + * + * Author: Oleg Kalachev + * + * Licensed under the MIT License. See LICENSE.md in the project root for license information. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using cv::Mat; + +class OpticalFlow : public nodelet::Nodelet +{ +public: + OpticalFlow(): + camera_matrix_(3, 3, CV_64F), + dist_coeffs_(8, 1, CV_64F), + tf_listener_(tf_buffer_) + {} + +private: + ros::Publisher flow_pub_, velo_pub_, shift_pub_; + ros::Time prev_stamp_; + std::string fcu_frame_id_; + image_transport::CameraSubscriber img_sub_; + image_transport::Publisher img_pub_; + mavros_msgs::OpticalFlowRad flow_; + int roi_, roi_2_; + Mat hann_; + Mat prev_, curr_; + Mat camera_matrix_, dist_coeffs_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + void onInit() + { + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& nh_priv = getPrivateNodeHandle(); + image_transport::ImageTransport it(nh); + image_transport::ImageTransport it_priv(nh_priv); + + nh_priv.param("frame_id", fcu_frame_id_, "fcu"); + nh_priv.param("roi", roi_, 128); + roi_2_ = roi_ / 2; + + img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this); + img_pub_ = it_priv.advertise("debug", 1); + flow_pub_ = nh.advertise("mavros/px4flow/raw/send", 1); + velo_pub_ = nh_priv.advertise("angular_velocity", 1); + shift_pub_ = nh_priv.advertise("shift", 1); + + flow_.integrated_xgyro = NAN; // no IMU available + flow_.integrated_ygyro = NAN; + flow_.integrated_zgyro = NAN; + flow_.time_delta_distance_us = 0; + flow_.distance = -1; // no distance sensor available + flow_.temperature = 0; + + ROS_INFO("Optical Flow initialized"); + } + + void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) { + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + camera_matrix_.at(i, j) = cinfo->K[3 * i + j]; + } + } + for (int k = 0; k < cinfo->D.size(); k++) { + dist_coeffs_.at(k) = cinfo->D[k]; + } + } + + void drawFlow(Mat& frame, double x, double y, double quality) const + { + double brightness = (1 - quality) * 25;; + cv::Scalar color(brightness, brightness, brightness); + double radius = std::sqrt(x * x + y * y); + + // draw a circle and line indicating the shift direction... + cv::Point center(frame.cols >> 1, frame.rows >> 1); + cv::circle(frame, center, (int)(radius*5), color, 3, cv::LINE_AA); + cv::line(frame, center, cv::Point(center.x + (int)(x*5), center.y + (int)(y*5)), color, 3, cv::LINE_AA); + } + + void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo) + { + parseCameraInfo(cinfo); + + auto img = cv_bridge::toCvShare(msg, "mono8")->image; + + // Apply ROI + if (roi_ != 0) { + img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_)); + } + + img.convertTo(curr_, CV_64F); + + if (prev_.empty()) { + prev_ = curr_.clone(); + prev_stamp_ = msg->header.stamp; + cv::createHanningWindow(hann_, curr_.size(), CV_64F); + + } else { + double response; + cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response); + + // Publish raw shift in pixels + static geometry_msgs::Vector3Stamped shift_vec; + shift_vec.header.stamp = msg->header.stamp; + shift_vec.header.frame_id = msg->header.frame_id; + shift_vec.vector.x = shift.x; + shift_vec.vector.y = shift.y; + shift_pub_.publish(shift_vec); + + // Undistort flow in pixels + uint32_t flow_center_x = msg->width / 2; + uint32_t flow_center_y = msg->height / 2; + shift.x += flow_center_x; + shift.y += flow_center_y; + + std::vector points_dist = { shift }; + std::vector points_undist(1); + + cv::undistortPoints(points_dist, points_undist, camera_matrix_, dist_coeffs_, cv::noArray(), camera_matrix_); + points_undist[0].x -= flow_center_x; + points_undist[0].y -= flow_center_y; + + // Calculate flow in radians + double focal_length_x = camera_matrix_.at(0, 0); + double focal_length_y = camera_matrix_.at(1, 1); + double flow_x = atan2(points_undist[0].x, focal_length_x); + double flow_y = atan2(points_undist[0].y, focal_length_y); + + // // Convert to FCU frame + static geometry_msgs::Vector3Stamped flow_camera, flow_fcu; + flow_camera.header.frame_id = msg->header.frame_id; + flow_camera.header.stamp = msg->header.stamp; + flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis + flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis + tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_); + + // Calculate integration time + ros::Duration integration_time = msg->header.stamp - prev_stamp_; + uint32_t integration_time_us = integration_time.toSec() * 1.0e6; + + // Publish flow in fcu frame + flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp; + flow_.integration_time_us = integration_time_us; + flow_.integrated_x = flow_fcu.vector.x; + flow_.integrated_y = flow_fcu.vector.y; + flow_.quality = (uint8_t)(response * 255); + flow_pub_.publish(flow_); + + // Publish debug image + if (img_pub_.getNumSubscribers() > 0) { + // publish debug image + drawFlow(img, shift_vec.vector.x, shift_vec.vector.y, response); + cv_bridge::CvImage out_msg; + out_msg.header.frame_id = msg->header.frame_id; + out_msg.header.stamp = msg->header.stamp; + out_msg.encoding = sensor_msgs::image_encodings::MONO8; + out_msg.image = img; + img_pub_.publish(out_msg.toImageMsg()); + } + + // Publish estimated angular velocity + static geometry_msgs::TwistStamped velo; + velo.header.stamp = msg->header.stamp; + velo.header.frame_id = fcu_frame_id_; + velo.twist.angular.x = flow_.integrated_x / integration_time.toSec(); + velo.twist.angular.y = flow_.integrated_y / integration_time.toSec(); + velo_pub_.publish(velo); + + prev_ = curr_.clone(); + prev_stamp_ = msg->header.stamp; + } + } +}; + +PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)