just save gps when create points

dregner · Mar 22, 2023 · 8f29624 · 8f29624
1 parent e8916b5
commit 8f29624
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 6 deletions.
diff --git a/include/path_generator.hh b/include/path_generator.hh
@@ -32,8 +32,10 @@ private:
 
     /// file name to store WP
     std::ofstream saved_wp_;
+    std::ofstream saved_wp_gps_;
     std::string file_path_ = "/home/vant3d/Documents";
     std::string file_name_ = "pathGenerator_WP.csv";
+    std::string file_name_gps_ = "pathGenerator_WP_GPS.csv";
     /// Initialize parameters
     int vertical_pts_ = 4; // number of horizontal path
     float delta_altitude_ = 0.3; // 80% image overlap (CAMERA PARAMETER) TODO: Find a way to bring it from image.

diff --git a/launch/stereo_detection.launch b/launch/stereo_detection.launch
@@ -2,20 +2,21 @@
 
 <launch>
 
+
+
     <!-- Use YOLOv3 -->
     <arg name="network_param_file"         default="$(find darknet_ros)/config/yolov3-tiny-simulacro.yaml"/>
     <arg name="image" default="camera/rgb/image_raw" />
 
+    <!-- M210 stereo dispartyi node -->
+    <node pkg="riser_inspection" type="m210_stereo_depth" name="m210_stere_vga" output="screen">
+    </node>
     <!-- Include main launch file -->
     <include file="$(find darknet_ros)/launch/darknet_ros.launch">
         <arg name="network_param_file"    value="$(arg network_param_file)"/>
         <arg name="image" value="$(arg image)" />
     </include>
 
-    <!-- M210 stereo dispartyi node -->
-    <node pkg="riser_inspection" type="m210_stereo_depth" name="m210_stere_vga" output="screen">
-    </node>
-
     <!-- Darknet simulacro detection and distance -->
     <node pkg="riser_inspection" type="darknet_disparity_node" name="darknet_distance" output="screen">
         <param name="darknet_topic"     type="string"   value="/darknet_ros/bounding_boxes"/>

diff --git a/src/path/path_generator.cpp b/src/path/path_generator.cpp
@@ -140,12 +140,12 @@ void PathGenerate::save_delta_cartesian() {
 
 void PathGenerate::save_gnss() {
     if (firstTime) {
-        saved_wp_ << "WP,LAT,LON,ALT,Yaw" << std::endl;
+        saved_wp_gps_ << "WP,LAT,LON,ALT,Yaw" << std::endl;
         firstTime = false;
     }
     if (saved_wp_.is_open()) {
         for (int k = 0; k < (int) gnss_points_.size(); k++) {
-            saved_wp_ << k + 1 << ","
+            saved_wp_gps_ << k + 1 << ","
                       << std::setprecision(10) << gnss_points_[k][0] << ","
                       << std::setprecision(10) << gnss_points_[k][1] << ","
                       << std::setprecision(4) << gnss_points_[k][2] << ","
@@ -182,6 +182,7 @@ void PathGenerate::createInspectionPoints(int csv_type) {
             break;
         case 2:
             PathGenerate::save_delta_cartesian();
+            PathGenerate::save_gnss();
             break;
         case 3:
             PathGenerate::save_gnss();
@@ -195,10 +196,12 @@ void PathGenerate::createInspectionPoints(int csv_type) {
 void PathGenerate::openFile() {
     std::string slash = "/";
     saved_wp_.open(file_path_ + slash + file_name_);
+    saved_wp_gps_.open(file_path_+slash+file_name_gps_);
 }
 
 void PathGenerate::closeFile() {
     saved_wp_.close();
+    saved_wp_gps_.close();
 }
 
 bool PathGenerate::exists(const std::string &name) {

diff --git a/src/ros/local_position_control.cpp b/src/ros/local_position_control.cpp
@@ -306,6 +306,7 @@ bool LocalController::generate_WP(int csv_type) {
     pathGenerator.setInspectionParam(riser_distance, (float) riser_diameter, h_points, v_points, delta_h,
                                      (float) delta_v);
     /** Define start positions to create waypoints */
+    pathGenerator.setInitCoord(current_gps.latitude, current_gps.longitude, rpa_height, (int) init_heading);
     pathGenerator.setInitCoord_XY(current_local_pos.point.x, current_local_pos.point.y, rpa_height,
                                   (int) init_heading);
     try {