Enhance ros2_control interfaces

.vscode/c_cpp_properties.json

@@ -10,11 +10,12 @@
                 "${env:HOME}/webots/include/controller/cpp",
                 "/usr/include/python3.10",
                 "/usr/include",
-                "${workspaceFolder}/mep3_behavior/include",
                 "${workspaceFolder}/../../install/mep3_msgs/include/mep3_msgs",
+                "${workspaceFolder}/../behaviortree-cpp/include",
                 "${workspaceFolder}/mep3_hardware/include",
                 "${workspaceFolder}/mep3_navigation/include",
                 "${workspaceFolder}/mep3_behavior/include",
+                "${workspaceFolder}/mep3_controllers/include",
                 "${workspaceFolder}/../dynamixel-workbench/dynamixel_workbench_toolbox/include",
                 "/opt/ros/humble/include",
                 "/opt/ros/humble/include/rclcpp",
@@ -52,7 +53,7 @@
                 "/opt/ros/humble/include/bond",
                 "/opt/ros/humble/include/tf2_ros",
                 "/opt/ros/humble/include/tf2_msgs",
-                "/opt/ros/humble/include/ament_index_cpp",
+                "/opt/ros/humble/include/ament_index_cpp"
             ],
             "intelliSenseMode": "gcc-x64",
             "compilerPath": "/usr/bin/g++",

README.md

@@ -41,7 +41,7 @@ sudo cp src/mep3/tools/rplidar.rules /etc/udev/rules.d/
 sudo cp src/mep3/tools/dynamixel.rules /etc/udev/rules.d/
 
 # Build the packages
-colcon build
+colcon build --symlink-install --packages-up-to mep3_bringup mep3_simulation --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
 
 # Source this workspace
 source install/local_setup.bash

docker/Dockerfile.base

@@ -16,13 +16,15 @@ RUN apt-get update && apt-get install -y \
         ros-humble-nav2-bringup \
         ros-humble-rviz2 \
         ros-humble-teleop-twist-keyboard \
-        ros-humble-webots-ros2 \
         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 \
         python3-pip \
         python3-pil \
         alsa \
@@ -46,11 +48,11 @@ RUN su memristor -c 'code --install-extension eamodio.gitlens' && \
     su memristor -c 'code --install-extension ms-python.python' && \
     su memristor -c 'code --install-extension ms-vscode.cpptools-extension-pack' && \
     su memristor -c 'code --install-extension usernamehw.errorlens' && \
-    su memristor -c 'code --install-extension ms-iot.vscode-ros'
+    su memristor -c 'code --install-extension redhat.vscode-xml'
 
 # Webots
 RUN curl -L -o /tmp/webots.deb \
-        'https://github.com/cyberbotics/webots/releases/download/R2022b/webots_2022b_amd64.deb' && \
+        'https://github.com/cyberbotics/webots/releases/download/R2023a/webots_2023a_amd64.deb' && \
     apt-get install -y /tmp/webots.deb && \
     rm -f /tmp/webots.deb && \
     mkdir -p /memristor/.config/Cyberbotics

docker/Makefile

@@ -1,6 +1,6 @@
 UID:=$(shell id -u)
 ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST))))
-NVIDIA_GPU:=$(shell docker info | grep Runtimes | grep nvidia 1> /dev/null && echo '--runtime nvidia --gpus all' || echo '')
+NVIDIA_GPU:=$(shell (docker info | grep Runtimes | grep nvidia 1> /dev/null && command -v nvidia-smi 1>/dev/null 2>/dev/null && nvidia-smi | grep Processes 1>/dev/null 2>/dev/null) && echo '--runtime nvidia --gpus all' || echo '')
 FLAVOR=devel
 IMAGE=mep3
 

docker/config/bashrc

@@ -1,4 +1,6 @@
 source /opt/ros/humble/local_setup.bash
+source /memristor/ros2_ws/install/local_setup.bash
+export WEBOTS_HOME=/usr/local/webots
 export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 export PS1="$(tput setaf 4)\w$(tput sgr0) [$(tput setaf 5)\$?$(tput sgr0)] $(tput setaf 3)\$(git branch --show-current 2>/dev/null | xargs -I{} echo \"({})\")$(tput sgr0)\n$(tput setaf 2)\\$ \\[$(tput sgr0)\\]"
 cp -p /memristor/.host/.Xauthority /memristor/.Xauthority

docker/config/setup.sh

@@ -15,9 +15,9 @@ 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 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
+    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
 
 if dialog --title 'mep3 config' --yesno 'Auto-source default ROS workspace' 5 38; then

mep3.repos

@@ -7,3 +7,11 @@ repositories:
     type: git
     url: https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git
     version: ros2
+  webots_ros2:
+    type: git
+    url: https://github.com/cyberbotics/webots_ros2.git
+    version: c02db7e765cd702507205a45680f2b73ffc5d766
+  behaviortree-cpp:
+    type: git
+    url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
+    version: 901696c100b43dd1075858816695993da2ac9b9b

mep3_behavior/CMakeLists.txt

@@ -17,10 +17,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -32,21 +29,19 @@ find_package(diagnostic_msgs REQUIRED)
 # Use static linking for BT Nodes
 add_compile_definitions(MANUAL_STATIC_LINKING)
 
-# Add all source files under the src directory
-file(GLOB_RECURSE SOURCES "src/*.cpp")
-
 # Set path to assets
-get_filename_component(ASSETS_DIRECTORY assets ABSOLUTE [CACHE])
+get_filename_component(ASSETS_DIRECTORY strategies ABSOLUTE [CACHE])
 add_compile_definitions(ASSETS_DIRECTORY="${ASSETS_DIRECTORY}")
 
-add_executable(${PROJECT_NAME} ${SOURCES})
+add_executable(${PROJECT_NAME} src/mep3_behavior_tree.cpp)
+target_compile_definitions(${PROJECT_NAME} PRIVATE USE_BTCPP3_OLD_NAMES=ON)
 
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
 target_link_libraries(${PROJECT_NAME}
-  BT::behaviortree_cpp_v3
+  BT::behaviortree_cpp
 )
 ament_target_dependencies(
   ${PROJECT_NAME}
@@ -61,15 +56,4 @@ ament_target_dependencies(
 install(TARGETS ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME})
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
 ament_package()