agro_coverage_planner/config/planner.yaml

 # polygon_points: [[0, 0], [100,0], [100, 100], [0, 100]] #IN WORLD FRAME
-polygon_points: [[0,0], [83.5,0],[30,50], [53.5, 103], [-30, 10]] #IN WORLD FRAME
-row_width: 5
+# polygon_points: [[0,0], [83.5,0],[30,50], [53.5, 103], [-30, 10]] #IN WORLD FRAME
+# polygon_points: [[-4,4], [-5.2, 0.6],[-1.2, -1.7], [0, 3.7]] #IN WORLD FRAME
+# polygon_points: [[-2.5,4], [-2.7, -2.5],[0.5, -1], [0.7, 3.6]] #IN WORLD FRAME
+# polygon_points: [[-3.7, -70.0], [9.95,-16.60],[20.25, -19.0], [5.7, -72.15]] #POLY1
+polygon_points: [[12.7,-69], [170, -82],[176.8, -31.5], [26.8,-8.85]] #POLY2
+
+row_width: 4
+go_right: False
 
 image_taking_dist: 3
 planning_height: 4
 target_velocity: 1
-path_start_index: 0
-reaching_dist: 0.5
-target_heading: 0
-reaching_wait_time: 1
-
-fcu_frame: "uav1/fcu"
-map_frame: "uav1/world_origin"
-trajectory_generator_input_topic: "uav1/trajectory_generation/path"
+path_start_index: 13
+reaching_dist: 1
+target_heading: -0.11
+reaching_wait_time: 2
+
+fcu_frame: "uav38/fcu"
+map_frame: "uav38/world_origin"
+trajectory_generator_input_topic: "control_manager/reference"
+
+# fcu_frame: "uav38/fcu"
+# map_frame: "uav38/world_origin"
+# trajectory_generator_input_topic: "/uav38/control_manager/reference"

agro_coverage_planner/config/world_loucna.yaml

+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 50.31700427
+  origin_y: 14.23687475
+
+safety_area:
+
+  enabled: false
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "latlon_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      50.0765653, 14.4178914,
+      50.0762102, 14.4179722,
+      50.0763015, 14.4181847,
+      50.0765825, 14.4181411,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 4.0
+    min_z: 1.0

agro_coverage_planner/launch/planner.launch

 <launch>
+
+    <arg name="UAV_NAME" default="$(env UAV_NAME)"/>
+
+    <!-- Run ov_extension.py node -->
+    <group ns="$(arg UAV_NAME)">
+
     <!-- Load ROS parameters from the config file -->
     <rosparam command="load" file="$(find agro_coverage_planner)/config/planner.yaml"/>
 
     <!-- Run ov_extension.py node -->
     <node pkg="agro_coverage_planner" type="planner.py" name="agro_coverage_planner" output="screen"/>
+  </group>
+
 </launch>

agro_coverage_planner/scripts/planner.py

@@ -6,6 +6,7 @@ import matplotlib.pyplot as plt
 from shapely.geometry import LineString, Polygon, Point
 import std_msgs.msg
 import mrs_msgs.msg
+import mrs_msgs.srv
 import geometry_msgs.msg
 import tf
 import time
@@ -24,7 +25,9 @@ class PlannerNode:
         # self.global_nav_timer = rospy.Timer(rospy.Duration(1.0 / self.global_nav_frequency), self.global_nav_loop_iter)
 
         # VIS PUB
-        self.path_for_trajectory_generator_pub = rospy.Publisher(rospy.get_param("trajectory_generator_input_topic"), mrs_msgs.msg.Path, queue_size=10)
+        # self.path_for_trajectory_generator_pub = rospy.Publisher(rospy.get_param("trajectory_generator_input_topic"), mrs_msgs.msg.Path, queue_size=10)
+        self.path_for_trajectory_generator_pub = rospy.Publisher(rospy.get_param("trajectory_generator_input_topic"), mrs_msgs.msg.ReferenceStamped, queue_size=10)
+        self.ref_srv = rospy.ServiceProxy("control_manager/reference", mrs_msgs.srv.ReferenceStampedSrv)
 
         # self.path_planning_vis_pub = rospy.Publisher('path_planning_vis', MarkerArray, queue_size=10)
         # self.visual_similarity_vis_pub = rospy.Publisher('visual_similarity_vis', MarkerArray, queue_size=10)
