Skip to content

Commit 68bbd2e

Browse files
Merge emmn nav (#91)
* building * no valid path * workign * obstacle avoidance * spiral * script * cleanup * more cleanup and testing * working * rover updates * added mag heading * added parameters * declination * mag_heading * building * remove old minimal Nav * fix cmake * more fixes for real life testing * removed heading * Delete .claude/settings.local.json * update vector field planner and mission executive * update yolo defaults * added new aruco and led support * obstacle avoidance update * intermediary testing * proper aruco support * ArUco visual-servo approach + sim camera/detector tuning --------- Co-authored-by: UMDLoop Jetson Orin NX <umdloop@gmail.com>
1 parent d687af3 commit 68bbd2e

52 files changed

Lines changed: 6055 additions & 24 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎.gitignore‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -208,4 +208,7 @@ cython_debug/
208208
*.app
209209

210210
.DS_Store
211-
*.mcap
211+
*.mcap
212+
213+
# Local-only navigation sim test harness (not tracked)
214+
src/subsystems/navigation/nav_sim_harness/

‎src/athena_sensors/launch/sensors.launch.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ def generate_launch_description():
6161
'publish_tf': publish_odom, # this is really the odom tf, idk why its just called tf
6262
'publish_map_tf': publish_map,
6363
'publish_urdf': 'false', # we take care of this elsewhere
64-
'enable_gnss': 'true',
64+
'enable_gnss': enable_gnss,
6565
}.items(),
6666
condition=UnlessCondition(sim),
6767
)

‎src/bringup/package.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,9 @@
1919
<exec_depend>science_bringup</exec_depend>
2020
<exec_depend>description</exec_depend>
2121
<exec_depend>athena_planner</exec_depend>
22+
<exec_depend>led_indicator</exec_depend>
23+
<exec_depend>aruco_bt</exec_depend>
24+
<exec_depend>yolo_ros_bt</exec_depend>
2225
<exec_depend>robot_state_publisher</exec_depend>
2326
<exec_depend>xacro</exec_depend>
2427

