fixing and tuning

ctu-mrs · Mar 24, 2024 · 7caf2e5 · 7caf2e5
1 parent 3f04ce1
commit 7caf2e5
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 84 deletions.
diff --git a/ros_packages/mrs_precise_landing/config/precise_landing.yaml b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
@@ -26,7 +26,7 @@ stages:
 
   aligning2:
 
-    timeout: 10.0 # [s]
+    timeout: 15.0 # [s]
 
     criterion:
 

diff --git a/ros_packages/mrs_precise_landing/src/precise_landing.cpp b/ros_packages/mrs_precise_landing/src/precise_landing.cpp
@@ -1406,7 +1406,7 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e
 
         double alignemnt_held_for = (ros::Time::now() - aligning2_in_radius_time_).toSec();
 
-        if (alignemnt_held_for >= _aligning2_in_alignment_duration_) {
+        if (alignemnt_held_for > _aligning2_in_alignment_duration_) {
 
           ROS_INFO("[PreciseLanding]: alignment finished, LANDING");
 

diff --git a/ros_packages/mrs_precise_landing_gazebo/models/Apriltag_recursive1/model.config b/ros_packages/mrs_precise_landing_gazebo/models/Apriltag_recursive1/model.config
@@ -1,14 +1,13 @@
 <?xml version="1.0" ?>
 <model>
-    <name>Apriltag_recursive1</name>
-    <version>1.0</version>
-    <sdf version="1.6">model.sdf</sdf>
-
-      <author>
-       <name>Kenji Koide</name>
-       <email>koide@dei.unipd.it</email>
-      </author>
-
-    <description>Apriltag model</description>
-</model>
+  <name>Apriltag_recursive1</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Tomas Baca</name>
+    <email>tomas.baca@fel.cvut.cz</email>
+  </author>
 
+  <description>Apriltag model</description>
+</model>
diff --git a/ros_packages/mrs_precise_landing_gazebo/models/Apriltag_recursive1/model.sdf b/ros_packages/mrs_precise_landing_gazebo/models/Apriltag_recursive1/model.sdf
@@ -1,14 +1,13 @@
 <?xml version='1.0'?>
 <sdf version='1.6'>
   <model name='Apriltag_recursive1'>
-    <link name='main'>
+    <link name='apriltag_main'>
     <collision name='collision'>
-      <pose>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <geometry>
-        <plane>
-          <normal>0 0 1</normal>
-          <size>0.5 0.5</size>
-        </plane>
+        <box>
+          <size>0.5 0.5 0.01</size>
+        </box>
       </geometry>
       <surface>
         <friction>
@@ -28,7 +27,7 @@
       <max_contacts>10</max_contacts>
     </collision>
     <pose frame=''>0 0 0 0 0 0</pose>
-      <visual name='main_Visual'>
+      <visual name='apriltag_main_visual'>
         <geometry>
           <box>
           <size>0.5 0.5 0.01</size>

diff --git a/ros_packages/mrs_precise_landing_gazebo/tmux/config/custom_config.yaml b/ros_packages/mrs_precise_landing_gazebo/tmux/config/custom_config.yaml
@@ -6,6 +6,7 @@ mrs_uav_managers:
     state_estimators: [
       "gps_baro",
       "gps_garmin",
+      "ground_truth",
     ]
 
     initial_state_estimator: "gps_garmin" # will be used as the first state estimator
@@ -16,10 +17,11 @@ mrs_uav_managers:
     takeoff:
 
       during_takeoff:
-        controller: "MpcController"
+        controller: "Se3Controller"
         tracker: "LandoffTracker"
 
       after_takeoff:
-        controller: "MpcController" # more robust
-        # controller: "Se3Controller" # more precise
+        controller: "Se3Controller" # more precise
         tracker: "MpcTracker"
+
+      takeoff_height: 3.0
diff --git a/ros_packages/mrs_precise_landing_gazebo/tmux/rviz.rviz b/ros_packages/mrs_precise_landing_gazebo/tmux/rviz.rviz
@@ -299,7 +299,7 @@ Visualization Manager:
               Marker Topic: /uav1/control_manager/trajectory_original/markers
               Name: Markers
               Namespaces:
-                "": true
+                {}
               Queue Size: 100
               Value: true
           Enabled: true
@@ -423,7 +423,7 @@ Visualization Manager:
               Marker Topic: /uav1/control_manager/mpc_tracker/trajectory_processed/markers
               Name: Markers
               Namespaces:
-                "": true
+                {}
               Queue Size: 100
               Value: true
           Enabled: true
@@ -437,19 +437,15 @@ Visualization Manager:
           Marker Topic: /uav1/trajectory_generation/markers/final
           Name: final
           Namespaces:
-            markers/final_lines: true
-            markers/final_points: true
-            markers/final_triangles: true
+            {}
           Queue Size: 100
           Value: true
         - Class: rviz/MarkerArray
           Enabled: true
           Marker Topic: /uav1/trajectory_generation/markers/original
           Name: original
           Namespaces:
-            markers/original_lines: true
-            markers/original_points: true
-            markers/original_triangles: true
+            {}
           Queue Size: 100
           Value: true
       Enabled: true
@@ -573,17 +569,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7047972679138184
+      Pitch: 0.389797180891037
       Target Frame: uav1/fcu
-      Yaw: 5.283559799194336
+      Yaw: 5.22355842590332
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
   Height: 506
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000191000001a4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001a4000000c700fffffffb0000001400760069006f005f00630061006d0065007200610000000122000000bd0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001400000009f0000000000000000fb0000001400760069006f005f00630061006d00650072006100000001400000009f00000000000000000000000100000116000001a4fc0200000004fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003b000000e40000005c00fffffffb0000000a005600690065007700730000000125000000ba000000a000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000016d0000016e0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000038f0000003efc0100000002fb0000000800540069006d006500000000000000038f0000035700fffffffb0000000800540069006d0065010000000000000450000000000000000000000211000001a400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000191000001a4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000001a4000000c700fffffffb0000001400760069006f005f00630061006d0065007200610000000122000000bd0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001400000009f0000000000000000fb0000001400760069006f005f00630061006d00650072006100000001400000009f00000000000000000000000100000116000001a4fc0200000004fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003b000000e40000005c00fffffffb0000000a005600690065007700730000000125000000ba000000a000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000016d0000016e0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000038f0000003efc0100000002fb0000000800540069006d006500000000000000038f0000035700fffffffb0000000800540069006d00650100000000000004500000000000000000000003a8000001a400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/ros_packages/mrs_precise_landing_gazebo/tmux/session.yml b/ros_packages/mrs_precise_landing_gazebo/tmux/session.yml
@@ -15,7 +15,7 @@ windows:
   - gazebo:
       layout: tiled
       panes:
-        - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_file:=`rospack find mrs_precise_landing_gazebo`/worlds/apriltag_recursive.world gui:=true
+        - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_file:=`rospack find mrs_precise_landing_gazebo`/worlds/apriltag.world gui:=true
         - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --pos 0 0 0.5 0 --enable-rangefinder --enable-ground-truth --enable-bluefox-camera"
         - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME
   - status:
@@ -38,11 +38,12 @@ windows:
       layout: tiled
       panes:
         - waitForTime; roslaunch mrs_precise_landing precise_landing.launch apriltag_config:=`rospack find mrs_landing_pad_estimation`/config/apriltag_recursive.yaml camera_node:=bluefox_optflow image_topic:=image_raw estimator_config:=./config/landing_estimator.yaml controller_config:=./config/landing_controller.yaml
+        - waitForControl; rostopic echo /$UAV_NAME/ground_truth/pose/pose/position
   - land:
       layout: tiled
       panes:
         - 'history -s rosservice call /$UAV_NAME/precise_landing/land'
-        - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[1.0, 1.0, 5.0, 1.0\]\"'
+        - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 0.0, 4.0, 1.0\]\"'
   - takeoff:
       layout: tiled
       panes:

diff --git a/...ng_gazebo/worlds/apriltag_recursive.world → ...cise_landing_gazebo/worlds/apriltag.world b/...ng_gazebo/worlds/apriltag_recursive.world → ...cise_landing_gazebo/worlds/apriltag.world
@@ -1,3 +1,5 @@
+<?xml version="1.0" ?>
+<?xml-model href="http://sdformat.org/schemas/root.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <sdf version='1.7'>
 
   <world name='default'>
@@ -14,7 +16,9 @@
     </spherical_coordinates>
     <!--}-->
 
