Skip to content

Commit

Permalink
Included DVL sensor, odom now publishes a reading
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexoj46
alexoj46 committed Feb 14, 2025
1 parent 1ee5e23 commit 8171f43
Show file tree
Hide file tree
Showing 5 changed files with 93 additions and 174 deletions.
51 changes: 26 additions & 25 deletions src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ def generate_launch_description():
False,
True,
True,
False,
True,
True,
True,
True,
Expand All @@ -42,30 +42,30 @@ def generate_launch_description():
Parameter("imu0_relative", True),
Parameter("imu0_remove_gravitational_acceleration", True),
Parameter("imu0_queue_size", 10),
# Parameter("odom0", "/dvl/odom"),
# Parameter(
# "odom0_config",
# [
# True,
# True,
# False,
# False,
# False,
# True,
# True,
# False,
# False,
# False,
# False,
# True,
# False,
# False,
# False,
# ],
# ),
# Parameter("odom0_differential", True),
# Parameter("odom0_relative", True),
# Parameter("odom0_queue_size", 10),
Parameter("odom0", "/dvl/velocity"),
Parameter(
"odom0_config",
[
False,
False,
False,
False,
False,
False,
True,
True,
True,
False,
False,
False,
False,
False,
False,
],
),
Parameter("odom0_differential", False),
Parameter("odom0_relative", True),
Parameter("odom0_queue_size", 10),
# Parameter("pose0", "/depth/pose"),
# Parameter("pose0_config", [False, False, True,
# False, False, False,
Expand All @@ -77,6 +77,7 @@ def generate_launch_description():
# Parameter("pose0_queue_size", 10),
Parameter("print_diagnostics", True),
Parameter("publish_acceleration", True),
# Parameter("initial_state", [0.0] * 15),
]
import rich

Expand Down
105 changes: 21 additions & 84 deletions src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
<?xml version="1.0" ?>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sub9">

<!-- XACRO INCLUDES -->
<xacro:include filename="$(find subjugator_description)/urdf/xacro/fixed_link.xacro"/>
<xacro:include filename="$(find subjugator_description)/urdf/xacro/camera.xacro"/>
Expand All @@ -17,7 +16,7 @@
<!-- Inertial Properties -->
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<!-- Need to be properly caluclated at some point -->
<!-- Need to be properly calculated at some point -->
<mass value="30.5"/>
<inertia ixx="0.96105" ixy="0.003" ixz="-0.02" iyy="1.799" iyz="0.0003" izz="2.1286"/>
</inertial>
Expand All @@ -39,11 +38,9 @@
</collision>
<!-- Need to add 'shell' collisions-->
</link>

<gazebo>
<plugin
filename="gz-sim-hydrodynamics-system"
name="gz::sim::systems::Hydrodynamics">
<plugin filename="gz-sim-hydrodynamics-system" name="gz::sim::systems::Hydrodynamics">
<link_name>base_link</link_name>
<xDotU>-2</xDotU>
<yDotV>-5</yDotV>
Expand All @@ -65,37 +62,21 @@
<nR>-10</nR>
</plugin>
</gazebo>

<!-- Cameras -->
<xacro:mil_camera
name="front_cam"
parent="base_link"
xyz="-0.32 -0.014274 -0.050"
rpy="0 0 3.14159"
/>
<xacro:mil_camera
name="down_cam"
parent="base_link"
xyz="-0.15222 0.027913 -0.22"
rpy="0 1.57 0"
/>

<!-- Cameras -->
<xacro:mil_camera name="front_cam" parent="base_link" xyz="-0.32 -0.014274 -0.050" rpy="0 0 3.14159"/>
<xacro:mil_camera name="down_cam" parent="base_link" xyz="-0.15222 0.027913 -0.22" rpy="0 1.57 0"/>

<!-- DVL Sensor -->
<xacro:mil_dvl_sensor
name="dvl_sensor"
parent="base_link"
xyz="0.21236 0.00254 -0.10233"
rpy="0 0 0"
/>
<xacro:mil_dvl_sensor name="dvl_sensor" parent="base_link" xyz="0.21236 0.00254 -0.10233" rpy="0 0 0"/>

<!-- IMU, Magnetometer -->
<!-- IMU, Magnetometer -->
<xacro:mil_imu_magnetometer
name="imu"
namespace='/imu'
parent="base_link"
xyz="0.0632 -0.164 -0.079"
rpy="0 -1.571 0"
rpy="0 0 0"
rate="210"
ax="0.015707963"
ay="0.015707963"
Expand All @@ -105,60 +86,16 @@
lz="0.08825985"
/>

<!-- Thrusters -->
<!-- Front/Back Left/Right Vertical/Horizontal -->
<!-- Orientation seem to be correct -->
<xacro:mil_thruster
name="FLV"
parent="base_link"
xyz="-0.15217 -0.16474 0.010449"
rpy="0 0 0"

/>
<xacro:mil_thruster
name="FRV"
parent="base_link"
xyz="-0.1492 0.13821 0.010449"
rpy="0 0 0"
/>
<xacro:mil_thruster
name="BRV"
parent="base_link"
xyz="0.31117 0.13821 0.010449"
rpy="0 0 0"
/>
<xacro:mil_thruster
name="BLV"
parent="base_link"
xyz="0.30868 -0.16621 0.010449"
rpy="0 0 0"
/>


