diff --git a/terrain_navigation/include/terrain_navigation/viewpoint.h b/terrain_navigation/include/terrain_navigation/viewpoint.h deleted file mode 100644 index ae6fac67..00000000 --- a/terrain_navigation/include/terrain_navigation/viewpoint.h +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-2023 Jaeyoung Lim, Autonomous Systems Lab, - * ETH Zürich. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @brief Adaptive view utility estimation node - * - * - * @author Jaeyoung Lim - */ - -#ifndef TERRAIN_PLANNER_VIEWPOINT_H -#define TERRAIN_PLANNER_VIEWPOINT_H - -#include -#include -#include -#include -#include -#include -#include -#include "opencv2/core.hpp" - -class ViewPoint { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ViewPoint(const int idx, const Eigen::Vector3d &local_position, const Eigen::Vector4d &orientation) { - index_ = idx; - center_local_ = local_position; - orientation_ = orientation; - /// TODO: Read camera parameters from a file - corner_ray_vectors_.push_back(RayVector(0, 0)); - corner_ray_vectors_.push_back(RayVector(0, image_width_)); - corner_ray_vectors_.push_back(RayVector(image_height_, image_width_)); - corner_ray_vectors_.push_back(RayVector(image_height_, 0)); - center_ray_vector_ = RayVector(image_height_ / 2, image_width_ / 2); - - orientation_ = orientation; - Eigen::Matrix3d R_att = quat2RotMatrix(orientation); - - // Transform corner ray vectors according to attitude - for (auto &corner_ray : corner_ray_vectors_) { - corner_ray = R_att * corner_ray; - } - center_ray_vector_ = R_att * center_ray_vector_; - }; - ViewPoint(const int idx, const double &longitude, const double &latitude, const double &altitude) { - index_ = idx; - center_global << longitude, latitude, altitude; - corner_ray_vectors_.push_back(RayVector(0, 0)); - corner_ray_vectors_.push_back(RayVector(0, image_width_)); - corner_ray_vectors_.push_back(RayVector(image_height_, image_width_)); - corner_ray_vectors_.push_back(RayVector(image_height_, 0)); - center_ray_vector_ = RayVector(image_height_ / 2, image_width_ / 2); - } - virtual ~ViewPoint(){}; - void setOrigin(const double &latitude, const double &longitude, const double &altitude) { - origin_global_ << latitude, longitude, altitude; - }; - Eigen::Vector3d RayVector(int pixel_x, int pixel_y) { - /// TODO: Get camera intrinsics from a file - int c1 = image_height_ / 2; - int c2 = image_width_ / 2; - double f = image_width_ / 2; - - Eigen::Vector3d ray; - ray << double(pixel_x - c1) / f, double(pixel_y - c2) / f, -1.0; - ray.normalize(); - return ray; - } - Eigen::Vector3d getCenterLocal() { return center_local_; } - double getTime() { return time_seconds_; } - void setTime(double time_seconds) { time_seconds_ = time_seconds; } - void setPosition(const Eigen::Vector3d &position) { center_local_ = position; } - void setOrientation(const Eigen::Vector4d &attitude) { - orientation_ = attitude; - Eigen::Matrix3d R_att = quat2RotMatrix(attitude); - - // Transform corner ray vectors according to attitude - for (auto &corner_ray : corner_ray_vectors_) { - corner_ray = R_att * corner_ray; - } - center_ray_vector_ = R_att * center_ray_vector_; - } - void setUtility(const double &utility) { utility_ = utility; }; - void setImage(const std::string &image_path) { - image_ = cv::imread(image_path, cv::IMREAD_GRAYSCALE); - // cv::imshow("viewpoint", image_); - // cv::waitKey(0.5); - }; - cv::Mat getImage() { return image_; } - std::vector getCornerRayVectors() { return corner_ray_vectors_; }; - Eigen::Vector3d getCenterRayVector() { return center_ray_vector_; }; - Eigen::Vector4d getOrientation() { return orientation_; }; - double getUtility() { return utility_; }; - int getIndex() { return index_; } - static Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q) { - Eigen::Matrix3d rotmat; - rotmat << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3), - 2 * q(0) * q(2) + 2 * q(1) * q(3), - - 2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3), - 2 * q(2) * q(3) - 2 * q(0) * q(1), - - 2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3), - q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3); - return rotmat; - }; - /** - * @brief Get the Pixel Resolution object - * - * @return double pixel_resolution [rad/pixel] - */ - /// TODO: get FOV from ray vectors - double getPixelResolution() { return 0.5 * M_PI / image_width_; } - - /** - * @brief - * - * @param pos - * @return true - * @return false - */ - bool isInsideViewFustrum(const Eigen::Vector3d &pos) { - bool is_inside{false}; - Eigen::Matrix3d R_att = quat2RotMatrix(orientation_); - auto camera_frame_pos = R_att.inverse() * pos; - /// TODO: Get camera intrinsics from a file - double f = image_width_ / 2; - int c1 = f * camera_frame_pos(0) / camera_frame_pos(2); - int c2 = f * camera_frame_pos(1) / camera_frame_pos(2); - - if (std::abs(c1) < image_width_ / 2 && std::abs(c2) < image_height_ / 2 && camera_frame_pos(2) > 0.0) { - is_inside = true; - } - return is_inside; - } - - private: - int index_; - Eigen::Vector3d center_local_{Eigen::Vector3d(0.0, 0.0, 0.0)}; - Eigen::Vector3d center_global{Eigen::Vector3d(0.0, 0.0, 0.0)}; - Eigen::Vector3d origin_global_{Eigen::Vector3d(0.0, 0.0, 0.0)}; - Eigen::Vector4d orientation_{Eigen::Vector4d(1.0, 0.0, 0.0, 0.0)}; - std::vector corner_ray_vectors_; - Eigen::Vector3d center_ray_vector_; - double time_seconds_{0.0}; - double utility_{0.0}; - cv::Mat image_; // Store image of the viewpoint - int image_width_ = 1440; - int image_height_ = 1080; -}; - -#endif diff --git a/terrain_navigation/include/terrain_navigation/visualization.h b/terrain_navigation/include/terrain_navigation/visualization.h deleted file mode 100644 index 90c4ea24..00000000 --- a/terrain_navigation/include/terrain_navigation/visualization.h +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-2023 Jaeyoung Lim, Autonomous Systems Lab, - * ETH Zürich. All rights reserved. - - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @brief Helper functions for visualizations - * - * - * @author Jaeyoung Lim - */ - -#ifndef VISUALIZATION_H -#define VISUALIZATION_H - -#include "geometry_msgs/Point.h" -#include "terrain_navigation/viewpoint.h" -#include "visualization_msgs/Marker.h" - -geometry_msgs::Point toPoint(const Eigen::Vector3d &p) { - geometry_msgs::Point position; - position.x = p(0); - position.y = p(1); - position.z = p(2); - return position; -} - -visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { - double scale{15}; // Size of the viewpoint markers - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "my_namespace"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - const Eigen::Vector3d position = viewpoint.getCenterLocal(); - std::vector points; - std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); - std::vector vertex; - for (auto &corner_ray : corner_ray_vectors) { - vertex.push_back(position + scale * corner_ray); - } - - for (size_t i = 0; i < vertex.size(); i++) { - points.push_back(toPoint(position)); // Viewpoint center - points.push_back(toPoint(vertex[i])); - points.push_back(toPoint(vertex[i])); - points.push_back(toPoint(vertex[(i + 1) % vertex.size()])); - } - - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1.0; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = color(0); - marker.color.g = color(1); - marker.color.b = color(2); - return marker; -} - -#endif // VISUALIZATION_H diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h index 6e827179..edf50e13 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h @@ -59,7 +59,6 @@ #include #include -#include #include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" @@ -173,8 +172,6 @@ class TerrainPlanner { ros::Publisher rallypoint_pub_; ros::Publisher candidate_goal_pub_; ros::Publisher candidate_start_pub_; - ros::Publisher viewpoint_pub_; - ros::Publisher planned_viewpoint_pub_; ros::Publisher tree_pub_; ros::Publisher vehicle_velocity_pub_; ros::Publisher path_segment_pub_; @@ -184,7 +181,6 @@ class TerrainPlanner { ros::Subscriber mavstate_sub_; ros::Subscriber mavmission_sub_; ros::Subscriber global_origin_sub_; - ros::Subscriber image_captured_sub_; ros::ServiceServer setlocation_serviceserver_; ros::ServiceServer setmaxaltitude_serviceserver_; @@ -234,11 +230,8 @@ class TerrainPlanner { std::mutex goal_mutex_; // protects g_i std::vector vehicle_position_history_; - std::vector added_viewpoint_list; - std::vector planned_viewpoint_list; std::vector posehistory_vector_; std::vector referencehistory_vector_; - std::vector viewpoints_; Eigen::Vector3d vehicle_position_{Eigen::Vector3d::Zero()}; Eigen::Vector3d vehicle_velocity_{Eigen::Vector3d::Zero()}; Eigen::Vector4d vehicle_attitude_{Eigen::Vector4d(1.0, 0.0, 0.0, 0.0)}; diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h index 4f4e0261..114b10ca 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h @@ -126,27 +126,6 @@ inline void publishCameraView(const ros::Publisher pub, const Eigen::Vector3d& p pub.publish(marker); } -inline void publishViewpoints(const ros::Publisher pub, std::vector& viewpoint_vector, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { - visualization_msgs::MarkerArray msg; - - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; - marker.push_back(mark); - msg.markers = marker; - pub.publish(msg); - - std::vector viewpoint_marker_vector; - int i = 0; - for (auto viewpoint : viewpoint_vector) { - viewpoint_marker_vector.insert(viewpoint_marker_vector.begin(), Viewpoint2MarkerMsg(i, viewpoint, color)); - i++; - } - msg.markers = viewpoint_marker_vector; - pub.publish(msg); -} - void publishCandidateManeuvers(const ros::Publisher& pub, const std::vector& candidate_maneuvers, bool visualize_invalid_trajectories = false) { visualization_msgs::MarkerArray msg; diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp index bded5154..ea45a9a1 100644 --- a/terrain_navigation_ros/src/terrain_planner.cpp +++ b/terrain_navigation_ros/src/terrain_planner.cpp @@ -80,7 +80,6 @@ TerrainPlanner::TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle camera_pose_pub_ = nh_.advertise("camera_pose_marker", 1); planner_status_pub_ = nh_.advertise("planner_status", 1); viewpoint_pub_ = nh_.advertise("viewpoints", 1); - planned_viewpoint_pub_ = nh_.advertise("planned_viewpoints", 1); path_segment_pub_ = nh_.advertise("path_segments", 1); tree_pub_ = nh_.advertise("tree", 1); @@ -92,8 +91,6 @@ TerrainPlanner::TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle ros::TransportHints().tcpNoDelay()); global_origin_sub_ = nh_.subscribe("mavros/global_position/gp_origin", 1, &TerrainPlanner::mavGlobalOriginCallback, this, ros::TransportHints().tcpNoDelay()); - image_captured_sub_ = nh_.subscribe("mavros/camera/image_captured", 1, &TerrainPlanner::mavImageCapturedCallback, - this, ros::TransportHints().tcpNoDelay()); setlocation_serviceserver_ = nh_.advertiseService("/terrain_planner/set_location", &TerrainPlanner::setLocationCallback, this); @@ -217,37 +214,6 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { publishPositionHistory(referencehistory_pub_, reference_position, referencehistory_vector_); tracking_error_ = reference_position - vehicle_position_; planner_enabled_ = true; - - if ((ros::Time::now() - last_triggered_time_).toSec() > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { - bool dummy_camera = false; - if (dummy_camera) { - const int id = viewpoints_.size() + added_viewpoint_list.size(); - ViewPoint viewpoint(id, vehicle_position_, vehicle_attitude_); - added_viewpoint_list.push_back(viewpoint); - } else { - /// TODO: Trigger camera when viewpoint reached - /// This can be done using the mavlink message MAV_CMD_IMAGE_START_CAPTURE - mavros_msgs::CommandLong image_capture_msg; - image_capture_msg.request.command = mavros_msgs::CommandCode::DO_DIGICAM_CONTROL; - image_capture_msg.request.param1 = 0; // id - image_capture_msg.request.param2 = 0; // interval - image_capture_msg.request.param3 = 0; // total images - image_capture_msg.request.param4 = 0; // sequence number - image_capture_msg.request.param5 = 1; // sequence number - image_capture_msg.request.param6 = 0; // sequence number - image_capture_msg.request.param7 = 0; // sequence number - msginterval_serviceclient_.call(image_capture_msg); - } - /// TODO: Get reference attitude from path reference states - const double pitch = std::atan(reference_tangent(2) / reference_tangent.head(2).norm()); - const double yaw = std::atan2(-reference_tangent(1), reference_tangent(0)); - const double roll = std::atan(-reference_curvature * std::pow(cruise_speed_, 2) / 9.81); - Eigen::Vector4d reference_attitude = rpy2quaternion(roll, -pitch, -yaw); - const int id = viewpoints_.size() + planned_viewpoint_list.size(); - ViewPoint viewpoint(id, reference_position, reference_attitude); - planned_viewpoint_list.push_back(viewpoint); - last_triggered_time_ = ros::Time::now(); - } } else { tracking_error_ = Eigen::Vector3d::Zero(); planner_enabled_ = false; @@ -498,9 +464,6 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { // double planner_time = planner_profiler_->toc(); publishTrajectory(reference_primitive_.position()); // publishGoal(goal_pub_, goal_pos_, 66.67, Eigen::Vector3d(0.0, 1.0, 0.0)); - - publishViewpoints(viewpoint_pub_, viewpoints_, Eigen::Vector3d(0.0, 1.0, 0.0)); - publishViewpoints(planned_viewpoint_pub_, planned_viewpoint_list, Eigen::Vector3d(1.0, 1.0, 0.0)); } PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_state, const PLANNER_STATE query_state) { @@ -919,15 +882,6 @@ void TerrainPlanner::mavMissionCallback(const mavros_msgs::WaypointListPtr &msg) } } -void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::CameraImageCaptured::ConstPtr &msg) { - // Publish recorded viewpoints - /// TODO: Transform image tag into local position - int id = viewpoints_.size(); - ViewPoint viewpoint(id, vehicle_position_, vehicle_attitude_); - viewpoints_.push_back(viewpoint); - // publishViewpoints(viewpoint_pub_, viewpoints_); -} - bool TerrainPlanner::setLocationCallback(planner_msgs::SetString::Request &req, planner_msgs::SetString::Response &res) { std::string set_location = req.string;