@@ -34,7 +37,7 @@ class PlannerNode:
 
         # --Load calib
         self.polygon_pts = rospy.get_param("polygon_points")
-        self.row_width = rospy.get_param("polygon_points")
+        self.row_width = rospy.get_param("row_width")
         self.path_index = rospy.get_param("path_start_index")
         self.planning_height = rospy.get_param("planning_height")
         self.fcu_frame = rospy.get_param("fcu_frame")
@@ -43,6 +46,7 @@ class PlannerNode:
         self.target_heading = rospy.get_param("target_heading")
         self.target_velocity = rospy.get_param("target_velocity")
         self.reaching_wait_time = rospy.get_param("reaching_wait_time")
+        self.go_right = rospy.get_param("go_right")
 
 
 
@@ -54,8 +58,6 @@ class PlannerNode:
         # self.marker_scale = rospy.get_param("marker_scale")
         # self.using_external_slam_pts = rospy.get_param("local_mapping/using_external_slam_pts")
 
-        self.node_initialized = True
-
 
         polygon_pts = np.array(self.polygon_pts)
         # polygon_pts = np.array([[0, 0], [10,0], [10, 11], [0, 11]])
@@ -63,7 +65,7 @@ class PlannerNode:
         # polygon_pts = np.array([[0, 0], [150,0], [100, 100], [0, 80]])
         # polygon_pts = np.array([[0, 0], [150,0], [100, 100], [0, 80]])
         
-        res_pts = self.plan_coverage_path(polygon_pts, polygon_pts[0], polygon_pts[1], 5, False)
+        res_pts = self.plan_coverage_path(polygon_pts, polygon_pts[0], polygon_pts[1], self.row_width, self.go_right)
         print("VISUALIZING PATH TO CONFIRM MISSION")
         self.visualize_pts_and_polygon(polygon_pts, res_pts)
 
@@ -75,36 +77,41 @@ class PlannerNode:
 
         # START THE MAIN MISSION EXECUTION TIMER
         self.tf_listener = tf.TransformListener()
-        mainloop_f = 10
+        mainloop_f = 1
         self.path_points = res_pts
         self.done = False
 
-        self.return_home_when_done = False
+        self.return_home_when_done = True
         self.home_pos = None
         self.force_sending_goal = True
 
         self.global_nav_timer = rospy.Timer(rospy.Duration(1.0 / mainloop_f ), self.mainloop_iter)
 
         # plan_coverage_path(self.polygon_pts, self.polygon_pts[0], self.polygon_pts[1], self.row_width)
+        self.node_initialized = True
 
         # # #}
 
 
     def mainloop_iter(self, event):
-        if self.done:
+        if self.done or not self.node_initialized:
             return
 
         next_pt_2d = self.path_points[self.path_index, :].flatten()
         next_pt_3d = np.array([next_pt_2d[0], next_pt_2d[1], self.planning_height]).flatten()
+        if self.path_index == 0:
+            self.home_pos = next_pt_3d
 
         # CHECK IF REACHED NEXT PT
         (trans, rotation) = self.tf_listener.lookupTransform(self.map_frame, self.fcu_frame, rospy.Time(0)) #Time0 = latest
         # print(trans)
         current_pos = np.array(trans)
 
-        dist = np.linalg.norm(next_pt_3d - current_pos)
+        deltavec = (next_pt_3d - current_pos).flatten()
+        deltavec[2] = 0
+        dist = np.linalg.norm(deltavec)
         traj_generator_point = None
-        # print("DIST: " + str(dist))
+        print("DIST: " + str(dist) + " INDEX: " + str(self.path_index))
 
         # TODO -rather check if standing
         if self.force_sending_goal:
@@ -117,39 +124,49 @@ class PlannerNode:
             print("WAITING " + str(self.reaching_wait_time) + "s")
             time.sleep(self.reaching_wait_time)
             if(self.path_index == self.path_points.shape[0]):
-                print("DONE, SETTING HOME POINT AS GOAL")
+                print("DONE")
                 self.done = True
                 if self.return_home_when_done:
