✨ Add support for mujoco

.vscode/launch.json

@@ -1,21 +1,26 @@
 {
-    // For information on how to use the debug with ROS see
-    // https://github.com/ms-iot/vscode-ros/blob/master/doc/debug-support.md
     "version": "0.2.0",
     "configurations": [
         {
-            // Note: Install GDB to use this configuration
-            "name": "ROS: Attach",
-            "type": "ros",
-            "request": "attach"
+            "name": "ROS Launch",
+            "request": "launch",
+            "type": "cppdbg",
+            "preLaunchTask": "ROS Launch Micras Simulation",
+            "serverLaunchTimeout": 10000,
+            "miDebuggerServerAddress": "localhost:3000",
+            "miDebuggerPath": "gdb",
+            "miDebuggerArgs": "-x ${workspaceFolder}/scripts/pause_gazebo.py",
+            "cwd": "${workspaceFolder}",
+            "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}",
         },
         {
-            "name": "ROS: Launch",
-            "type": "ros",
+            "name": "ROS Attach",
             "request": "launch",
-            "target": "${workspaceFolder}/launch/include/node.launch.xml",
-            "preLaunchTask": "ROS Launch World",
-            "arguments": ["name:=${input:args}"]
+            "type": "cppdbg",
+            "miDebuggerPath": "gdb",
+            "miDebuggerServerAddress": "localhost:3000",
+            "cwd": "${workspaceFolder}",
+            "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}",
         }
     ],
     "inputs": [

.vscode/tasks.json

@@ -2,27 +2,27 @@
 	"version": "2.0.0",
 	"tasks": [
         {
-            "label": "ROS Launch World",
+            "label": "ROS Launch Micras Simulation",
             "type": "ros",
-            "command": "ros2 launch micras_simulation world.launch.xml",
+            "command": "bash -c 'source /opt/ros/jazzy/setup.bash && source ${workspaceFolder}/../../install/setup.bash && ros2 launch micras_simulation micras.launch.xml'",
             "isBackground": true,
             "problemMatcher": [
                 {
                     "pattern": [
                         {
-                            "regexp": ".",
+                            "regexp": "",
                             "file": 1,
                             "location": 2,
                             "message": 3
                         }
                     ],
                     "background": {
                         "activeOnStart": true,
-                        "beginsPattern": ".",
-                        "endsPattern": ".",
+                        "beginsPattern": "\\[INFO\\] \\[launch\\]",
+                        "endsPattern": "Listening on port"
                     }
                 }
             ]
-        },
+        }
     ]
 }

CMakeLists.txt

@@ -1,10 +1,9 @@
-# Name: CMakeLists.txt
-# Micras Team
-# Brief: CMake configuration file for the project
-# 04/2024
-
 cmake_minimum_required(VERSION 3.16.3)
 
+###############################################################################
+## CMake Configuration
+###############################################################################
+
 project(micras_simulation)
 
 if(NOT CMAKE_CXX_STANDARD)
@@ -26,6 +25,10 @@ endif()
 
 add_subdirectory(gazebo/micras_plugin)
 