6.01 KB
Loading
Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
3+
<model>
4+
<name>aruco_post</name>
5+
<version>1.0</version>
6+
<sdf version="1.6">model.sdf</sdf>
7+
8+
<author>
9+
<name>UMD Loop Navigation</name>
10+
</author>
11+
12+
<description>
13+
URC-style ArUco post: a 3-sided (triangular-prism) marker carrying three
14+
identical DICT_4X4_50 tag faces, one per side, mounted on a support post.
15+
Faces are 20x20 cm (15 cm marker + 2.5 cm white quiet zone). The tag texture
16+
is pre-generated once with nav_sim_harness/scripts/gen_aruco_texture.py and
17+
baked into materials/textures/ — nothing generates it at spawn time.
18+
Default tag id baked in = 4. Used by the nav_sim_harness Gazebo test harness.
19+
</description>
20+
21+
</model>
Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<!--
4+
3-sided ArUco post (URC mission post).
5+
6+
Geometry:
7+
* Support post: cylinder, r=0.03 m, from ground (z=0) up to z=0.9 m.
8+
* Three identical tag faces forming a triangular prism, centred at z=1.0 m.
9+
Each face is a thin 0.20 x 0.20 m panel (X=thickness, Y=width, Z=height)
10+
textured with the same DICT_4X4_50 marker on its outward face.
11+
The three panels sit at yaw 0 / 120 / 240 deg, offset radially by the
12+
equilateral-triangle apothem (0.20 / (2*sqrt(3)) = 0.057735 m) so their
13+
edges meet to form a closed prism.
14+
15+
Texture: materials/textures/aruco_4x4_50_id4.png — pre-baked, NOT generated
16+
at spawn time. Regenerate for a different id with
17+
nav_sim_harness/scripts/gen_aruco_texture.py and re-commit the PNG.
18+
19+
Model origin is the base of the post (z=0), so when spawned you set the
20+
include <pose> z to the ground height at the drop point.
21+
-->
22+
<model name="aruco_post">
23+
<static>true</static>
24+
<link name="link">
25+
26+
<!-- Support post -->
27+
<visual name="post_visual">
28+
<pose>0 0 0.45 0 0 0</pose>
29+
<geometry>
30+
<cylinder>
31+
<radius>0.03</radius>
32+
<length>0.9</length>
33+
</cylinder>
34+
</geometry>
35+
<material>
36+
<ambient>0.5 0.5 0.5 1</ambient>
37+
<diffuse>0.6 0.6 0.6 1</diffuse>
38+
<specular>0.1 0.1 0.1 1</specular>
39+
</material>
40+
</visual>
41+
<collision name="post_collision">
42+
<pose>0 0 0.45 0 0 0</pose>
43+
<geometry>
44+
<cylinder>
45+
<radius>0.03</radius>
46+
<length>0.9</length>
47+
</cylinder>
48+
</geometry>
49+
</collision>
50+
51+
<!-- Tag face 0 (yaw 0) -->
52+
<visual name="tag_face_0">
53+
<pose>0.057735 0 1.0 0 0 0</pose>
54+
<geometry>
55+
<box>
56+
<size>0.005 0.20 0.20</size>
57+
</box>
58+
</geometry>
59+
<material>
60+
<ambient>1 1 1 1</ambient>
61+
<diffuse>1 1 1 1</diffuse>
62+
<pbr>
63+
<metal>
64+
<albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
65+
<metalness>0.0</metalness>
66+
<roughness>1.0</roughness>
67+
</metal>
68+
</pbr>
69+
</material>
70+
</visual>
71+
72+
<!-- Tag face 1 (yaw 120 deg) -->
73+
<visual name="tag_face_1">
74+
<pose>-0.0288675 0.05 1.0 0 0 2.0943951</pose>
75+
<geometry>
76+
<box>
77+
<size>0.005 0.20 0.20</size>
78+
</box>
79+
</geometry>
80+
<material>
81+
<ambient>1 1 1 1</ambient>
82+
<diffuse>1 1 1 1</diffuse>
83+
<pbr>
84+
<metal>
85+
<albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
86+
<metalness>0.0</metalness>
87+
<roughness>1.0</roughness>
88+
</metal>
89+
</pbr>
90+
</material>
91+
</visual>
92+
93+
<!-- Tag face 2 (yaw 240 deg) -->
94+
<visual name="tag_face_2">
95+
<pose>-0.0288675 -0.05 1.0 0 0 4.1887902</pose>
96+
<geometry>
97+
<box>
98+
<size>0.005 0.20 0.20</size>
99+
</box>
100+
</geometry>
101+
<material>
102+
<ambient>1 1 1 1</ambient>
103+
<diffuse>1 1 1 1</diffuse>
104+
<pbr>
105+
<metal>
106+
<albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
107+
<metalness>0.0</metalness>
108+
<roughness>1.0</roughness>
109+
</metal>
110+
</pbr>
111+
</material>
112+
</visual>
113+
114+
<!-- Prism collision (bounding cylinder around the three faces) -->
115+
<collision name="prism_collision">
116+
<pose>0 0 1.0 0 0 0</pose>
117+
<geometry>
118+
<cylinder>
119+
<radius>0.063</radius>
120+
<length>0.20</length>
121+
</cylinder>
122+
</geometry>
123+
</collision>
124+
125+
</link>
126+
</model>
127+
</sdf>

‎src/description/urdf/athena_drive.urdf.xacro‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
<xacro:include filename="$(find description)/urdf/athena_drive/athena_drive_macro.ros2_control.xacro"/>
1616
<xacro:include filename="imu.xacro"/>
1717
<xacro:include filename="gps.xacro"/>
18+
<xacro:include filename="magnetometer.xacro"/>
1819
<xacro:include filename="ground_truth_odom.xacro"/>
1920
<xacro:include filename="zed_camera.xacro"/>
2021

