ros2: remove viewpoint object (#29)

Co-authored-by: JaeyoungLim <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

Author: Jaeyoung-Lim

terrain_navigation/include/terrain_navigation/viewpoint.h

@@ -43,7 +43,6 @@
 #define TERRAIN_PLANNER_H
 
 #include <terrain_navigation/profiler.h>
-#include <terrain_navigation/viewpoint.h>
 #include <terrain_planner/common.h>
 #include <terrain_planner/terrain_ompl_rrt.h>
 #include <terrain_planner/visualization.h>
@@ -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<planner_msgs::srv::SetString::Request> req,
                            std::shared_ptr<planner_msgs::srv::SetString::Response> res);
@@ -177,7 +175,6 @@ class TerrainPlanner : public rclcpp::Node {
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr vehicle_path_pub_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr grid_map_pub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_pose_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr camera_pose_pub_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr posehistory_pub_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr referencehistory_pub_;
   rclcpp::Publisher<mavros_msgs::msg::GlobalPositionTarget>::SharedPtr global_position_setpoint_pub_;
@@ -187,8 +184,6 @@ class TerrainPlanner : public rclcpp::Node {
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr rallypoint_pub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr candidate_goal_pub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr candidate_start_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr viewpoint_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr planned_viewpoint_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr tree_pub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_velocity_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_segment_pub_;
@@ -199,7 +194,6 @@ class TerrainPlanner : public rclcpp::Node {
   rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr mavstate_sub_;
   rclcpp::Subscription<mavros_msgs::msg::WaypointList>::SharedPtr mavmission_sub_;
   rclcpp::Subscription<geographic_msgs::msg::GeoPointStamped>::SharedPtr global_origin_sub_;
-  rclcpp::Subscription<mavros_msgs::msg::CameraImageCaptured>::SharedPtr image_captured_sub_;
 
   rclcpp::Service<planner_msgs::srv::SetString>::SharedPtr setlocation_serviceserver_;
   rclcpp::Service<planner_msgs::srv::SetString>::SharedPtr setmaxaltitude_serviceserver_;
@@ -257,11 +251,8 @@ class TerrainPlanner : public rclcpp::Node {
   std::mutex goal_mutex_;  // protects g_i
 
   std::vector<Eigen::Vector3d> vehicle_position_history_;
-  std::vector<ViewPoint> added_viewpoint_list;
-  std::vector<ViewPoint> planned_viewpoint_list;
   std::vector<geometry_msgs::msg::PoseStamped> posehistory_vector_;
   std::vector<geometry_msgs::msg::PoseStamped> referencehistory_vector_;
-  std::vector<ViewPoint> 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)};

terrain_navigation/include/terrain_navigation/visualization.h

@@ -34,8 +34,6 @@
 #ifndef TERRAIN_NAVIGATION_ROS_VISUALIZATION_H
 #define TERRAIN_NAVIGATION_ROS_VISUALIZATION_H
 
-#include <terrain_navigation/viewpoint.h>
-
 #include <Eigen/Dense>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -53,14 +51,4 @@ void publishVehiclePose(rclcpp::Publisher<visualization_msgs::msg::Marker>::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<visualization_msgs::msg::Marker>::SharedPtr pub,
-                       const Eigen::Vector3d &position, const Eigen::Vector4d &attitude);
-
-void publishViewpoints(rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub,
-                       std::vector<ViewPoint> &viewpoint_vector,
-                       Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0));
-
 #endif