From cbca0b0b9885a654e0e101a002bebdcaef876a2e Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sun, 10 Dec 2023 02:59:11 +0100 Subject: [PATCH] ros2: remove viewpoint object (#29) Co-authored-by: JaeyoungLim Co-authored-by: Rhys Mainwaring Signed-off-by: Rhys Mainwaring --- .../include/terrain_navigation/viewpoint.h | 183 ------------------ .../terrain_navigation/visualization.h | 97 ---------- .../terrain_navigation_ros/terrain_planner.h | 9 - .../terrain_navigation_ros/visualization.h | 12 -- .../src/terrain_planner.cpp | 89 --------- terrain_navigation_ros/src/visualization.cpp | 68 ------- 6 files changed, 458 deletions(-) delete mode 100644 terrain_navigation/include/terrain_navigation/viewpoint.h delete mode 100644 terrain_navigation/include/terrain_navigation/visualization.h diff --git a/terrain_navigation/include/terrain_navigation/viewpoint.h b/terrain_navigation/include/terrain_navigation/viewpoint.h deleted file mode 100644 index b4561050..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 - -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 f8f0ce80..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 -#include -#include - -geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { - geometry_msgs::msg::Point position; - position.x = p(0); - position.y = p(1); - position.z = p(2); - return position; -} - -visualization_msgs::msg::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::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Clock().now(); - marker.ns = "my_namespace"; - marker.id = id; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.action = visualization_msgs::msg::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 926a511e..d77dd1ef 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h @@ -43,7 +43,6 @@ #define TERRAIN_PLANNER_H #include -#include #include #include #include @@ -100,7 +99,6 @@ class TerrainPlanner : public rclcpp::Node { void mavstateCallback(const mavros_msgs::msg::State &msg); void mavGlobalOriginCallback(const geographic_msgs::msg::GeoPointStamped &msg); void mavMissionCallback(const mavros_msgs::msg::WaypointList &msg); - void mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured &msg); bool setLocationCallback(const std::shared_ptr req, std::shared_ptr res); @@ -177,7 +175,6 @@ class TerrainPlanner : public rclcpp::Node { rclcpp::Publisher::SharedPtr vehicle_path_pub_; rclcpp::Publisher::SharedPtr grid_map_pub_; rclcpp::Publisher::SharedPtr vehicle_pose_pub_; - rclcpp::Publisher::SharedPtr camera_pose_pub_; rclcpp::Publisher::SharedPtr posehistory_pub_; rclcpp::Publisher::SharedPtr referencehistory_pub_; rclcpp::Publisher::SharedPtr global_position_setpoint_pub_; @@ -187,8 +184,6 @@ class TerrainPlanner : public rclcpp::Node { rclcpp::Publisher::SharedPtr rallypoint_pub_; rclcpp::Publisher::SharedPtr candidate_goal_pub_; rclcpp::Publisher::SharedPtr candidate_start_pub_; - rclcpp::Publisher::SharedPtr viewpoint_pub_; - rclcpp::Publisher::SharedPtr planned_viewpoint_pub_; rclcpp::Publisher::SharedPtr tree_pub_; rclcpp::Publisher::SharedPtr vehicle_velocity_pub_; rclcpp::Publisher::SharedPtr path_segment_pub_; @@ -199,7 +194,6 @@ class TerrainPlanner : public rclcpp::Node { rclcpp::Subscription::SharedPtr mavstate_sub_; rclcpp::Subscription::SharedPtr mavmission_sub_; rclcpp::Subscription::SharedPtr global_origin_sub_; - rclcpp::Subscription::SharedPtr image_captured_sub_; rclcpp::Service::SharedPtr setlocation_serviceserver_; rclcpp::Service::SharedPtr setmaxaltitude_serviceserver_; @@ -257,11 +251,8 @@ class TerrainPlanner : public rclcpp::Node { 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 ca3ae70d..3b347c91 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h @@ -34,8 +34,6 @@ #ifndef TERRAIN_NAVIGATION_ROS_VISUALIZATION_H #define TERRAIN_NAVIGATION_ROS_VISUALIZATION_H -#include - #include #include #include @@ -53,14 +51,4 @@ void publishVehiclePose(rclcpp::Publisher::Shar const Eigen::Vector3d &position, const Eigen::Vector4d &attitude, std::string mesh_resource_path); -visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)); - -void publishCameraView(rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d &position, const Eigen::Vector4d &attitude); - -void publishViewpoints(rclcpp::Publisher::SharedPtr pub, - std::vector &viewpoint_vector, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)); - #endif diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp index 9e5a9254..302995d1 100644 --- a/terrain_navigation_ros/src/terrain_planner.cpp +++ b/terrain_navigation_ros/src/terrain_planner.cpp @@ -121,10 +121,7 @@ TerrainPlanner::TerrainPlanner() : Node("terrain_planner") { global_position_setpoint_pub_ = this->create_publisher("mavros/setpoint_raw/global", 1); vehicle_pose_pub_ = this->create_publisher("vehicle_pose_marker", 1); - camera_pose_pub_ = this->create_publisher("camera_pose_marker", 1); planner_status_pub_ = this->create_publisher("planner_status", 1); - viewpoint_pub_ = this->create_publisher("viewpoints", 1); - planned_viewpoint_pub_ = this->create_publisher("planned_viewpoints", 1); path_segment_pub_ = this->create_publisher("path_segments", 1); tree_pub_ = this->create_publisher("tree", 1); @@ -139,8 +136,6 @@ TerrainPlanner::TerrainPlanner() : Node("terrain_planner") { global_origin_sub_ = this->create_subscription( "mavros/global_position/gp_origin", mavros_position_qos, std::bind(&TerrainPlanner::mavGlobalOriginCallback, this, _1)); - image_captured_sub_ = this->create_subscription( - "mavros/camera/image_captured", 1, std::bind(&TerrainPlanner::mavImageCapturedCallback, this, _1)); setlocation_serviceserver_ = this->create_service( "/terrain_planner/set_location", std::bind(&TerrainPlanner::setLocationCallback, this, _1, _2)); @@ -299,77 +294,6 @@ void TerrainPlanner::cmdloopCallback() { publishPositionHistory(referencehistory_pub_, reference_position, referencehistory_vector_); tracking_error_ = reference_position - vehicle_position_; planner_enabled_ = true; - - auto time_now = this->get_clock()->now(); - double time_since_trigger_s = (time_now - last_triggered_time_).seconds(); - - // RCLCPP_INFO_STREAM(this->get_logger(), "time_now: " << time_now.seconds()); - // RCLCPP_INFO_STREAM(this->get_logger(), "last_triggered_time: " << last_triggered_time_.seconds()); - - if (time_since_trigger_s > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { - RCLCPP_INFO_STREAM(this->get_logger(), "time_since_trigger: " << time_since_trigger_s); - - 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 - auto image_capture_req = std::make_shared(); - image_capture_req->command = mavros_msgs::msg::CommandCode::DO_DIGICAM_CONTROL; - image_capture_req->param1 = 0; // id - image_capture_req->param2 = 0; // interval - image_capture_req->param3 = 0; // total images - image_capture_req->param4 = 0; // sequence number - image_capture_req->param5 = 1; // sequence number - image_capture_req->param6 = 0; // sequence number - image_capture_req->param7 = 0; // sequence number - - while (!msginterval_serviceclient_->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(this->get_logger(), - "Service [" << msginterval_serviceclient_->get_service_name() << "] not available."); - return; - } - - bool received_response = false; - using ServiceResponseFuture = rclcpp::Client::SharedFuture; - auto response_received_callback = [&received_response](ServiceResponseFuture future) { - auto result = future.get(); - received_response = true; - }; - auto future = msginterval_serviceclient_->async_send_request(image_capture_req, response_received_callback); - - //! @todo wait for response_received_callback to set done - //! @todo add timeout - while (!received_response) { - ; - } - - // return; - - // auto future = msginterval_serviceclient_->async_send_request(image_capture_req); - - // if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != - // rclcpp::FutureReturnCode::SUCCESS) - // { - // RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" - // << msginterval_serviceclient_->get_service_name() - // << "] failed."); - // return; - // } - } - /// 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_ = this->get_clock()->now(); - } } else { tracking_error_ = Eigen::Vector3d::Zero(); planner_enabled_ = false; @@ -389,7 +313,6 @@ void TerrainPlanner::cmdloopCallback() { publishVehiclePose(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_, mesh_resource_path_); publishVelocityMarker(vehicle_velocity_pub_, vehicle_position_, vehicle_velocity_); publishPositionHistory(posehistory_pub_, vehicle_position_, posehistory_vector_); - publishCameraView(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_); } void TerrainPlanner::statusloopCallback() { @@ -639,9 +562,6 @@ void TerrainPlanner::plannerloopCallback() { // 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) { @@ -1069,15 +989,6 @@ void TerrainPlanner::mavMissionCallback(const mavros_msgs::msg::WaypointList &ms } } -void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured & /*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(const std::shared_ptr req, std::shared_ptr res) { //! @todo(srmainwaring) consolidate duplicate code from here and TerrainPlanner::plannerloopCallback diff --git a/terrain_navigation_ros/src/visualization.cpp b/terrain_navigation_ros/src/visualization.cpp index 0ceddfed..81ae4ff3 100644 --- a/terrain_navigation_ros/src/visualization.cpp +++ b/terrain_navigation_ros/src/visualization.cpp @@ -68,71 +68,3 @@ void publishVehiclePose(rclcpp::Publisher::Shar marker.pose = vehicle_pose; pub->publish(marker); } - -visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, Eigen::Vector3d color) { - double scale{15}; // Size of the viewpoint markers - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Clock().now(); - marker.ns = "my_namespace"; - marker.id = id; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.action = visualization_msgs::msg::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; -} - -void publishCameraView(rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d &position, const Eigen::Vector4d &attitude) { - visualization_msgs::msg::Marker marker; - ViewPoint viewpoint(-1, position, attitude); - marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint); - pub->publish(marker); -} - -void publishViewpoints(rclcpp::Publisher::SharedPtr pub, - std::vector &viewpoint_vector, Eigen::Vector3d color) { - visualization_msgs::msg::MarkerArray msg; - - std::vector marker; - visualization_msgs::msg::Marker mark; - mark.action = visualization_msgs::msg::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); -}