-                    traj_generator_point = home_pos.flatten()
-            else:
-                next_pt_2d = self.path_points[self.path_index, :].flatten()
-                traj_generator_point = np.array([next_pt_2d[0], next_pt_2d[1], self.planning_height]).flatten()
+                    print("DONE, SETTING HOME POINT AS GOAL")
+                    traj_generator_point = self.home_pos.flatten()
+        elif self.path_index != self.path_points.shape[0]:
+            next_pt_2d = self.path_points[self.path_index, :].flatten()
+            traj_generator_point = np.array([next_pt_2d[0], next_pt_2d[1], self.planning_height]).flatten()
+
+            # else:
+            #     next_pt_2d = self.path_points[self.path_index, :].flatten()
+            #     traj_generator_point = np.array([next_pt_2d[0], next_pt_2d[1], self.planning_height]).flatten()
                 
         # SEND NEW PT TO TRAJ GENERATION
         if not traj_generator_point is None:
             print("SENDING PT TO TRAJ GENERATOR: ")
             print(traj_generator_point)
 
-            msg = mrs_msgs.msg.Path()
-            msg.header = std_msgs.msg.Header()
-            msg.header.frame_id = self.map_frame
-            msg.header.stamp = rospy.Time.now()
-            msg.use_heading = True
-            msg.fly_now = True
-            msg.stop_at_waypoints = False
             # msg.override_max_velocity_horizontal = self.target_velocity
 
             xx = traj_generator_point[0]
             yy = traj_generator_point[1]
             zz = traj_generator_point[2]
 
-            ref = mrs_msgs.msg.Reference()
-            ref.position = geometry_msgs.msg.Point(x=xx, y=yy, z=zz)
-            ref.heading = self.target_heading
+            # ref = mrs_msgs.msg.ReferenceStamped()
+            ref = mrs_msgs.srv.ReferenceStampedSrvRequest()
+            ref.reference.position = geometry_msgs.msg.Point(x=xx, y=yy, z=zz)
+            ref.reference.heading = self.target_heading
+
+            # ref = mrs_msgs.msg.Path()
+            ref.header = std_msgs.msg.Header()
+            ref.header.frame_id = self.map_frame
+            ref.header.stamp = rospy.Time.now()
+            # ref.use_heading = True
+            # ref.fly_now = True
+            # ref.stop_at_waypoints = False
+
+            # msg.points.append(ref)
 
-            msg.points.append(ref)
+            # self.path_for_trajectory_generator_pub.publish(msg)
+            # self.path_for_trajectory_generator_pub.publish(ref)
+            self.ref_srv(ref)
 
-            self.path_for_trajectory_generator_pub.publish(msg)
             print("SENT!")
 
     def plan_coverage_path(self, polygon_pts, firstrow_start, firstrow_end, row_width, go_right_from_first_row = True, visualize_plt = True):# # #{
@@ -203,7 +220,7 @@ class PlannerNode:
             print(intersections_out)
     
             #if cant hit polygon in any dir -> OUTSIDE OF POLYGON IN THE DIR OF TRAVEL! END!
-            if intersections_in.is_empty and intersections_out.is_empty:
+            if intersections_in.is_empty:
                 print("REACHED END WITH N_PTS: " + str(len(res)) + ", N_ROWS: " + str(n_rows))
                 return np.array(res)
     

agro_coverage_planner/tmux/loucna/config/custom_config.yaml

+mrs_uav_managers:
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, aloam, ground_truth, dummy
+    state_estimators: [
+    # "gps_baro",
+    "gps_garmin",
+    "rtk",
+    # "aloam",
+    # "ground_truth",
+    # "dummy"
+    ]
+
+    initial_state_estimator: "rtk" # will be used as the first state estimator
+    agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback)

agro_coverage_planner/tmux/loucna/record.sh

