Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[tag tracking] Compute tag orientation. (#3313)

Browse Source 
* [tag tracking] Compute tag orientation.

* [tag tracking] it works

* Fix errors and allow tracking any tag

* improve code readability

* tag tracking doc

* no need for the size

* [tag tracking] silence unused variable warning.

---------

Co-authored-by: Fabien-B <Fabien-B@github.com>
This commit is contained in:
Fabien-B
2024-06-22 16:34:45 +02:00
committed by GitHub
parent 7100d157c7
commit 8fa8415117
6 changed files with 82 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+1 -2
View File
@@ -236,14 +236,13 @@
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
<define name="BODY_TO_CAM_PSI" value="0"/>
<define name="CAM_POS_Y" value="-0.12"/>
<define name="NB_WP_MAX" value="2"/>
<define name="WPS" type="array">
<field type="struct">
<field name="wp_id" value="WP_TAG"/>
<field name="tag_id" value="0"/>
</field>
<field type="struct">
<field name="wp_id" value="WP_ROVER"/>
<field name="wp_id" value="WP_TRUCK"/>
<field name="tag_id" value="4"/>
</field>
</define>
conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml
+4 -2
View File
@@ -104,7 +104,10 @@
<call_once fun="jevois_stream(false)"/>
<stay wp="STDBY"/>
</block>
<block name="Stay Tag" strip_button="Stay Tag" group="tag">
<call_once fun="jevois_stream(true)"/>
<stay wp="TAG" height="search_height" pre_call="nav_set_heading_deg(tag_tracking_get_heading(11))"/>
</block>
<block name="Search Tag" strip_button="Search Target" group="tag">
<call_once fun="jevois_stream(true)"/>
<call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, 1, NS)"/>
@@ -115,7 +118,6 @@
<exception cond="rectangle_survey_sweep_num == 1" deroute="Standby"/>
<call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S3)"/>
</block>
<block name="Follow Tag" strip_button="Follow Tag" group="tag">
<call_once fun="jevois_stream(true)"/>
<set var="tag_tracking_get(0)->motion_type" value="TAG_TRACKING_MOVING"/>
conf/flight_plans/competitions/IMAV2022_drop.xml
+14 -14
View File
@@ -273,7 +273,7 @@
<call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
<call_once fun="NavSetWaypointHere(WP_TAG)"/>
<call_once fun="jevois_stream(true)"/>
<stay wp="DELIVERY" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<stay wp="DELIVERY" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<deroute block="Drop Package"/>
</block>
@@ -287,7 +287,7 @@
<call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
<call_once fun="NavSetWaypointHere(WP_TAG)"/>
<call_once fun="jevois_stream(true)"/>
<stay wp="SILENT" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<stay wp="SILENT" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<deroute block="Drop Package"/>
</block>
@@ -315,7 +315,7 @@
<call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
<call_once fun="NavSetWaypointHere(WP_TAG)"/>
<call_once fun="jevois_stream(true)"/>
<stay wp="UNCERTAIN" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<stay wp="UNCERTAIN" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<call_once fun="NavSetWaypointHere(WP_UNCERTAIN)"/>
<deroute block="Drop Package"/>
</block>
@@ -330,13 +330,13 @@
<call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
<call_once fun="NavSetWaypointHere(WP_TAG)"/>
<call_once fun="jevois_stream(true)"/>
<stay wp="AREA_TAG" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<stay wp="AREA_TAG" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
<deroute block="Drop Package"/>
</block>
<block name="Goto Truck">
<set var="mission_nb" value="DROP_NONE"/>
<set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
<set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_MOVING"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<!--call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/-->
@@ -348,13 +348,13 @@
<call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
<call_once fun="NavSetWaypointHere(WP_TAG)"/>
<stay wp="TRUCK" until="stage_time @GT 5" height="truck_height"/>
<stay wp="TRUCK" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="truck_height"/>
<stay wp="TRUCK" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="truck_height"/>
<deroute block="Drop Package on Truck"/>
</block>
<block name="Run Search Uncertain">
<set var="mission_nb" value="DROP_NONE"/>
<exception cond="tag_tracking.status == TAG_TRACKING_RUNNING" deroute="Drop Package"/>
<exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING" deroute="Drop Package"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
@@ -387,8 +387,8 @@
<block name="Run Search Truck">
<set var="mission_nb" value="DROP_NONE"/>
<exception cond="tag_tracking.status == TAG_TRACKING_RUNNING" deroute="Drop Package on Truck"/>
<set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
<exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING" deroute="Drop Package on Truck"/>
<set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_MOVING"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
@@ -418,21 +418,21 @@
</block>
<block name="Drop Package">
<stay wp="TAG" until="(stage_time @GT 2 @AND tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 5)" height="search_height"/>
<stay wp="TAG" until="(stage_time @GT 2 @AND tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 5)" height="search_height"/>
<stay wp="TAG" climb="-0.5" vmode="climb" until="((tag_distance @LT drop_height) @AND tag_valid) @OR (GetPosHeight() @LT drop_height/2.)" post_call="tag_valid = false"/>
<call_once fun="DropOpen()"/>
<stay wp="TAG" until="stage_time @GT 3" height="goto_height"/>
<set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
<set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
<deroute block="Land"/>
</block>
<block name="Drop Package on Truck">
<exception cond="tag_tracking.status == TAG_TRACKING_SEARCHING @OR tag_tracking.status == TAG_TRACKING_LOST" deroute="Drop on Truck fallback"/>
<exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_SEARCHING @OR tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_LOST" deroute="Drop on Truck fallback"/>
<stay wp="TAG" until="stage_time @GT 3" height="truck_height"/>
<stay wp="TAG" climb="-0.5" vmode="climb" until="((tag_distance @LT drop_height+0.5) @AND tag_valid) @OR (GetPosHeight() @LT drop_height+1.)" post_call="tag_valid = false"/>
<call_once fun="DropOpen()"/>
<stay wp="TAG" until="stage_time @GT 3" height="goto_height"/>
<set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
<set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
<set var="follow_follow_wp_status" value="MODULES_STOP"/>
<deroute block="Land"/>
</block>
@@ -442,7 +442,7 @@
<stay wp="TRUCK" climb="-0.5" vmode="climb" until="GetPosHeight() @LT drop_height+1.0"/>
<call_once fun="DropOpen()"/>
<stay wp="TRUCK" until="stage_time @GT 3" height="goto_height"/>
<set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
<set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
<set var="follow_follow_wp_status" value="MODULES_STOP"/>
<deroute block="Land"/>
</block>
conf/modules/tag_tracking.xml
+2 -2
View File
@@ -2,11 +2,11 @@
<module name="tag_tracking" dir="computer_vision">
<doc>
<description>
Track poistion of a tag (ArUco, QRcode, ...) detected by an onboard camera
Track position of a tag (ArUco, QRcode, ...) detected by an onboard camera
The tag detection and pose computation is done outside of the module, only the estimation by fusion of AHRS and visual detection with a Kalman filter is performed in this module
</description>
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
<define name="WP" value="WP_TAG" description="if defined, update the position of the waypoint at the location of the tag; if TAG waypoint exists in flight plan, it is used by default"/>
<define name="WPS" value="[{.wp_id=WP_TAG, .tag_id=42}, ...]" description="Array of structs. If defined, update the position of the waypoints at the location of the tags"/>
<define name="SIM_WP" value="WP_TARGET" description="designate a waypoint to be used as virtual tag in simulation; if TARGET waypoint exists in flight plan, it is used by default"/>
<define name="BODY_TO_CAM_PHI" value="0." description="rotation from body to cam frame (phi)"/>
<define name="BODY_TO_CAM_THETA" value="0." description="rotation from body to cam frame (theta)"/>