Skip to content

Commit 105fa19

Browse files
rakeshv24rakeshv24
authored
Merge pull request #21 from IOES-Lab/robot_launch
2 parents 8e0d8bd + 33e2068 commit 105fa19

34 files changed

+4173
-107
lines changed

‎.codespellrc

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
[codespell]
2+
skip = *.json
3+
ignore-words-list = parm,ned,sitl

‎examples/dave_demos/README.md

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -60,22 +60,22 @@ Before launching, ensure to build and source the workspace:
6060
colcon build && source install/setup.bash
6161
```
6262

63-
1. Launching REXROV in an empty world:
63+
1. Launching REXROV in dave_ocean_waves.world:
6464

6565
```bash
66-
ros2 launch dave_demos dave_robot.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false
66+
ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
6767
```
6868

69-
2. Launching REXROV in dave_ocean_waves.world:
69+
2. Launching BlueROV2 in dave_ocean_waves.world:
7070

7171
```bash
72-
ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
72+
ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=bluerov2 world_name:=dave_ocean_waves paused:=false
7373
```
7474

75-
3. Launching Slocum Glider in an empty world:
75+
3. Launching BlueROV2 Heavy in dave_ocean_waves.world:
7676

7777
```bash
78-
ros2 launch dave_demos dave_robot.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false
78+
ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=bluerov2_heavy world_name:=dave_ocean_waves paused:=false
7979
```
8080

8181
4. Launching Slocum Glider in dave_ocean_waves.world:

‎extras/repos/dave.jazzy.repos

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,3 +13,4 @@ repositories:
1313
type: git
1414
url: https://github.com/IOES-Lab/dave.git
1515
version: ros2
16+

‎models/dave_robot_models/CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ project(dave_robot_models)
44
find_package(ament_cmake REQUIRED)
55

66
install(
7-
DIRECTORY description launch meshes
7+
DIRECTORY description launch meshes config
88
DESTINATION share/dave_robot_models
99
)
1010

‎models/dave_robot_models/config/bluerov2/ardusub.parm

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
# These settings were copied from ardupilot/Tools/autotest/default_params/sub.parm
2+
BATT_MONITOR 4
3+
BTN0_FUNCTION 0
4+
BTN10_SFUNCTION 0
5+
BTN10_FUNCTION 22
6+
BTN10_SFUNCTION 0
7+
BTN11_FUNCTION 42
8+
BTN11_SFUNCTION 47
9+
BTN12_FUNCTION 43
10+
BTN12_SFUNCTION 46
11+
BTN13_FUNCTION 33
12+
BTN13_SFUNCTION 45
13+
BTN14_FUNCTION 32
14+
BTN14_SFUNCTION 44
15+
BTN15_FUNCTION 0
16+
BTN15_SFUNCTION 0
17+
BTN1_FUNCTION 6
18+
BTN1_SFUNCTION 0
19+
BTN2_FUNCTION 8
20+
BTN2_SFUNCTION 0
21+
BTN3_FUNCTION 7
22+
BTN3_SFUNCTION 0
23+
BTN4_FUNCTION 4
24+
BTN4_SFUNCTION 0
25+
BTN5_FUNCTION 1
26+
BTN5_SFUNCTION 0
27+
BTN6_FUNCTION 3
28+
BTN6_SFUNCTION 0
29+
BTN7_FUNCTION 21
30+
BTN7_SFUNCTION 0
31+
BTN8_FUNCTION 48
32+
BTN8_SFUNCTION 0
33+
BTN9_FUNCTION 23
34+
BTN9_SFUNCTION 0
35+
COMPASS_OFS_X 5
36+
COMPASS_OFS_Y 13
37+
COMPASS_OFS_Z -18
38+
COMPASS_OFS2_X 5
39+
COMPASS_OFS2_Y 13
40+
COMPASS_OFS2_Z -18
41+
COMPASS_OFS3_X 5
42+
COMPASS_OFS3_Y 13
43+
COMPASS_OFS3_Z -18
44+
INS_ACCOFFS_X 0.001
45+
INS_ACCOFFS_Y 0.001
46+
INS_ACCOFFS_Z 0.001
47+
INS_ACCSCAL_X 1.001
48+
INS_ACCSCAL_Y 1.001
49+
INS_ACCSCAL_Z 1.001
50+
INS_ACC2OFFS_X 0.001
51+
INS_ACC2OFFS_Y 0.001
52+
INS_ACC2OFFS_Z 0.001
53+
INS_ACC2SCAL_X 1.001
54+
INS_ACC2SCAL_Y 1.001
55+
INS_ACC2SCAL_Z 1.001
56+
INS_ACC3OFFS_X 0.000
57+
INS_ACC3OFFS_Y 0.000
58+
INS_ACC3OFFS_Z 0.000
59+
INS_ACC3SCAL_X 1.000
60+
INS_ACC3SCAL_Y 1.000
61+
INS_ACC3SCAL_Z 1.000
62+
MNT_ANGMAX_TIL 4500
63+
MNT_ANGMIN_TIL -4500
64+
MNT_DEFLT_MODE 3
65+
MNT_JSTICK_SPD 100
66+
MNT_RC_IN_TILT 8
67+
MNT_STAB_TILT 0
68+
MNT_TYPE 1
69+
PSC_ACCZ_P 2
70+
PSC_ACCZ_I 4
71+
PSC_ACCZ_FF 0.75
72+
PSC_VELZ_P 8
73+
PSC_POSZ_P 3
74+
PSC_POSXY_P 2.5
75+
PSC_VELXY_D 0.8
76+
PSC_VELXY_I 0.5
77+
PSC_VELXY_P 5.0
78+
RNGFND1_MAX_CM 3000
79+
RNGFND1_PIN 0
80+
RNGFND1_SCALING 12.12
81+
RNGFND1_TYPE 1
82+
SERVO8_FUNCTION 7
83+
SERVO8_MAX 1900
84+
SERVO8_MIN 1100
85+
BARO_EXT_BUS 1
86+
87+
# Use EK3
88+
EK2_ENABLE 0
89+
EK3_ENABLE 1
90+
AHRS_EKF_TYPE 3
91+
92+
# Enable external nav
93+
# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
94+
GPS_TYPE 0 # Disable GPS
95+
VISO_TYPE 1 # External vision
96+
EK3_SRC1_POSXY 6 # External nav
97+
EK3_SRC1_VELXY 6 # External nav
98+
EK3_SRC1_POSZ 1 # Baro is the primary z source
99+
EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
100+
COMPASS_USE 0 # Disable compass 1
101+
COMPASS_USE2 0 # Disable compass 2
102+
COMPASS_USE3 0 # Disable compass 3
103+
EK3_SRC1_YAW 6 # External nav
104+
105+
# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
106+
SIM_BARO_RND 0.01
107+
108+
# Store thruster parameters as backups
109+
SERVO1_FUNCTION 33
110+
SERVO2_FUNCTION 34
111+
SERVO3_FUNCTION 35
112+
SERVO4_FUNCTION 36
113+
SERVO5_FUNCTION 37
114+
SERVO6_FUNCTION 38

‎models/dave_robot_models/config/bluerov2/robot_config.py

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
from launch import LaunchDescription
2+
from launch.actions import (
3+
DeclareLaunchArgument,
4+
OpaqueFunction,
5+
ExecuteProcess,
6+
)
7+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
8+
from launch_ros.actions import Node
9+
from launch_ros.substitutions import FindPackageShare
10+
11+
12+
def launch_setup(context, *args, **kwargs):
13+
namespace = LaunchConfiguration("namespace").perform(context)
14+
15+
thruster_joints = []
16+
for thruster in range(1, 7):
17+
thruster_joints.append(f"/model/{namespace}/joint/thruster{thruster}_joint")
18+
19+
bluerov2_arguments = (
20+
[f"{joint}/cmd_thrust@std_msgs/msg/[email protected]" for joint in thruster_joints]
21+
+ [f"{joint}/ang_vel@std_msgs/msg/[email protected]" for joint in thruster_joints]
22+
+ [
23+
f"{joint}/enable_deadband@std_msgs/msg/[email protected]"
24+
for joint in thruster_joints
25+
]
26+
+ [
27+
f"/model/{namespace}/odometry@nav_msgs/msg/[email protected]",
28+
f"/model/{namespace}/odometry_with_covariance@nav_msgs/msg/[email protected]",
29+
f"/model/{namespace}/pose@geometry_msgs/msg/[email protected]_V",
30+
f"/model/{namespace}/imu@sensor_msgs/msg/[email protected]",
31+
f"/model/{namespace}/magnetometer@sensor_msgs/msg/[email protected]",
32+
f"/model/{namespace}/camera/image@sensor_msgs/msg/[email protected]",
33+
f"/model/{namespace}/camera/camera_info@sensor_msgs/msg/[email protected]",
34+
]
35+
)
36+
37+
bluerov2_bridge = Node(
38+
package="ros_gz_bridge",
39+
executable="parameter_bridge",
40+
arguments=bluerov2_arguments,
41+
output="screen",
42+
)
43+
44+
mavros_file = LaunchConfiguration("mavros_file")
45+
46+
mavros_node = Node(
47+
package="mavros",
48+
executable="mavros_node",
49+
output="screen",
50+
parameters=[mavros_file, {"use_sim_time": True}],
51+
)
52+
53+
nodes = [bluerov2_bridge, mavros_node]
54+
55+
ardusub_params = LaunchConfiguration("ardusub_params").perform(context)
56+
57+
ardusub_cmd = [
58+
"ardusub -S -w -M gazebo --defaults "
59+
+ ardusub_params
60+
+ " -IO --home 44.65870,-124.06556,0.0,270.0"
61+
]
62+
63+
ardusub_process = ExecuteProcess(cmd=ardusub_cmd, shell=True, output="screen")
64+
65+
processes = [ardusub_process]
66+
67+
return nodes + processes
68+
69+
70+
def generate_launch_description():
71+
args = [
72+
DeclareLaunchArgument(
73+
"namespace",
74+
default_value="",
75+
description="Namespace",
76+
),
77+
DeclareLaunchArgument(
78+
"mavros_file",
79+
default_value=PathJoinSubstitution(
80+
[FindPackageShare("dave_robot_models"), "config", "mavros", "mavros.yaml"]
81+
),
82+
description="Path to mavros.yaml file",
83+
),
84+
DeclareLaunchArgument(
85+
"ardusub_params",
86+
default_value=PathJoinSubstitution(
87+
[FindPackageShare("dave_robot_models"), "config", "bluerov2", "ardusub.parm"]
88+
),
89+
description="Path to ardusub.parm file",
90+
),
91+
]
92+
93+
return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

‎models/dave_robot_models/config/bluerov2_heavy/ardusub.parm

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
# These settings were copied from ardupilot/Tools/autotest/default_params/sub-6dof.parm
2+
BARO_RND 0.02
3+
BATT_MONITOR 4
4+
BTN0_FUNCTION 0
5+
BTN10_SFUNCTION 0
6+
BTN10_FUNCTION 22
7+
BTN10_SFUNCTION 0
8+
BTN11_FUNCTION 42
9+
BTN11_SFUNCTION 47
10+
BTN12_FUNCTION 43
11+
BTN12_SFUNCTION 46
12+
BTN13_FUNCTION 33
13+
BTN13_SFUNCTION 45
14+
BTN14_FUNCTION 32
15+
BTN14_SFUNCTION 44
16+
BTN15_FUNCTION 0
17+
BTN15_SFUNCTION 0
18+
BTN1_FUNCTION 6
19+
BTN1_SFUNCTION 0
20+
BTN2_FUNCTION 8
21+
BTN2_SFUNCTION 0
22+
BTN3_FUNCTION 7
23+
BTN3_SFUNCTION 0
24+
BTN4_FUNCTION 4
25+
BTN4_SFUNCTION 0
26+
BTN5_FUNCTION 1
27+
BTN5_SFUNCTION 0
28+
BTN6_FUNCTION 3
29+
BTN6_SFUNCTION 0
30+
BTN7_FUNCTION 21
31+
BTN7_SFUNCTION 0
32+
BTN8_FUNCTION 48
33+
BTN8_SFUNCTION 0
34+
BTN9_FUNCTION 23
35+
BTN9_SFUNCTION 0
36+
COMPASS_OFS_X 5
37+
COMPASS_OFS_Y 13
38+
COMPASS_OFS_Z -18
39+
COMPASS_OFS2_X 5
40+
COMPASS_OFS2_Y 13
41+
COMPASS_OFS2_Z -18
42+
COMPASS_OFS3_X 5
43+
COMPASS_OFS3_Y 13
44+
COMPASS_OFS3_Z -18
45+
INS_ACCOFFS_X 0.001
46+
INS_ACCOFFS_Y 0.001
47+
INS_ACCOFFS_Z 0.001
48+
INS_ACCSCAL_X 1.001
49+
INS_ACCSCAL_Y 1.001
50+
INS_ACCSCAL_Z 1.001
51+
INS_ACC2OFFS_X 0.001
52+
INS_ACC2OFFS_Y 0.001
53+
INS_ACC2OFFS_Z 0.001
54+
INS_ACC2SCAL_X 1.001
55+
INS_ACC2SCAL_Y 1.001
56+
INS_ACC2SCAL_Z 1.001
57+
INS_ACC3OFFS_X 0.000
58+
INS_ACC3OFFS_Y 0.000
59+
INS_ACC3OFFS_Z 0.000
60+
INS_ACC3SCAL_X 1.000
61+
INS_ACC3SCAL_Y 1.000
62+
INS_ACC3SCAL_Z 1.000
63+
FRAME_CONFIG 2.000
64+
65+
# Use EK3
66+
EK2_ENABLE 0
67+
EK3_ENABLE 1
68+
AHRS_EKF_TYPE 3
69+
70+
# Enable external nav
71+
# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
72+
GPS_TYPE 0 # Disable GPS
73+
VISO_TYPE 1 # External vision
74+
EK3_SRC1_POSXY 6 # External nav
75+
EK3_SRC1_VELXY 6 # External nav
76+
EK3_SRC1_POSZ 1 # Baro is the primary z source
77+
EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
78+
COMPASS_USE 0 # Disable compass 1
79+
COMPASS_USE2 0 # Disable compass 2
80+
COMPASS_USE3 0 # Disable compass 3
81+
EK3_SRC1_YAW 6 # External nav
82+
83+
# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
84+
SIM_BARO_RND 0.01
85+
86+
# Store thruster parameters as backups
87+
SERVO1_FUNCTION 33
88+
SERVO2_FUNCTION 34
89+
SERVO3_FUNCTION 35
90+
SERVO4_FUNCTION 36
91+
SERVO5_FUNCTION 37
92+
SERVO6_FUNCTION 38
93+
SERVO7_FUNCTION 39
94+
SERVO8_FUNCTION 40

0 commit comments

Comments
 (0)