@@ -55,6 +56,10 @@
5556
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
5657
</xacro:gps_sensor>
5758

59+
<xacro:magnetometer_sensor parent="base_footprint">
60+
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
61+
</xacro:magnetometer_sensor>
62+
5863
<xacro:ground_truth_odometry robot_base_frame="base_footprint"/>
5964

6065
<xacro:zed_camera_sensor parent="base_footprint" camera_name="zed" sim_gazebo="$(arg sim_gazebo)">
Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
<xacro:macro name="magnetometer_sensor" params="parent *origin">
4+
<joint name="mag_joint" type="fixed">
5+
<xacro:insert_block name="origin" />
6+
<parent link="${parent}" />
7+
<child link="mag_link" />
8+
</joint>
9+
10+
<link name="mag_link">
11+
<visual>
12+
<geometry>
13+
<box size="0.05 0.05 0.05" />
14+
</geometry>
15+
<material name="red">
16+
<color rgba="1 0 0 1" />
17+
</material>
18+
</visual>
19+
</link>
20+
21+
<gazebo reference="mag_link">
22+
<sensor name="magnetometer" type="magnetometer">
23+
<always_on>true</always_on>
24+
<update_rate>50</update_rate>
25+
<topic>/mag</topic>
26+
<ign_frame_id>mag_link</ign_frame_id>
27+
<plugin filename="ignition-gazebo-magnetometer-system" name="ignition::gazebo::systems::Magnetometer"/>
28+
</sensor>
29+
</gazebo>
30+
</xacro:macro>
31+
</robot>

‎src/description/urdf/zed_camera.xacro‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@
3333
<camera name="${camera_name}_depth">
3434
<horizontal_fov>1.8151</horizontal_fov>
3535
<image>
36-
<width>672</width>
37-
<height>376</height>
36+
<width>320</width>
37+
<height>180</height>
3838
<format>R_FLOAT32</format>
3939
</image>
4040
<clip>
@@ -52,15 +52,15 @@
5252

5353
<sensor name="${camera_name}_rgb_sensor" type="camera">
5454
<always_on>1</always_on>
55-
<update_rate>15</update_rate>
55+
<update_rate>30</update_rate>
5656
<visualize>true</visualize>
5757
<topic>${camera_name}/zed_node/left/image_rect_color</topic>
5858
<ign_frame_id>${camera_name}_left_camera_frame_optical</ign_frame_id>
5959
<camera name="${camera_name}_rgb">
6060
<horizontal_fov>1.8151</horizontal_fov>
6161
<image>
62-
<width>672</width>
63-
<height>376</height>
62+
<width>1280</width>
63+
<height>720</height>
6464
<format>RGB_INT8</format>
6565
</image>
6666
<clip>

‎src/msgs/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,17 +19,24 @@ rosidl_generate_interfaces(${PROJECT_NAME}
1919
"msg/Currentdraw.msg"
2020
"msg/JointStatus.msg"
2121
"msg/SystemInfo.msg"
22+
"msg/PlannerEvent.msg"
23+
"msg/ActiveTarget.msg"
24+
"msg/NavStatus.msg"
2225
"msg/Heading.msg"
2326
"msg/WaypointEntry.msg"
2427
"msg/WaypointManagerState.msg"
28+
"msg/LocalPlannerStuck.msg"
2529
"srv/SetController.srv"
2630
"srv/AddWaypoint.srv"
2731
"srv/ClearWaypoints.srv"
2832
"msg/LedStatus.msg"
2933
"srv/StatusReq.srv"
3034
"srv/MaintenanceReq.srv"
35+
"srv/LatLonToENU.srv"
36+
"srv/SetTarget.srv"
3137
"action/NavigateToGPS.action"
3238
"action/NavigateToWaypoint.action"
39+
"action/NavigateToTarget.action"
3340
DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
3441
)
3542

0 commit comments

Comments
 (0)