+#!/bin/bash
+
+path="/home/\$(optenv USER mrs)/bag_files/latest/"
+
+# By default, we record everything.
+# Except for this list of EXCLUDED topics:
+exclude=(
+
+# IN GENERAL, DON'T RECORD CAMERAS
+#
+# If you want to record cameras, create a copy of this script
+# and place it at your tmux session.
+#
+# Please, seek an advice of a senior researcher of MRS about
+# what can be recorded. Recording too much data can lead to
+# ROS communication hiccups, which can lead to eland, failsafe
+# or just a CRASH.
+
+# Every topic containing "compressed"
+# '(.*)compressed(.*)'
+# Every topic containing "image_raw"
+# '(.*)image_raw(.*)'
+# Every topic containing "theora"
+# '(.*)theora(.*)'
+# Every topic containing "h264"
+# '(.*)h264(.*)'
+
+)
+
+# file's header
+filename=`mktemp`
+echo "<launch>" > "$filename"
+echo "<arg name=\"UAV_NAME\" default=\"\$(env UAV_NAME)\" />" >> "$filename"
+echo "<group ns=\"\$(arg UAV_NAME)\">" >> "$filename"
+
+echo -n "<node pkg=\"rosbag\" type=\"record\" name=\"rosbag_record\" output=\"screen\" args=\"-o $path -a" >> "$filename"
+
+# if there is anything to exclude
+if [ "${#exclude[*]}" -gt 0 ]; then
+
+  echo -n " -x " >> "$filename"
+
+  # list all the string and separate the with |
+  for ((i=0; i < ${#exclude[*]}; i++));
+  do
+    echo -n "${exclude[$i]}" >> "$filename"
+    if [ "$i" -lt "$( expr ${#exclude[*]} - 1)" ]; then
+      echo -n "|" >> "$filename"
+    fi
+  done
+
+fi
+
+echo "\">" >> "$filename"
+
+echo "<remap from=\"~status_msg_out\" to=\"mrs_uav_status/display_string\" />" >> "$filename"
+echo "<remap from=\"~data_rate_out\" to=\"~data_rate_MB_per_s\" />" >> "$filename"
+
+# file's footer
+echo "</node>" >> "$filename"
+echo "</group>" >> "$filename"
+echo "</launch>" >> "$filename"
+
+cat $filename
+roslaunch $filename

agro_coverage_planner/tmux/loucna/tmux.sh

+#!/bin/bash
+### BEGIN INIT INFO
+# Provides: tmux
+# Required-Start:    $local_fs $network dbus
+# Required-Stop:     $local_fs $network
+# Default-Start:     2 3 4 5
+# Default-Stop:      0 1 6
+# Short-Description: start the uav
+### END INIT INFO
+if [ "$(id -u)" == "0" ]; then
+  exec sudo -u mrs "$0" "$@"
+fi
+
+source $HOME/.bashrc
+
+# location for storing the bag files
+# * do not change unless you know what you are doing
+MAIN_DIR=~/"bag_files"
+
+# the project name
+# * is used to define folder name in ~/$MAIN_DIR
+PROJECT_NAME=just_flying
+
+# the name of the TMUX session
+# * can be used for attaching as 'tmux a -t <session name>'
+SESSION_NAME=mav
+
+# following commands will be executed first in each window
+# * do NOT put ; at the end
+pre_input=""
+
+# define commands
+# 'name' 'command'
+# * DO NOT PUT SPACES IN THE NAMES
+# * "new line" after the command    => the command will be called after start
+# * NO "new line" after the command => the command will wait for user's <enter>
+input=(
+  'Rosbag' 'waitForOffboard; ./record.sh
+'
+  'Sensors' 'waitForTime; roslaunch mrs_uav_deployment sensors.launch
+'
+  'Nimbro' 'waitForTime; rosrun mrs_uav_deployment run_nimbro.py ./config/network_config.yaml `rospack find mrs_uav_deployment`/config/communication_config.yaml
+'
+  'HwApi' 'waitForTime; roslaunch mrs_uav_px4_api api.launch
+'
+  'Status' 'waitForHw; roslaunch mrs_uav_status status.launch
+'
+  'Core' 'waitForTime; roslaunch mrs_uav_core core.launch platform_config:=`rospack find mrs_uav_deployment`/config/mrs_uav_system/$UAV_TYPE.yaml world_config:=`rospack find mrs_uav_deployment`/config/worlds/world_$WORLD_NAME.yaml custom_config:=./config/custom_config.yaml network_config:=./config/network_config.yaml
+'
+  'Agro' 'roslaunch agro_coverage_planner planner.launch'
+  'AutoStart' 'waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+'
+# do NOT modify the command list below
+  'EstimDiag' 'waitForHw; rostopic echo /'"$UAV_NAME"'/estimation_manager/diagnostics
+'
+  'kernel_log' 'tail -f /var/log/kern.log -n 100
+'
+  'roscore' 'roscore
+'
+  'simtime' 'waitForRos; rosparam set use_sim_time false
+'
+)
+
+# the name of the window to focus after start
+init_window="Status"
+
+# automatically attach to the new session?
+# {true, false}, default true
+attach=true
+
+###########################
+### DO NOT MODIFY BELOW ###
+###########################
+
+export TMUX_BIN="/usr/bin/tmux -L mrs -f /etc/ctu-mrs/tmux.conf"
+
+# find the session
+FOUND=$( $TMUX_BIN ls | grep $SESSION_NAME )
+
+if [ $? == "0" ]; then
+  echo "The session already exists"
+  $TMUX_BIN -2 attach-session -t $SESSION_NAME
+  exit
+fi
+
+# Absolute path to this script. /home/user/bin/foo.sh
+SCRIPT=$(readlink -f $0)
+# Absolute path this script is in. /home/user/bin
+SCRIPTPATH=`dirname $SCRIPT`
+
+TMUX= $TMUX_BIN new-session -s "$SESSION_NAME" -d
+echo "Starting new session."
+
+# get the iterator
+ITERATOR_FILE="$MAIN_DIR/$PROJECT_NAME"/iterator.txt
+if [ -e "$ITERATOR_FILE" ]
+then
+  ITERATOR=`cat "$ITERATOR_FILE"`
+  ITERATOR=$(($ITERATOR+1))
+else
+  echo "iterator.txt does not exist, creating it"
+  mkdir -p "$MAIN_DIR/$PROJECT_NAME"
+  touch "$ITERATOR_FILE"
+  ITERATOR="1"
+fi
+echo "$ITERATOR" > "$ITERATOR_FILE"
+
+# create file for logging terminals' output
+LOG_DIR="$MAIN_DIR/$PROJECT_NAME/"
+SUFFIX=$(date +"%Y_%m_%d_%H_%M_%S")
+SUBLOG_DIR="$LOG_DIR/"$ITERATOR"_"$SUFFIX""
+TMUX_DIR="$SUBLOG_DIR/tmux"
+mkdir -p "$SUBLOG_DIR"
+mkdir -p "$TMUX_DIR"
+
+# link the "latest" folder to the recently created one
+rm "$LOG_DIR/latest" > /dev/null 2>&1
+rm "$MAIN_DIR/latest" > /dev/null 2>&1
+ln -sf "$SUBLOG_DIR" "$LOG_DIR/latest"
+ln -sf "$SUBLOG_DIR" "$MAIN_DIR/latest"
+
+# create arrays of names and commands
+for ((i=0; i < ${#input[*]}; i++));
+do
+  ((i%2==0)) && names[$i/2]="${input[$i]}"
+  ((i%2==1)) && cmds[$i/2]="${input[$i]}"
+done
+
+# run tmux windows
+for ((i=0; i < ${#names[*]}; i++));
+do
+  $TMUX_BIN new-window -t $SESSION_NAME:$(($i+1)) -n "${names[$i]}"
+done
+
+sleep 3
+
+# start loggers
+for ((i=0; i < ${#names[*]}; i++));
+do
+  $TMUX_BIN pipe-pane -t $SESSION_NAME:$(($i+1)) -o "ts | cat >> $TMUX_DIR/$(($i+1))_${names[$i]}.log"
+done
+
+# send commands
+for ((i=0; i < ${#cmds[*]}; i++));
+do
+  $TMUX_BIN send-keys -t $SESSION_NAME:$(($i+1)) "cd $SCRIPTPATH;${pre_input};${cmds[$i]}"
+done
+
+# identify the index of the init window
+init_index=0
+for ((i=0; i < ((${#names[*]})); i++));
+do
+  if [ ${names[$i]} == "$init_window" ]; then
+    init_index=$(expr $i + 1)
+  fi
+done
+
+$TMUX_BIN select-window -t $SESSION_NAME:$init_index
+
+if $attach; then
+
+  if [ -z ${TMUX} ];
+  then
+    $TMUX_BIN -2 attach-session -t $SESSION_NAME
+  else
+    tmux detach-client -E "tmux -L mrs a -t $SESSION_NAME"
+  fi
+else
+  echo "The session was started"
+  echo "You can later attach by calling:"
+  echo "  tmux -L mrs a -t $SESSION_NAME"
+fi

agro_coverage_planner/tmux/simulation/config/network_config.yaml

+# 1. This list is used by NimbroNetwork for mutual communication of the UAVs
+#    The names of the robots have to match hostnames described in /etc/hosts.
+#
+# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs.
+#    The names should match the true "UAV_NAMES" (the topic prefixes).
+#
+#     network_config:=~/config/network_config.yaml
+#
+#    to the core.launch and nimbro.launch.
+
+network:
+
+  robot_names: [
+    uav1,
+  ]