Skip to content

Commit d4f4e92

Browse files
jorgenfjjorgenfj
authored
navigation easter testing changes (#708)
1 parent f766981 commit d4f4e92

9 files changed

Lines changed: 733 additions & 3 deletions

File tree

‎auv_setup/description/nautilus.urdf.xacro‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
<joint name="base_to_imu" type="fixed">
1919
<parent link="base_link"/>
2020
<child link="imu_link"/>
21-
<origin xyz="0.0057 0.0 0.0827" rpy="0.0 0.0 0.0"/>
21+
<origin xyz="-0.1 -0.001 0.085" rpy="0.0 0.0 0.0"/>
2222
</joint>
2323

2424
<!-- base to dvl -->
@@ -46,7 +46,7 @@
4646
<joint name="base_to_downwards_camera" type="fixed">
4747
<parent link="base_link"/>
4848
<child link="downwards_camera_link"/>
49-
<origin xyz="0.283615 -0.085021 0.094186" rpy="-1.5708 -1.5708 -1.5708"/>
49+
<origin xyz="0.283615 -0.158 0.12" rpy="-1.5708 -1.5708 -1.5708"/>
5050
</joint>
5151
<xacro:flir_blackfly_s frame="downwards_camera_link" base="downwards_camera" name="flir_blackfly"/>
5252

@@ -66,7 +66,7 @@
6666
<joint name="shoulder_joint" type="fixed">
6767
<parent link="base_link"/>
6868
<child link="shoulder"/>
69-
<origin xyz="0.282589 0.07822 0.115803" rpy="0.0 0.0 0.0"/>
69+
<origin xyz="0.282589 0.028 0.115803" rpy="0.0 0.0 0.0"/>
7070
</joint>
7171

7272
<link name="arm"></link>

‎auv_setup/launch/nucleus_odom_transformer.launch.py‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
import os
2+
3+
from ament_index_python.packages import get_package_share_directory
4+
from launch import LaunchDescription
5+
from launch.actions import IncludeLaunchDescription, OpaqueFunction
6+
from launch.launch_description_sources import PythonLaunchDescriptionSource
7+
from launch_ros.actions import Node
8+
9+
from auv_setup.launch_arg_common import (
10+
declare_drone_and_namespace_args,
11+
resolve_drone_and_namespace,
12+
)
13+
14+
15+
def launch_setup(context, *args, **kwargs):
16+
drone, namespace = resolve_drone_and_namespace(context)
17+
18+
drone_params = os.path.join(
19+
get_package_share_directory("auv_setup"),
20+
"config",
21+
"robots",
22+
f"{drone}.yaml",
23+
)
24+
25+
drone_description_launch = IncludeLaunchDescription(
26+
PythonLaunchDescriptionSource(
27+
os.path.join(
28+
get_package_share_directory("auv_setup"),
29+
"launch",
30+
"drone_description.launch.py",
31+
)
32+
),
33+
launch_arguments={
34+
"drone": drone,
35+
"namespace": namespace,
36+
}.items(),
37+
)
38+
39+
nortek_nucleus_ros_interface_node = Node(
40+
package="nortek_nucleus_ros_interface",
41+
executable="nortek_nucleus_ros_interface_node",
42+
name="nortek_nucleus_ros_interface_node",
43+
namespace=namespace,
44+
parameters=[
45+
{
46+
"frame_id": f"{namespace}/nucleus_frame",
47+
"connection_params.remote_ip": "10.0.0.42",
48+
"connection_params.data_remote_port": 9000,
49+
"connection_params.password": "",
50+
"reset_pose_on_start": True,
51+
"enable_imu": True,
52+
"enable_ins_odom": True,
53+
"enable_dvl": True,
54+
"enable_pressure": True,
55+
"enable_altimeter": True,
56+
"enable_magnetometer": False,
57+
"enable_ins_twist": False,
58+
"enable_ins_position": False,
59+
"enable_ins_pose": False,
60+
"imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw",
61+
"imu_data_pub_topic": f"/{namespace}/imu/data",
62+
"ins_pub_topic": f"/{namespace}/nucleus/odom",
63+
"dvl_pub_topic": f"/{namespace}/nucleus/dvl",
64+
"pressure_pub_topic": f"/{namespace}/nucleus/pressure",
65+
"altimeter_pub_topic": f"/{namespace}/nucleus/altitude",
66+
"magnetometer_pub_topic": f"/{namespace}/imu/mag",
67+
"ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist",
68+
"ins_position_pub_topic": f"/{namespace}/nucleus/ins/position",
69+
"ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose",
70+
"imu_settings.freq": 125,
71+
"ahrs_settings.freq": 10,
72+
"ahrs_settings.mode": 0,
73+
"bottom_track_settings.mode": 2,
74+
"bottom_track_settings.velocity_range": 5,
75+
"bottom_track_settings.enable_watertrack": False,
76+
"altimeter_settings.power_level": 0, # 0=default
77+
"magnetometer_settings.freq": 75,
78+
"magnetometer_settings.mode": 0,
79+
"instrument_settings.rotxy": 180.0, # Transform currently not working. Maybe fix later
80+
"instrument_settings.rotyz": 0.0,
81+
"instrument_settings.rotxz": 0.0,
82+
},
83+
drone_params,
84+
],
85+
output="screen",
86+
)
87+
88+
odom_transformer_node = Node(
89+
package="odom_transformer",
90+
executable="odom_transformer_node",
91+
name="odom_transformer_node",
92+
namespace=namespace,
93+
parameters=[
94+
{
95+
"sensor_frame": "dvl_link",
96+
"publish_tf": True,
97+
"publish_pose": True,
98+
"publish_twist": True,
99+
"rotate_yaw_180": True,
100+
"topics.input": f"/{namespace}/nucleus/odom",
101+
"topics.output": f"/{namespace}/odom",
102+
"topics.pose": f"/{namespace}/pose",
103+
"topics.twist": f"/{namespace}/twist",
104+
},
105+
drone_params,
106+
{"frame_prefix": namespace},
107+
],
108+
output="screen",
109+
)
110+
111+
return [
112+
drone_description_launch,
113+
nortek_nucleus_ros_interface_node,
114+
odom_transformer_node,
115+
]
116+
117+
118+
def generate_launch_description():
119+
return LaunchDescription(
120+
declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)]
121+
)

