Fix controller_manager (#284)

* bt fix

* Remove mep3.repos, fix bt and params in robot launch file

* fix

* fix

---------

Co-authored-by: Darko Lukic <[email protected]>

docker/Dockerfile.base

@@ -13,36 +13,39 @@ RUN useradd -d /memristor -m \
     echo "memristor ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers
 
 # Essentials
-RUN apt-get update && apt-get install -y \
-    ros-humble-navigation2 \
-    ros-humble-nav2-bringup \
-    ros-humble-rviz2 \
-    ros-humble-teleop-twist-keyboard \
-    ros-humble-dynamixel-sdk \
-    ros-humble-can-msgs \
-    ros-humble-ruckig \
-    ros-humble-laser-filters \
-    ros-humble-domain-bridge \
-    ros-humble-rmw-cyclonedds-cpp \
-    ros-humble-ros2-control \
-    ros-humble-ros2-controllers \
-    ros-humble-rqt-common-plugins \
-    ros-humble-webots-ros2 \
-    ros-humble-dynamixel-workbench-toolbox \
-    libopencv-dev \
-    python3-pip \
-    python3-pil \
-    alsa \
-    libxshmfence1 \
-    libgtk-3-dev \
-    git \
-    git-lfs \
-    curl \
-    wget \
-    vim \
-    rsync \
-    dialog \
-    fish
+RUN apt-get update && apt-get install -y -o Dpkg::Options::="--force-overwrite" \
+        ros-humble-navigation2 \
+        ros-humble-nav2-bringup \
+        ros-humble-rviz2 \
+        ros-humble-teleop-twist-keyboard \
+        ros-humble-dynamixel-sdk \
+        ros-humble-can-msgs \
+        ros-humble-ruckig \
+        ros-humble-laser-filters \
+        ros-humble-domain-bridge \
+        ros-humble-rmw-cyclonedds-cpp \
+        ros-humble-ros2-control \
+        ros-humble-ros2-controllers \
+        ros-humble-rqt-common-plugins \
+        ros-humble-webots-ros2 \
+        ros-humble-dynamixel-workbench-toolbox \ 
+        ros-humble-behaviortree-cpp \
+        libopencv-dev \
+        python3-pip \
+        python3-pil \
+        alsa \
+        libxshmfence1 \
+        libgtk-3-dev \
+        git \
+        git-lfs \
+        curl \
+        wget \
+        vim \
+        rsync \
+        dialog \
+        fish
+
+
 
 # VS Code
 RUN curl -L -o /tmp/vscode.deb \
@@ -58,14 +61,14 @@ RUN su memristor -c 'code --install-extension eamodio.gitlens' && \
 
 # Webots
 RUN curl -L -o /tmp/webots.deb \
-    'https://github.com/cyberbotics/webots/releases/download/R2023a/webots_2023a_amd64.deb' && \
+    'https://github.com/cyberbotics/webots/releases/download/R2023b/webots_2023b_amd64.deb' && \
     apt-get install -y /tmp/webots.deb && \
     rm -f /tmp/webots.deb && \
     mkdir -p /memristor/.config/Cyberbotics
 
 RUN python3 -m pip install scipy transforms3d
 
-# HOTFIX: https://github.com/ros-controls/ros2_controllers/issues/482
+#HOTFIX: https://github.com/ros-controls/ros2_controllers/issues/482
 RUN wget -O /tmp/diff_drive_controller.deb http://snapshots.ros.org/humble/2022-11-23/ubuntu/pool/main/r/ros-humble-diff-drive-controller/ros-humble-diff-drive-controller_2.12.0-1jammy.20221108.202153_amd64.deb && \
     apt install -y --allow-downgrades /tmp/diff_drive_controller.deb && \
     rm -f /tmp/diff_drive_controller.deb

docker/config/bashrc

@@ -1,6 +1,9 @@
 export WEBOTS_HOME=/usr/local/webots
+export USERNAME=default
 export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 
+alias cb='colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug'
+
 test -f /opt/ros/humble/local_setup.bash && \
     source /opt/ros/humble/local_setup.bash
 test -f /memristor/ros2_ws/install/local_setup.bash && \

docker/config/setup.sh

@@ -27,7 +27,6 @@ if dialog --title 'mep3 config' --yesno 'Run first time ROS setup' 5 30; then
     clear
     sudo -E rosdep init
     sudo -E apt-get install -y python3-vcstool
-    cd /memristor/ros2_ws && vcs import --recursive src < /memristor/ros2_ws/src/mep3/mep3.repos
     rosdep --rosdistro "${ROS_DISTRO}" update
     cd /memristor/ros2_ws && yes | rosdep --rosdistro "${ROS_DISTRO}" install -r --from-paths src --ignore-src
 fi

mep3_behavior/CMakeLists.txt

@@ -1,25 +1,15 @@
 cmake_minimum_required(VERSION 3.5)
 project(mep3_behavior)
 
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(mep3_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
@@ -40,9 +30,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
-target_link_libraries(${PROJECT_NAME}
-  BT::behaviortree_cpp
-)
 ament_target_dependencies(
   ${PROJECT_NAME}
   rclcpp
@@ -51,9 +38,15 @@ ament_target_dependencies(
   nav2_msgs
   can_msgs
   diagnostic_msgs
+  behaviortree_cpp
 )
 
 install(TARGETS ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME})
 
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
 ament_package()

mep3_behavior/package.xml

@@ -7,7 +7,7 @@
   <maintainer email="[email protected]">memristor</maintainer>
   <license>http://www.apache.org/licenses/LICENSE-2.0</license>
 
-  <depend>diagnostic_msgs</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
   <build_depend>rclcpp_action</build_depend>
@@ -19,15 +19,16 @@
   <build_depend>nav2_bt_navigator</build_depend>
   <build_depend>nav2_planner</build_depend>
   <build_depend>nav2_recoveries</build_depend>
-  <build_depend>behaviortree_cpp</build_depend>
   <build_depend>can_msgs</build_depend>
-  <buildtool_depend>ament_cmake</buildtool_depend>
+
   <test_depend>ament_cppcheck </test_depend>
   <test_depend>ament_cpplint</test_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_lint_cmake</test_depend>
   <test_depend>ament_xmllint</test_depend>
+
   <depend>mep3_msgs</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

mep3_bringup/launch/robot_launch.py

@@ -2,7 +2,7 @@
 #
 # Example usage:
 #
-#   ros2 launch mep3_bringup robot_launch.py sim:=true color:=green namespace:=big strategy:=blue_strategy
+#   ros2 launch mep3_bringup robot_launch.py sim:=true color:= namespace:=big strategy:=blue_strategy
 #
 
 from math import pi
@@ -22,14 +22,7 @@
 
 INITIAL_POSE_MATRIX = [
     ('big', 'blue', [-0.65, -0.43, 0]),
-    ('small', 'blue', [-0.69, -0.405, -pi/2]),
-    ('big', 'green', [-0.65, 0.43, 0]),
-    ('small', 'blue_1', [0.69, -0.405, -pi/2]),
-
-    ('big', 'blue_a', [0.83, 0.43, -pi]),
-    ('small', 'blue_a', [-0.80, 1.34, pi/2]),
-    ('big', 'green_a', [0.83, -0.43, -pi]),
-    ('small', 'blue_2', [0.80, 1.34, pi/2]),
+    ('small', 'blue', [0.80, 1.34, pi/2])
 ]
 PREDEFINED_TABLE_NAMES = [
     'table1',
@@ -88,8 +81,8 @@ def launch_setup(context, *args, **kwargs):
     color = LaunchConfiguration('color')
     table = LaunchConfiguration('table', default='')
     should_live_reload = ('live' in strategy.perform(context))
-    if color.perform(context) not in ['blue', 'green', 'blue_a', 'blue_2', 'blue_1', 'green_a']:
-        print('ERROR: The `color` parameter must be either `blue` or `green`.')
+    if color.perform(context) not in ['blue', 'yellow']:
+        print('ERROR: The `color` parameter must be either `blue` or `yellow`.')
         sys.exit(1)
     if namespace.perform(context) not in ['big', 'small']:
         print('ERROR: The `namespace` parameter must be either `big` or `small`.')

mep3_bringup/mep3_bringup/launch_utils.py

@@ -6,13 +6,22 @@ def get_controller_spawners(controller_params_file):
     with open(controller_params_file, 'r') as f:
         controller_params = yaml.safe_load(f)
 
+    controller_names = []
+
     # Resolve namespace
     namespace = ''
-    if 'controller_manager' not in controller_params.keys():
+    for item in controller_params.keys():
+        tokens = item.split('/')
+        if tokens[-1] == 'controller_manager':
+            namespace = '/'.join(tokens[:-1])
+            controller_names = list(controller_params[item]['ros__parameters'].keys())
+            break
+    if namespace == '' and 'controller_manager' not in controller_params.keys():
+        print(controller_params.keys())
         namespace = list(controller_params.keys())[0]
-        controller_params = controller_params[namespace]
-
-    controller_names = list(controller_params['controller_manager']['ros__parameters'].keys())
+        controller_names = list(controller_params[namespace]['controller_manager']['ros__parameters'].keys())
+    if namespace == '':
+        controller_names = list(controller_params['controller_manager']['ros__parameters'].keys())
 
     # Create controller spawners
     controller_spawners = []

mep3_hardware/launch/hardware_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 import subprocess
 
 from ament_index_python.packages import get_package_share_directory
@@ -35,7 +34,7 @@ def launch_setup(context, *args, **kwargs):
     usb_port = '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_9e82eec869f4c84fb1901fc50d00c93c-if00-port0' if performed_namespace == 'big' else '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_0e10a6d7001d8f4fad5376bfd2f6f1ad-if00-port0'
 
     controller_params_file = os.path.join(package_dir, 'resource', f'{performed_namespace}_controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(package_dir, 'resource', f'{performed_namespace}_description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'resource', f'{performed_namespace}_description.urdf')
 
     enable_can_interface()
 

mep3_hardware/test/dynamixel/dynamixel_joint_trajectory_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
@@ -8,8 +7,7 @@ def generate_launch_description():
     package_dir = os.path.dirname(os.path.abspath(__file__))
     controller_params_file = os.path.join(
         package_dir, 'controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(
-        package_dir, 'description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'description.urdf')
 
     return LaunchDescription([
         Node(

mep3_hardware/test/dynamixel/dynamixel_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
@@ -8,8 +7,7 @@ def generate_launch_description():
     package_dir = os.path.dirname(os.path.abspath(__file__))
     controller_params_file = os.path.join(
         package_dir, 'controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(
-        package_dir, 'description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'description.urdf')
 
     return LaunchDescription([
         Node(

mep3_hardware/test/dynamixel_big/dynamixel_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
@@ -8,8 +7,7 @@ def generate_launch_description():
     package_dir = os.path.dirname(os.path.abspath(__file__))
     controller_params_file = os.path.join(
         package_dir, 'controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(
-        package_dir, 'description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'description.urdf')
 
     return LaunchDescription([
         Node(

mep3_hardware/test/dynamixels_and_pump/dynamixels_and_pump_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from mep3_bringup.launch_utils import get_controller_spawners
@@ -9,8 +8,7 @@ def generate_launch_description():
     package_dir = os.path.dirname(os.path.abspath(__file__))
     controller_params_file = os.path.join(
         package_dir, 'controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(
-        package_dir, 'description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'description.urdf')
 
     return LaunchDescription([
         Node(

mep3_hardware/test/pumps/pumps_launch.py

@@ -1,5 +1,4 @@
 import os
-import pathlib
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
@@ -8,8 +7,7 @@ def generate_launch_description():
     package_dir = os.path.dirname(os.path.abspath(__file__))
     controller_params_file = os.path.join(
         package_dir, 'controllers.yaml')
-    robot_description = pathlib.Path(os.path.join(
-        package_dir, 'description.urdf')).read_text()
+    robot_description = os.path.join(package_dir, 'description.urdf')
 
     return LaunchDescription([
         Node(