diff --git a/.devcontainer/nouveau/Dockerfile b/.devcontainer/nouveau/Dockerfile index 8122286a..6bdc0abb 100644 --- a/.devcontainer/nouveau/Dockerfile +++ b/.devcontainer/nouveau/Dockerfile @@ -1,4 +1,4 @@ -FROM ghcr.io/robotic-decision-making-lab/blue:rolling-desktop +FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop # Install ROS dependencies # This is done in a previous stage, but we include it again here in case anyone wants to diff --git a/.devcontainer/nvidia/Dockerfile b/.devcontainer/nvidia/Dockerfile index 1c55005d..2e87f47b 100644 --- a/.devcontainer/nvidia/Dockerfile +++ b/.devcontainer/nvidia/Dockerfile @@ -1,4 +1,5 @@ -FROM ghcr.io/robotic-decision-making-lab/blue:rolling-desktop-nvidia +FROM ghcr.io/jbvakshaya/blue:iron-nvidia-ompl +# FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia # Install ROS dependencies # This is done in a previous stage, but we include it again here in case anyone wants to @@ -16,6 +17,34 @@ RUN sudo apt-get -q update \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* +# Install OMPL with ROS2 +RUN curl http://repo.ros2.org/repos.key | sudo apt-key add -\ + && sudo apt-get update \ + && sudo apt-get install -y nano \ + && sudo apt-get install -y ros-iron-ompl + +# RUN cd ${USER_WORKSPACE}/src/learn_ros2 \ +# && echo "Check me" \ +# && echo ls -d -- */ + +# Install pinocchio +RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-pinocchio + +# Update rqt to find plugins work with ROS2 +RUN sudo apt update \ + && sudo apt install -y ros-${ROS_DISTRO}-rqt* + +# tf2 depedencies +# RUN sudo apt-get install ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-turtle-tf2-py ros-${ROS_DISTRO}-tf2-ros ros-${ROS_DISTRO}-tf2-tools ros-${ROS_DISTRO}-turtlesim + +# Install the learn_ros2 package +# RUN cd ${USER_WORKSPACE} \ +# && echo "Check me" \ +# && pwd \ +# && colcon build --packages-select learn_ros2 \ +# && . install/setup.bash + +RUN echo 'nvidia' > ${USER_WORKSPACE}/nvidia.txt # Install debugging/linting Python packages RUN python3 -m pip install \ pre-commit \ diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index 27f58aa6..c06cb5bb 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -14,6 +14,7 @@ "--privileged", "--volume=/tmp/.X11-unix:/tmp/.X11-unix", "--volume=/mnt/wslg:/mnt/wslg", + "--volume=/home/akshaya/Research/doc_vol/learn_ros2:/home/blue/ws_blue/src/learn_ros2", "--gpus=all" ], "containerEnv": { diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 9f38a9ee..a4a98294 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -1,10 +1,15 @@ ARG ROS_DISTRO=rolling FROM ros:$ROS_DISTRO-ros-base AS ci +FROM ros:$ROS_DISTRO-ros-base AS ci +ARG USERNAME +ARG USER_UID +ARG USER_GID ENV DEBIAN_FRONTEND=noninteractive WORKDIR /root/ws_blue COPY . src/blue +RUN echo 'ompl' > ompl.txt # Install apt packages needed for CI RUN apt-get -q update \ @@ -53,7 +58,7 @@ FROM ci AS robot # jazzy and rolling images, now includes a user "ubuntu" at UID 1000 ARG USERNAME=ubuntu ARG USER_UID=1000 -ARG USER_GID=$USER_UID +ARG USER_GID=1000 RUN echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ && chmod 0440 /etc/sudoers.d/$USERNAME \ @@ -89,6 +94,8 @@ COPY --chown=$USER_UID:$USER_GID . src/blue # Install the Python requirements that aren't available as rosdeps RUN python3 -m pip install -r $(pwd)/src/blue/requirements-build.txt +RUN sudo apt-get -q update && sudo apt-get install nano + # Install gstreamer RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ @@ -206,7 +213,7 @@ RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ && vcs import src < src/blue/sim.repos \ && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ && sudo apt-get autoremove -y \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* @@ -217,6 +224,12 @@ RUN sudo apt-get -q update \ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ && colcon build +# Install OMPL with ROS2 +RUN curl http://repo.ros2.org/repos.key | sudo apt-key add -\ + && sudo apt-get update \ + && sudo apt-get install -y nano \ + && sudo apt-get install -y ros-iron-ompl + # Setup the simulation environment variables RUN <> /home/$USERNAME/.bashrc @@ -235,6 +248,17 @@ EOT FROM desktop AS desktop-nvidia +# Add ardupilot_gazebo plugin +export GZ_SIM_SYSTEM_PLUGIN_PATH=\$HOME/ardupilot_gazebo/build:\$GZ_SIM_SYSTEM_PLUGIN_PATH + +# Add ardupilot_gazebo models and worlds +export GZ_SIM_RESOURCE_PATH=\$HOME/ardupilot_gazebo/models:\$HOME/ardupilot_gazebo/worlds:\$GZ_SIM_RESOURCE_PATH +EOT + +FROM desktop AS desktop-nvidia +ARG USERNAME +ARG USER_UID +ARG USER_GID # Install NVIDIA software RUN sudo apt-get update \ && sudo apt-get -q -y upgrade \ @@ -249,6 +273,10 @@ RUN sudo apt-get update \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* +#Install nano +RUN sudo apt-get update \ + && sudo apt-get install -y nano + # Env vars for the nvidia-container-runtime. ENV NVIDIA_VISIBLE_DEVICES=all ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute diff --git a/.docker/compose/nouveau-desktop.yaml b/.docker/compose/nouveau-desktop.yaml index d7ccecac..a2fa8907 100644 --- a/.docker/compose/nouveau-desktop.yaml +++ b/.docker/compose/nouveau-desktop.yaml @@ -1,6 +1,6 @@ services: blue: - image: ghcr.io/robotic-decision-making-lab/blue:rolling-desktop + image: ghcr.io/robotic-decision-making-lab/blue:iron-desktop environment: - DISPLAY=${DISPLAY} - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR} diff --git a/.docker/compose/nvidia-desktop.yaml b/.docker/compose/nvidia-desktop.yaml index 86526f43..0afd8fe7 100644 --- a/.docker/compose/nvidia-desktop.yaml +++ b/.docker/compose/nvidia-desktop.yaml @@ -1,6 +1,6 @@ services: blue: - image: ghcr.io/robotic-decision-making-lab/blue:rolling-desktop-nvidia + image: ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia environment: - DISPLAY=${DISPLAY} - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR} diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e8ff75ad..33ffca7f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -3,7 +3,7 @@ name: Check Implementation on: push: branches: - - main + - iron pull_request: workflow_dispatch: @@ -19,8 +19,8 @@ jobs: fail-fast: false matrix: env: - - IMAGE: rolling-ci - ROS_DISTRO: rolling + - IMAGE: iron-ci + ROS_DISTRO: iron steps: - name: Checkout repository uses: actions/checkout@v4 diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 04d38298..178b055d 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -5,7 +5,7 @@ on: - cron: "0 17 * * 6" push: branches: - - main + - iron pull_request: paths: - .docker/** @@ -22,7 +22,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [iron] runs-on: ubuntu-latest permissions: packages: write diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 2024987f..60802941 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -4,7 +4,7 @@ on: pull_request: push: branches: - - main + - iron workflow_dispatch: jobs: diff --git a/blue.repos b/blue.repos index 20a32377..619a685e 100644 --- a/blue.repos +++ b/blue.repos @@ -8,7 +8,7 @@ repositories: auv_controllers: type: git url: https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git - version: main + version: iron ardusub_driver: type: git diff --git a/blue_bringup/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml new file mode 100644 index 00000000..2bfa0089 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml @@ -0,0 +1,168 @@ +launch: + + - let: + name: model_name + value: bluerov2_heavy + + # Arguments + - arg: + name: use_camera + default: "false" + + - arg: + name: use_mocap + default: "false" + + - arg: + name: localization_source + default: gazebo + choice: + - value: gazebo + - value: mocap + - value: camera + + - arg: + name: use_sim + default: "false" + + - arg: + name: prefix + default: "" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: use_manager + default: "true" + + - arg: + name: rviz_config + default: $(find-pkg-share blue_description)/rviz/$(var model_name).rviz + + - arg: + name: mavros_file + default: $(find-pkg-share blue_description)/config/ardusub/ns_mavros.yaml + + - arg: + name: manager_file + default: $(find-pkg-share blue_description)/config/ardusub/ns_ardusub_manager.yaml + + - arg: + name: ardusub_params_file + default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm + + - arg: + name: localization_file + default: $(find-pkg-share blue_description)/config/$(var model_name)/ns_localization.yaml + + # Load the description file + - let: + name: description_file + value: $(find-pkg-share blue_description)/description/$(var model_name)/config.xacro + + - arg: + name: robot_description + default: $(command 'xacro $(var description_file) prefix:=$(var prefix) use_sim:=$(var use_sim)') + + - arg: + name: sitl_file + default: $(find-pkg-share ardusub_bringup)/launch/sitl_multi_robot.launch.yaml + + - arg: + name: ardusub_instance + default: "0" + + - arg: + name: sysid_thismav + default: "1" + + - arg: + name: rob_x + default: "0.0" + + - arg: + name: rob_y + default: "0.0" + + - arg: + name: rob_z + default: "0.0" + + # Nodes + + - node: + pkg: robot_state_publisher + exec: robot_state_publisher + param: + - name: robot_description + value: $(var robot_description) + - name: use_sim_time + value: $(var use_sim) + - name: tf_prefix + value: $(var prefix) + + - node: + pkg: rviz2 + exec: rviz2 + if: $(var use_rviz) + args: -d $(var rviz_config) + param: + - name: robot_description + value: $(var robot_description) + +# Includes + - include: + file: $(find-pkg-share ardusub_bringup)/launch/ns_ardusub.launch.yaml + arg: + - name: mavros_file + value: $(var mavros_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) + - name: manager_file + value: $(var manager_file) + - name: model_name + value: $(var model_name) + - name: use_sim + value: $(var use_sim) + - name: use_manager + value: $(var use_manager) + - name: sitl_file + value: $(var sitl_file) + - name: ardusub_instance + value: $(var ardusub_instance) + - name: sysid_thismav + value: $(var sysid_thismav) + - name: rob_x + value: $(var rob_x) + - name: rob_y + value: $(var rob_y) + - name: rob_z + value: $(var rob_z) + + - include: + file: $(find-pkg-share blue_localization)/localization.launch.py + arg: + - name: localization_source + value: $(var localization_source) + - name: use_camera + value: $(var use_camera) + - name: use_mocap + value: $(var use_mocap) + - name: use_sim_time + value: $(var use_sim) + - name: config_filepath + value: $(var localization_file) + + - include: + file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml + arg: + - name: prefix + value: $(var prefix) + + - include: + file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml + arg: + - name: prefix + value: $(var prefix) diff --git a/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml b/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml index dba2b883..20cd4b59 100644 --- a/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml @@ -1,49 +1,52 @@ launch: + - arg: + name: prefix + default: "" - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster1 - args: --x 0.14 --y -0.092 --roll -1.571 --pitch 1.571 --yaw -0.785 --frame-id base_link --child-frame-id thruster1 + args: --x 0.14 --y -0.092 --roll -1.571 --pitch 1.571 --yaw -0.785 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster1 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster2 - args: --x 0.14 --y 0.092 --roll -1.571 --pitch 1.571 --yaw -2.356 --frame-id base_link --child-frame-id thruster2 + args: --x 0.14 --y 0.092 --roll -1.571 --pitch 1.571 --yaw -2.356 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster2 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster3 - args: --x -0.15 --y -0.092 --roll -1.571 --pitch 1.571 --yaw 0.785 --frame-id base_link --child-frame-id thruster3 + args: --x -0.15 --y -0.092 --roll -1.571 --pitch 1.571 --yaw 0.785 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster3 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster4 - args: --x -0.15 --y 0.092 --roll -1.571 --pitch 1.571 --yaw 2.356 --frame-id base_link --child-frame-id thruster4 + args: --x -0.15 --y 0.092 --roll -1.571 --pitch 1.571 --yaw 2.356 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster4 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster5 - args: --x 0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster5 + args: --x 0.118 --y -0.215 --z 0.064 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster5 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster6 - args: --x 0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster6 + args: --x 0.118 --y 0.215 --z 0.064 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster6 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster7 - args: --x -0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster7 + args: --x -0.118 --y -0.215 --z 0.064 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster7 - node: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_thruster8 - args: --x -0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster8 + args: --x -0.118 --y 0.215 --z 0.064 --frame-id $(var prefix)base_link --child-frame-id $(var prefix)thruster8 diff --git a/blue_demos/CMakeLists.txt b/blue_demos/CMakeLists.txt index 977d6090..7cbd9d3c 100644 --- a/blue_demos/CMakeLists.txt +++ b/blue_demos/CMakeLists.txt @@ -16,4 +16,11 @@ install( DESTINATION share/blue_demos/teleoperation ) +install( + DIRECTORY multi_robot/control_integration + multi_robot/teleoperation + multi_robot/description + DESTINATION share/blue_demos/multi_robot +) + ament_package() diff --git a/blue_demos/control_integration/config/bluerov2_heavy_joint_traj_controller.yaml b/blue_demos/control_integration/config/bluerov2_heavy_joint_traj_controller.yaml new file mode 100644 index 00000000..f3cdc2e7 --- /dev/null +++ b/blue_demos/control_integration/config/bluerov2_heavy_joint_traj_controller.yaml @@ -0,0 +1,207 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + jt_controller: + type: "joint_trajectory_controller/JointTrajectoryController" + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +jt_controller: + ros__parameters: + action_monitor_rate: 20.0 + allow_integration_in_goal_trajectories: false + allow_nonzero_velocity_at_trajectory_end: false + allow_partial_joints_goal: false + cmd_timeout: 1.0 + joints: + - joint_x + - joint_y + - joint_z + - joint_rx + - joint_ry + - joint_rz + + command_interfaces: + - velocity + + command_joints: + - integral_sliding_mode_controller/x + - integral_sliding_mode_controller/y + - integral_sliding_mode_controller/z + - integral_sliding_mode_controller/rx + - integral_sliding_mode_controller/ry + - integral_sliding_mode_controller/rz + + state_interfaces: + - position + - velocity + + action_monitor_rate: 20.0 + + allow_partial_joints_goal: true + open_loop_control: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + joint1: + trajectory: 0.05 + goal: 0.03 + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] + tf: + base_frame: "base_link_fsd" + odom_frame: "map_ned" + hydrodynamics: + mass: 13.5 + weight: 114.80 + buoyancy: 112.80 + moments_of_inertia: [0.16, 0.16, 0.16] + added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] + center_of_buoyancy: [0.0, 0.0, 0.0] + center_of_gravity: [0.0, 0.0, 0.0] + linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] + quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + +thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - thruster1_joint + - thruster2_joint + - thruster3_joint + - thruster4_joint + - thruster5_joint + - thruster6_joint + - thruster7_joint + - thruster8_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + - thruster7_controller + - thruster8_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0] + +thruster1_controller: + ros__parameters: + thruster: thruster1_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro index 0ab9119a..da19f0c2 100644 --- a/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro +++ b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro @@ -1,12 +1,14 @@ - + thruster_hardware/ThrusterHardware ${max_set_param_attempts} + ${prefix} @@ -66,6 +68,17 @@ + + + + mavros_odom_sensor/MavrosOdomSensor + ${max_set_param_attempts} + ${mavros_odom_topic} + ${prefix} + + + + diff --git a/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro b/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro index ece97666..c25a7119 100644 --- a/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro +++ b/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro @@ -3,6 +3,11 @@ + + + + + @@ -11,12 +16,16 @@ - + - + filename="$(find + blue_demos)/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro" /> + diff --git a/blue_demos/control_integration/launch/bluerov2_heavy_with_jt_controllers.launch.py b/blue_demos/control_integration/launch/bluerov2_heavy_with_jt_controllers.launch.py new file mode 100644 index 00000000..85b2bfe7 --- /dev/null +++ b/blue_demos/control_integration/launch/bluerov2_heavy_with_jt_controllers.launch.py @@ -0,0 +1,193 @@ +# Copyright 2024, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2. + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + # The ISMC expects state information to be provided in the FSD frame + message_transformer = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("message_transforms"), + "launch", + "message_transforms.launch.py", + ] + ) + ), + launch_arguments={ + "parameters_file": PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "control_integration", + "config", + "transforms.yaml", + ] + ), + "ns": TextSubstitution(text="control_integration"), + }.items(), + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "control_integration", + "config", + "bluerov2_heavy_joint_traj_controller.yaml", + ] + ), + ], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + ) + + jt_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "jt_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + for i in range(8) # BlueROV2 Heavy has 8 thrusters + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + delay_jt_controller_spawner_after_velocity_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=velocity_controller_spawner, + on_exit=[jt_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + message_transformer, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + delay_jt_controller_spawner_after_velocity_controller_spawner, + ] + ) diff --git a/blue_demos/control_integration/launch/obstacles.launch.yaml b/blue_demos/control_integration/launch/obstacles.launch.yaml new file mode 100644 index 00000000..2239f677 --- /dev/null +++ b/blue_demos/control_integration/launch/obstacles.launch.yaml @@ -0,0 +1,18 @@ +launch: + + # Arguments + - arg: + name: gazebo_world_file + + # Nodes + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock + + # Includes + - include: + file: $(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py + arg: + - name: gz_args + value: -v 4 -r $(var gazebo_world_file) diff --git a/blue_demos/multi_robot/control_integration/config/rob_1_bluerov2_heavy_controllers.yaml b/blue_demos/multi_robot/control_integration/config/rob_1_bluerov2_heavy_controllers.yaml new file mode 100644 index 00000000..e9badfaa --- /dev/null +++ b/blue_demos/multi_robot/control_integration/config/rob_1_bluerov2_heavy_controllers.yaml @@ -0,0 +1,163 @@ +rob_1/controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +rob_1/integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] + tf: + base_frame: "rob_1/base_link_fsd" + odom_frame: "map_ned" + hydrodynamics: + mass: 13.5 + weight: 114.80 + buoyancy: 112.80 + moments_of_inertia: [0.16, 0.16, 0.16] + added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] + center_of_buoyancy: [0.0, 0.0, 0.0] + center_of_gravity: [0.0, 0.0, 0.0] + linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] + quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + +rob_1/thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - rob_1/thruster1_joint + - rob_1/thruster2_joint + - rob_1/thruster3_joint + - rob_1/thruster4_joint + - rob_1/thruster5_joint + - rob_1/thruster6_joint + - rob_1/thruster7_joint + - rob_1/thruster8_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + - thruster7_controller + - thruster8_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0] + +rob_1/thruster1_controller: + ros__parameters: + thruster: rob_1/thruster1_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster2_controller: + ros__parameters: + thruster: rob_1/thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster3_controller: + ros__parameters: + thruster: rob_1/thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster4_controller: + ros__parameters: + thruster: rob_1/thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster5_controller: + ros__parameters: + thruster: rob_1/thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster6_controller: + ros__parameters: + thruster: rob_1/thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster7_controller: + ros__parameters: + thruster: rob_1/thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_1/thruster8_controller: + ros__parameters: + thruster: rob_1/thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/multi_robot/control_integration/config/rob_1_transforms.yaml b/blue_demos/multi_robot/control_integration/config/rob_1_transforms.yaml new file mode 100644 index 00000000..fe331004 --- /dev/null +++ b/blue_demos/multi_robot/control_integration/config/rob_1_transforms.yaml @@ -0,0 +1,11 @@ +rob_1/control_integration/message_transforms: + ros__parameters: + + incoming_topics: + - /rob_1/local_position/velocity_body + + transforms: + /rob_1/local_position/velocity_body: + outgoing_topic: /rob_1/integral_sliding_mode_controller/system_state + message_type: geometry_msgs/msg/TwistStamped + frame_id: rob_1/base_link_fsd diff --git a/blue_demos/multi_robot/control_integration/config/rob_3_bluerov2_heavy_controllers.yaml b/blue_demos/multi_robot/control_integration/config/rob_3_bluerov2_heavy_controllers.yaml new file mode 100644 index 00000000..52280988 --- /dev/null +++ b/blue_demos/multi_robot/control_integration/config/rob_3_bluerov2_heavy_controllers.yaml @@ -0,0 +1,163 @@ +rob_3/controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +rob_3/integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] + tf: + base_frame: "rob_3/base_link_fsd" + odom_frame: "map_ned" + hydrodynamics: + mass: 13.5 + weight: 114.80 + buoyancy: 112.80 + moments_of_inertia: [0.16, 0.16, 0.16] + added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] + center_of_buoyancy: [0.0, 0.0, 0.0] + center_of_gravity: [0.0, 0.0, 0.0] + linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] + quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + +rob_3/thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - rob_3/thruster1_joint + - rob_3/thruster2_joint + - rob_3/thruster3_joint + - rob_3/thruster4_joint + - rob_3/thruster5_joint + - rob_3/thruster6_joint + - rob_3/thruster7_joint + - rob_3/thruster8_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + - thruster7_controller + - thruster8_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0] + +rob_3/thruster1_controller: + ros__parameters: + thruster: rob_3/thruster1_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster2_controller: + ros__parameters: + thruster: rob_3/thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster3_controller: + ros__parameters: + thruster: rob_3/thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster4_controller: + ros__parameters: + thruster: rob_3/thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster5_controller: + ros__parameters: + thruster: rob_3/thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster6_controller: + ros__parameters: + thruster: rob_3/thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster7_controller: + ros__parameters: + thruster: rob_3/thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +rob_3/thruster8_controller: + ros__parameters: + thruster: rob_3/thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/multi_robot/control_integration/config/rob_3_transforms.yaml b/blue_demos/multi_robot/control_integration/config/rob_3_transforms.yaml new file mode 100644 index 00000000..75ee4f0c --- /dev/null +++ b/blue_demos/multi_robot/control_integration/config/rob_3_transforms.yaml @@ -0,0 +1,11 @@ +rob_3/control_integration/message_transforms: + ros__parameters: + + incoming_topics: + - /rob_3/local_position/velocity_body + + transforms: + /rob_3/local_position/velocity_body: + outgoing_topic: /rob_3/integral_sliding_mode_controller/system_state + message_type: geometry_msgs/msg/TwistStamped + frame_id: rob_3/base_link_fsd diff --git a/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_controllers.launch.py b/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_controllers.launch.py new file mode 100644 index 00000000..d0600f47 --- /dev/null +++ b/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_controllers.launch.py @@ -0,0 +1,189 @@ +# Copyright 2024, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node, PushROSNamespace +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2. + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="rob_1/", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + # The ISMC expects state information to be provided in the FSD frame + message_transformer = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("message_transforms"), + "launch", + "message_transforms.launch.py", + ] + ) + ), + launch_arguments={ + "parameters_file": PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "multi_robot", + "control_integration", + "config", + "rob_1_transforms.yaml", + ] + ), + "ns": TextSubstitution(text="control_integration"), + }.items(), + ) + + message_transformer_with_ns = GroupAction( + actions=[ + PushROSNamespace("rob_1"), + message_transformer, + ] + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + namespace="rob_1", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "multi_robot", + "control_integration", + "config", + "rob_1_bluerov2_heavy_controllers.yaml", + ] + ), + ], + remappings=[ + ("controller_manager/robot_description", "robot_description"), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_1", + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_1", + ], + ) + for i in range(8) # BlueROV2 Heavy has 8 thrusters + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_1", + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + message_transformer_with_ns, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) diff --git a/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_demo.launch.yaml b/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_demo.launch.yaml new file mode 100644 index 00000000..44ee01db --- /dev/null +++ b/blue_demos/multi_robot/control_integration/launch/rob_1_bluerov2_heavy_demo.launch.yaml @@ -0,0 +1,118 @@ +launch: + + - arg: + name: namespace + default: "rob_1" + + - arg: + name: gazebo_world_file + default: $(find-pkg-share blue_demos)/multi_robot/description/gazebo/worlds/underwater_with_obstacles.world + + - arg: + name: use_sim + default: "false" + + - arg: + name: prefix + default: "rob_1/" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: mavros_file + default: $(find-pkg-share blue_demos)/multi_robot/description/config/ardusub/rob_1_mavros.yaml + + - let: + name: manager_file + value: $(find-pkg-share blue_demos)/multi_robot/description/config/ardusub/rob_1_ardusub_manager.yaml + + - let: + name: localization_file + value: $(find-pkg-share blue_demos)/multi_robot/description/config/bluerov2_heavy/rob_1_localization.yaml + + - let: + name: ardusub_instance + value: "0" + + - let: + name: sysid_thismav + value: "1" + + - let: + name: rob_x + value: "0.0" + + - let: + name: rob_y + value: "4.0" + + - let: + name: rob_z + value: "0.0" + + - let: + name: sitl_file + value: $(find-pkg-share ardusub_bringup)/launch/sitl_multi_robot.launch.yaml + + - let: + name: fdm_port_in + value: "9002" + + - let: + name: fdm_port_out + value: "9003" + + - let: + name: use_mavros_odom + value: "true" + + - let: + name: mavros_odom_topic + value: /rob_1/local_position/odom + + - group: + - push-ros-namespace: + namespace: "$(var namespace)" + + - let: + name: description_file + value: $(find-pkg-share blue_demos)/control_integration/description/urdf/bluerov2_heavy.config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) prefix:=$(var prefix) use_sim:=$(var use_sim) fdm_port_in:=$(var fdm_port_in) fdm_port_out:=$(var fdm_port_out) use_mavros_odom:=$(var use_mavros_odom) mavros_odom_topic:=$(var mavros_odom_topic)') + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml + arg: + - name: robot_description + value: $(var robot_description) + - name: prefix + value: $(var prefix) + - name: mavros_file + value: $(var mavros_file) + - name: manager_file + value: $(var manager_file) + - name: sitl_file + value: $(var sitl_file) + - name: ardusub_instance + value: $(var ardusub_instance) + - name: sysid_thismav + value: $(var sysid_thismav) + - name: localization_file + value: $(var localization_file) + - name: rob_x + value: $(var rob_x) + - name: rob_y + value: $(var rob_y) + - name: rob_z + value: $(var rob_z) + + #Spawn obstacles + - include: + file: $(find-pkg-share blue_demos)/control_integration/launch/obstacles.launch.yaml + arg: + - name: gazebo_world_file + value: $(var gazebo_world_file) diff --git a/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_controllers.launch.py b/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_controllers.launch.py new file mode 100644 index 00000000..3d10968d --- /dev/null +++ b/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_controllers.launch.py @@ -0,0 +1,189 @@ +# Copyright 2024, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node, PushROSNamespace +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2. + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="rob_3/", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + # The ISMC expects state information to be provided in the FSD frame + message_transformer = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("message_transforms"), + "launch", + "message_transforms.launch.py", + ] + ) + ), + launch_arguments={ + "parameters_file": PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "multi_robot", + "control_integration", + "config", + "rob_3_transforms.yaml", + ] + ), + "ns": TextSubstitution(text="control_integration"), + }.items(), + ) + + message_transformer_with_ns = GroupAction( + actions=[ + PushROSNamespace("rob_3"), + message_transformer, + ] + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + namespace="rob_3", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("blue_demos"), + "multi_robot", + "control_integration", + "config", + "rob_3_bluerov2_heavy_controllers.yaml", + ] + ), + ], + remappings=[ + ("controller_manager/robot_description", "robot_description"), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_3", + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_3", + ], + ) + for i in range(8) # BlueROV2 Heavy has 8 thrusters + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + "--namespace", + "rob_3", + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + message_transformer_with_ns, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) diff --git a/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_demo.launch.yaml b/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_demo.launch.yaml new file mode 100644 index 00000000..2f8418a3 --- /dev/null +++ b/blue_demos/multi_robot/control_integration/launch/rob_3_bluerov2_heavy_demo.launch.yaml @@ -0,0 +1,110 @@ +launch: + + - arg: + name: namespace + default: "rob_3" + + - arg: + name: gazebo_world_file + default: $(find-pkg-share blue_demos)/multi_robot/description/gazebo/worlds/underwater_with_obstacles.world + + - arg: + name: use_sim + default: "false" + + - arg: + name: prefix + default: "rob_3/" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: mavros_file + default: $(find-pkg-share blue_description)/config/ardusub/rob_3_mavros.yaml + + - let: + name: manager_file + value: $(find-pkg-share blue_description)/config/ardusub/ROB_3_ardusub_manager.yaml + + - let: + name: localization_file + value: $(find-pkg-share blue_description)/config/bluerov2_heavy/ROB_3_localization.yaml + + - let: + name: ardusub_instance + value: "1" + + - let: + name: sysid_thismav + value: "2" + + - let: + name: rob_x + value: "0.0" + + - let: + name: rob_y + value: "0.0" + + - let: + name: rob_z + value: "0.0" + + - let: + name: sitl_file + value: $(find-pkg-share ardusub_bringup)/launch/sitl_multi_robot.launch.yaml + + - let: + name: fdm_port_in + value: "9012" + + - let: + name: fdm_port_out + value: "9013" + + - group: + - push-ros-namespace: + namespace: "$(var namespace)" + + - let: + name: description_file + value: $(find-pkg-share blue_demos)/control_integration/description/urdf/bluerov2_heavy.config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) prefix:=$(var prefix) use_sim:=$(var use_sim) fdm_port_in:=$(var fdm_port_in) fdm_port_out:=$(var fdm_port_out)') + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml + arg: + - name: robot_description + value: $(var robot_description) + - name: prefix + value: $(var prefix) + - name: mavros_file + value: $(var mavros_file) + - name: manager_file + value: $(var manager_file) + - name: sitl_file + value: $(var sitl_file) + - name: ardusub_instance + value: $(var ardusub_instance) + - name: sysid_thismav + value: $(var sysid_thismav) + - name: localization_file + value: $(var localization_file) + - name: rob_x + value: $(var rob_x) + - name: rob_y + value: $(var rob_y) + - name: rob_z + value: $(var rob_z) + + #Spawn obstacles + # - include: + # file: $(find-pkg-share blue_demos)/control_integration/launch/obstacles.launch.yaml + # arg: + # - name: gazebo_world_file + # value: $(var gazebo_world_file) diff --git a/blue_demos/multi_robot/description/config/ardusub/rob_1_ardusub_manager.yaml b/blue_demos/multi_robot/description/config/ardusub/rob_1_ardusub_manager.yaml new file mode 100644 index 00000000..71ee336c --- /dev/null +++ b/blue_demos/multi_robot/description/config/ardusub/rob_1_ardusub_manager.yaml @@ -0,0 +1,9 @@ +rob_1: + ardusub_manager: + ros__parameters: + message_intervals: + ids: [31, 32] + rates: [50.0, 50.0] + set_ekf_origin: true + publish_tf: false + prefix: "rob_1/" diff --git a/blue_demos/multi_robot/description/config/ardusub/rob_1_mavros.yaml b/blue_demos/multi_robot/description/config/ardusub/rob_1_mavros.yaml new file mode 100644 index 00000000..e2268fca --- /dev/null +++ b/blue_demos/multi_robot/description/config/ardusub/rob_1_mavros.yaml @@ -0,0 +1,49 @@ +rob_1: + mavros: + ros__parameters: + system_id: 255 + base_link_frame_id: rob_1/base_link + plugin_allowlist: + - sys_status + - command + - imu + - rc_io + - param + - vision* + - local* + - global_position + + mavros_node: + ros__parameters: + base_link_frame: "rob_1/base_link" + map_frame: "map" + odom_frame: "odom" + fcu_url: "tcp://localhost:5760" + # gcs_url: "udp://@localhost:14550" + gcs_url: "" + tgt_system: 1 + + local_position: + ros__parameters: + frame_id: "map" + tf: + child_frame_id: "rob_1/base_link" + send: false + + imu: + ros__parameters: + frame_id: "rob_1/base_link" + + global_position: + ros__parameters: + child_frame_id: "rob_1/base_link" + frame_id: "map" + tf: + child_frame_id: "rob_1/base_link" + global_frame_id: "earth" + send: false + + vision_pose: + ros__parameters: + tf/child_frame_id: "rob_1/vision_estimate" + tf/frame_id: map diff --git a/blue_demos/multi_robot/description/config/ardusub/rob_3_ardusub_manager.yaml b/blue_demos/multi_robot/description/config/ardusub/rob_3_ardusub_manager.yaml new file mode 100644 index 00000000..8e414ea3 --- /dev/null +++ b/blue_demos/multi_robot/description/config/ardusub/rob_3_ardusub_manager.yaml @@ -0,0 +1,9 @@ +rob_3: + ardusub_manager: + ros__parameters: + message_intervals: + ids: [31, 32] + rates: [50.0, 50.0] + set_ekf_origin: true + publish_tf: false + prefix: "rob_3/" diff --git a/blue_demos/multi_robot/description/config/ardusub/rob_3_mavros.yaml b/blue_demos/multi_robot/description/config/ardusub/rob_3_mavros.yaml new file mode 100644 index 00000000..d74a12de --- /dev/null +++ b/blue_demos/multi_robot/description/config/ardusub/rob_3_mavros.yaml @@ -0,0 +1,48 @@ +rob_3: + mavros: + ros__parameters: + system_id: 255 + base_link_frame_id: rob_3/base_link + plugin_allowlist: + - sys_status + - command + - imu + - rc_io + - param + - vision* + - local* + - global_position + + mavros_node: + ros__parameters: + base_link_frame: "rob_3/base_link" + map_frame: "map" + odom_frame: "odom" + fcu_url: "tcp://localhost:5770" + gcs_url: "udp://@localhost:14550" + tgt_system: 2 + + local_position: + ros__parameters: + frame_id: "map" + tf: + child_frame_id: "rob_3/base_link" + send: false + + imu: + ros__parameters: + frame_id: "rob_3/base_link" + + global_position: + ros__parameters: + child_frame_id: "rob_3/base_link" + frame_id: "map" + tf: + child_frame_id: "rob_3/base_link" + global_frame_id: "earth" + send: false + + vision_pose: + ros__parameters: + tf/child_frame_id: "rob_3/vision_estimate" + tf/frame_id: map diff --git a/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_1_localization.yaml b/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_1_localization.yaml new file mode 100644 index 00000000..816acf82 --- /dev/null +++ b/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_1_localization.yaml @@ -0,0 +1,39 @@ +rob_1: + camera: + ros__parameters: + port: 5600 + distortion_model: "plumb_bob" + frame: + height: 1080 + width: 1920 + camera_matrix: + [1078.17559, 0.0, 1010.57086, + 0.0, 1076.46176, 463.06243, + 0.0, 0.0, 1.0] + distortion_coefficients: + [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] + projection_matrix: + [1108.25366, 0.0, 1003.75555, 0.0, + 0.0, 1108.39001, 456.92861, 0.0, + 0.0, 0.0, 1.0, 0.0] + + qualisys_mocap: + ros__parameters: + ip: "192.168.254.1" + port: 22223 + version: "1.22" + body: "bluerov" + prefix: "rob_1/" + + qualisys_localizer: + ros__parameters: + update_rate: 30.0 + body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node + prefix: "rob_1/" + + gazebo_localizer: + ros__parameters: + update_rate: 30.0 + gazebo_odom_topic: "/model/rob_1/bluerov2_heavy/odometry" + prefix: "rob_1/" + diff --git a/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_3_localization.yaml b/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_3_localization.yaml new file mode 100644 index 00000000..cc774d4d --- /dev/null +++ b/blue_demos/multi_robot/description/config/bluerov2_heavy/rob_3_localization.yaml @@ -0,0 +1,39 @@ +rob_3: + camera: + ros__parameters: + port: 5600 + distortion_model: "plumb_bob" + frame: + height: 1080 + width: 1920 + camera_matrix: + [1078.17559, 0.0, 1010.57086, + 0.0, 1076.46176, 463.06243, + 0.0, 0.0, 1.0] + distortion_coefficients: + [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] + projection_matrix: + [1108.25366, 0.0, 1003.75555, 0.0, + 0.0, 1108.39001, 456.92861, 0.0, + 0.0, 0.0, 1.0, 0.0] + + qualisys_mocap: + ros__parameters: + ip: "192.168.254.1" + port: 22223 + version: "1.22" + body: "bluerov" + prefix: "rob_3/" + + qualisys_localizer: + ros__parameters: + update_rate: 30.0 + body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node + prefix: "rob_3/" + + gazebo_localizer: + ros__parameters: + update_rate: 30.0 + gazebo_odom_topic: "/model/rob_3/bluerov2_heavy/odometry" + prefix: "rob_3/" + diff --git a/blue_demos/multi_robot/description/gazebo/worlds/underwater_with_obstacles.world b/blue_demos/multi_robot/description/gazebo/worlds/underwater_with_obstacles.world new file mode 100644 index 00000000..5fddf90f --- /dev/null +++ b/blue_demos/multi_robot/description/gazebo/worlds/underwater_with_obstacles.world @@ -0,0 +1,553 @@ + + + + + + + + + ogre2 + + + + + + + + + + + + + + 1000 + + 0 + 1 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun + + + + model://sand_heightmap + 0 7 -10 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Coast Water + + 0 0 0 0 0 0 + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + + 50 0 150 0 0 0 + 1 1 1 1 + .1 .1 .1 1 + 0.3 0.3 -1 + false + + + + + 6 0 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 970.0 + + + + + 1 1 1 + + + + + + + + 1 2 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + true + 6 -6 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 900.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + true + 6 6 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 900.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + + 4 -3 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 971.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + + 4 3 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 965.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + + 3 -3 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 965.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + + 2 -4 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 968.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + + 2 0 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 969.0 + + + + + 1 1 1 + + + + + + + + 1 4 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + true + 1 4 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 900.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + true + 1 -3 0 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 900.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + false + false + false + false + true + true + + + + + diff --git a/blue_demos/multi_robot/teleoperation/config/rob_1_transforms.yaml b/blue_demos/multi_robot/teleoperation/config/rob_1_transforms.yaml new file mode 100644 index 00000000..beef87dd --- /dev/null +++ b/blue_demos/multi_robot/teleoperation/config/rob_1_transforms.yaml @@ -0,0 +1,10 @@ +rob_1/message_transforms: + ros__parameters: + + incoming_topics: + - /rob_1/cmd_vel + + transforms: + /rob_1/cmd_vel: + outgoing_topic: /rob_1/integral_sliding_mode_controller/reference + message_type: geometry_msgs/msg/Twist diff --git a/blue_demos/multi_robot/teleoperation/config/rob_3_transforms.yaml b/blue_demos/multi_robot/teleoperation/config/rob_3_transforms.yaml new file mode 100644 index 00000000..8409e03d --- /dev/null +++ b/blue_demos/multi_robot/teleoperation/config/rob_3_transforms.yaml @@ -0,0 +1,10 @@ +rob_3/message_transforms: + ros__parameters: + + incoming_topics: + - /rob_3/cmd_vel + + transforms: + /rob_3/cmd_vel: + outgoing_topic: /rob_3/integral_sliding_mode_controller/reference + message_type: geometry_msgs/msg/Twist diff --git a/blue_description/config/ardusub/ns_ardusub_manager.yaml b/blue_description/config/ardusub/ns_ardusub_manager.yaml new file mode 100644 index 00000000..71ee336c --- /dev/null +++ b/blue_description/config/ardusub/ns_ardusub_manager.yaml @@ -0,0 +1,9 @@ +rob_1: + ardusub_manager: + ros__parameters: + message_intervals: + ids: [31, 32] + rates: [50.0, 50.0] + set_ekf_origin: true + publish_tf: false + prefix: "rob_1/" diff --git a/blue_description/config/ardusub/ns_mavros.yaml b/blue_description/config/ardusub/ns_mavros.yaml new file mode 100644 index 00000000..a7171957 --- /dev/null +++ b/blue_description/config/ardusub/ns_mavros.yaml @@ -0,0 +1,48 @@ +rob_1: + mavros: + ros__parameters: + system_id: 255 + base_link_frame_id: rob_1/base_link + plugin_allowlist: + - sys_status + - command + - imu + - rc_io + - param + - vision* + - local* + - global_position + + mavros_node: + ros__parameters: + base_link_frame: "rob_1/base_link" + map_frame: "map" + odom_frame: "odom" + fcu_url: "tcp://localhost" + gcs_url: "udp://@localhost:14550" + tgt_system: 1 + + local_position: + ros__parameters: + frame_id: "map" + tf: + child_frame_id: "rob_1/base_link" + send: false + + imu: + ros__parameters: + frame_id: "rob_1/base_link" + + global_position: + ros__parameters: + child_frame_id: "rob_1/base_link" + frame_id: "map" + tf: + child_frame_id: "rob_1/base_link" + global_frame_id: "earth" + send: false + + vision_pose: + ros__parameters: + tf/child_frame_id: "rob_1/vision_estimate" + tf/frame_id: map diff --git a/blue_description/config/bluerov2_heavy/ns_localization.yaml b/blue_description/config/bluerov2_heavy/ns_localization.yaml new file mode 100644 index 00000000..501c0338 --- /dev/null +++ b/blue_description/config/bluerov2_heavy/ns_localization.yaml @@ -0,0 +1,38 @@ +rob_1: + camera: + ros__parameters: + port: 5600 + distortion_model: "plumb_bob" + frame: + height: 1080 + width: 1920 + camera_matrix: + [1078.17559, 0.0, 1010.57086, + 0.0, 1076.46176, 463.06243, + 0.0, 0.0, 1.0] + distortion_coefficients: + [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] + projection_matrix: + [1108.25366, 0.0, 1003.75555, 0.0, + 0.0, 1108.39001, 456.92861, 0.0, + 0.0, 0.0, 1.0, 0.0] + + qualisys_mocap: + ros__parameters: + ip: "192.168.254.1" + port: 22223 + version: "1.22" + body: "bluerov" + prefix: "rob_1/" + + qualisys_localizer: + ros__parameters: + update_rate: 30.0 + body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node + prefix: "rob_1/" + + gazebo_localizer: + ros__parameters: + update_rate: 30.0 + gazebo_odom_topic: "/model/rob_1/bluerov2_heavy/odometry" + prefix: "rob_1/" diff --git a/blue_description/description/bluerov2_heavy/gazebo.xacro b/blue_description/description/bluerov2_heavy/gazebo.xacro index 011dc392..b7c67888 100644 --- a/blue_description/description/bluerov2_heavy/gazebo.xacro +++ b/blue_description/description/bluerov2_heavy/gazebo.xacro @@ -1,13 +1,14 @@ - + - + 127.0.0.1 - 9002 + ${fdm_port_in} + ${fdm_port_out} 5 1 @@ -20,7 +21,7 @@ 0 0 0 3.142 0 1.571 - imu_sensor + ${prefix}imu_sensor @@ -128,7 +129,7 @@ - + 0 0 0 3.142 0 0 @@ -149,9 +150,14 @@ + + - + diff --git a/blue_description/description/bluerov2_heavy/urdf.xacro b/blue_description/description/bluerov2_heavy/urdf.xacro index 89f91c63..3c2916fb 100644 --- a/blue_description/description/bluerov2_heavy/urdf.xacro +++ b/blue_description/description/bluerov2_heavy/urdf.xacro @@ -26,7 +26,7 @@ - + diff --git a/blue_description/description/camera/gazebo.xacro b/blue_description/description/camera/gazebo.xacro index 0bd0aa7a..762bce7c 100644 --- a/blue_description/description/camera/gazebo.xacro +++ b/blue_description/description/camera/gazebo.xacro @@ -1,19 +1,19 @@ - + - + - + 1.0855 @@ -28,7 +28,7 @@ true 10 true - /camera/image_raw + /${prefix}camera/image_raw diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index a12a65f5..92007930 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -49,8 +49,10 @@ ) from scipy.spatial.transform import Rotation as R from sensor_msgs.msg import CameraInfo, Image -from tf2_ros import TransformException # type: ignore -from tf2_ros import Time +from tf2_ros import ( + Time, + TransformException, # type: ignore +) from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener @@ -73,6 +75,13 @@ def __init__(self, node_name: str) -> None: Node.__init__(self, node_name) ABC.__init__(self) + # Set frame_names according to namespace + self.declare_parameter("prefix", "") + self.prefix = self.get_parameter("prefix").get_parameter_value().string_value + self.BASE_LINK_FRAME = self.prefix + self.BASE_LINK_FRAME + self.BASE_LINK_FRD_FRAME = self.prefix + self.BASE_LINK_FRD_FRAME + self.CAMERA_FRAME = self.prefix + self.CAMERA_FRAME + self.declare_parameter("update_rate", 30.0) # Provide access to TF2 @@ -143,18 +152,28 @@ def __init__(self, node_name: str) -> None: node_name: The name of the localizer node. """ super().__init__(node_name) - - # Poses are sent to the ArduPilot EKF - self.vision_pose_pub = self.create_publisher( - PoseStamped, - "/mavros/vision_pose/pose", - qos_profile_default, - ) - self.vision_pose_cov_pub = self.create_publisher( - PoseWithCovarianceStamped, - "/mavros/vision_pose/pose_cov", - qos_profile_default, - ) + if self.prefix != "": + self.vision_pose_pub = self.create_publisher( + PoseStamped, + f"/{self.prefix}vision_pose/pose", + qos_profile_default, + ) + self.vision_pose_cov_pub = self.create_publisher( + PoseWithCovarianceStamped, + f"/{self.prefix}vision_pose/pose_cov", + qos_profile_default, + ) + else: + self.vision_pose_pub = self.create_publisher( + PoseStamped, + "/mavros/vision_pose/pose", + qos_profile_default, + ) + self.vision_pose_cov_pub = self.create_publisher( + PoseWithCovarianceStamped, + "/mavros/vision_pose/pose_cov", + qos_profile_default, + ) def publish(self) -> None: """Publish a pose message to the ArduSub EKF.""" @@ -183,7 +202,9 @@ def __init__(self, node_name: str) -> None: # Twists are sent to the ArduPilot EKF self.vision_speed_pub = self.create_publisher( - TwistStamped, "/mavros/vision_speed/speed", qos_profile_default + TwistStamped, + "/mavros/vision_speed/speed", + qos_profile_default, ) self.vision_speed_cov_pub = self.create_publisher( TwistWithCovarianceStamped, @@ -475,7 +496,7 @@ def __init__(self) -> None: self.mocap_sub = self.create_subscription( PoseStamped, - f"/blue/mocap/qualisys/{body}", + "/blue/mocap/qualisys/{body}", self.update_pose_cb, qos_profile_sensor_data, ) diff --git a/blue_planner_py/LICENSE b/blue_planner_py/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/blue_planner_py/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/blue_planner_py/blue_planner_py/__init__.py b/blue_planner_py/blue_planner_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/blue_planner_py/blue_planner_py/plan_node.py b/blue_planner_py/blue_planner_py/plan_node.py new file mode 100644 index 00000000..0c473b10 --- /dev/null +++ b/blue_planner_py/blue_planner_py/plan_node.py @@ -0,0 +1,103 @@ +import rclpy +from nav_msgs.msg import Odometry +from rclpy.duration import Duration +from rclpy.node import Node +from tf_transformations import euler_from_quaternion +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +def position_to_list(position): + return [position.x, position.y, position.z] + + +def quaternion_to_list(orientation): + return [orientation.x, orientation.y, orientation.z, orientation.w] + + +class MoveBluerov(Node): + def __init__(self): + super().__init__("move_bluerov") + + self.subscription = self.create_subscription( + Odometry, "/model/bluerov2_heavy/odometry", self.odom_callback, 10 + ) + self.subscription + + self.odom_pose_position = [] + self.odom_pose_orientation_rpy = [] + + self.publisher_ = self.create_publisher( + JointTrajectory, "/jt_controller/joint_trajectory", 10 + ) + timer_period = 2.0 # seconds + self.timer = self.create_timer(timer_period, self.issue_jt_control_command) + + def odom_callback(self, msg): + self.odom_pose_position = position_to_list(msg.pose.pose.position) + self.odom_pose_orientation_rpy = list( + euler_from_quaternion(quaternion_to_list(msg.pose.pose.orientation)) + ) + + def issue_jt_control_command(self): + # TODO: Implement this method + """ + create two points, point1 and point2, with position and velocity values + """ + self.get_logger().info( + "pose: {0}, {1}".format( + self.odom_pose_position, self.odom_pose_orientation_rpy + ) + ) + joint_state = [] + joint_state_position = self.odom_pose_position.copy() + joint_state_orientation = self.odom_pose_orientation_rpy.copy() + joint_state = joint_state_position + joint_state.extend(joint_state_orientation) + + if len(joint_state) != 0: + # self.get_logger().info("planning: {0}".format(joint_state)) + point1 = JointTrajectoryPoint() + point2 = JointTrajectoryPoint() + duration1 = Duration(seconds=0.0) + duration2 = Duration(seconds=4.0) + point1.time_from_start = duration1.to_msg() + point2.time_from_start = duration2.to_msg() + point1.positions = joint_state + vel_1 = [0.0] * 6 + vel_1[0] = 0.2 + point1.velocities = vel_1 + pos_2 = joint_state.copy() + pos_2[0] = pos_2[0] + (0.2 * 4.0) + point2.positions = pos_2 + vel_2 = [0.0] * 6 + point2.velocities = vel_2 + trajectory = JointTrajectory() + trajectory.joint_names = [ + "joint_x", + "joint_y", + "joint_z", + "joint_rx", + "joint_ry", + "joint_rz", + ] + trajectory.points = [point1, point2] + trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory.header.frame_id = "base_link" + self.publisher_.publish(trajectory) + self.get_logger().info("published: {0}".format(trajectory)) + + +def main(args=None): + rclpy.init(args=args) + + stow_command = MoveBluerov() + + rclpy.spin_once(stow_command) + stow_command.issue_jt_control_command() + rclpy.spin(stow_command) + stow_command.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/blue_planner_py/blue_planner_py/publish_to_ismc_ref_node.py b/blue_planner_py/blue_planner_py/publish_to_ismc_ref_node.py new file mode 100644 index 00000000..194247ed --- /dev/null +++ b/blue_planner_py/blue_planner_py/publish_to_ismc_ref_node.py @@ -0,0 +1,63 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.node import Node + + +class MinimalPublisher(Node): + def __init__(self): + super().__init__("minimal_publisher") + self.publisher_ = self.create_publisher( + Twist, "/integral_sliding_mode_controller/reference", 10 + ) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + vel_msg = Twist() + vel_msg.linear.x = 0.5 + self.get_logger().info( + "Publishing: %f, %f, %f, %f, %f, %f" + % ( + vel_msg.linear.x, + vel_msg.linear.y, + vel_msg.linear.z, + vel_msg.angular.x, + vel_msg.angular.y, + vel_msg.angular.z, + ) + ) + self.publisher_.publish(vel_msg) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/blue_planner_py/blue_planner_py/subscribe_to_odom_node.py b/blue_planner_py/blue_planner_py/subscribe_to_odom_node.py new file mode 100644 index 00000000..3fa71003 --- /dev/null +++ b/blue_planner_py/blue_planner_py/subscribe_to_odom_node.py @@ -0,0 +1,57 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node + + +class MinimalSubscriber(Node): + def __init__(self): + super().__init__("minimal_subscriber") + self.subscription = self.create_subscription( + Odometry, "/model/bluerov2_heavy/odometry", self.listener_callback, 10 + ) + self.subscription # prevent unused variable warning + + def listener_callback(self, msg): + self.get_logger().info( + "I heard: %f, %f, %f, %f, %f, %f," + % ( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + ) + ) + + +def main(args=None): + rclpy.init(args=args) + + minimal_subscriber = MinimalSubscriber() + + rclpy.spin(minimal_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/blue_planner_py/package.xml b/blue_planner_py/package.xml new file mode 100644 index 00000000..7d331f51 --- /dev/null +++ b/blue_planner_py/package.xml @@ -0,0 +1,24 @@ + + + + blue_planner_py + 0.0.0 + python planners + blue + Apache-2.0 + + rclpy + geometry_msgs + nav_msgs + trajectory_msgs + tf_transformation + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/blue_planner_py/resource/blue_planner_py b/blue_planner_py/resource/blue_planner_py new file mode 100644 index 00000000..e69de29b diff --git a/blue_planner_py/setup.cfg b/blue_planner_py/setup.cfg new file mode 100644 index 00000000..3e2eac8d --- /dev/null +++ b/blue_planner_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/blue_planner_py +[install] +install_scripts=$base/lib/blue_planner_py diff --git a/blue_planner_py/setup.py b/blue_planner_py/setup.py new file mode 100644 index 00000000..993a9888 --- /dev/null +++ b/blue_planner_py/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = "blue_planner_py" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="blue", + maintainer_email="agrawaak@oregonstate.edu", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "plan_node = blue_planner_py.plan_node:main", + "publish_to_ismc_ref_node = blue_planner_py.publish_to_ismc_ref_node:main", + "subscribe_to_odom_node = blue_planner_py.subscribe_to_odom_node:main", + ], + }, +) diff --git a/blue_planner_py/test/test_copyright.py b/blue_planner_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/blue_planner_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/blue_planner_py/test/test_flake8.py b/blue_planner_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/blue_planner_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/blue_planner_py/test/test_pep257.py b/blue_planner_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/blue_planner_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/docs/package-lock.json b/docs/package-lock.json index c4cba4c0..da251958 100644 --- a/docs/package-lock.json +++ b/docs/package-lock.json @@ -14203,6 +14203,7 @@ "url": "https://github.com/sponsors/ai" } ], + "license": "MIT", "bin": { "nanoid": "bin/nanoid.cjs" },