Skip to content

Commit 9691490

Browse files
AntTraAntTra
committed
Merge branch 'main' into feature/eskf-landmark-valve
2 parents aba7a85 + 309b10a commit 9691490

File tree

72 files changed

+4833
-826
lines changed

Some content is hidden

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

72 files changed

+4833
-826
lines changed

‎.github/workflows/simulator-test.yml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@ jobs:
1414
with:
1515
vcs_repos_file: "tests/dependencies.repos"
1616
setup_script: "tests/setup.sh"
17+
pre_test_script: "scripts/ci_install_dependencies.sh"
1718
test_scripts: '[
1819
"tests/simulator_tests/waypoint_navigation/simulator_test.sh",
1920
"tests/simulator_tests/los_test/simulator_test.sh",

‎.gitignore‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ msg/*Result.msg
2121
msg/_*.py
2222
build_isolated/
2323
devel_isolated/
24+
*.cache/
2425

2526
# Generated by dynamic reconfigure
2627
*.cfgc
@@ -46,6 +47,7 @@ srv/_*.py
4647
*.pyc
4748
qtcreator-*
4849
*.user
50+
*hintmux.sh
4951

5052
/planning/cfg
5153
/planning/docs

‎.pre-commit-config.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ repos:
111111
rev: v2.4.1
112112
hooks:
113113
- id: codespell
114-
args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU']
114+
args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU,Bu']
115115

116116
ci:
117117
autoupdate_schedule: quarterly

‎auv_setup/config/robots/moby.yaml‎

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,6 @@
2626
num: 3
2727
thrusters:
2828
num: 8
29-
min: -100
30-
max: 100
3129
thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1
3230
0.70711,
3331
0.00000,
@@ -63,14 +61,14 @@
6361
-0.163904,
6462
0.140095,
6563
0.413892, # x-positions of the thrusters
66-
-0.313022,
67-
-0.313022,
68-
-0.313022,
69-
-0.313022,
7064
0.313022,
7165
0.313022,
7266
0.313022,
73-
0.313022, # y-positions of the thrusters
67+
0.313022,
68+
-0.313022,
69+
-0.313022,
70+
-0.313022,
71+
-0.313022, # y-positions of the thrusters
7472
0.021736,
7573
0.021736,
7674
0.021736,
@@ -80,6 +78,13 @@
8078
0.021736,
8179
0.021736, # z-positions of the thrusters
8280
]
81+
82+
constraints:
83+
max_force: 40.0
84+
min_force: -40.0
85+
input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
86+
slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0]
87+
8388
rate_of_change:
8489
max: 1 # Maximum rate of change in newton per second for a thruster
8590

@@ -110,6 +115,7 @@
110115
imu: "imu/data_raw"
111116
sonar_info: "fls/sonar_info"
112117
pressure_sensor: "pressure_sensor"
118+
gripper_servos: "gripper_servos"
113119

114120

115121
action_servers:

‎auv_setup/config/robots/orca.yaml‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,9 @@
1717
/**:
1818
ros__parameters:
1919
physical:
20-
center_of_mass: [0.0, 0.0, 0.035] # m (x,y,z)
20+
center_of_mass: [0.0, 0.0, 0.035] # Position (x,y,z) in meters (M)
2121
mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34]
22+
# 6x6 mass_inertia_matrix
2223

2324
propulsion:
2425
dofs:
@@ -27,9 +28,7 @@
2728
num: 3
2829
thrusters:
2930
num: 8
30-
min: -100
31-
max: 100
32-
thruster_force_direction: [
31+
thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1
3332
0.70711,
3433
0.00000,
3534
0.00000,
@@ -53,9 +52,9 @@
5352
0.00000,
5453
1.00000,
5554
1.00000,
56-
0.00000,
57-
] # Heave
58-
thruster_position: [
55+
0.00000, # Heave
56+
]
57+
thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0)
5958
0.41500,
6059
0.28400,
6160
-0.31800,
@@ -79,8 +78,14 @@
7978
0.07600,
8079
0.08200,
8180
0.08200,
82-
0.07600,
83-
] # z-position of thrusters
81+
0.07600, # z-position of thrusters
82+
]
83+
84+
constraints:
85+
max_force: 100.0
86+
min_force: -100.0
87+
input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
88+
slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0]
8489

8590
rate_of_change:
8691
max: 1 # Maximum rate of change in newton per second for a thruster
@@ -136,6 +141,7 @@
136141
dvl_altitude: "dvl/altitude"
137142
imu: "imu/data_raw"
138143
pressure_sensor: "pressure_sensor"
144+
gripper_servos: "gripper_servos"
139145

140146
action_servers:
141147
reference_filter: "reference_filter"

‎auv_setup/description/moby.urdf.xacro‎

Lines changed: 40 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -50,19 +50,6 @@
5050
</joint>
5151
<xacro:flir_blackfly_s frame="downwards_camera_link" base="downwards_camera" name="flir_blackfly"/>
5252

53-
<xacro:include filename="gripper.urdf.xacro"/>
54-
55-
<!-- Gripper needs updated urdf with the new model once ready-->
56-
<link name="gripper_link"/>
57-
58-
<joint name="base_to_gripper" type="fixed">
59-
<parent link="base_link"/>
60-
<child link="gripper_link"/>
61-
<origin xyz="0.26 0.095 0.08" rpy="0 0 0"/>
62-
</joint>
63-
64-
<xacro:gripper frame="gripper_link" static_shoulder="true"/>
65-
6653
<!-- RealSense -->
6754
<xacro:include filename="realsense_d555.urdf.xacro"/>
6855
<joint name="base_to_front_camera" type="fixed">
@@ -73,4 +60,44 @@
7360

7461
<xacro:realsense_d555 frame="front_camera_link" base="front_camera" name="realsense"/>
7562

63+
<!-- Gripper -->
64+
<link name="shoulder"></link>
65+
66+
<joint name="shoulder_joint" type="fixed">
67+
<parent link="base_link"/>
68+
<child link="shoulder"/>
69+
<origin xyz="0.282589 0.07822 0.115803" rpy="0.0 0.0 0.0"/>
70+
</joint>
71+
72+
<link name="arm"></link>
73+
74+
<joint name="moby/arm_joint" type="revolute">
75+
<parent link="shoulder"/>
76+
<child link="arm"/>
77+
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
78+
<axis xyz="1.0 0.0 0.0"/>
79+
<limit lower="-3.14159" upper="3.14159" effort="0" velocity="0"/>
80+
</joint>
81+
82+
<link name="finger1"></link>
83+
84+
85+
<joint name="moby/finger_joint1" type="prismatic">
86+
<parent link="arm"/>
87+
<child link="finger1"/>
88+
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
89+
<axis xyz="0.70711 0.70711 0.0"/>
90+
<limit lower="-0.033" upper="0" effort="0" velocity="0"/>
91+
</joint>
92+
93+
<link name="finger2"></link>
94+
95+
<joint name="moby/finger_joint2" type="prismatic">
96+
<parent link="arm"/>
97+
<child link="finger2"/>
98+
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
99+
<axis xyz="0.70711 -0.70711 0.0"/>
100+
<limit lower="-0.033" upper="0" effort="0" velocity="0"/>
101+
</joint>
102+
76103
</robot>

‎auv_setup/description/orca.urdf.xacro‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -62,4 +62,42 @@
6262
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 1.571"/>
6363
</joint>
6464

65+
<!-- ======================== Gripper ======================== -->
66+
67+
<link name="shoulder"/>
68+
<joint name="orca/shoulder_joint" type="revolute">
69+
<parent link="base_link"/>
70+
<child link="shoulder"/>
71+
<origin xyz="0.31 -0.02 0.16" rpy="0.0 0.0 0.0"/>
72+
<axis xyz="0.0 1.0 0.0"/>
73+
<limit lower="-1.57" upper="0.3" effort="0" velocity="0"/>
74+
</joint>
75+
76+
<link name="arm"/>
77+
<joint name="orca/arm_joint" type="revolute">
78+
<parent link="shoulder"/>
79+
<child link="arm"/>
80+
<origin xyz="0.1 0.02 -0.1" rpy="0.0 0.0 3.14"/>
81+
<axis xyz="1.0 0.0 0.0"/>
82+
<limit lower="-3.14159" upper="3.14159" effort="0" velocity="0"/>
83+
</joint>
84+
85+
<link name="finger1"/>
86+
<joint name="orca/finger_joint1" type="revolute">
87+
<parent link="arm"/>
88+
<child link="finger1"/>
89+
<origin xyz="-0.15 0 0" rpy="0 -1.57 3.14"/>
90+
<axis xyz="0.0 1.0 0.0"/>
91+
<limit lower="0.0" upper="1.57" effort="0" velocity="0"/>
92+
</joint>
93+
94+
<link name="finger2"/>
95+
<joint name="orca/finger_joint2" type="revolute">
96+
<parent link="arm"/>
97+
<child link="finger2"/>
98+
<origin xyz="-0.15 0 0" rpy="0 1.57 0"/>
99+
<axis xyz="0.0 1.0 0.0"/>
100+
<limit lower="0.0" upper="1.57" effort="0" velocity="0"/>
101+
</joint>
102+
65103
</robot>

‎auv_setup/launch/drone_description.launch.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ def launch_setup(context, *args, **kwargs):
2929
name="robot_state_publisher",
3030
namespace=namespace,
3131
output="screen",
32+
remappings=[("joint_states", "servo_state")],
3233
parameters=[
3334
{
3435
"robot_description": ParameterValue(

‎filtering/line_filtering/CMakeLists.txt‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(line_filtering)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 20)
6+
endif()
7+
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
endif()
11+
12+
find_package(ament_cmake REQUIRED)
13+
find_package(rclcpp REQUIRED)
14+
find_package(rclcpp_components REQUIRED)
15+
find_package(geometry_msgs REQUIRED)
16+
find_package(visualization_msgs REQUIRED)
17+
find_package(tf2 REQUIRED)
18+
find_package(tf2_ros REQUIRED)
19+
find_package(message_filters REQUIRED)
20+
find_package(vortex_utils REQUIRED)
21+
find_package(vortex_utils_ros REQUIRED)
22+
find_package(vortex_filtering REQUIRED)
23+
find_package(vortex_msgs REQUIRED)
24+
25+
include_directories(include)
26+
27+
set(CORE_LIB_NAME "${PROJECT_NAME}")
28+
29+
add_library(${CORE_LIB_NAME} SHARED
30+
src/lib/line_track_manager.cpp
31+
)
32+
33+
target_include_directories(${CORE_LIB_NAME}
34+
PUBLIC
35+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
36+
$<INSTALL_INTERFACE:include>
37+
)
38+
39+
ament_target_dependencies(${CORE_LIB_NAME}
40+
vortex_filtering
41+
vortex_utils
42+
)
43+
44+
set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
45+
46+
add_library(${COMPONENT_LIB_NAME} SHARED
47+
src/ros/line_filtering_ros.cpp
48+
)
49+
50+
ament_target_dependencies(${COMPONENT_LIB_NAME}
51+
rclcpp
52+
rclcpp_components
53+
geometry_msgs
54+
visualization_msgs
55+
tf2
56+
tf2_ros
57+
message_filters
58+
vortex_filtering
59+
vortex_utils
60+
vortex_utils_ros
61+
vortex_msgs
62+
)
63+
64+
target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME})
65+
66+
rclcpp_components_register_node(
67+
${COMPONENT_LIB_NAME}
68+
PLUGIN "LineFilteringNode"
69+
EXECUTABLE ${PROJECT_NAME}_node
70+
)
71+
72+
install(
73+
TARGETS
74+
${CORE_LIB_NAME}
75+
${COMPONENT_LIB_NAME}
76+
EXPORT export_${PROJECT_NAME}
77+
ARCHIVE DESTINATION lib
78+
LIBRARY DESTINATION lib
79+
RUNTIME DESTINATION bin
80+
)
81+
82+
install(
83+
DIRECTORY include/
84+
DESTINATION include
85+
)
86+
87+
install(DIRECTORY
88+
launch
89+
config
90+
DESTINATION share/${PROJECT_NAME}/
91+
)
92+
93+
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
94+
ament_export_dependencies(
95+
vortex_filtering
96+
vortex_utils
97+
)
98+
ament_export_include_directories(include)
99+
ament_export_libraries(${CORE_LIB_NAME})
100+
101+
if(BUILD_TESTING)
102+
add_subdirectory(test)
103+
endif()
104+
105+
ament_package()

0 commit comments

Comments
 (0)