<xacro:mil_thruster
name="FLH"
parent="base_link"
xyz="-0.14714 -0.26409 0.038317"
rpy="-1.5708 0 0.7854"
/>
<xacro:mil_thruster
name="FRH"
parent="base_link"
xyz="-0.147 0.2413 0.038324"
rpy="1.5708 0 -0.7854"
/>
<xacro:mil_thruster
name="BRH"
parent="base_link"
xyz="0.32369 0.22645 0.038317"
rpy="-1.5708 0 0.7854"
/>
<xacro:mil_thruster
name="BLH"
parent="base_link"
xyz="0.32397 -0.25204 0.038324"
rpy="1.5708 0 -0.7854"
/>

<!-- Thrusters -->
<!-- Front/Back Left/Right Vertical/Horizontal -->
<!-- Orientation seem to be correct -->
<xacro:mil_thruster name="FLV" parent="base_link" xyz="-0.15217 -0.16474 0.010449" rpy="0 0 0"/>
<xacro:mil_thruster name="FRV" parent="base_link" xyz="-0.1492 0.13821 0.010449" rpy="0 0 0"/>
<xacro:mil_thruster name="BRV" parent="base_link" xyz="0.31117 0.13821 0.010449" rpy="0 0 0"/>
<xacro:mil_thruster name="BLV" parent="base_link" xyz="0.30868 -0.16621 0.010449" rpy="0 0 0"/>

<xacro:mil_thruster name="FLH" parent="base_link" xyz="-0.14714 -0.26409 0.038317" rpy="-1.5708 0 0.7854"/>
<xacro:mil_thruster name="FRH" parent="base_link" xyz="-0.147 0.2413 0.038324" rpy="1.5708 0 -0.7854"/>
<xacro:mil_thruster name="BRH" parent="base_link" xyz="0.32369 0.22645 0.038317" rpy="-1.5708 0 0.7854"/>
<xacro:mil_thruster name="BLH" parent="base_link" xyz="0.32397 -0.25204 0.038324" rpy="1.5708 0 -0.7854"/>
</robot>
35 changes: 13 additions & 22 deletions src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" xmlns:experimental="http://sdformat.org/schemas/experimental" xmlns:gz="http://gazebosim.org/schema">
<!-- Macro for inserting a DVL -->
<xacro:macro
name="mil_dvl_sensor"
params="name parent xyz rpy"
>

<xacro:macro name="mil_dvl_sensor" params="name parent xyz rpy">
<!-- DVL Link -->
<link name="${name}_link">
<link name="${name}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<mass value="0.1"/>
<inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
Expand All @@ -25,15 +21,13 @@

<!-- This ensures that the fixed link is not simplified away -->
<gazebo reference="${name}_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
<disableFixedJointLumping>true</disableFixedJointLumping>
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>

<!-- Sensor/Plugin Info-->
<gazebo reference="${name}_link">
<sensor
name="A50_Waterlinked" type="custom"
gz:type="dvl">
<sensor name="A50_Waterlinked" type="custom" gz:type="dvl">
<pose>0 0 0 0 0 0</pose>
<always_on>1</always_on>
<update_rate>8</update_rate>
Expand Down Expand Up @@ -97,13 +91,10 @@
</gazebo>

<gazebo>
<plugin
filename="DVLBridge"
name="dave_ros_gz_plugins::DVLBridge">

<!-- Grabbed in DVL Bridge script -->
<topic>/dvl/velocity</topic>
</plugin>
</gazebo>
</xacro:macro>
</robot>
<plugin filename="DVLBridge" name="dave_ros_gz_plugins::DVLBridge">
<!-- Grabbed in DVL Bridge script -->
<topic>/dvl/velocity</topic>
</plugin>
</gazebo>
</xacro:macro>
</robot>
34 changes: 12 additions & 22 deletions src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro
name="mil_imu_magnetometer"
params="name namespace parent xyz rpy
rate ax ay az lx ly lz"
>

<!-- Imu link -->
<link name="${name}_link">
<xacro:macro name="mil_imu_magnetometer" params="name namespace parent xyz rpy
rate ax ay az lx ly lz">
<!-- Imu link -->
<link name="${name}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<mass value="0.1"/>
<inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
Expand All @@ -25,16 +21,17 @@

<!-- This ensures that the fixed link is not simplified away -->
<gazebo reference="${name}_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
<disableFixedJointLumping>true</disableFixedJointLumping>
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>

<!-- Sensor/Plugin Info-->
<gazebo reference="${name}_link">
<sensor name="VectorNav_IMU" type="imu">
<sensor name="${name}_link" type="imu">
<always_on>true</always_on>
<update_rate>${rate}</update_rate>
<topic>${namespace}/data_raw</topic>
<gz_frame_id>imu_link</gz_frame_id>
<imu>
<linear_acceleration>
<x>
Expand Down Expand Up @@ -71,21 +68,14 @@
</z>
</angular_velocity>
</imu>
<plugin
filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>

<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
</sensor>
<sensor name="VectorNav_MAG" type="magnetometer">
<always_on>true</always_on>
<update_rate>${rate}</update_rate>
<topic>${namespace}/mag</topic>
<plugin
filename="gz-sim-magnetometer-system"
name="gz::sim::systems::Magnetometer">
</plugin>
<plugin filename="gz-sim-magnetometer-system" name="gz::sim::systems::Magnetometer"/>
</sensor>
</gazebo>
</xacro:macro>
</robot>
</xacro:macro>
</robot>
Loading

0 comments on commit 8171f43

Please sign in to comment.