Merge pull request #21 from IOES-Lab/robot_launch

.codespellrc

@@ -0,0 +1,3 @@
+[codespell]
+skip = *.json
+ignore-words-list = parm,ned,sitl

examples/dave_demos/README.md

@@ -60,22 +60,22 @@ Before launching, ensure to build and source the workspace:
 colcon build && source install/setup.bash
 ```
 
-1. Launching REXROV in an empty world:
+1. Launching REXROV in dave_ocean_waves.world:
 
 ```bash
-ros2 launch dave_demos dave_robot.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false
+ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
 ```
 
-2. Launching REXROV in dave_ocean_waves.world:
+2. Launching BlueROV2 in dave_ocean_waves.world:
 
 ```bash
-ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
+ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=bluerov2 world_name:=dave_ocean_waves paused:=false
 ```
 
-3. Launching Slocum Glider in an empty world:
+3. Launching BlueROV2 Heavy in dave_ocean_waves.world:
 
 ```bash
-ros2 launch dave_demos dave_robot.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false
+ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=bluerov2_heavy world_name:=dave_ocean_waves paused:=false
 ```
 
 4. Launching Slocum Glider in dave_ocean_waves.world:

extras/repos/dave.jazzy.repos

@@ -13,3 +13,4 @@ repositories:
     type: git
     url: https://github.com/IOES-Lab/dave.git
     version: ros2
+

models/dave_robot_models/CMakeLists.txt

@@ -4,7 +4,7 @@ project(dave_robot_models)
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY description launch meshes
+  DIRECTORY description launch meshes config
   DESTINATION share/dave_robot_models
 )
 

models/dave_robot_models/config/bluerov2/ardusub.parm

@@ -0,0 +1,114 @@
+# These settings were copied from ardupilot/Tools/autotest/default_params/sub.parm
+BATT_MONITOR     4
+BTN0_FUNCTION    0
+BTN10_SFUNCTION  0
+BTN10_FUNCTION   22
+BTN10_SFUNCTION  0
+BTN11_FUNCTION   42
+BTN11_SFUNCTION  47
+BTN12_FUNCTION   43
+BTN12_SFUNCTION  46
+BTN13_FUNCTION   33
+BTN13_SFUNCTION  45
+BTN14_FUNCTION   32
+BTN14_SFUNCTION  44
+BTN15_FUNCTION   0
+BTN15_SFUNCTION  0
+BTN1_FUNCTION    6
+BTN1_SFUNCTION   0
+BTN2_FUNCTION    8
+BTN2_SFUNCTION   0
+BTN3_FUNCTION    7
+BTN3_SFUNCTION   0
+BTN4_FUNCTION    4
+BTN4_SFUNCTION   0
+BTN5_FUNCTION    1
+BTN5_SFUNCTION   0
+BTN6_FUNCTION    3
+BTN6_SFUNCTION   0
+BTN7_FUNCTION    21
+BTN7_SFUNCTION   0
+BTN8_FUNCTION    48
+BTN8_SFUNCTION   0
+BTN9_FUNCTION    23
+BTN9_SFUNCTION   0
+COMPASS_OFS_X   5
+COMPASS_OFS_Y   13
+COMPASS_OFS_Z   -18
+COMPASS_OFS2_X   5
+COMPASS_OFS2_Y   13
+COMPASS_OFS2_Z   -18
+COMPASS_OFS3_X   5
+COMPASS_OFS3_Y   13
+COMPASS_OFS3_Z   -18
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
+INS_ACC2OFFS_X   0.001
+INS_ACC2OFFS_Y   0.001
+INS_ACC2OFFS_Z   0.001
+INS_ACC2SCAL_X   1.001
+INS_ACC2SCAL_Y   1.001
+INS_ACC2SCAL_Z   1.001
+INS_ACC3OFFS_X   0.000
+INS_ACC3OFFS_Y   0.000
+INS_ACC3OFFS_Z   0.000
+INS_ACC3SCAL_X   1.000
+INS_ACC3SCAL_Y   1.000
+INS_ACC3SCAL_Z   1.000
+MNT_ANGMAX_TIL 4500
+MNT_ANGMIN_TIL -4500
+MNT_DEFLT_MODE 3
+MNT_JSTICK_SPD 100
+MNT_RC_IN_TILT 8
+MNT_STAB_TILT 0
+MNT_TYPE 1
+PSC_ACCZ_P   2
+PSC_ACCZ_I   4
+PSC_ACCZ_FF  0.75
+PSC_VELZ_P   8
+PSC_POSZ_P   3
+PSC_POSXY_P 2.5
+PSC_VELXY_D 0.8
+PSC_VELXY_I 0.5
+PSC_VELXY_P 5.0
+RNGFND1_MAX_CM   3000
+RNGFND1_PIN   0
+RNGFND1_SCALING   12.12
+RNGFND1_TYPE   1
+SERVO8_FUNCTION 7
+SERVO8_MAX 1900
+SERVO8_MIN 1100
+BARO_EXT_BUS 1
+
+# Use EK3
+EK2_ENABLE 0
+EK3_ENABLE 1
+AHRS_EKF_TYPE 3
+
+# Enable external nav
+# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
+GPS_TYPE 0            # Disable GPS
+VISO_TYPE 1           # External vision
+EK3_SRC1_POSXY 6      # External nav
+EK3_SRC1_VELXY 6      # External nav
+EK3_SRC1_POSZ 1       # Baro is the primary z source
+EK3_SRC1_VELZ 6       # External nav z velocity influences EKF
+COMPASS_USE 0         # Disable compass 1
+COMPASS_USE2 0        # Disable compass 2
+COMPASS_USE3 0        # Disable compass 3
+EK3_SRC1_YAW 6        # External nav
+
+# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
+SIM_BARO_RND 0.01
+
+# Store thruster parameters as backups
+SERVO1_FUNCTION 33
+SERVO2_FUNCTION 34
+SERVO3_FUNCTION 35
+SERVO4_FUNCTION 36
+SERVO5_FUNCTION 37
+SERVO6_FUNCTION 38

models/dave_robot_models/config/bluerov2/robot_config.py

@@ -0,0 +1,93 @@
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    ExecuteProcess,
+)
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    namespace = LaunchConfiguration("namespace").perform(context)
+
+    thruster_joints = []
+    for thruster in range(1, 7):
+        thruster_joints.append(f"/model/{namespace}/joint/thruster{thruster}_joint")
+
+    bluerov2_arguments = (
+        [f"{joint}/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints]
+        + [f"{joint}/ang_vel@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints]
+        + [
+            f"{joint}/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean"
+            for joint in thruster_joints
+        ]
+        + [
+            f"/model/{namespace}/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry",
+            f"/model/{namespace}/odometry_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
+            f"/model/{namespace}/pose@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V",
+            f"/model/{namespace}/imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
+            f"/model/{namespace}/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer",
+            f"/model/{namespace}/camera/image@sensor_msgs/msg/Image@gz.msgs.Image",
+            f"/model/{namespace}/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
+        ]
+    )
+
+    bluerov2_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=bluerov2_arguments,
+        output="screen",
+    )
+
+    mavros_file = LaunchConfiguration("mavros_file")
+
+    mavros_node = Node(
+        package="mavros",
+        executable="mavros_node",
+        output="screen",
+        parameters=[mavros_file, {"use_sim_time": True}],
+    )
+
+    nodes = [bluerov2_bridge, mavros_node]
+
+    ardusub_params = LaunchConfiguration("ardusub_params").perform(context)
+
+    ardusub_cmd = [
+        "ardusub -S -w -M gazebo --defaults "
+        + ardusub_params
+        + " -IO --home 44.65870,-124.06556,0.0,270.0"
+    ]
+
+    ardusub_process = ExecuteProcess(cmd=ardusub_cmd, shell=True, output="screen")
+
+    processes = [ardusub_process]
+
+    return nodes + processes
+
+
+def generate_launch_description():
+    args = [
+        DeclareLaunchArgument(
+            "namespace",
+            default_value="",
+            description="Namespace",
+        ),
+        DeclareLaunchArgument(
+            "mavros_file",
+            default_value=PathJoinSubstitution(
+                [FindPackageShare("dave_robot_models"), "config", "mavros", "mavros.yaml"]
+            ),
+            description="Path to mavros.yaml file",
+        ),
+        DeclareLaunchArgument(
+            "ardusub_params",
+            default_value=PathJoinSubstitution(
+                [FindPackageShare("dave_robot_models"), "config", "bluerov2", "ardusub.parm"]
+            ),
+            description="Path to ardusub.parm file",
+        ),
+    ]
+
+    return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

models/dave_robot_models/config/bluerov2_heavy/ardusub.parm

@@ -0,0 +1,94 @@
+# These settings were copied from ardupilot/Tools/autotest/default_params/sub-6dof.parm
+BARO_RND         0.02
+BATT_MONITOR     4
+BTN0_FUNCTION    0
+BTN10_SFUNCTION  0
+BTN10_FUNCTION   22
+BTN10_SFUNCTION  0
+BTN11_FUNCTION   42
+BTN11_SFUNCTION  47
+BTN12_FUNCTION   43
+BTN12_SFUNCTION  46
+BTN13_FUNCTION   33
+BTN13_SFUNCTION  45
+BTN14_FUNCTION   32
+BTN14_SFUNCTION  44
+BTN15_FUNCTION   0
+BTN15_SFUNCTION  0
+BTN1_FUNCTION    6
+BTN1_SFUNCTION   0
+BTN2_FUNCTION    8
+BTN2_SFUNCTION   0
+BTN3_FUNCTION    7
+BTN3_SFUNCTION   0
+BTN4_FUNCTION    4
+BTN4_SFUNCTION   0
+BTN5_FUNCTION    1
+BTN5_SFUNCTION   0
+BTN6_FUNCTION    3
+BTN6_SFUNCTION   0
+BTN7_FUNCTION    21
+BTN7_SFUNCTION   0
+BTN8_FUNCTION    48
+BTN8_SFUNCTION   0
+BTN9_FUNCTION    23
+BTN9_SFUNCTION   0
+COMPASS_OFS_X   5
+COMPASS_OFS_Y   13
+COMPASS_OFS_Z   -18
+COMPASS_OFS2_X   5
+COMPASS_OFS2_Y   13
+COMPASS_OFS2_Z   -18
+COMPASS_OFS3_X   5
+COMPASS_OFS3_Y   13
+COMPASS_OFS3_Z   -18
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
+INS_ACC2OFFS_X   0.001
+INS_ACC2OFFS_Y   0.001
+INS_ACC2OFFS_Z   0.001
+INS_ACC2SCAL_X   1.001
+INS_ACC2SCAL_Y   1.001
+INS_ACC2SCAL_Z   1.001
+INS_ACC3OFFS_X   0.000
+INS_ACC3OFFS_Y   0.000
+INS_ACC3OFFS_Z   0.000
+INS_ACC3SCAL_X   1.000
+INS_ACC3SCAL_Y   1.000
+INS_ACC3SCAL_Z   1.000
+FRAME_CONFIG     2.000
+
+# Use EK3
+EK2_ENABLE 0
+EK3_ENABLE 1
+AHRS_EKF_TYPE 3
+
+# Enable external nav
+# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
+GPS_TYPE 0            # Disable GPS
+VISO_TYPE 1           # External vision
+EK3_SRC1_POSXY 6      # External nav
+EK3_SRC1_VELXY 6      # External nav
+EK3_SRC1_POSZ 1       # Baro is the primary z source
+EK3_SRC1_VELZ 6       # External nav z velocity influences EKF
+COMPASS_USE 0         # Disable compass 1
+COMPASS_USE2 0        # Disable compass 2
+COMPASS_USE3 0        # Disable compass 3
+EK3_SRC1_YAW 6        # External nav
+
+# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
+SIM_BARO_RND 0.01
+
+# Store thruster parameters as backups
+SERVO1_FUNCTION 33
+SERVO2_FUNCTION 34
+SERVO3_FUNCTION 35
+SERVO4_FUNCTION 36
+SERVO5_FUNCTION 37
+SERVO6_FUNCTION 38
+SERVO7_FUNCTION 39
+SERVO8_FUNCTION 40