-    <physics name='default_physics' default='0' type='ode'>
+    <!-- physics engine {-->
+    <physics name="default_physics" default="0" type="ode">
+      <gravity>0 0 -9.8066</gravity>
       <ode>
         <solver>
           <type>quick</type>
@@ -32,18 +36,23 @@
       <max_step_size>0.004</max_step_size>
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>250</real_time_update_rate>
+      <magnetic_field>6.0e-06 2.3e-05 -4.2e-05</magnetic_field>
     </physics>
+    <!--}-->
 
+    <!-- setup shadows {-->
     <scene>
-      <shadows>1</shadows>
-      <ambient>0.4 0.4 0.4 1</ambient>
-      <background>0.7 0.7 0.7 1</background>
+      <shadows>false</shadows>
+      <!-- <sky> -->
+      <!--   <clouds/> -->
+      <!-- </sky> -->
     </scene>
+    <!--}-->
 
-    <!-- light {-->
+    <!-- sun {-->
 
     <light name='sun' type='directional'>
-      <pose>0 0 1000 0.4 0.2 0</pose>
+      <pose frame=''>0 0 1000 0.4 0.2 0</pose>
       <diffuse>1 1 1 1</diffuse>
       <specular>0.6 0.6 0.6 1</specular>
       <direction>0.1 0.1 -0.9</direction>
@@ -54,50 +63,10 @@
         <quadratic>0.001</quadratic>
       </attenuation>
       <cast_shadows>1</cast_shadows>
-      <spot>
-        <inner_angle>0</inner_angle>
-        <outer_angle>0</outer_angle>
-        <falloff>0</falloff>
-      </spot>
     </light>
 
     <!--}-->
 
-    <!-- gui {-->
-
-    <gui fullscreen='0'>
-      <camera name='camera'>
-        <pose>-6.06072 -2.40886 4.63304 0 0.244 -0.157996</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <!--}-->
-
-    <!-- cam synchronizer plugin {-->
-
-    <plugin name='mrs_gazebo_rviz_cam_synchronizer' filename='libMrsGazeboCommonResources_RvizCameraSynchronizer.so'>
-      <target_frame_id>gazebo_user_camera</target_frame_id>
-      <world_origin_frame_id>uav1/gps_origin</world_origin_frame_id>
-      <frame_to_follow>uav1</frame_to_follow>
-    </plugin>
-
-    <!--}-->
-
-    <gravity>0 0 -9.8066</gravity>
-
-    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
-
-    <atmosphere type='adiabatic'/>
-
-    <wind/>
-
-    <include>
-      <uri>model://Apriltag_recursive1</uri>
-      <pose>1 1 1 0 0 1.2</pose>
-    </include>
-
     <!-- ground plane {-->
 
     <model name='ground_plane'>
@@ -145,6 +114,27 @@
 
     <!--}-->
 
+    <include>
+      <uri>model://Apriltag_recursive1</uri>
+      <pose>1 1 1 0 0 1.2</pose>
+    </include>
+
+    <!-- user camera {-->
+    <gui>
+      <camera name="camera">
+        <pose>-60 -100 30 0 0.4 0.89</pose>
+      </camera>
+    </gui>
+    <!--}-->
+
+    <!-- GUI frame synchronization {-->
+    <plugin name="mrs_gazebo_rviz_cam_synchronizer" filename="libMrsGazeboCommonResources_RvizCameraSynchronizer.so" >
+      <target_frame_id>gazebo_user_camera</target_frame_id>
+      <world_origin_frame_id>uav1/gps_origin</world_origin_frame_id>
+      <frame_to_follow>uav1</frame_to_follow>
+    </plugin>
+    <!--}-->
+
   </world>
 
 </sdf>