Iron mp underdev tmp

.devcontainer/nvidia/Dockerfile

@@ -1,4 +1,7 @@
-FROM ghcr.io/jbvakshaya/blue:iron-nvidia-ompl
+# FROM ghcr.io/jbvakshaya/blue:iron-nvidia-ompl
+FROM ghcr.io/jbvakshaya/blue:iron-nvidia-multi-robots-recent
+# With above image we probably don't need to install any other dependencies
+
 # FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia
 
 # Install ROS dependencies
@@ -9,19 +12,19 @@ ENV USER_WORKSPACE=/home/$USERNAME/ws_blue
 WORKDIR $USER_WORKSPACE
 
 COPY --chown=$USER_UID:$USER_GID . src/blue
-RUN sudo apt-get -q update \
-    && sudo apt-get -q -y upgrade \
-    && rosdep update \
-    && 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/*
-
-# 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 sudo apt-get -q update \
+#     && sudo apt-get -q -y upgrade \
+#     && rosdep update \
+#     && 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/*
+
+# # 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" \

blue_bringup/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml

@@ -79,15 +79,15 @@ launch:
       default: "1"
 
   - arg:
-      name: rob_x
+      name: init_rob_x
       default: "0.0"
 
   - arg:
-      name: rob_y
+      name: init_rob_y
       default: "0.0"
 
   - arg:
-      name: rob_z
+      name: init_rob_z
       default: "0.0"
 
   # Nodes
@@ -134,12 +134,12 @@ launch:
           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)
+        - name: init_rob_x
+          value: $(var init_rob_x)
+        - name: init_rob_y
+          value: $(var init_rob_y)
+        - name: init_rob_z
+          value: $(var init_rob_z)
 
   - include:
       file: $(find-pkg-share blue_localization)/localization.launch.py

blue_demos/CMakeLists.txt

@@ -20,6 +20,7 @@ install(
   DIRECTORY multi_robot/control_integration
             multi_robot/teleoperation
             multi_robot/description
+            multi_robot/motion_planning
   DESTINATION share/blue_demos/multi_robot
 )
 

blue_demos/control_integration/config/bluerov2_heavy_joint_traj_controller.yaml

@@ -65,9 +65,6 @@ jt_controller:
       - position
       - velocity
 
-    action_monitor_rate: 20.0
-
-    allow_partial_joints_goal: true
     open_loop_control: true
     constraints:
       stopped_velocity_tolerance: 0.01

blue_demos/control_integration/launch/test_gz_to_ros_bridge.launch.py

@@ -0,0 +1,31 @@
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+
+# add ros_gz_bridge to the Python path
+# ros_gz_bridge_root = os.path.join(os.path.dirname(__file__), "..", "..")
+# sys.path.insert(0, os.path.abspath(ros_gz_bridge_root))
+
+# from ros_gz_bridge import mappings  # noqa: E402
+
+
+def bridge_setup(context, *args, **kwargs):
+    # gz_msgs_ver = LaunchConfiguration("gz_msgs_ver").perform(context)
+    # gz_msgs_ver = tuple(map(int, gz_msgs_ver.split(".")))
+
+    # Bridge
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/model/obs_1/pose@geometry_msgs/msg/Pose[gz.msgs.Pose"],
+        output="screen",
+    )
+    return [bridge]
+
+
+# f'/{m.unique()}@{m.ros2_string()}[{m.gz_string()}'
+#           for m in mappings(gz_msgs_ver)
+
+
+def generate_launch_description():
+    return LaunchDescription([OpaqueFunction(function=bridge_setup)])

blue_demos/multi_robot/control_integration/config/rob_1_bluerov2_heavy_jtc.yaml

@@ -0,0 +1,204 @@
+rob_1/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
+
+rob_1/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
+
+    open_loop_control: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+      joint1:
+        trajectory: 0.05
+        goal: 0.03
+
+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]