Fixed dependency issues and updated package.xml, cmakelists, README

.docker/Dockerfile

@@ -85,8 +85,8 @@ RUN git clone https://github.com/acados/acados.git \
 # Install acados python interface
 WORKDIR /home/$USERNAME
 RUN pip install -e /home/$USERNAME/acados/interfaces/acados_template \
-    && export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/home/$USERNAME/acados/lib" \
-    && export ACADOS_SOURCE_DIR="/home/$USERNAME/acados"
+    && echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/home/$USERNAME/acados/lib"" >> /home/$USERNAME/.bashrc \
+    && echo "ACADOS_SOURCE_DIR="/home/$USERNAME/acados"" >> /home/$USERNAME/.bashrc
 
 
 ENV USER_WORKSPACE=/home/$USERNAME/ws_dock

README.md

@@ -21,7 +21,8 @@ This package was developed to control a BlueROV2 to perform autonomous underwate
 
 - Before proceeding further, make sure that all the steps mentioned in the guide for [Software Setup](https://bluerobotics.com/learn/bluerov2-software-setup
 ) has been followed.
-- From a terminal, run `roslaunch bluerov2_dock mission_control.launch`
+- Open a terminal and source the workspace: `source ~/ws_dock/devel/setup.bash`
+- Run `roslaunch docking_control mission_control.launch`
   - If the terminal initially outputs *Controller error:'joy'*, move the sticks to clear the error.
   - Press button "A" on the joystick to enable autonomous docking mode.
     - To switch back to manual mode, move either of the sticks in any direction.

docking_control/CMakeLists.txt

@@ -68,15 +68,15 @@ ${catkin_INCLUDE_DIRS}
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-  src/docking_control/mission_control.py
-  src/docking_control/pwm_publish_node.py
-  src/docking_control/video.py
-  src/docking_control/manager.py
-  src/docking_control/auto_dock.py
-  src/docking_control/mpc_casadi.py
-  src/docking_control/mpc_acados.py
-  src/docking_control/auv_hinsdale.py
-  src/docking_control/marker_detection.py
+  src/mission_control.py
+  src/pwm_publish_node.py
+  src/video.py
+  src/manager.py
+  src/auto_dock.py
+  src/mpc_casadi.py
+  src/mpc_acados.py
+  src/auv_hinsdale.py
+  src/marker_detection.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

docking_control/package.xml

@@ -32,6 +32,9 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>xacro</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>joystick_drivers</depend>
   <depend>cv_bridge</depend>
 
   <exec_depend>mavros</exec_depend>

docking_control/src/docking_control/auto_dock.py → docking_control/src/auto_dock.py

@@ -2,15 +2,14 @@
 import time
 import numpy as np
 import rospy
+import sys
 
-# sys.path.insert(
-#     0, "/home/darth/workspace/bluerov2_ws/src/docking_control/src/docking_control"
-# )
+sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
 
-from auv_hinsdale import AUV
+from auv_hinsdale import AUV  # noqa: E402
 
 # from mpc_casadi import MPC
-from mpc_acados import MPC
+from mpc_acados import MPC  # noqa: E402
 
 
 class MPControl:
@@ -20,11 +19,11 @@ def __init__(self):
         """
         cwd = os.path.dirname(__file__)
 
-        # auv_yaml = cwd + "/../../config/auv_bluerov2.yaml"
-        # mpc_yaml = cwd + "/../../config/mpc_bluerov2.yaml"
+        # auv_yaml = cwd + "/../config/auv_bluerov2.yaml"
+        # mpc_yaml = cwd + "/../config/mpc_bluerov2.yaml"
 
-        auv_yaml = cwd + "/../../config/auv_bluerov2_heavy.yaml"
-        mpc_yaml = cwd + "/../../config/mpc_bluerov2_heavy.yaml"
+        auv_yaml = cwd + "/../config/auv_bluerov2_heavy.yaml"
+        mpc_yaml = cwd + "/../config/mpc_bluerov2_heavy.yaml"
 
         # Change these values as desired
         self.tolerance = 0.05

docking_control/src/docking_control/marker_detection.py → docking_control/src/marker_detection.py

@@ -105,7 +105,7 @@ def __init__(self):
     def load_camera_config(self):
         """This function is used to load the camera calibration parameters."""
         cwd = os.path.dirname(__file__)
-        filename = cwd + "/../../config/in_air/ost.yaml"
+        filename = cwd + "/../config/in_air/ost.yaml"
         f = open(filename, "r")
         camera_params = yaml.load(f.read(), Loader=yaml.SafeLoader)
         self.cam_mat = np.array(

docking_control/src/docking_control/mission_control.py → docking_control/src/mission_control.py

@@ -5,7 +5,6 @@
 import numpy as np
 import pandas as pd
 import os
-from auto_dock import MPControl
 from std_msgs.msg import Float32MultiArray, MultiArrayDimension
 from geometry_msgs.msg import PoseStamped, WrenchStamped
 from sensor_msgs.msg import Joy, BatteryState, FluidPressure
@@ -17,6 +16,11 @@
 from std_srvs.srv import SetBool
 from scipy.spatial.transform import Rotation as R
 
+import sys
+
+sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
+from auto_dock import MPControl  # noqa: E402
+
 
 class BlueROV2:
     def __init__(self) -> None:
@@ -78,7 +82,7 @@ def __init__(self) -> None:
     def load_pwm_lookup(self):
         """Load the lookup table for converting thrust to pwm values"""
         cwd = os.path.dirname(__file__)
-        csv = pd.read_csv(cwd + "/../../data/T200_data_16V.csv")
+        csv = pd.read_csv(cwd + "/../data/T200_data_16V.csv")
 
         thrust_vals = csv["Force"].tolist()
         neg_thrust = [i for i in thrust_vals if i < 0]

docking_control/src/docking_control/mpc_acados.py → docking_control/src/mpc_acados.py

@@ -2,7 +2,10 @@
 import yaml
 from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
 from casadi import evalf, SX, mtimes, pinv
-from auv_hinsdale import AUV
+import sys
+
+sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
+from auv_hinsdale import AUV  # noqa: E402
 
 
 class MPC:

docking_control/src/docking_control/mpc_casadi.py → docking_control/src/mpc_casadi.py

@@ -1,7 +1,10 @@
 import yaml
 import numpy as np
 from casadi import SX, Function, integrator, Opti, evalf, mtimes, vertcat, pinv
-from auv_hinsdale import AUV
+import sys
+
+sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
+from auv_hinsdale import AUV  # noqa: E402
 
 
 class MPC(object):