‎auv_setup/launch/state_estimation.launch.py‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
import os
2+
3+
from ament_index_python.packages import get_package_share_directory
4+
from launch import LaunchDescription
5+
from launch.actions import IncludeLaunchDescription, OpaqueFunction
6+
from launch.launch_description_sources import PythonLaunchDescriptionSource
7+
from launch_ros.actions import ComposableNodeContainer
8+
from launch_ros.descriptions import ComposableNode
9+
10+
from auv_setup.launch_arg_common import (
11+
declare_drone_and_namespace_args,
12+
resolve_drone_and_namespace,
13+
)
14+
15+
16+
def launch_setup(context, *args, **kwargs):
17+
drone, namespace = resolve_drone_and_namespace(context)
18+
19+
drone_params = os.path.join(
20+
get_package_share_directory("auv_setup"),
21+
"config",
22+
"robots",
23+
f"{drone}.yaml",
24+
)
25+
26+
drone_description_launch = IncludeLaunchDescription(
27+
PythonLaunchDescriptionSource(
28+
os.path.join(
29+
get_package_share_directory("auv_setup"),
30+
"launch",
31+
"drone_description.launch.py",
32+
)
33+
),
34+
launch_arguments={
35+
"drone": drone,
36+
"namespace": namespace,
37+
}.items(),
38+
)
39+
40+
container = ComposableNodeContainer(
41+
name="eskf_container",
42+
namespace=namespace,
43+
package="rclcpp_components",
44+
executable="component_container_mt",
45+
composable_node_descriptions=[
46+
ComposableNode(
47+
package="eskf",
48+
plugin="ESKFNode",
49+
name="eskf_node",
50+
namespace=namespace,
51+
parameters=[
52+
drone_params,
53+
{
54+
"diag_Q_std": [
55+
0.05,
56+
0.05,
57+
0.1,
58+
0.01,
59+
0.01,
60+
0.02,
61+
0.001,
62+
0.001,
63+
0.001,
64+
0.0001,
65+
0.0001,
66+
0.0001,
67+
],
68+
"diag_p_init": [
69+
1.0,
70+
1.0,
71+
1.0,
72+
0.5,
73+
0.5,
74+
0.5,
75+
0.1,
76+
0.1,
77+
0.1,
78+
0.001,
79+
0.001,
80+
0.001,
81+
0.001,
82+
0.001,
83+
0.001,
84+
],
85+
"transform.imu_frame_r": [
86+
-1.0,
87+
0.0,
88+
0.0,
89+
0.0,
90+
1.0,
91+
0.0,
92+
0.0,
93+
0.0,
94+
-1.0,
95+
],
96+
"transform.imu_frame_t": [0.0, 0.0, 0.0],
97+
"transform.dvl_frame_r": [
98+
0.0,
99+
-1.0,
100+
0.0,
101+
1.0,
102+
0.0,
103+
0.0,
104+
0.0,
105+
0.0,
106+
1.0,
107+
],
108+
"transform.dvl_frame_t": [0.4, 0.0, 0.2],
109+
"transform.depth_frame_t": [0.0, 0.0, 0.0],
110+
"use_tf_transforms": True,
111+
"publish_tf": True,
112+
"publish_pose": True,
113+
"publish_twist": True,
114+
"publish_rate_ms": 5,
115+
"add_gravity_to_imu": True,
116+
"frame_prefix": namespace,
117+
"initial_gyro_bias": [0.0, 0.0, 0.0],
118+
"initial_accel_bias": [0.0, 0.0, -0.05],
119+
},
120+
],
121+
extra_arguments=[{"use_intra_process_comms": True}],
122+
),
123+
ComposableNode(
124+
package="mru_ros_interface",
125+
plugin="MruRosInterface",
126+
name="mru_ros_interface_node",
127+
namespace=namespace,
128+
parameters=[
129+
{
130+
"imu_pub_topic": f"/{namespace}/imu/data_raw",
131+
"frame_id": f"/{namespace}/imu_link",
132+
"connection_params.remote_ip": "10.0.1.20", # MRU IP
133+
"connection_params.data_remote_port": 7550,
134+
"connection_params.data_local_port": 7551,
135+
"connection_params.control_local_port": 7552,
136+
"mru_settings.channel": "UDP1",
137+
"mru_settings.port": 7551,
138+
"mru_settings.ip_addr": "10.0.0.69", # Host computer IP
139+
"mru_settings.format": "MRUBIN",
140+
"mru_settings.interval": 5,
141+
"mru_settings.token": 21,
142+
}
143+
],
144+
extra_arguments=[{"use_intra_process_comms": True}],
145+
),
146+
ComposableNode(
147+
package="nortek_nucleus_ros_interface",
148+
plugin="NortekNucleusRosInterface",
149+
name="nortek_nucleus_ros_interface_node",
150+
namespace=namespace,
151+
parameters=[
152+
{
153+
"frame_id": f"/{namespace}/nucleus_frame",
154+
"connection_params.remote_ip": "10.0.0.42",
155+
"connection_params.data_remote_port": 9000,
156+
"connection_params.password": "",
157+
"reset_pose_on_start": True,
158+
"enable_imu": False,
159+
"enable_ins_odom": False,
160+
"enable_dvl": True,
161+
"enable_pressure": True,
162+
"enable_altimeter": True,
163+
"enable_magnetometer": False,
164+
"enable_ins_twist": False,
165+
"enable_ins_position": False,
166+
"enable_ins_pose": False,
167+
"imu_data_raw_pub_topic": f"/{namespace}/nucleus/imu/data_raw",
168+
"imu_data_pub_topic": f"/{namespace}/imu/data",
169+
"ins_pub_topic": f"/{namespace}/nucleus/odom",
170+
"dvl_pub_topic": f"/{namespace}/nucleus/dvl",
171+
"pressure_pub_topic": f"/{namespace}/nucleus/pressure",
172+
"magnetometer_pub_topic": f"/{namespace}/imu/mag",
173+
"ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist",
174+
"ins_position_pub_topic": f"/{namespace}/nucleus/ins/position",
175+
"ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose",
176+
"imu_settings.freq": 125,
177+
"ahrs_settings.freq": 10,
178+
"ahrs_settings.mode": 0,
179+
"bottom_track_settings.mode": 2,
180+
"bottom_track_settings.velocity_range": 5,
181+
"bottom_track_settings.enable_watertrack": False,
182+
"fast_pressure_settings.enable": True,
183+
"fast_pressure_settings.sampling_rate": 16,
184+
"magnetometer_settings.freq": 75,
185+
"magnetometer_settings.mode": 0,
186+
"instrument_settings.rotxy": 180.0, # Transform currently not working. Maybe fix later
187+
"instrument_settings.rotyz": 0.0,
188+
"instrument_settings.rotxz": 0.0,
189+
}
190+
],
191+
extra_arguments=[{"use_intra_process_comms": True}],
192+
),
193+
],
194+
output="screen",
195+
arguments=["--ros-args", "--log-level", "error"],
196+
)
197+
198+
return [drone_description_launch, container]
199+
200+
201+
def generate_launch_description():
202+
return LaunchDescription(
203+
declare_drone_and_namespace_args()
204+
+ [
205+
OpaqueFunction(function=launch_setup),
206+
]
207+
)

0 commit comments

Comments
 (0)