+###############################################################################
+## Find ROS2 Dependencies
+###############################################################################
+
 find_package(ament_cmake REQUIRED)
 
 set(ROS_CODE_DEPS
@@ -34,19 +37,47 @@ set(ROS_CODE_DEPS
     sensor_msgs
     geometry_msgs
     nav_msgs
+    example_interfaces
     ament_index_cpp
 )
 
 foreach(dependency ${ROS_CODE_DEPS})
     find_package(${dependency} REQUIRED)
 endforeach()
 
+###############################################################################
+## Find WebSocketpp
+###############################################################################
+
+find_package(Boost REQUIRED COMPONENTS system thread)
+
+include(FetchContent)
+
+FetchContent_Declare(
+    websocketpp
+    GIT_REPOSITORY https://github.com/zaphoyd/websocketpp.git
+    GIT_TAG b9aeec6eaf3d5610503439b4fae3581d9aff08e8
+)
+
+# Suppress CMake deprecation warnings from websocketpp
+set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "Whether to issue deprecation warnings." FORCE)
+FetchContent_MakeAvailable(websocketpp)
+set(CMAKE_WARN_DEPRECATED ON CACHE BOOL "Whether to issue deprecation warnings." FORCE)
+
+###############################################################################
+## Input Files
+###############################################################################
+
 file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/**/*.c*" "MicrasFirmware/src/*.c*")
-file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "MicrasFirmware/micras_nav/src/*.c*" "MicrasFirmware/micras_core/src/*.c*")
+file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "MicrasFirmware/micras_nav/src/*.c*" "MicrasFirmware/micras_comm/src/*.c*" "MicrasFirmware/micras_core/src/*.c*")
 file(GLOB_RECURSE SIMULATION_PROXY_SOURCES CONFIGURE_DEPENDS "src/proxy/*.c*")
 file(GLOB_RECURSE PROJECT_TESTS CONFIGURE_DEPENDS "MicrasFirmware/tests/src/*/*.c*")
 list(REMOVE_ITEM PROJECT_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/src/main.cpp")
 
+###############################################################################
+## Add Micras Simulation Library
+###############################################################################
+
 add_library(${PROJECT_NAME}_lib
     ${LIB_SOURCES}
     ${SIMULATION_PROXY_SOURCES}
@@ -61,9 +92,20 @@ target_include_directories(${PROJECT_NAME}_lib PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/tests/include>
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/include>
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_nav/include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_comm/include>
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_core/include>
+    $<BUILD_INTERFACE:${websocketpp_SOURCE_DIR}>
+)
+
+target_link_libraries(${PROJECT_NAME}_lib
+    Boost::system
+    Boost::thread
 )
 
+###############################################################################
+## Add main executable
+###############################################################################
+
 add_executable(micras_node
     src/micras_node.cpp
     ${PROJECT_SOURCES}
@@ -77,6 +119,10 @@ target_link_libraries(micras_node
 
 add_dependencies(micras_node MicrasPlugin)
 
+###############################################################################
+## Install targets and directories
+###############################################################################
+
 install(DIRECTORY include
     DESTINATION include
 )
@@ -86,6 +132,10 @@ install(DIRECTORY gazebo
     PATTERN gazebo/micras_plugin EXCLUDE
 )
 
+install(DIRECTORY mujoco
+    DESTINATION share/${PROJECT_NAME}
+)
+
 install(DIRECTORY launch
     DESTINATION share/${PROJECT_NAME}
 )
@@ -106,6 +156,10 @@ install(TARGETS MicrasPlugin
     DESTINATION share/${PROJECT_NAME}/micras_plugin
 )
 
+###############################################################################
+## Add Tests
+###############################################################################
+
 if(COMPILE_TESTS)
     foreach(TEST_FILE ${PROJECT_TESTS})
 
@@ -123,7 +177,9 @@ if(COMPILE_TESTS)
             ${CMAKE_CURRENT_SOURCE_DIR}/tests/include
             ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/include
             ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_nav/include
+            ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_comm/include
             ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_core/include
+            ${websocketpp_SOURCE_DIR}
         )
 
         target_link_libraries(${TEST_NAME}

config/constants.hpp

@@ -15,19 +15,20 @@
 #include "micras/nav/speed_controller.hpp"
 
 namespace micras {
-/*****************************************
+/***************
  * Constants
- *****************************************/
+ ***************/
 
+constexpr bool     debug_mode{false};
 constexpr uint8_t  maze_width{16};
 constexpr uint8_t  maze_height{16};
 constexpr float    cell_size{0.18};
 constexpr uint32_t loop_time_us{1042};
-constexpr float    wall_thickness{0.0126F};
-constexpr float    start_offset{0.04F + wall_thickness / 2.0F};
-constexpr float    exploration_speed{0.5F};
-constexpr float    max_linear_acceleration{1.0F};
-constexpr float    max_angular_acceleration{200.0F};
+constexpr float    wall_thickness{0.012F};
+constexpr float    start_offset{0.05F};
+constexpr float    max_angular_acceleration{400.0F};
+constexpr float    crash_acceleration{1000000.0F};
+constexpr float    fan_speed{100.0F};
 
 constexpr core::WallSensorsIndex wall_sensors_index{
     .left_front = 0,
@@ -36,55 +37,56 @@ constexpr core::WallSensorsIndex wall_sensors_index{
     .right_front = 3,
 };
 
-/*****************************************
+/***************
  * Template Instantiations
- *****************************************/
+ ***************/
 
 namespace nav {
 using Maze = TMaze<maze_width, maze_height>;
 }  // namespace nav
 
-/*****************************************
+/***************
  * Configurations
- *****************************************/
+ ***************/
 
 const nav::ActionQueuer::Config action_queuer_config{
     .cell_size = cell_size,
     .start_offset = start_offset,
+    .curve_safety_margin = 0.053F,
     .exploring =
         {
-            .max_linear_speed = exploration_speed,
-            .max_linear_acceleration = max_linear_acceleration,
-            .max_linear_deceleration = max_linear_acceleration,
-            .curve_radius = cell_size / 2.0F,
-            .max_centrifugal_acceleration = 2.78F,
-            .max_angular_acceleration = max_angular_acceleration,
+            .max_linear_speed = 0.3F,
+            .max_linear_acceleration = 5.0F,
+            .max_linear_deceleration = 5.0F,
+            .max_centrifugal_acceleration = 1.0F,
+            .max_angular_acceleration = 200.0F,
         },
     .solving =
         {
-            .max_linear_speed = exploration_speed,
-            .max_linear_acceleration = max_linear_acceleration,
-            .max_linear_deceleration = max_linear_acceleration,
-            .curve_radius = cell_size / 2.0F,
-            .max_centrifugal_acceleration = 1.0F,
+            .max_linear_speed = 5.0F,
+            .max_linear_acceleration = 12.0F,
+            .max_linear_deceleration = 20.0F,
+            .max_centrifugal_acceleration = 40.0F,
             .max_angular_acceleration = max_angular_acceleration,
         },
 };
 
 const nav::FollowWall::Config follow_wall_config{
     .pid =
         {
-            .kp = 0.5F,
+            .kp = 30.0F,
             .ki = 0.0F,
-            .kd = 0.0F,
+            .kd = 0.008F,
             .setpoint = 0.0F,
             .saturation = 1.0F,
             .max_integral = -1.0F,
         },
-    .max_linear_speed = 0.1F,
-    .post_threshold = 16.5F,
+    .wall_sensor_index = wall_sensors_index,
+    .max_angular_acceleration = max_angular_acceleration,
     .cell_size = cell_size,
-    .post_clearance = 0.2F * cell_size,
+    .post_threshold = 400.0F,
+    .post_reference = 0.44F * cell_size,
+    .post_clearance = 0.035F,
 };
 
 const nav::Maze::Config maze_config{
@@ -95,48 +97,48 @@ const nav::Maze::Config maze_config{
         {maze_width / 2, (maze_height - 1) / 2},
         {(maze_width - 1) / 2, (maze_height - 1) / 2},
     }},
+    .cost_margin = 1.2F,
+    .action_queuer_config = action_queuer_config,
 };
 
 const nav::Odometry::Config odometry_config{
-    .linear_cutoff_frequency = 5.0F,
-    .wheel_radius = 0.0112F,
-    .initial_pose = {{0.0F, 0.0F}, 0.0F},
+    .linear_cutoff_frequency = 10.0F,
+    .wheel_radius = 0.011F,
+    .initial_pose = {{cell_size / 2.0f, start_offset}, std::numbers::pi_v<float> / 2.0f},
 };
 
 const nav::SpeedController::Config speed_controller_config{
-    .max_linear_acceleration = max_linear_acceleration,
-    .max_angular_acceleration = max_angular_acceleration,
     .linear_pid =
         {
-            .kp = 10.0F,
-            .ki = 1.0F,
+            .kp = 0.0F,
+            .ki = 10.0F,
             .kd = 0.0F,
             .setpoint = 0.0F,
-            .saturation = 40.0F,
+            .saturation = 20.0F,
             .max_integral = -1.0F,
         },
     .angular_pid =
         {
-            .kp = 2.0F,
-            .ki = 1.0F,
+            .kp = 0.0F,
+            .ki = 5.0F,
             .kd = 0.0F,
             .setpoint = 0.0F,
-            .saturation = 40.0F,
+            .saturation = 20.0F,
             .max_integral = -1.0F,
         },
     .left_feed_forward =
         {
-            .linear_speed = 12.706F,
-            .linear_acceleration = 2.796F,
-            .angular_speed = -0.971F,
-            .angular_acceleration = -0.0258F,
+            .linear_speed = 102.0F,
+            .linear_acceleration = 0.3F,
+            .angular_speed = -4.9F,
+            .angular_acceleration = -0.1F,
         },
     .right_feed_forward =
         {
-            .linear_speed = 13.319F,
-            .linear_acceleration = 2.878F,
-            .angular_speed = 0.901F,
-            .angular_acceleration = -0.0244F,
+            .linear_speed = 102.0F,
+            .linear_acceleration = 0.3F,
+            .angular_speed = +4.9F,
+            .angular_acceleration = +0.1F,
         },
 };
 }  // namespace micras