fixed avoidance bug

ctu-mrs · Apr 21, 2024 · 1c5d5ce · 1c5d5ce
1 parent 6a885b4
commit 1c5d5ce
Show file tree

Hide file tree

Showing 5 changed files with 1,074 additions and 4 deletions.
diff --git a/src/mpc_tracker/mpc_tracker.cpp b/src/mpc_tracker/mpc_tracker.cpp
@@ -1608,7 +1608,7 @@ void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::Co
     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
 
     // update the diagnostics
-    other_uav_avoidance_trajectories_.emplace(trajectory.uav_name, trajectory);
+    other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
   }
 }
 
@@ -1631,7 +1631,7 @@ void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnosti
   {
     std::scoped_lock lock(mutex_other_uav_diagnostics_);
 
-    other_uav_diagnostics_.emplace(diagnostics.uav_name, diagnostics);
+    other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
   }
 }
 
@@ -1790,6 +1790,7 @@ double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
         }
       }
     }
+
     u++;
   }
 

diff --git a/test/mpc_tracker/mpc_avoidance_two_should_avoid/config/mrs_simulator.yaml b/test/mpc_tracker/mpc_avoidance_two_should_avoid/config/mrs_simulator.yaml
@@ -13,15 +13,15 @@ uav_names: [
 uav1:
   type: "x500"
   spawn:
-    x: 0.0
+    x: 5.0
     y: 0.0
     z: 5.0
     heading: 0.0
 
 uav2:
   type: "x500"
   spawn:
-    x: 5.0
+    x: 0.0
     y: 0.0
     z: 5.0
     heading: 0.0
diff --git a/test/mpc_tracker/mpc_avoidance_two_should_avoid/mpc_avoidance_two_should_avoid.test b/test/mpc_tracker/mpc_avoidance_two_should_avoid/mpc_avoidance_two_should_avoid.test
@@ -46,6 +46,9 @@
     <arg name="UAV_NAME" default="uav2" />
   </include>
 
+  <!-- <node pkg="rviz" type="rviz" name="rviz1" args="-d $(dirname)/rviz1.rviz" /> -->
+  <!-- <node pkg="rviz" type="rviz" name="rviz2" args="-d $(dirname)/rviz2.rviz" /> -->
+
   <test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="60.0">
     <param name="test" value="$(arg test_name)" />
   </test>