├── .gitignore ├── gz_ws ├── src │ ├── encoder_sensor_system │ │ ├── README.md │ │ ├── CMakeLists.txt │ │ ├── EncoderSystem.hh │ │ └── EncoderSystem.cc │ ├── micromouse │ │ ├── README.md │ │ ├── micromouse.qrc │ │ ├── CMakeLists.txt │ │ ├── MicromousePlugin.hh │ │ ├── MicromousePlugin.qml │ │ └── MicromousePlugin.cc │ ├── encoder_sensor │ │ ├── CMakeLists.txt │ │ ├── Encoder.hh │ │ └── Encoder.cc │ └── micromouse_system │ │ ├── CMakeLists.txt │ │ ├── MicromouseSystem.hh │ │ └── MicromouseSystem.cc ├── installws.sh ├── cleanws.sh ├── buildws.sh └── README.md ├── ros2_ws ├── src │ └── micromouse │ │ ├── gen_urdf.sh │ │ ├── README.md │ │ ├── xacro │ │ └── urdf │ │ │ ├── common │ │ │ ├── inertial │ │ │ │ ├── sphere.xacro │ │ │ │ ├── box.xacro │ │ │ │ └── cylinder.xacro │ │ │ └── links │ │ │ │ ├── body.xacro │ │ │ │ ├── caster.xacro │ │ │ │ └── wheel.xacro │ │ │ ├── micromouse_robot.urdf.xacro │ │ │ └── micromouse_robot.gazebo.xacro │ │ ├── package.xml │ │ ├── include │ │ └── micromouse │ │ │ ├── bitwise_utils.hpp │ │ │ ├── solver_utils.hpp │ │ │ ├── pose2.hpp │ │ │ ├── robot_specs.hpp │ │ │ ├── maze_utils.hpp │ │ │ ├── angle_utils.hpp │ │ │ ├── vector2int.hpp │ │ │ ├── math_utils.hpp │ │ │ ├── cell.hpp │ │ │ ├── ros │ │ │ ├── dc_motor.hpp │ │ │ ├── cmdvel.hpp │ │ │ ├── range_sensor.hpp │ │ │ ├── encoder.hpp │ │ │ └── imu.hpp │ │ │ ├── vector2.hpp │ │ │ ├── direction.hpp │ │ │ ├── micromouse.hpp │ │ │ ├── maze.hpp │ │ │ ├── solver.hpp │ │ │ └── robot.hpp │ │ ├── src │ │ ├── bitwise_utils.cpp │ │ ├── pose2.cpp │ │ ├── dc_motor.cpp │ │ ├── cmdvel.cpp │ │ ├── range_sensor.cpp │ │ ├── angle_utils.cpp │ │ ├── solver_utils.cpp │ │ ├── vector2int.cpp │ │ ├── vector2.cpp │ │ ├── cell.cpp │ │ ├── encoder.cpp │ │ ├── math_utils.cpp │ │ ├── maze_utils.cpp │ │ ├── imu.cpp │ │ ├── robot.cpp │ │ ├── direction.cpp │ │ ├── maze.cpp │ │ ├── solver.cpp │ │ └── micromouse.cpp │ │ ├── launch │ │ └── micromouse_launch.py │ │ ├── params │ │ └── micromouse_bridge.yaml │ │ ├── CMakeLists.txt │ │ ├── worlds │ │ ├── micromouse.world │ │ └── gui.config │ │ └── urdf │ │ └── micromouse_robot.urdf └── README.md ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | install/ 3 | log/ 4 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor_system/README.md: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/README.md: -------------------------------------------------------------------------------- 1 | # Micromouse GUI Plugin for Gazebo 2 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/micromouse.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | MicromousePlugin.qml 4 | 5 | 6 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/gen_urdf.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ $# -eq 0 ]; then 4 | >&2 echo "No arguments provided" 5 | exit 1 6 | fi 7 | 8 | xacro "$1"/xacro/urdf/micromouse_robot.urdf.xacro -o "$1"/urdf/micromouse_robot.urdf 9 | -------------------------------------------------------------------------------- /ros2_ws/README.md: -------------------------------------------------------------------------------- 1 | # ros2_ws 2 | 3 | This is the workspace for ROS2-related assets and files. 4 | 5 | ## Quick Start 6 | 7 | Ensure ROS2 has been sourced: 8 | 9 | ```sh 10 | source /opt/ros/jazzy/setup.bash 11 | ``` 12 | 13 | Build and source packages. Note: run the following from the ROS2 workspace folder `ros2_ws`: 14 | 15 | ```sh 16 | colcon build 17 | source ./install/setup.bash 18 | ``` 19 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/README.md: -------------------------------------------------------------------------------- 1 | # Micromouse 2 | 3 | ## Quick Start 4 | 5 | Ensure this package has been built, installed and sourced. 6 | 7 | Launch simulator with micromouse robot node controller 8 | 9 | ```sh 10 | ros2 launch micromouse micromouse_launch.py 11 | ``` 12 | 13 | If not using the launch file provided and not starting running the micromouse package from the launch file: 14 | 15 | ```sh 16 | ros2 run micromouse micromouse --ros-args -p use_sim_time:=true 17 | ``` 18 | -------------------------------------------------------------------------------- /gz_ws/installws.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ $# -eq 0 ]; then 4 | >&2 echo "No arguments provided" 5 | exit 1 6 | fi 7 | 8 | mkdir -p "$1/install/system" 9 | mkdir -p "$1/install/gui" 10 | 11 | cp "$1/src/micromouse/build/libMicromousePlugin.so" "$1/install/gui" 12 | cp "$1/src/micromouse_system/build/libMicromouseSystem.so" "$1/install/system/" 13 | cp "$1/src/encoder_sensor/build/libencoder.so" "$1/install/system/" 14 | cp "$1/src/encoder_sensor_system/build/libEncoderSystem.so" "$1/install/system/" 15 | -------------------------------------------------------------------------------- /gz_ws/cleanws.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ $# -eq 0 ]; then 4 | >&2 echo "No arguments provided" 5 | exit 1 6 | fi 7 | 8 | rm -rf "$1/src/micromouse/build" 9 | rm -rf "$1/src/micromouse/.cache" 10 | rm -rf "$1/src/micromouse_system/build" 11 | rm -rf "$1/src/micromouse_system/.cache" 12 | rm -rf "$1/src/encoder_sensor/build" 13 | rm -rf "$1/src/encoder_sensor/.cache" 14 | rm -rf "$1/src/encoder_sensor_system/build" 15 | rm -rf "$1/src/encoder_sensor_system/.cache" 16 | rm -rf "$1/src/encoder_sensor_system/encoder_sensor" 17 | rm -rf "$1/install" 18 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/inertial/sphere.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) 2 | 3 | project(encoder) 4 | 5 | find_package(gz-cmake3 REQUIRED) 6 | find_package(gz-sensors8 REQUIRED) 7 | find_package(gz-sim8 REQUIRED) 8 | 9 | add_library(${PROJECT_NAME} SHARED Encoder.cc) 10 | target_link_libraries(${PROJECT_NAME} 11 | PUBLIC 12 | gz-sensors8::gz-sensors8 13 | gz-sim8::gz-sim8 14 | ) 15 | 16 | target_compile_options(${PROJECT_NAME} PRIVATE 17 | $<$:/W4 /WX> 18 | $<$>:-Wall -Wextra -Wpedantic -Werror> 19 | ) 20 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/inertial/box.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /gz_ws/buildws.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ $# -eq 0 ]; then 4 | >&2 echo "No arguments provided" 5 | exit 1 6 | fi 7 | 8 | cmake -B "$1/src/micromouse/build" -S "$1/src/micromouse" && cmake --build "$1/src/micromouse/build" 9 | cmake -B "$1/src/micromouse_system/build" -S "$1/src/micromouse_system" && cmake --build "$1/src/micromouse_system/build" 10 | cmake -B "$1/src/encoder_sensor/build" -S "$1/src/encoder_sensor" && cmake --build "$1/src/encoder_sensor/build" 11 | cmake -B "$1/src/encoder_sensor_system/build" -S "$1/src/encoder_sensor_system" && cmake --build "$1/src/encoder_sensor_system/build" 12 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/inertial/cylinder.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/links/body.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/links/caster.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | micromouse 5 | 0.1.0 6 | TODO: Package description 7 | Pedro Zawadniak 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | ros_gz_sim 13 | ros_gz_bridge 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/common/links/wheel.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/bitwise_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef BITWISE_UTILS_HPP 19 | #define BITWISE_UTILS_HPP 20 | 21 | unsigned int genMask(int bits); 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) 2 | 3 | find_package(gz-cmake3 REQUIRED) 4 | 5 | project(MicromouseSystem) 6 | 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) 10 | set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) 11 | 12 | gz_find_package(gz-sim8 REQUIRED) 13 | set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) 14 | 15 | gz_find_package(gz-sensors8 REQUIRED) 16 | set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR}) 17 | 18 | add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) 19 | target_link_libraries(${PROJECT_NAME} 20 | PRIVATE 21 | gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} 22 | gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} 23 | gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} 24 | ) 25 | 26 | target_compile_options(${PROJECT_NAME} PRIVATE 27 | $<$:/W4 /WX> 28 | $<$>:-Wall -Wextra -Wpedantic> 29 | ) 30 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/bitwise_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/bitwise_utils.hpp" 19 | 20 | #include 21 | 22 | unsigned int genMask(int bits) 23 | { 24 | unsigned int mask; 25 | int sizeInBits = sizeof(mask) * CHAR_BIT; 26 | mask = bits >= sizeInBits ? ~0u : (1u << bits) - 1; 27 | return mask; 28 | } 29 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/solver_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SOLVER_UTILS_HPP 19 | #define SOLVER_UTILS_HPP 20 | 21 | #include "micromouse/pose2.hpp" 22 | #include "micromouse/solver.hpp" 23 | 24 | // Returns the ideal pose representing a state 25 | Pose2 stateToPose(State s, double xSize, double ySize); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/pose2.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef POSE2_HPP 19 | #define POSE2_HPP 20 | 21 | #include "micromouse/vector2.hpp" 22 | 23 | struct Pose2 24 | { 25 | Vector2 position; 26 | double angle; 27 | double normalizedAngle() const; 28 | }; 29 | 30 | Pose2 operator-(const Pose2& lhs, const Pose2& rhs); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/robot_specs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ROBOT_SPECS_HPP 19 | #define ROBOT_SPECS_HPP 20 | 21 | struct RobotConstructionSpecification 22 | { 23 | double leftWheelPPR; 24 | double rightWheelPPR; 25 | double leftWheelRadius; 26 | double rightWheelRadius; 27 | double wheelBaseDistance; 28 | }; 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/maze_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MAZE_UTILS_HPP 19 | #define MAZE_UTILS_HPP 20 | 21 | #include "micromouse/maze.hpp" 22 | #include "micromouse/vector2.hpp" 23 | 24 | static constexpr Vector2 CLASSIC_MICROMOUSE_MAZE_SIZE = Vector2{0.18, 0.18}; 25 | 26 | LogicMaze classicMicromouseMaze(); 27 | 28 | void surroundMazeWithWalls(LogicMaze& maze); 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/angle_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ANGLE_ULTILS_H 19 | #define ANGLE_ULTILS_H 20 | 21 | // returns b - a, constrained to [-PI, PI] 22 | double signedAngleDifference(double a, double b); 23 | 24 | //returns angle constrained to [0, 2*PI[ 25 | double angleNormalize(double angle); 26 | 27 | double rad2deg(double rad); 28 | double deg2rad(double deg); 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/pose2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/pose2.hpp" 19 | #include "micromouse/angle_utils.hpp" 20 | 21 | double Pose2::normalizedAngle() const 22 | { 23 | return angleNormalize(angle); 24 | } 25 | 26 | Pose2 operator-(const Pose2& lhs, const Pose2& rhs) 27 | { 28 | Pose2 result; 29 | result.position = lhs.position - rhs.position; 30 | result.angle = lhs.angle - rhs.angle; 31 | return result; 32 | } 33 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/vector2int.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef VECTOR2INT_HPP 19 | #define VECTOR2INT_HPP 20 | 21 | #include "micromouse/direction.hpp" 22 | 23 | struct Vector2Int 24 | { 25 | int x; 26 | int y; 27 | Vector2Int neighbor(Direction direction); 28 | }; 29 | 30 | bool operator==(const Vector2Int& lhs, const Vector2Int& rhs); 31 | bool operator!=(const Vector2Int& lhs, const Vector2Int& rhs); 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/math_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MATH_UTILS_H 19 | #define MATH_UTILS_H 20 | 21 | #include "micromouse/vector2.hpp" 22 | 23 | Vector2 rayCast(Vector2 origin, double angle, double length); 24 | 25 | bool isWithinRectangle(Vector2 point, Vector2 recSize); 26 | bool isWithinRectangle(Vector2 point, Vector2 recCenter, Vector2 recSize); 27 | bool isWithinCircle(Vector2 point, double radius); 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) 2 | 3 | project(gz-gui-micromouse-plugin) 4 | 5 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 6 | 7 | if(POLICY CMP0100) 8 | cmake_policy(SET CMP0100 NEW) 9 | endif() 10 | 11 | set (CMAKE_AUTOMOC ON) 12 | 13 | # Find Qt5 14 | find_package (Qt5 15 | COMPONENTS 16 | Core 17 | Quick 18 | QuickControls2 19 | REQUIRED 20 | ) 21 | 22 | # Find the Gazebo SIM library 23 | find_package(gz-sim8 REQUIRED COMPONENTS gui) 24 | 25 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GZ-GUI_CXX_FLAGS}") 26 | 27 | QT5_ADD_RESOURCES(resources_RCC micromouse.qrc) 28 | 29 | # Generate examples 30 | add_library(MicromousePlugin SHARED ${headers_MOC} 31 | MicromousePlugin.cc 32 | ${resources_RCC} 33 | ) 34 | target_link_libraries(MicromousePlugin 35 | PRIVATE gz-sim8::gui 36 | ${Qt5Core_LIBRARIES} 37 | ${Qt5Qml_LIBRARIES} 38 | ${Qt5Quick_LIBRARIES} 39 | ${Qt5QuickControls2_LIBRARIES} 40 | ) 41 | 42 | target_compile_options(MicromousePlugin PRIVATE 43 | $<$:/W4 /WX> 44 | $<$>:-Wall -Wextra -Wpedantic> 45 | ) 46 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/cell.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef CELL_HPP 19 | #define CELL_HPP 20 | 21 | #include "micromouse/direction.hpp" 22 | #include 23 | 24 | class Cell 25 | { 26 | public: 27 | static int wallPositionToIndex(Direction direction); 28 | bool placeWall(Direction direction); 29 | bool removeWall(Direction direction); 30 | bool hasWall(Direction direction) const; 31 | 32 | private: 33 | std::uint8_t m_walls; 34 | }; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/ros/dc_motor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef DC_MOTOR_HPP 19 | #define DC_MOTOR_HPP 20 | 21 | #include 22 | #include 23 | 24 | class DcMotor 25 | { 26 | public: 27 | DcMotor(rclcpp::Node& parent_node, std::string topic_name); 28 | void SetVoltage(double voltage); 29 | 30 | private: 31 | rclcpp::Node& _parent_node; 32 | rclcpp::Publisher::SharedPtr pub; 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) 2 | 3 | find_package(gz-cmake3 REQUIRED) 4 | 5 | project(EncoderSystem) 6 | 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) 10 | set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) 11 | 12 | gz_find_package(gz-sim8 REQUIRED) 13 | set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) 14 | 15 | find_package(gz-sensors8 REQUIRED) 16 | set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR}) 17 | 18 | add_subdirectory(../encoder_sensor ../encoder_sensor/build) 19 | 20 | add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) 21 | target_link_libraries(${PROJECT_NAME} 22 | PRIVATE 23 | gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} 24 | gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} 25 | gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} 26 | encoder 27 | ) 28 | target_include_directories(${PROJECT_NAME} 29 | PUBLIC ../encoder_sensor) 30 | 31 | target_compile_options(${PROJECT_NAME} PRIVATE 32 | $<$:/W4 /WX> 33 | $<$>:-Wall -Wextra -Wpedantic -Werror> 34 | ) 35 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/ros/cmdvel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef CMDVEL_HPP 19 | #define CMDVEL_HPP 20 | 21 | #include 22 | #include 23 | 24 | class CmdVel 25 | { 26 | public: 27 | CmdVel(rclcpp::Node& parent_node, std::string topic_name); 28 | void SetVel(double linear, double angular); 29 | 30 | private: 31 | rclcpp::Node& _parent_node; 32 | rclcpp::Publisher::SharedPtr pub; 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/vector2.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef VECTOR2_HPP 19 | #define VECTOR2_HPP 20 | 21 | struct Vector2 22 | { 23 | double x; 24 | double y; 25 | double magnitude() const; 26 | Vector2 normalize() const; 27 | }; 28 | 29 | Vector2 operator-(const Vector2& lhs, const Vector2& rhs); 30 | Vector2 operator+(const Vector2& lhs, const Vector2& rhs); 31 | Vector2 operator/(const Vector2& lhs, double num); 32 | 33 | double pointDistance(Vector2 a, Vector2 b); 34 | 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/ros/range_sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef RANGE_SENSOR_HPP 19 | #define RANGE_SENSOR_HPP 20 | 21 | #include 22 | #include 23 | 24 | class RangeSensor 25 | { 26 | public: 27 | RangeSensor(rclcpp::Node& parent_node, std::string topic_name); 28 | float Read() const; 29 | 30 | private: 31 | rclcpp::Node& _parent_node; 32 | rclcpp::Subscription::SharedPtr sub; 33 | float lastRange; 34 | }; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/dc_motor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/ros/dc_motor.hpp" 19 | 20 | DcMotor::DcMotor(rclcpp::Node& parent_node, std::string topic_name) 21 | : _parent_node(parent_node) 22 | { 23 | auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); 24 | pub = _parent_node.create_publisher(topic_name, qos); 25 | } 26 | 27 | void DcMotor::SetVoltage(double voltage) 28 | { 29 | std_msgs::msg::Float64 voltage_cmd; 30 | voltage_cmd.data = voltage; 31 | this->pub->publish(voltage_cmd); 32 | } 33 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/cmdvel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/ros/cmdvel.hpp" 19 | 20 | CmdVel::CmdVel(rclcpp::Node& parent_node, std::string topic_name) 21 | : _parent_node(parent_node) 22 | { 23 | auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); 24 | pub = _parent_node.create_publisher(topic_name, qos); 25 | } 26 | 27 | void CmdVel::SetVel(double linear, double angular) 28 | { 29 | geometry_msgs::msg::Twist cmd_vel; 30 | cmd_vel.linear.x = linear; 31 | cmd_vel.angular.z = angular; 32 | this->pub->publish(cmd_vel); 33 | } 34 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/range_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/ros/range_sensor.hpp" 19 | 20 | RangeSensor::RangeSensor(rclcpp::Node& parent_node, std::string topic_name) 21 | : _parent_node(parent_node) 22 | { 23 | auto cb_data = [this](sensor_msgs::msg::LaserScan::UniquePtr msg) -> void 24 | { 25 | this->lastRange = msg.get()->ranges.at(0); 26 | }; 27 | this->sub = _parent_node.create_subscription(topic_name, 10, cb_data); 28 | } 29 | 30 | float RangeSensor::Read() const 31 | { 32 | return this->lastRange; 33 | } 34 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/angle_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/angle_utils.hpp" 19 | #include 20 | 21 | #define TWO_M_PI 2 * M_PI 22 | 23 | double signedAngleDifference(double a, double b) 24 | { 25 | double dif = b - a; 26 | while (dif < -M_PI) dif += TWO_M_PI; 27 | while (dif > M_PI) dif -= TWO_M_PI; 28 | return dif; 29 | } 30 | 31 | double angleNormalize(double angle) 32 | { 33 | while (angle < 0) angle += TWO_M_PI; 34 | while (angle >= TWO_M_PI) angle -= TWO_M_PI; 35 | return angle; 36 | } 37 | 38 | double rad2deg(double rad) 39 | { 40 | return rad * 180 / M_PI; 41 | } 42 | 43 | double deg2rad(double deg) 44 | { 45 | return deg * M_PI / 180; 46 | } -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/direction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef POSITION_HPP 19 | #define POSITION_HPP 20 | 21 | enum class Direction 22 | { 23 | up, 24 | right, 25 | down, 26 | left 27 | }; 28 | 29 | Direction oppositeDirection(Direction direction); 30 | Direction ccwDirection(Direction direction); 31 | Direction ccwDirection(Direction direction, int steps); 32 | Direction cwDirection(Direction direction); 33 | Direction cwDirection(Direction direction, int steps); 34 | Direction localToGlobalDirection(Direction localForward, Direction subject); 35 | int ccwStepDistance(Direction from, Direction to); 36 | int cwStepDistance(Direction from, Direction to); 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/solver_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/solver_utils.hpp" 19 | #include 20 | 21 | Pose2 stateToPose(State s, double xSize, double ySize) 22 | { 23 | Pose2 pose; 24 | pose.position.x = s.position.x * xSize; 25 | pose.position.y = s.position.y * ySize; 26 | double angle = 0.0; 27 | switch(s.orientation) 28 | { 29 | case Direction::left: 30 | angle = M_PI; 31 | break; 32 | case Direction::down: 33 | angle = 3 * M_PI_2; 34 | break; 35 | case Direction::right: 36 | angle = 0.0; 37 | break; 38 | case Direction::up: 39 | angle = M_PI_2; 40 | break; 41 | } 42 | pose.angle = angle; 43 | return pose; 44 | } 45 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/vector2int.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/vector2int.hpp" 19 | 20 | Vector2Int Vector2Int::neighbor(Direction direction) 21 | { 22 | Vector2Int result = *this; 23 | switch(direction) 24 | { 25 | case Direction::up: 26 | result.y += 1; 27 | break; 28 | case Direction::right: 29 | result.x += 1; 30 | break; 31 | case Direction::down: 32 | result.y -= 1; 33 | break; 34 | case Direction::left: 35 | result.x -= 1; 36 | break; 37 | } 38 | return result; 39 | } 40 | 41 | bool operator==(const Vector2Int& lhs, const Vector2Int& rhs) 42 | { 43 | return (lhs.x == rhs.x) && (lhs.y == rhs.y); 44 | } 45 | 46 | bool operator!=(const Vector2Int& lhs, const Vector2Int& rhs) 47 | { 48 | return !operator==(lhs, rhs); 49 | } 50 | -------------------------------------------------------------------------------- /gz_ws/README.md: -------------------------------------------------------------------------------- 1 | # gz_ws 2 | 3 | ## Introduction 4 | 5 | gz_ws is the workspace for Gazebo plugins part of Ratada. 6 | 7 | Source files are located in `src` and are installed to `install`. 8 | 9 | - `encoder_sensor` is the implementation of an incremental encoder for Gazebo, as a custom sensor. 10 | - `encoder_sensor_system` is the system that manages the encoder instances. 11 | - `micromouse` is a GUI plugin for setting up and interacting with the simulation. 12 | - `micromouse_system` is the underlying system that processes commands sent via the GUI plugin and keeps track of lap times. 13 | 14 | There are helper scripts for building, installing and cleaning the entire workspace (also removes installed binaries). 15 | 16 | ## Quick Start 17 | 18 | Make the scripts executable: 19 | 20 | ```sh 21 | chmod +x buildws.sh 22 | chmod +x installws.sh 23 | chmod +x cleanws.sh 24 | ``` 25 | 26 | Build and install the plugins: 27 | 28 | ```sh 29 | ./buildws.sh . && ./installws.sh . 30 | ``` 31 | 32 | Point Gazebo to the plugin installation directories: 33 | 34 | ```sh 35 | export GZ_GUI_PLUGIN_PATH=~/gz_ws/install/gui 36 | export GZ_SIM_SYSTEM_PLUGIN_PATH=~/gz_ws/install/system 37 | ``` 38 | 39 | Run this if running Gazebo on Wayland ([source](https://gazebosim.org/docs/harmonic/troubleshooting/#wayland-issues)): 40 | 41 | ```sh 42 | export QT_QPA_PLATFORM=xcb 43 | ``` 44 | 45 | **Tip: you can add the commands above to `~/.bashrc` so they're run every time you open a terminal.** 46 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/ros/encoder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ENCODER_HPP 19 | #define ENCODER_HPP 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using ticks_t = int32_t; 27 | using ros_msg_t = std_msgs::msg::Int32; 28 | 29 | class Encoder 30 | { 31 | public: 32 | Encoder(rclcpp::Node& parent_node, std::string topic_name); 33 | ticks_t TicksDelta(); 34 | ticks_t Ticks() const; 35 | void SetResolution(double resolution_); 36 | double Resolution() const; 37 | 38 | private: 39 | rclcpp::Node& _parent_node; 40 | ticks_t ticks{0}; 41 | ticks_t lastTicks{0}; 42 | rclcpp::Subscription::SharedPtr sub; 43 | double resolution; 44 | mutable std::mutex mutex; 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor_system/EncoderSystem.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ENCODERSYSTEM_HH_ 19 | #define ENCODERSYSTEM_HH_ 20 | 21 | #include 22 | #include 23 | 24 | namespace custom { 25 | class EncoderSystemPrivate; 26 | 27 | class EncoderSystem : public gz::sim::System, 28 | public gz::sim::ISystemPreUpdate, 29 | public gz::sim::ISystemPostUpdate { 30 | public: 31 | explicit EncoderSystem(); 32 | ~EncoderSystem() override; 33 | void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) final; 34 | void PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) final; 35 | 36 | private: 37 | std::unique_ptr dataPtr; 38 | }; 39 | } // namespace custom 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/ros/imu.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef IMU_HPP 19 | #define IMU_HPP 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | class Imu 26 | { 27 | public: 28 | Imu(rclcpp::Node& parent_node, std::string topic_name); 29 | double MagZ() const; 30 | 31 | private: 32 | rclcpp::Node& _parent_node; 33 | rclcpp::Subscription::SharedPtr sub; 34 | // quaternion 35 | tf2::Quaternion q; 36 | double mag_x{0.0}; 37 | double mag_y{0.0}; 38 | double mag_z{0.0}; 39 | double mag_w{0.0}; 40 | 41 | double gyro_x{0.0}; 42 | double gyro_y{0.0}; 43 | double gyro_z{0.0}; 44 | 45 | double accel_x{0.0}; 46 | double accel_y{0.0}; 47 | double accel_z{0.0}; 48 | 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor/Encoder.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ENCODER_HH_ 19 | #define ENCODER_HH_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace custom 26 | { 27 | class EncoderPrivate; 28 | 29 | class Encoder : public gz::sensors::Sensor 30 | { 31 | public: 32 | Encoder(); 33 | virtual ~Encoder(); 34 | virtual bool Load(const sdf::Sensor &_sdf) override; 35 | using Sensor::Update; // For the Update(duration, bool) definition: 36 | virtual bool Update(const std::chrono::steady_clock::duration &_now) override; 37 | void SetJoint(gz::sim::Entity joint); 38 | gz::sim::Entity Joint() const; 39 | void SetPosition(double position); 40 | double Position() const; 41 | 42 | private: 43 | std::unique_ptr dataPtr; 44 | }; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/vector2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/vector2.hpp" 19 | #include 20 | 21 | double Vector2::magnitude() const 22 | { 23 | return std::sqrt(x * x + y * y); 24 | } 25 | 26 | Vector2 Vector2::normalize() const 27 | { 28 | return *this / magnitude(); 29 | } 30 | 31 | double pointDistance(Vector2 a, Vector2 b) 32 | { 33 | return (a - b).magnitude(); 34 | } 35 | 36 | Vector2 operator-(const Vector2& lhs, const Vector2& rhs) 37 | { 38 | Vector2 result; 39 | result.x = lhs.x - rhs.x; 40 | result.y = lhs.y - rhs.y; 41 | return result; 42 | } 43 | 44 | Vector2 operator+(const Vector2& lhs, const Vector2& rhs) 45 | { 46 | Vector2 result; 47 | result.x = lhs.x + rhs.x; 48 | result.y = lhs.y + rhs.y; 49 | return result; 50 | } 51 | 52 | Vector2 operator/(const Vector2& lhs, double num) 53 | { 54 | Vector2 result; 55 | result.x = lhs.x / num; 56 | result.y = lhs.y / num; 57 | return result; 58 | } 59 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/cell.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/cell.hpp" 19 | 20 | bool Cell::placeWall(Direction position) 21 | { 22 | bool hadWall = hasWall(position); 23 | int index = wallPositionToIndex(position); 24 | m_walls |= (1 << index); 25 | return !hadWall; 26 | } 27 | 28 | bool Cell::removeWall(Direction position) 29 | { 30 | bool hadWall = hasWall(position); 31 | int index = wallPositionToIndex(position); 32 | m_walls &= ~(1 << index); 33 | return hadWall; 34 | } 35 | 36 | bool Cell::hasWall(Direction position) const 37 | { 38 | int index = wallPositionToIndex(position); 39 | return (1 << index) & m_walls; 40 | } 41 | 42 | int Cell::wallPositionToIndex(Direction position) 43 | { 44 | int ret = -1; 45 | switch(position) 46 | { 47 | case Direction::up: 48 | ret = 0; 49 | break; 50 | case Direction::right: 51 | ret = 1; 52 | break; 53 | case Direction::down: 54 | ret = 2; 55 | break; 56 | case Direction::left: 57 | ret = 3; 58 | break; 59 | } 60 | return ret; 61 | } -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/encoder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/ros/encoder.hpp" 19 | 20 | Encoder::Encoder(rclcpp::Node& parent_node, std::string topic_name) 21 | : _parent_node(parent_node) 22 | { 23 | auto cb_data = [this](ros_msg_t::UniquePtr msg) -> void { 24 | std::lock_guard lock(this->mutex); 25 | this->ticks = msg.get()->data; 26 | }; 27 | this->sub = _parent_node.create_subscription(topic_name, 10, cb_data); 28 | RCLCPP_INFO(_parent_node.get_logger(), "Encoder has been initialised"); 29 | } 30 | 31 | ticks_t Encoder::Ticks() const 32 | { 33 | std::lock_guard lock(this->mutex); 34 | return this->ticks; 35 | } 36 | 37 | ticks_t Encoder::TicksDelta() 38 | { 39 | ticks_t ticks = Ticks(); 40 | ticks_t delta = ticks - this->lastTicks; 41 | this->lastTicks = ticks; 42 | return delta; 43 | } 44 | 45 | void Encoder::SetResolution(double resolution_) 46 | { 47 | this->resolution = resolution_; 48 | } 49 | 50 | double Encoder::Resolution() const 51 | { 52 | return this->resolution; 53 | } 54 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/math_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/math_utils.hpp" 19 | #include 20 | 21 | Vector2 rayCast(Vector2 origin, double angle, double length) 22 | { 23 | origin.x += length * std::cos(angle); 24 | origin.y += length * std::sin(angle); 25 | return origin; 26 | } 27 | 28 | bool isWithinRectangle(Vector2 point, Vector2 recSize) 29 | { 30 | return point.x >= -recSize.x/2 && point.x <= recSize.x/2 31 | && point.y >= -recSize.y/2 && point.y <= recSize.y/2; 32 | } 33 | 34 | bool isWithinRectangle(Vector2 point, Vector2 recCenter, Vector2 recSize) 35 | { 36 | double forwardBoundary = recCenter.y + recSize.y / 2; 37 | double rightBoundary = recCenter.x + recSize.x / 2; 38 | double backwardBoundary = recCenter.y - recSize.y / 2; 39 | double leftBoundary = recCenter.x - recSize.x / 2; 40 | 41 | return (point.x >= leftBoundary && point.x < rightBoundary ) 42 | && (point.y >= backwardBoundary && point.y < forwardBoundary); 43 | } 44 | 45 | bool isWithinCircle(Vector2 point, double radius) 46 | { 47 | return point.magnitude() <= radius; 48 | } 49 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/launch/micromouse_launch.py: -------------------------------------------------------------------------------- 1 | from ament_index_python.packages import get_package_share_directory 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | import os 7 | 8 | 9 | def generate_launch_description(): 10 | ros_gz_sim = get_package_share_directory('ros_gz_sim') 11 | 12 | world = os.path.join( 13 | get_package_share_directory('micromouse'), 14 | 'worlds', 15 | 'micromouse.world' 16 | ) 17 | 18 | gui_config = os.path.join( 19 | get_package_share_directory('micromouse'), 20 | 'worlds', 21 | 'gui.config' 22 | ) 23 | 24 | bridge_config = os.path.join( 25 | get_package_share_directory('micromouse'), 26 | 'params', 27 | 'micromouse_bridge.yaml' 28 | ) 29 | 30 | bridge_cmd = Node( 31 | package='ros_gz_bridge', 32 | executable='parameter_bridge', 33 | arguments=[ 34 | '--ros-args', 35 | '-p', f'config_file:={bridge_config}' 36 | ] 37 | ) 38 | 39 | gz_cmd = IncludeLaunchDescription( 40 | PythonLaunchDescriptionSource( 41 | os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py'), 42 | ), 43 | launch_arguments={'gz_args': [world, ' --gui-config ', gui_config, ' -v4']}.items() 44 | ) 45 | 46 | micromouse_node = Node( 47 | package='micromouse', 48 | executable='micromouse', 49 | parameters=[{'use_sim_time': True}] 50 | ) 51 | 52 | ld = LaunchDescription() 53 | ld.add_action(bridge_cmd) 54 | ld.add_action(gz_cmd) 55 | ld.add_action(micromouse_node) 56 | return ld 57 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/maze_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/maze_utils.hpp" 19 | 20 | LogicMaze classicMicromouseMaze() 21 | { 22 | LogicMaze maze(16, 16); 23 | surroundMazeWithWalls(maze); 24 | maze.placeWall({0, 0}, Direction::right); 25 | maze.setHomePosition({0, 0}); 26 | maze.addGoalPosition({7, 7}); 27 | maze.addGoalPosition({7, 8}); 28 | maze.addGoalPosition({8, 7}); 29 | maze.addGoalPosition({8, 8}); 30 | return maze; 31 | } 32 | 33 | void surroundMazeWithWalls(LogicMaze& maze) 34 | { 35 | for (auto i = 0; i < maze.xSize(); ++i) 36 | { 37 | for (auto j = 0; j < maze.ySize(); ++j) 38 | { 39 | Vector2Int cellPosition = {i, j}; 40 | if (i == 0) 41 | { 42 | maze.placeWall(cellPosition, Direction::left); 43 | } 44 | else if (i == maze.xSize() - 1) 45 | { 46 | maze.placeWall(cellPosition, Direction::right); 47 | } 48 | if (j == 0) 49 | { 50 | maze.placeWall(cellPosition, Direction::down); 51 | } 52 | else if (j == maze.ySize() - 1) 53 | { 54 | maze.placeWall(cellPosition, Direction::up); 55 | } 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Ratada 2 | 3 | ![Screenshot of Ratada with RoboChallenge 2024 Day 3 maze](https://github.com/user-attachments/assets/3e1510fe-5551-4718-a904-39e6cc5a3e53) 4 | 5 | ## Introduction 6 | 7 | Ratada is a 3D simulation environment for Micromouse robots. 8 | 9 | ## Supported Platform 10 | 11 | Ratada is implemented as a set of plugins for Gazebo and ROS2. 12 | 13 | It has been tested and developed with [Gazebo Harmonic](https://gazebosim.org/docs/harmonic/getstarted/) and [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/) on an [Ubuntu 24.04 (Noble Numbat)](https://releases.ubuntu.com/noble/) virtual machine. 14 | 15 | ## Features 16 | 17 | - Import robots from [URDF](http://docs.ros.org/en/jazzy/Tutorials/Intermediate/URDF/URDF-Main.html) or [SDF](http://sdformat.org/spec?elem=sdf&ver=1.11) files 18 | - Import Classic 16 x 16 mazes in [Micromouse Online's mazefiles repository](https://github.com/micromouseonline/mazefiles) file format 19 | - Support for built-in [sensors](https://gazebosim.org/docs/harmonic/sensors/) 20 | - Keeps track of solve times 21 | - Implementation of wheel encoder sensor 22 | - Example robot with range sensors, differential drive and wheel encoders 23 | - Maze solving algorithm implementation based on the flood fill strategy 24 | 25 | ## Quick Start 26 | 27 | This assumes an environment running Ubuntu 24.04, since ROS2 distributions are tightly tied to particular Ubuntu releases. 28 | 29 | ### Install ROS2 30 | 31 | Follow the [ROS2 installation guide](http://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) 32 | 33 | ### Install Gazebo 34 | 35 | See [Installing Gazebo with ROS](https://gazebosim.org/docs/harmonic/ros_installation/) for detailed instructions. 36 | 37 | The following command may be enough: 38 | 39 | ```sh 40 | sudo apt-get install ros-jazzy-ros-gz 41 | ``` 42 | 43 | ### Run the simulator 44 | 45 | See the README files in the `gz_ws` and `ros_ws` folders for further instructions. 46 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse_system/MicromouseSystem.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MICROMOUSE_SYSTEM_HH_ 19 | #define MICROMOUSE_SYSTEM_HH_ 20 | 21 | #include 22 | #include 23 | 24 | namespace custom 25 | { 26 | 27 | class MicromouseSystemPrivate; 28 | 29 | class MicromouseSystem 30 | : public gz::sim::System, 31 | public gz::sim::ISystemConfigure, 32 | public gz::sim::ISystemPreUpdate, 33 | public gz::sim::ISystemPostUpdate 34 | { 35 | public: 36 | MicromouseSystem(); 37 | 38 | ~MicromouseSystem() override; 39 | 40 | void Configure( 41 | const gz::sim::Entity &_entity, 42 | const std::shared_ptr &_sdf, 43 | gz::sim::EntityComponentManager &_ecm, 44 | gz::sim::EventManager &_eventMgr) override; 45 | 46 | void PreUpdate( 47 | const gz::sim::UpdateInfo &_info, 48 | gz::sim::EntityComponentManager &_ecm) override; 49 | 50 | void PostUpdate( 51 | const gz::sim::UpdateInfo &_info, 52 | const gz::sim::EntityComponentManager &_ecm) override; 53 | 54 | private: 55 | std::unique_ptr dataPtr; 56 | }; 57 | 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/micromouse.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MICROMOUSE_MICROMOUSE_HPP 19 | #define MICROMOUSE_MICROMOUSE_HPP 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "micromouse/ros/cmdvel.hpp" 29 | #include "micromouse/ros/encoder.hpp" 30 | #include "micromouse/ros/imu.hpp" 31 | #include "micromouse/ros/range_sensor.hpp" 32 | #include "micromouse/ros/dc_motor.hpp" 33 | 34 | class MicromouseNode 35 | : public rclcpp::Node 36 | { 37 | public: 38 | MicromouseNode(); 39 | ~MicromouseNode(); 40 | 41 | private: 42 | void update_callback(); 43 | void update_cmd_vel(double linear, double angular); 44 | CmdVel cmdVel; 45 | DcMotor motor_left; 46 | DcMotor motor_right; 47 | RangeSensor range_center; 48 | RangeSensor range_right; 49 | RangeSensor range_left; 50 | Imu imu; 51 | Encoder encoder_left; 52 | Encoder encoder_right; 53 | rclcpp::Subscription::SharedPtr micromoue_sub; 54 | bool micromouseStarted{false}; 55 | rclcpp::TimerBase::SharedPtr update_timer_; 56 | }; 57 | 58 | #endif // MICROMOUSE_MICROMOUSE_HPP 59 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/params/micromouse_bridge.yaml: -------------------------------------------------------------------------------- 1 | # Lidar 1 2 | - ros_topic_name: "lidar_left" 3 | gz_topic_name: "/lidar_left" 4 | gz_type_name: "gz.msgs.LaserScan" 5 | ros_type_name: "sensor_msgs/msg/LaserScan" 6 | direction: GZ_TO_ROS 7 | 8 | # Lidar 2 9 | - ros_topic_name: "lidar_center" 10 | gz_topic_name: "/lidar_center" 11 | gz_type_name: "gz.msgs.LaserScan" 12 | ros_type_name: "sensor_msgs/msg/LaserScan" 13 | direction: GZ_TO_ROS 14 | 15 | # Lidar 3 16 | - ros_topic_name: "lidar_right" 17 | gz_topic_name: "/lidar_right" 18 | gz_type_name: "gz.msgs.LaserScan" 19 | ros_type_name: "sensor_msgs/msg/LaserScan" 20 | direction: GZ_TO_ROS 21 | 22 | # IMU 23 | - ros_topic_name: "imu" 24 | gz_topic_name: "/imu" 25 | gz_type_name: "gz.msgs.IMU" 26 | ros_type_name: "sensor_msgs/msg/Imu" 27 | direction: GZ_TO_ROS 28 | 29 | # Motor controller 30 | - ros_topic_name: "cmd_vel" 31 | gz_topic_name: "/model/micromouse_robot/cmd_vel" 32 | gz_type_name: "gz.msgs.Twist" 33 | ros_type_name: "geometry_msgs/msg/Twist" 34 | direction: ROS_TO_GZ 35 | 36 | # Odometry 1 37 | - ros_topic_name: "left_wheel_encoder" 38 | gz_topic_name: "/left_wheel_encoder" 39 | gz_type_name: "gz.msgs.Int32" 40 | ros_type_name: "std_msgs/msg/Int32" 41 | direction: GZ_TO_ROS 42 | 43 | # Odometry 2 44 | - ros_topic_name: "right_wheel_encoder" 45 | gz_topic_name: "/right_wheel_encoder" 46 | gz_type_name: "gz.msgs.Int32" 47 | ros_type_name: "std_msgs/msg/Int32" 48 | direction: GZ_TO_ROS 49 | 50 | # Micromouse start message 51 | - ros_topic_name: "micromouse" 52 | gz_topic_name: "/micromouse/status" 53 | gz_type_name: "gz.msgs.Boolean" 54 | ros_type_name: "std_msgs/msg/Bool" 55 | direction: GZ_TO_ROS 56 | 57 | # Simulation clock for nodes with "use_sim_time" parameter set to True 58 | - ros_topic_name: "clock" 59 | gz_topic_name: "/clock" 60 | gz_type_name: gz.msgs.Clock 61 | ros_type_name: rosgraph_msgs/msg/Clock 62 | direction: GZ_TO_ROS 63 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/imu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/ros/imu.hpp" 19 | #include 20 | 21 | Imu::Imu(rclcpp::Node& parent_node, std::string topic_name) 22 | : _parent_node(parent_node) 23 | { 24 | auto cb_data = [this](sensor_msgs::msg::Imu::UniquePtr msg) -> void 25 | { 26 | this->mag_x = msg.get()->orientation.x; 27 | this->mag_y = msg.get()->orientation.y; 28 | this->mag_z = msg.get()->orientation.z; 29 | this->mag_w = msg.get()->orientation.w; 30 | this->q = tf2::Quaternion(this->mag_x, this->mag_y, this->mag_z, this->mag_w); // Todo: ensure tf2scalar and double are compatible... How to avoid conversion between the two? 31 | 32 | this->gyro_x = msg.get()->angular_velocity.x; 33 | this->gyro_y = msg.get()->angular_velocity.y; 34 | this->gyro_z = msg.get()->angular_velocity.z; 35 | 36 | this->accel_x = msg.get()->linear_acceleration.x; 37 | this->accel_y = msg.get()->linear_acceleration.y; 38 | this->accel_z = msg.get()->linear_acceleration.z; 39 | }; 40 | 41 | this->sub = _parent_node.create_subscription(topic_name, 10, cb_data); 42 | } 43 | 44 | double Imu::MagZ() const 45 | { 46 | tf2::Matrix3x3 m{this->q}; 47 | tf2Scalar row, pitch, yaw; 48 | m.getRPY(row, pitch, yaw); 49 | return yaw; 50 | } 51 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(micromouse) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(ros_gz_sim REQUIRED) 13 | find_package(ros_gz_bridge REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(tf2 REQUIRED) 18 | # find_package(rclcpp_components REQUIRED) 19 | 20 | include_directories( 21 | include 22 | ) 23 | 24 | set(dependencies 25 | "geometry_msgs" 26 | "nav_msgs" 27 | "rclcpp" 28 | "sensor_msgs" 29 | "tf2" 30 | ) 31 | 32 | set(EXEC_NAME "micromouse") 33 | 34 | add_executable(${EXEC_NAME} src/micromouse.cpp src/cmdvel.cpp src/encoder.cpp src/imu.cpp src/range_sensor.cpp 35 | src/solver_utils.cpp src/math_utils.cpp src/pose2.cpp src/direction.cpp src/vector2.cpp src/vector2int.cpp 36 | src/angle_utils.cpp src/maze.cpp src/cell.cpp src/solver.cpp src/maze_utils.cpp 37 | src/bitwise_utils.cpp src/robot.cpp src/dc_motor.cpp) 38 | ament_target_dependencies(${EXEC_NAME} ${dependencies}) 39 | 40 | install( 41 | DIRECTORY launch worlds urdf params 42 | DESTINATION share/${PROJECT_NAME} 43 | ) 44 | 45 | install(DIRECTORY include/ 46 | DESTINATION include/ 47 | ) 48 | 49 | install(TARGETS ${EXEC_NAME} 50 | DESTINATION lib/${PROJECT_NAME} 51 | ) 52 | 53 | if(BUILD_TESTING) 54 | find_package(ament_lint_auto REQUIRED) 55 | # the following line skips the linter which checks for copyrights 56 | # comment the line when a copyright and license is added to all source files 57 | set(ament_cmake_copyright_FOUND TRUE) 58 | # the following line skips cpplint (only works in a git repo) 59 | # comment the line when this package is in a git repo and when 60 | # a copyright and license is added to all source files 61 | set(ament_cmake_cpplint_FOUND TRUE) 62 | ament_lint_auto_find_test_dependencies() 63 | endif() 64 | 65 | ament_package() 66 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/maze.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MAZE_HPP 19 | #define MAZE_HPP 20 | 21 | #include "micromouse/cell.hpp" 22 | #include "micromouse/vector2int.hpp" 23 | #include 24 | #include 25 | 26 | class IMaze 27 | { 28 | public: 29 | virtual int xSize() const = 0; 30 | virtual int ySize() const = 0; 31 | virtual bool hasWall(Vector2Int cellPosition, Direction direction) const = 0; 32 | virtual Vector2Int homePosition() const = 0; 33 | virtual std::vector goalPositions() const = 0; 34 | }; 35 | 36 | class LogicMaze 37 | : public IMaze 38 | { 39 | public: 40 | LogicMaze(int xSize, int ySize); 41 | int xSize() const override; 42 | int ySize() const override; 43 | bool placeWall(Vector2Int cellPosition, Direction direction); 44 | bool removeWall(Vector2Int cellPosition, Direction direction); 45 | bool hasWall(Vector2Int cellPosition, Direction direction) const override; 46 | 47 | void setHomePosition(Vector2Int home); 48 | void addGoalPosition(Vector2Int goal); 49 | 50 | Vector2Int homePosition() const override; 51 | virtual std::vector goalPositions() const override; 52 | 53 | private: 54 | int _xSize; 55 | int _ySize; 56 | Vector2Int _home; 57 | std::vector _goals; 58 | Cell *_cells; 59 | Vector2Int _homePosition; 60 | std::vector _goalPositions; 61 | const Cell& cellAt(Vector2Int position) const; 62 | Cell& cellAt(Vector2Int position); 63 | bool isWithinBounds(Vector2Int position) const; 64 | bool hasNeighbor(Vector2Int cellPosition, Direction direction) const; 65 | }; 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/worlds/micromouse.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.001 6 | 1.0 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | ogre2 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | true 30 | 0 0 10 0 0 0 31 | 0.8 0.8 0.8 1 32 | 0.2 0.2 0.2 1 33 | 34 | 1000 35 | 0.9 36 | 0.01 37 | 0.001 38 | 39 | -0.5 0.1 -0.9 40 | 41 | 42 | 43 | true 44 | 0 0 0 0 0 0 45 | 46 | 47 | 48 | 49 | 0 0 1 50 | 100 100 51 | 52 | 53 | 54 | 55 | 56 | 57 | 0 0 1 58 | 100 100 59 | 60 | 61 | 62 | 0 0 0 1 63 | 0 0 0 1 64 | 0 0 0 1 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/solver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SOLVER_HPP 19 | #define SOLVER_HPP 20 | 21 | #include "micromouse/vector2int.hpp" 22 | #include "micromouse/direction.hpp" 23 | #include "micromouse/maze.hpp" 24 | #include 25 | 26 | // The states that a robot can be in. 27 | struct State 28 | { 29 | Vector2Int position; 30 | Direction orientation; 31 | }; 32 | 33 | bool operator==(const State& lhs, const State& rhs); 34 | 35 | enum class StateTransition 36 | { 37 | forward, turnLeft, turnRight, invalid 38 | }; 39 | 40 | StateTransition stateFromTo(State from, State to); 41 | 42 | class FloodFillAdvSolver 43 | { 44 | public: 45 | FloodFillAdvSolver(const IMaze& maze); 46 | void addGoal(Vector2Int position); 47 | void setGoalToHome(); 48 | void setGoalToMazeGoal(); 49 | void resetGoals(); 50 | bool isSeed(State s) const; 51 | int cost(State s) const; 52 | void calculateCosts(); 53 | void calculateCostsOptimized(State seedState); 54 | State bestMovement(State current) const; 55 | 56 | private: 57 | const IMaze& _maze; 58 | bool _seeds[16][16][4]; 59 | int _costs[16][16][4]; 60 | std::queue q; 61 | void setCost(State s, int cost); 62 | void enqueueBackwardNeighborsIfNotSeed(State s); 63 | std::vector backwardNeighbors(State s) const; 64 | std::vector forwardNeighbors(State s) const; 65 | }; 66 | 67 | class PathManager 68 | { 69 | public: 70 | PathManager(const IMaze& maze); 71 | StateTransition nextMovement(); 72 | void updatedWalls(); 73 | State getRobotState() const; 74 | 75 | private: 76 | FloodFillAdvSolver _solver; 77 | const IMaze& _maze; 78 | bool _goingToCenter; 79 | State _robotState; 80 | bool isGoal(State s) const; 81 | bool isHome(State s) const; 82 | }; 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/include/micromouse/robot.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef ROBOT_HPP 19 | #define ROBOT_HPP 20 | 21 | #include "micromouse/robot_specs.hpp" 22 | #include "micromouse/pose2.hpp" 23 | 24 | struct Displacement 25 | { 26 | double linear; 27 | double angular; 28 | }; 29 | 30 | struct Velocity 31 | { 32 | double linear; 33 | double angular; 34 | }; 35 | 36 | struct MazeSpecification 37 | { 38 | double cellWallSize = 16.8 / 100; 39 | int sizeX = 16; 40 | int sizeY = 16; 41 | }; 42 | 43 | struct BoundingBoxToTurningAxis 44 | { 45 | double leftToAxis; 46 | double rightToAxis; 47 | double frontToAxis; 48 | double backToAxis; 49 | }; 50 | 51 | class ICalculateDisplacementFromPulses 52 | { 53 | public: 54 | virtual Displacement getDisplacement(int leftWheelPulses, int rightWheelPulses) = 0; 55 | }; 56 | 57 | class SimpleVelocityFromPulsesCalculator : public ICalculateDisplacementFromPulses 58 | { 59 | public: 60 | SimpleVelocityFromPulsesCalculator(const RobotConstructionSpecification& specs); 61 | Displacement getDisplacement(int leftWheelPulses, int rightWheelPulses) override; 62 | 63 | private: 64 | const RobotConstructionSpecification& _specs; 65 | }; 66 | 67 | class SimpleRobotPoseManager 68 | { 69 | private: 70 | const RobotConstructionSpecification& m_specs; 71 | Pose2 m_pose; 72 | SimpleVelocityFromPulsesCalculator m_calc; 73 | 74 | public: 75 | SimpleRobotPoseManager(const RobotConstructionSpecification& specs); 76 | // void updatePose(int leftWheelPulses, int rightWheelPulses, unsigned long deltaTime); 77 | void updatePose(int leftWheelPulses, int rightWheelPulses); 78 | Pose2 getPose() const; 79 | void setPose(Pose2 pose); 80 | float deltaDistance() const; 81 | }; 82 | 83 | enum class RobotMovement 84 | { 85 | forward, turnLeft, turnRight, halt 86 | }; 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/MicromousePlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef MICROMOUSEPLUGIN_HH_ 19 | #define MICROMOUSEPLUGIN_HH_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace custom 28 | { 29 | 30 | class MicromousePluginPrivate; 31 | 32 | class MicromousePlugin : public gz::sim::GuiSystem 33 | { 34 | 35 | Q_OBJECT 36 | 37 | Q_PROPERTY( 38 | QString mazefile 39 | READ Mazefile 40 | WRITE SetMazefile 41 | NOTIFY MazefileChanged 42 | ) 43 | 44 | Q_PROPERTY( 45 | QString robotfile 46 | READ Robotfile 47 | WRITE SetRobotfile 48 | NOTIFY RobotfileChanged 49 | ) 50 | 51 | Q_PROPERTY( 52 | QVector3D position 53 | READ Position 54 | WRITE SetPosition 55 | NOTIFY PositionChanged 56 | ) 57 | 58 | Q_PROPERTY( 59 | QVector3D orientation 60 | READ Orientation 61 | WRITE SetOrientation 62 | NOTIFY OrientationChanged 63 | ) 64 | 65 | public: 66 | MicromousePlugin(); 67 | virtual ~MicromousePlugin(); 68 | virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; 69 | void Update(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; 70 | Q_INVOKABLE QString QurlToQString(QUrl url) const; 71 | 72 | protected slots: 73 | void OnLoadButton(); 74 | void OnResetButton(); 75 | void StartMicromouse(); 76 | 77 | public: 78 | Q_INVOKABLE QString Mazefile() const; 79 | Q_INVOKABLE QString Robotfile() const; 80 | Q_INVOKABLE QVector3D Position() const; 81 | Q_INVOKABLE QVector3D Orientation() const; 82 | Q_INVOKABLE void SetMazefile(const QString &_mazefile); 83 | Q_INVOKABLE void SetRobotfile(const QString &_robotfile); 84 | Q_INVOKABLE void SetPosition(const QVector3D &_position); 85 | Q_INVOKABLE void SetOrientation(const QVector3D &_orientation); 86 | 87 | signals: 88 | void MazefileChanged(); 89 | void RobotfileChanged(); 90 | void PositionChanged(); 91 | void OrientationChanged(); 92 | 93 | private: 94 | std::unique_ptr dataPtr; 95 | }; 96 | 97 | } 98 | 99 | #endif // MICROMOUSEPLUGIN_HH_ 100 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/robot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/robot.hpp" 19 | #include 20 | 21 | SimpleRobotPoseManager::SimpleRobotPoseManager(const RobotConstructionSpecification& specs) 22 | : m_specs(specs), m_pose{0, 0, 0}, m_calc{m_specs} 23 | { 24 | } 25 | 26 | void SimpleRobotPoseManager::updatePose(int leftWheelPulses, int rightWheelPulses) 27 | { 28 | Displacement d = m_calc.getDisplacement(leftWheelPulses, rightWheelPulses); 29 | 30 | m_pose.angle += d.angular; 31 | m_pose.position.x += d.linear * cos(m_pose.angle); 32 | m_pose.position.y += d.linear * sin(m_pose.angle); 33 | } 34 | 35 | //void SimpleRobotPoseManager::updatePose(int leftWheelPulses, int rightWheelPulses, unsigned long deltaTime) 36 | //{ 37 | // Velocity v = m_calc.getVelocity(leftWheelPulses, rightWheelPulses, deltaTime); 38 | // m_pose.theta += v.angular * deltaTime; 39 | // m_pose.x += v.linear * cos(m_pose.theta) * deltaTime; 40 | // m_pose.y += v.linear * sin(m_pose.theta) * deltaTime; 41 | //} 42 | 43 | Pose2 SimpleRobotPoseManager::getPose() const 44 | { 45 | return m_pose; 46 | } 47 | 48 | void SimpleRobotPoseManager::setPose(Pose2 pose) 49 | { 50 | m_pose = pose; 51 | } 52 | 53 | float SimpleRobotPoseManager::deltaDistance() const 54 | { 55 | return m_pose.position.magnitude(); 56 | } 57 | 58 | SimpleVelocityFromPulsesCalculator::SimpleVelocityFromPulsesCalculator(const RobotConstructionSpecification& specs) 59 | : _specs(specs) 60 | { 61 | } 62 | 63 | Displacement SimpleVelocityFromPulsesCalculator::getDisplacement(int leftWheelPulses, int rightWheelPulses) 64 | { 65 | float leftWheelAngularDisplacement = leftWheelPulses * 2 * M_PI / _specs.leftWheelPPR; 66 | float rightWheelAngularDisplacement = rightWheelPulses * 2 * M_PI / _specs.rightWheelPPR; 67 | 68 | float leftWheelLinearDisplacement = leftWheelAngularDisplacement * _specs.leftWheelRadius; 69 | float rightWheelLinearDisplacement = rightWheelAngularDisplacement * _specs.rightWheelRadius; 70 | 71 | float robotLinearDisplacement = (rightWheelLinearDisplacement + leftWheelLinearDisplacement) / 2; 72 | float robotAngularDisplacement = (rightWheelLinearDisplacement - leftWheelLinearDisplacement) / _specs.wheelBaseDistance; 73 | 74 | Displacement displacement; 75 | displacement.linear = robotLinearDisplacement; 76 | displacement.angular = robotAngularDisplacement; 77 | return displacement; 78 | } 79 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/direction.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/direction.hpp" 19 | 20 | Direction oppositeDirection(Direction direction) 21 | { 22 | switch(direction) 23 | { 24 | case Direction::up: 25 | direction = Direction::down; 26 | break; 27 | case Direction::right: 28 | direction = Direction::left; 29 | break; 30 | case Direction::down: 31 | direction = Direction::up; 32 | break; 33 | case Direction::left: 34 | direction = Direction::right; 35 | break; 36 | } 37 | return direction; 38 | } 39 | 40 | Direction ccwDirection(Direction direction) 41 | { 42 | switch(direction) 43 | { 44 | case Direction::up: 45 | direction = Direction::left; 46 | break; 47 | case Direction::right: 48 | direction = Direction::up; 49 | break; 50 | case Direction::down: 51 | direction = Direction::right; 52 | break; 53 | case Direction::left: 54 | direction = Direction::down; 55 | break; 56 | } 57 | return direction; 58 | } 59 | 60 | Direction ccwDirection(Direction direction, int steps) 61 | { 62 | if (steps < 0) 63 | { 64 | return cwDirection(direction, -steps); 65 | } 66 | steps %= 4; 67 | for (int i = 0; i < steps; ++i) 68 | { 69 | direction = ccwDirection(direction); 70 | } 71 | return direction; 72 | } 73 | 74 | Direction cwDirection(Direction direction) 75 | { 76 | switch(direction) 77 | { 78 | case Direction::up: 79 | direction = Direction::right; 80 | break; 81 | case Direction::right: 82 | direction = Direction::down; 83 | break; 84 | case Direction::down: 85 | direction = Direction::left; 86 | break; 87 | case Direction::left: 88 | direction = Direction::up; 89 | break; 90 | } 91 | return direction; 92 | } 93 | 94 | Direction cwDirection(Direction direction, int steps) 95 | { 96 | if(steps < 0) 97 | { 98 | return ccwDirection(direction, -steps); 99 | } 100 | steps %= 4; 101 | for (int i = 0; i < steps; ++i) 102 | { 103 | direction = cwDirection(direction); 104 | } 105 | return direction; 106 | } 107 | 108 | int ccwStepDistance(Direction from, Direction to) 109 | { 110 | int stepDistance = 0; 111 | while (from != to) 112 | { 113 | from = ccwDirection(from); 114 | stepDistance++; 115 | } 116 | return stepDistance; 117 | } 118 | 119 | int cwStepDistance(Direction from, Direction to) 120 | { 121 | int stepDistance = 0; 122 | while (from != to) 123 | { 124 | from = cwDirection(from); 125 | stepDistance++; 126 | } 127 | return stepDistance; 128 | } 129 | 130 | Direction localToGlobalDirection(Direction localForward, Direction subject) 131 | { 132 | int localToGlobalStepOffset = ccwStepDistance(localForward, Direction::up); 133 | Direction result = cwDirection(subject, localToGlobalStepOffset); 134 | return result; 135 | } 136 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/maze.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/maze.hpp" 19 | #include 20 | 21 | LogicMaze::LogicMaze(int xSize, int ySize) 22 | : _xSize(xSize), _ySize(ySize) 23 | { 24 | this->_cells = (Cell *)std::malloc(xSize * ySize); 25 | } 26 | 27 | int LogicMaze::xSize() const 28 | { 29 | return this->_xSize; 30 | } 31 | 32 | int LogicMaze::ySize() const 33 | { 34 | return this->_ySize; 35 | } 36 | 37 | bool LogicMaze::placeWall(Vector2Int cellPosition, Direction wallPosition) 38 | { 39 | if (!isWithinBounds(cellPosition)) return false; 40 | Cell& targetCell = cellAt(cellPosition); 41 | bool targetCellSuccessFlag = targetCell.placeWall(wallPosition); 42 | if (!hasNeighbor(cellPosition, wallPosition)) 43 | { 44 | return targetCellSuccessFlag; 45 | } 46 | Vector2Int neighborPosition = cellPosition.neighbor(wallPosition); 47 | Cell& neighborCell = cellAt(neighborPosition); 48 | Direction neighborWallPosition = oppositeDirection(wallPosition); 49 | bool neighborCellSuccessFlag = neighborCell.placeWall(neighborWallPosition); 50 | return targetCellSuccessFlag && neighborCellSuccessFlag; 51 | } 52 | 53 | bool LogicMaze::removeWall(Vector2Int cellPosition, Direction wallPosition) 54 | { 55 | if (!isWithinBounds(cellPosition)) return false; 56 | Cell& targetCell = cellAt(cellPosition); 57 | bool targetCellSuccessFlag = targetCell.removeWall(wallPosition); 58 | if (!hasNeighbor(cellPosition, wallPosition)) 59 | { 60 | return targetCellSuccessFlag; 61 | } 62 | Vector2Int neighborPosition = cellPosition.neighbor(wallPosition); 63 | Cell& neighborCell = cellAt(neighborPosition); 64 | Direction neighborWallPosition = oppositeDirection(wallPosition); 65 | bool neighborCellSuccessFlag = neighborCell.removeWall(neighborWallPosition); 66 | return targetCellSuccessFlag && neighborCellSuccessFlag; 67 | } 68 | 69 | bool LogicMaze::hasWall(Vector2Int cellPosition, Direction wallPosition) const 70 | { 71 | if (!isWithinBounds(cellPosition)) return false; 72 | const Cell& targetCell = cellAt(cellPosition); 73 | bool targetCellSuccessFlag = targetCell.hasWall(wallPosition); 74 | if (!hasNeighbor(cellPosition, wallPosition)) 75 | { 76 | return targetCellSuccessFlag; 77 | } 78 | Vector2Int neighborPosition = cellPosition.neighbor(wallPosition); 79 | const Cell& neighborCell = cellAt(neighborPosition); 80 | Direction neighborWallPosition = oppositeDirection(wallPosition); 81 | bool neighborCellSuccessFlag = neighborCell.hasWall(neighborWallPosition); 82 | return targetCellSuccessFlag && neighborCellSuccessFlag; 83 | } 84 | 85 | Vector2Int LogicMaze::homePosition() const 86 | { 87 | return _home; 88 | } 89 | 90 | void LogicMaze::setHomePosition(Vector2Int home) 91 | { 92 | _home = home; 93 | } 94 | 95 | std::vector LogicMaze::goalPositions() const 96 | { 97 | std::vector ret(_goals); 98 | return ret; 99 | } 100 | 101 | void LogicMaze::addGoalPosition(Vector2Int goal) 102 | { 103 | _goals.push_back(goal); 104 | } 105 | 106 | bool LogicMaze::isWithinBounds(Vector2Int position) const 107 | { 108 | return position.x >= 0 && position.x < xSize() && 109 | position.y >= 0 && position.y < ySize(); 110 | } 111 | 112 | bool LogicMaze::hasNeighbor(Vector2Int cellPosition, Direction direction) const 113 | { 114 | Vector2Int neighborPosition = cellPosition.neighbor(direction); 115 | return isWithinBounds(neighborPosition); 116 | } 117 | 118 | Cell& LogicMaze::cellAt(Vector2Int position) 119 | { 120 | int x = position.x; 121 | int y = position.y; 122 | return _cells[x *ySize() + y]; // TODO: is this right? 123 | } 124 | 125 | const Cell& LogicMaze::cellAt(Vector2Int position) const 126 | { 127 | int x = position.x; 128 | int y = position.y; 129 | return _cells[x *ySize() + y]; // TODO: is this right? 130 | } 131 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/micromouse_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor/Encoder.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "Encoder.hh" 29 | 30 | using namespace custom; 31 | 32 | using resolution_t = double; 33 | using gearbox_ratio_t = double; 34 | using EncoderMsg = gz::msgs::Int32; 35 | 36 | class custom::EncoderPrivate { 37 | public: bool initialized = false; 38 | public: gz::transport::Node node; 39 | public: gz::transport::Node::Publisher pub; 40 | public: resolution_t resolution = 360.0; 41 | public: gearbox_ratio_t gearbox_ratio = 1.0; 42 | 43 | public: double position = 0.0; 44 | public: gz::sim::Entity joint{gz::sim::kNullEntity}; 45 | public: gz::sim::Entity parentEntity{gz::sim::kNullEntity}; 46 | }; 47 | 48 | Encoder::Encoder() 49 | : dataPtr(std::make_unique()) 50 | { 51 | } 52 | 53 | Encoder::~Encoder() 54 | { 55 | } 56 | 57 | void Encoder::SetJoint(gz::sim::Entity joint) 58 | { 59 | this->dataPtr->joint = joint; 60 | } 61 | 62 | void Encoder::SetPosition(double position) 63 | { 64 | this->dataPtr->position = position; 65 | } 66 | 67 | double Encoder::Position() const 68 | { 69 | return this->dataPtr->position; 70 | } 71 | 72 | gz::sim::Entity Encoder::Joint() const 73 | { 74 | return this->dataPtr->joint; 75 | } 76 | 77 | bool Encoder::Load(const sdf::Sensor &_sdf) 78 | { 79 | if (!gz::sensors::Sensor::Load(_sdf)) { 80 | return false; 81 | } 82 | 83 | if (_sdf.Type() != sdf::SensorType::CUSTOM) { 84 | gzerr << "Attempting to a load a custom sensor, but received a " << _sdf.TypeStr() << std::endl; 85 | return false; 86 | } 87 | 88 | auto type = gz::sensors::customType(_sdf); 89 | if ("encoder" != type) 90 | { 91 | return false; 92 | } 93 | 94 | if (this->Topic().empty()) { 95 | auto topic = "/encoder"; 96 | gzdbg << "Topic name not set. Using default topic name: " << topic << std::endl; 97 | this->SetTopic(topic); 98 | } 99 | 100 | // Advertise topic where data will be published 101 | this->dataPtr->pub = this->dataPtr->node.Advertise(this->Topic()); 102 | if (!this->dataPtr->pub) 103 | { 104 | gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; 105 | return false; 106 | } 107 | 108 | gzdbg << "Encoder data for [" << this->Name() << "] advertised on [" << this->Topic() << "]" << std::endl; 109 | 110 | if (!_sdf.Element()->HasElement("gz:encoder")) 111 | { 112 | gzerr << "No custom configuration for [" << this->Topic() << "]" << std::endl; 113 | return false; 114 | } 115 | 116 | // Load custom sensor params 117 | auto customElem = _sdf.Element()->GetElement("gz:encoder"); 118 | 119 | sdf::Errors errors; 120 | // Required fields 121 | if (!customElem->HasElement("resolution")) { 122 | gzerr << "Required element [resolution] not found" << std::endl; 123 | return false; 124 | } 125 | auto resolutionElement = customElem->GetElement("resolution"); 126 | this->dataPtr->resolution = resolutionElement->Get(errors); 127 | 128 | // Optional fields 129 | if (customElem->HasElement("gearbox_ratio")) { 130 | auto gearboxRatioElement = customElem->GetElement("gearbox_ratio"); 131 | this->dataPtr->gearbox_ratio = gearboxRatioElement->Get(errors, "", dataPtr->gearbox_ratio).first; 132 | if (this->dataPtr->gearbox_ratio < 1.0) { 133 | double inv = 1.0 / this->dataPtr->gearbox_ratio; 134 | gzwarn << "Gearbox ratio value " << this->dataPtr->gearbox_ratio << " is below 1. Using " << inv << " instead" << std::endl; 135 | this->dataPtr->gearbox_ratio = inv; 136 | } 137 | } 138 | 139 | if (!errors.empty()) { 140 | gzerr << "Errors" << std::endl; 141 | return false; 142 | } 143 | 144 | this->dataPtr->initialized = true; 145 | return true; 146 | } 147 | 148 | bool Encoder::Update(const std::chrono::steady_clock::duration &_now) 149 | { 150 | GZ_PROFILE("Encoder::Update"); 151 | 152 | if (!this->dataPtr->initialized) 153 | { 154 | gzerr << "Not initialized, update ignored.\n"; 155 | return false; 156 | } 157 | 158 | EncoderMsg msg; 159 | *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); 160 | auto frame = msg.mutable_header()->add_data(); 161 | frame->set_key("frame_id"); 162 | frame->add_value(this->FrameId()); 163 | 164 | double anglePerTick = ((2 * M_PI) / (this->dataPtr->resolution * this->dataPtr->gearbox_ratio)); 165 | int ticks = std::round(this->Position() / anglePerTick); 166 | msg.set_data(ticks); 167 | 168 | this->AddSequence(msg.mutable_header()); 169 | this->dataPtr->pub.Publish(msg); 170 | 171 | return true; 172 | } 173 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/MicromousePlugin.qml: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | import QtQuick 2.9 19 | import QtQuick.Controls 2.15 20 | import QtQuick.Dialogs 1.3 21 | import QtQuick.Layouts 1.15 22 | import "qrc:/qml" 23 | 24 | GridLayout { 25 | columns: 8 26 | columnSpacing: 10 27 | Layout.minimumWidth: 350 28 | Layout.minimumHeight: 550 29 | anchors.fill: parent 30 | anchors.leftMargin: 10 31 | anchors.rightMargin: 10 32 | 33 | TextField { 34 | Layout.columnSpan: 7 35 | Layout.fillWidth: true 36 | id: textField 37 | } 38 | 39 | FileDialog { 40 | id: mazeFileDialog 41 | title: "Please choose a file" 42 | folder: shortcuts.home 43 | onAccepted: { 44 | var url = mazeFileDialog.fileUrl; 45 | var urlStr = MicromousePlugin.QurlToQString(url); 46 | MicromousePlugin.SetMazefile(urlStr); 47 | textField.text = urlStr; 48 | } 49 | } 50 | 51 | Button { 52 | Layout.columnSpan: 1 53 | text: qsTr("Select maze") 54 | onClicked: { mazeFileDialog.open(); } 55 | } 56 | 57 | TextField { 58 | Layout.columnSpan: 7 59 | Layout.fillWidth: true 60 | id: robotTextField 61 | } 62 | 63 | FileDialog { 64 | id: robotFileDialog 65 | title: "Please choose a file" 66 | folder: shortcuts.home 67 | onAccepted: { 68 | var url = robotFileDialog.fileUrl; 69 | var urlStr = MicromousePlugin.QurlToQString(url); 70 | MicromousePlugin.SetRobotfile(urlStr); 71 | robotTextField.text = urlStr; 72 | } 73 | } 74 | 75 | Button { 76 | Layout.columnSpan: 1 77 | text: qsTr("Select Robot") 78 | onClicked: { robotFileDialog.open(); } 79 | } 80 | 81 | Button { 82 | Layout.columnSpan: 8 83 | Layout.fillWidth: true 84 | text: qsTr("Load") 85 | highlighted: true 86 | onClicked: { MicromousePlugin.OnLoadButton(); } 87 | } 88 | 89 | Button { 90 | Layout.columnSpan: 8 91 | Layout.fillWidth: true 92 | text: qsTr("Reset") 93 | highlighted: true 94 | onClicked: { MicromousePlugin.OnResetButton(); } 95 | } 96 | 97 | Button { 98 | Layout.columnSpan: 8 99 | Layout.fillWidth: true 100 | text: qsTr("Start!"); 101 | onClicked: {MicromousePlugin.StartMicromouse(); } 102 | } 103 | 104 | Label { 105 | Layout.columnSpan: 2 106 | horizontalAlignment: Text.AlignRight 107 | id: positionXText 108 | color: "black" 109 | text: qsTr("X (mm)") 110 | } 111 | 112 | SpinBox { 113 | Layout.columnSpan: 6 114 | Layout.fillWidth: true 115 | id: positionX 116 | editable: true 117 | onValueChanged: MicromousePlugin.position.x = positionX.value / 1000 118 | 119 | from: -10000 120 | value: 0 121 | to: 10000 122 | stepSize: 1 123 | } 124 | 125 | Label { 126 | Layout.columnSpan: 2 127 | horizontalAlignment: Text.AlignRight 128 | id: positionYText 129 | color: "black" 130 | text: qsTr("Y (mm)") 131 | } 132 | 133 | SpinBox { 134 | Layout.columnSpan: 6 135 | Layout.fillWidth: true 136 | id: positionY 137 | editable: true 138 | onValueChanged: MicromousePlugin.position.y = positionY.value / 1000 139 | 140 | from: -10000 141 | value: 0 142 | to: 10000 143 | stepSize: 1 144 | } 145 | 146 | Label { 147 | Layout.columnSpan: 2 148 | horizontalAlignment: Text.AlignRight 149 | id: positionZText 150 | color: "black" 151 | text: qsTr("Z (mm)") 152 | } 153 | 154 | SpinBox { 155 | Layout.columnSpan: 6 156 | Layout.fillWidth: true 157 | id: positionZ 158 | editable: true 159 | onValueChanged: MicromousePlugin.position.z = positionZ.value / 1000 160 | 161 | from: -10000 162 | value: 0 163 | to: 10000 164 | stepSize: 1 165 | } 166 | 167 | 168 | 169 | 170 | 171 | 172 | Label { 173 | Layout.columnSpan: 2 174 | horizontalAlignment: Text.AlignRight 175 | id: orientationXText 176 | color: "black" 177 | text: qsTr("Roll (deg)") 178 | } 179 | 180 | SpinBox { 181 | Layout.columnSpan: 6 182 | Layout.fillWidth: true 183 | id: orientationX 184 | editable: true 185 | onValueChanged: MicromousePlugin.orientation.x = orientationX.value 186 | 187 | from: -360 188 | value: 0 189 | to: 360 190 | stepSize: 1 191 | } 192 | 193 | Label { 194 | Layout.columnSpan: 2 195 | horizontalAlignment: Text.AlignRight 196 | id: orientationYText 197 | color: "black" 198 | text: qsTr("Pitch (deg)") 199 | } 200 | 201 | SpinBox { 202 | Layout.columnSpan: 6 203 | Layout.fillWidth: true 204 | id: orientationY 205 | editable: true 206 | onValueChanged: MicromousePlugin.orientation.y = orientationY.value 207 | 208 | from: -360 209 | value: 0 210 | to: 360 211 | stepSize: 1 212 | } 213 | 214 | Label { 215 | Layout.columnSpan: 2 216 | horizontalAlignment: Text.AlignRight 217 | id: orientationZText 218 | color: "black" 219 | text: qsTr("Yaw (deg)") 220 | } 221 | 222 | SpinBox { 223 | Layout.columnSpan: 6 224 | Layout.fillWidth: true 225 | id: orientationZ 226 | editable: true 227 | onValueChanged: MicromousePlugin.orientation.z = orientationZ.value 228 | 229 | from: -360 230 | value: 0 231 | to: 360 232 | stepSize: 1 233 | } 234 | 235 | } 236 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/xacro/urdf/micromouse_robot.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.2 5 | 0.75 6 | Gazebo/Red 7 | 8 | 9 | 10 | 0.2 11 | 0.75 12 | Gazebo/Green 13 | 14 | 15 | 16 | 0.2 17 | 0.75 18 | Gazebo/Yellow 19 | 20 | 21 | 22 | 0.2 23 | 0.75 24 | Gazebo/Yellow 25 | 26 | 27 | 28 | 0.0 29 | 0.0 30 | Gazebo/Green 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | left_wheel_joint 41 | right_wheel_joint 42 | ${wheel_separation} 43 | ${wheel_link_radius} 44 | 50 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 1 57 | 30 58 | true 59 | left_wheel_encoder 60 | 61 | 12.0 62 | 29.86 63 | 64 | 65 | 66 | 67 | 68 | 69 | 1 70 | 30 71 | true 72 | right_wheel_encoder 73 | 74 | 12.0 75 | 29.86 76 | 77 | 78 | 79 | 80 | 81 | 82 | ${imu_x_location} ${imu_y_location} ${imu_z_location} 0 0 0 83 | 1 84 | 1 85 | true 86 | imu 87 | 88 | 89 | ${sensor_lidar_x_location} 0 ${sensor_lidar_z_location} 0 90 | 0 0 91 | lidar_center 92 | 10 93 | 94 | 95 | 96 | 1 97 | 1 98 | 0 99 | 0 100 | 101 | 102 | 103 | 0.0001 104 | 10.0 105 | 106 | 107 | 1 108 | true 109 | 110 | 111 | ${sensor_lidar_x_location} ${body_link_y_dim/2} 112 | ${sensor_lidar_z_location} 0 0 ${pi/4} 113 | lidar_left 114 | 10 115 | 116 | 117 | 118 | 1 119 | 1 120 | 0 121 | 0 122 | 123 | 124 | 125 | 0.0001 126 | 10.0 127 | 128 | 129 | 1 130 | true 131 | 132 | 133 | ${sensor_lidar_x_location} ${-body_link_y_dim/2} 134 | ${sensor_lidar_z_location} 0 0 ${-pi/4} 135 | lidar_right 136 | 10 137 | 138 | 139 | 140 | 1 141 | 1 142 | 0 143 | 0 144 | 145 | 146 | 147 | 0.0001 148 | 10.0 149 | 150 | 151 | 1 152 | true 153 | 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse/MicromousePlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "MicromousePlugin.hh" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | using namespace custom; 33 | using namespace gz; 34 | 35 | class custom::MicromousePluginPrivate 36 | { 37 | public: 38 | void AskForMaze(); 39 | void AskForRobotPose(); 40 | void AskForRobot(); 41 | transport::Node node; 42 | std::string worldname; 43 | QString mazefile; 44 | QString robotfile; 45 | QVector3D position; 46 | QVector3D orientation; 47 | }; 48 | 49 | MicromousePlugin::MicromousePlugin() 50 | : dataPtr(std::make_unique()) 51 | { 52 | } 53 | 54 | MicromousePlugin::~MicromousePlugin() 55 | { 56 | } 57 | 58 | void MicromousePlugin::LoadConfig(const tinyxml2::XMLElement *_pluginElem) 59 | { 60 | if (this->title.empty()) 61 | { 62 | this->title = "Micromouse"; 63 | } 64 | 65 | this->dataPtr->worldname = gz::gui::worldNames()[0].toStdString(); 66 | 67 | if (!_pluginElem) 68 | return; 69 | 70 | // Take maze filename during runtime 71 | auto messageElem = _pluginElem->FirstChildElement("mazefile"); 72 | if (nullptr != messageElem && nullptr != messageElem->GetText()) 73 | { 74 | QString url(messageElem->GetText()); 75 | this->SetMazefile(url); 76 | gzmsg << "Micromouse loaded maze" << url.toStdString() << std::endl; 77 | } 78 | 79 | messageElem = _pluginElem->FirstChildElement("robotfile"); 80 | if (nullptr != messageElem && nullptr != messageElem->GetText()) 81 | { 82 | QString url(messageElem->GetText()); 83 | this->SetRobotfile(url); 84 | gzmsg << "Robot set to " << url.toStdString() << std::endl; 85 | } 86 | } 87 | 88 | void MicromousePlugin::Update(const sim::UpdateInfo &_info, 89 | sim::EntityComponentManager &_ecm) 90 | { 91 | (void)_info; 92 | (void)_ecm; 93 | } 94 | 95 | void MicromousePluginPrivate::AskForMaze() 96 | { 97 | QFile file{mazefile}; 98 | bool open_result = file.open(QIODevice::ReadOnly | QIODevice::Text); 99 | if (!open_result) { 100 | gzerr << "Failed to open file [" << mazefile.toStdString() << "]" << std::endl; 101 | return; 102 | } 103 | 104 | QTextStream in(&file); 105 | QString content = in.readAll(); 106 | file.close(); 107 | 108 | std::string topic = "/micromouse/set_maze"; 109 | msgs::StringMsg req; 110 | req.set_data(content.toStdString()); 111 | int timeout = 3500; 112 | msgs::Boolean rep; 113 | bool result; 114 | bool executed = this->node.Request(topic, req, timeout, rep, result); 115 | if (!executed) { 116 | gzerr << "Failed to request service [" << topic << "]" << std::endl; 117 | gzerr << req.data() << std::endl; 118 | return; 119 | } 120 | if (result) { 121 | if (!rep.data()) { 122 | gzerr << "Invalid maze" << std::endl; 123 | } 124 | } else { 125 | gzerr << "Failed service call [" << topic << "]" << std::endl; 126 | } 127 | } 128 | 129 | 130 | void MicromousePluginPrivate::AskForRobot() 131 | { 132 | QFile file{robotfile}; 133 | bool open_result = file.open(QIODevice::ReadOnly | QIODevice::Text); 134 | if (!open_result) { 135 | gzerr << "Failed to open file [" << robotfile.toStdString() << "]" << std::endl; 136 | return; 137 | } 138 | 139 | QTextStream in(&file); 140 | QString content = in.readAll(); 141 | file.close(); 142 | 143 | std::string topic = "/micromouse/set_robot"; 144 | msgs::StringMsg req; 145 | req.set_data(content.toStdString()); 146 | int timeout = 3500; 147 | msgs::Boolean rep; 148 | bool result; 149 | bool executed = this->node.Request(topic, req, timeout, rep, result); 150 | if (!executed) { 151 | gzerr << "Failed to request service [" << topic << "]" << std::endl; 152 | return; 153 | } 154 | if (result) { 155 | if (!rep.data()) { 156 | gzerr << "Invalid maze" << std::endl; 157 | } 158 | } else { 159 | gzerr << "Failed service call [" << topic << "]" << std::endl; 160 | } 161 | } 162 | 163 | void MicromousePluginPrivate::AskForRobotPose() 164 | { 165 | std::string topic = "/micromouse/set_robot_pose"; 166 | msgs::Pose req; 167 | req.mutable_position()->set_x(this->position.x()); 168 | req.mutable_position()->set_y(this->position.y()); 169 | req.mutable_position()->set_z(this->position.z()); 170 | auto q = QQuaternion::fromEulerAngles(this->orientation.x(), this->orientation.y(), this->orientation.z()); 171 | req.mutable_orientation()->set_w(q.scalar()); 172 | req.mutable_orientation()->set_x(q.x()); 173 | req.mutable_orientation()->set_y(q.y()); 174 | req.mutable_orientation()->set_z(q.z()); 175 | int timeout = 3500; 176 | msgs::Boolean rep; 177 | bool result; 178 | bool executed = this->node.Request(topic, req, timeout, rep, result); 179 | if (!executed) { 180 | gzerr << "Failed to request service [" << topic << "]" << std::endl; 181 | return; 182 | } 183 | if (result) { 184 | if (!rep.data()) { 185 | gzerr << "Invalid maze" << std::endl; 186 | } 187 | } else { 188 | gzerr << "Failed service call [" << topic << "]" << std::endl; 189 | } 190 | } 191 | 192 | void MicromousePlugin::OnLoadButton() 193 | { 194 | this->dataPtr->AskForMaze(); 195 | this->dataPtr->AskForRobotPose(); 196 | this->dataPtr->AskForRobot(); 197 | } 198 | 199 | void MicromousePlugin::OnResetButton() 200 | { 201 | this->dataPtr->AskForRobotPose(); 202 | std::string reset = "micromouse/reset";; 203 | msgs::StringMsg req; 204 | int timeout = 3500; 205 | msgs::StringMsg rep; 206 | bool result; 207 | this->dataPtr->node.Request(reset, req, timeout, rep, result); 208 | } 209 | 210 | void MicromousePlugin::StartMicromouse() 211 | { 212 | std::string topic = "micromouse/start_comp"; 213 | msgs::Empty req; 214 | int timeout = 3500; 215 | msgs::Boolean rep; 216 | bool result; 217 | bool executed = this->dataPtr->node.Request(topic, req, timeout, rep, result); 218 | if (!executed) { 219 | gzerr << "Failed to request service [" << topic << "]" << std::endl; 220 | return; 221 | } 222 | if (result) { 223 | if (!rep.data()) { 224 | gzerr << "Could not start competition" << std::endl; 225 | } 226 | } else { 227 | gzerr << "Failed service call [" << topic << "]" << std::endl; 228 | } 229 | } 230 | 231 | QString MicromousePlugin::QurlToQString(QUrl url) const 232 | { 233 | return url.toLocalFile(); 234 | } 235 | 236 | QString MicromousePlugin::Mazefile() const 237 | { 238 | return this->dataPtr->mazefile; 239 | } 240 | 241 | void MicromousePlugin::SetMazefile(const QString &_mazefile) 242 | { 243 | this->dataPtr->mazefile = _mazefile; 244 | this->MazefileChanged(); 245 | } 246 | 247 | QString MicromousePlugin::Robotfile() const 248 | { 249 | return this->dataPtr->robotfile; 250 | } 251 | 252 | void MicromousePlugin::SetRobotfile(const QString &_robotfile) 253 | { 254 | this->dataPtr->robotfile = _robotfile; 255 | this->RobotfileChanged(); 256 | } 257 | 258 | void MicromousePlugin::SetOrientation(const QVector3D &_orientation) 259 | { 260 | this->dataPtr->orientation = _orientation; 261 | this->OrientationChanged(); 262 | } 263 | 264 | void MicromousePlugin::SetPosition(const QVector3D &_position) 265 | { 266 | this->dataPtr->position = _position; 267 | this->PositionChanged(); 268 | } 269 | 270 | QVector3D MicromousePlugin::Position() const 271 | { 272 | return this->dataPtr->position; 273 | } 274 | 275 | QVector3D MicromousePlugin::Orientation() const 276 | { 277 | return this->dataPtr->orientation; 278 | } 279 | 280 | GZ_ADD_PLUGIN(MicromousePlugin, sim::GuiSystem, gui::Plugin); 281 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/worlds/gui.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 3D View 6 | false 7 | docked 8 | 9 | ogre2 10 | scene 11 | 0.4 0.4 0.4 12 | 0.8 0.8 0.8 13 | 1.44 -1.44 2 0 1.57079632679489661923 0 14 | 15 | 0.05 16 | 25000 17 | 18 | 19 | 20 | 21 | floating 22 | false 23 | false 24 | false 25 | 26 | 27 | 28 | 29 | floating 30 | false 31 | false 32 | false 33 | 34 | 35 | 36 | 37 | floating 38 | false 39 | false 40 | 41 | 42 | 43 | 44 | floating 45 | false 46 | false 47 | false 48 | 49 | 50 | 51 | 52 | floating 53 | false 54 | false 55 | false 56 | 57 | 58 | 59 | 60 | floating 61 | false 62 | false 63 | false 64 | 65 | 66 | 67 | 68 | floating 69 | false 70 | false 71 | false 72 | 73 | 74 | 75 | 76 | 77 | 78 | World control 79 | false 80 | false 81 | 72 82 | 1 83 | floating 84 | 85 | 86 | 87 | 88 | 89 | true 90 | true 91 | true 92 | /world/micromouse/control 93 | /world/micromouse/stats 94 | 95 | 96 | 97 | 98 | 99 | World stats 100 | false 101 | false 102 | 110 103 | 290 104 | 1 105 | floating 106 | 107 | 108 | 109 | 110 | 0.65 111 | #FFFFFF 112 | 113 | 114 | true 115 | true 116 | true 117 | true 118 | /world/micromouse/stats 119 | 120 | 121 | 122 | 123 | 300 124 | 50 125 | 100 126 | 50 127 | floating 128 | false 129 | false 130 | #777777 131 | 132 | 133 | 134 | 135 | 250 136 | 50 137 | 50 138 | 50 139 | floating 140 | false 141 | false 142 | #777777 143 | 144 | 145 | 146 | 147 | 0 148 | 50 149 | 250 150 | 50 151 | floating 152 | false 153 | false 154 | #777777 155 | 156 | 157 | 158 | 159 | 250 160 | 00 161 | 150 162 | 50 163 | floating 164 | false 165 | false 166 | #666666 167 | 168 | 169 | 170 | 171 | 0 172 | 0 173 | 250 174 | 50 175 | floating 176 | false 177 | false 178 | #666666 179 | 180 | 181 | 182 | 183 | floating 184 | false 185 | false 186 | false 187 | 188 | 189 | 190 | 191 | 192 | 193 | docked 194 | false 195 | 196 | 197 | 198 | 199 | 200 | docked 201 | false 202 | 203 | 204 | 205 | 206 | 207 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/urdf/micromouse_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 0.2 9 | 0.75 10 | Gazebo/Red 11 | 12 | 13 | 0.2 14 | 0.75 15 | Gazebo/Green 16 | 17 | 18 | 0.2 19 | 0.75 20 | Gazebo/Yellow 21 | 22 | 23 | 0.2 24 | 0.75 25 | Gazebo/Yellow 26 | 27 | 28 | 0.0 29 | 0.0 30 | Gazebo/Green 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | left_wheel_joint 39 | right_wheel_joint 40 | 0.096 41 | 0.016 42 | 50 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 1 53 | 30 54 | true 55 | left_wheel_encoder 56 | 57 | 12.0 58 | 29.86 59 | 60 | 61 | 62 | 63 | 64 | 1 65 | 30 66 | true 67 | right_wheel_encoder 68 | 69 | 12.0 70 | 29.86 71 | 72 | 73 | 74 | 75 | 76 | 0 0 0 0 0 0 77 | 1 78 | 1 79 | true 80 | imu 81 | 82 | 83 | 0.06 0 0 0 84 | 0 0 85 | lidar_center 86 | 10 87 | 88 | 89 | 90 | 1 91 | 1 92 | 0 93 | 0 94 | 95 | 96 | 97 | 0.0001 98 | 10.0 99 | 100 | 101 | 1 102 | true 103 | 104 | 105 | 0.06 0.048 106 | 0 0 0 0.7853981633974483 107 | lidar_left 108 | 10 109 | 110 | 111 | 112 | 1 113 | 1 114 | 0 115 | 0 116 | 117 | 118 | 119 | 0.0001 120 | 10.0 121 | 122 | 123 | 1 124 | true 125 | 126 | 127 | 0.06 -0.048 128 | 0 0 0 -0.7853981633974483 129 | lidar_right 130 | 10 131 | 132 | 133 | 134 | 1 135 | 1 136 | 0 137 | 0 138 | 139 | 140 | 141 | 0.0001 142 | 10.0 143 | 144 | 145 | 1 146 | true 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | -------------------------------------------------------------------------------- /gz_ws/src/encoder_sensor_system/EncoderSystem.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "gz/sim/components/JointPosition.hh" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "Encoder.hh" 40 | #include "EncoderSystem.hh" 41 | 42 | using namespace gz; 43 | using namespace sim; 44 | 45 | using namespace custom; 46 | 47 | class custom::EncoderSystemPrivate { 48 | public: 49 | std::unordered_map> entitySensorMap; 50 | gz::sensors::SensorFactory sensorFactory; 51 | std::unordered_set newSensors; 52 | std::unordered_set entitiesToSkip; 53 | bool initialized = false; 54 | void CreateSensors(const EntityComponentManager &_ecm); 55 | void Update(const EntityComponentManager &_ecm); 56 | void AddSensor(const EntityComponentManager &_ecm, const Entity _entity, const gz::sim::components::CustomSensor *_custom, const components::ParentEntity *_parent); 57 | void RemoveEncoderEntities(const gz::sim::EntityComponentManager &_ecm); 58 | }; 59 | 60 | void EncoderSystemPrivate::CreateSensors(const EntityComponentManager &_ecm) { 61 | GZ_PROFILE("EncoderSystemPrivate::CreateSensors"); 62 | 63 | if (!this->initialized) { 64 | _ecm.Each( 65 | [&](const Entity &_entity, const gz::sim::components::CustomSensor *_custom, const components::ParentEntity *_parent) -> bool { 66 | this->AddSensor(_ecm, _entity, _custom, _parent); 67 | return true; 68 | } 69 | ); 70 | this->initialized = true; 71 | } else { 72 | _ecm.EachNew( 73 | [&](const Entity &_entity, const gz::sim::components::CustomSensor *_sensor, const components::ParentEntity *_parent) -> bool { 74 | this->AddSensor(_ecm, _entity, _sensor, _parent); 75 | return true; 76 | } 77 | ); 78 | } 79 | } 80 | 81 | void EncoderSystemPrivate::Update(const EntityComponentManager &_ecm) { 82 | GZ_PROFILE("EncoderSystemPrivate::Update"); 83 | _ecm.Each( 84 | [&](const Entity &_entity, const gz::sim::components::CustomSensor * /*_custom*/, const gz::sim::components::ParentEntity *_parent) -> bool { 85 | if (this->entitiesToSkip.find(_entity) != this->entitiesToSkip.end()) { 86 | return true; 87 | } 88 | auto it = this->entitySensorMap.find(_entity); 89 | if (it != this->entitySensorMap.end()) { 90 | auto &sensor = it->second; 91 | auto joint = _parent->Data(); 92 | auto jointPosition = _ecm.ComponentData(joint); 93 | if (!jointPosition) { 94 | gzerr << "JointPosition not found. Will be created during next PreUpdate loop execution." << std::endl; 95 | return true; 96 | } 97 | auto positionVec = jointPosition.value(); 98 | if (positionVec.size() > 1) { 99 | gzerr << "More than one Dof" << std::endl; 100 | } 101 | double position = positionVec.at(0); 102 | sensor->SetPosition(position); 103 | } else { 104 | gzerr << "Failed to update Encoder: " << _entity << ". " << "Entity not found." << std::endl; 105 | } 106 | return true; 107 | } 108 | ); 109 | } 110 | 111 | void EncoderSystemPrivate::AddSensor(const EntityComponentManager &_ecm, const Entity _entity, const gz::sim::components::CustomSensor *_custom, const components::ParentEntity *_parent) { 112 | GZ_PROFILE("EncoderSystemPrivate::AddSensor"); 113 | 114 | // create sensor 115 | std::string sensorScopedName = removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); 116 | sdf::Sensor data = _custom->Data(); 117 | data.SetName(sensorScopedName); 118 | // check topic 119 | if (data.Topic().empty()) { 120 | gzdbg << "Using default topic" << std::endl; 121 | std::string topic = scopedName(_entity, _ecm) + "/custom"; 122 | data.SetTopic(topic); 123 | } 124 | std::unique_ptr sensor = this->sensorFactory.CreateSensor(data); 125 | if (nullptr == sensor) { 126 | gzerr << "Failed to create sensor [" << sensorScopedName << "]. " << "This might be caused by the system processing a custom sensor of a different type, in which case it's safe to ignore. " << std::endl; 127 | gzdbg << "Addind sensor to skiplist" << std::endl; 128 | this->entitiesToSkip.insert(_entity); 129 | return; 130 | } 131 | auto joint = _parent->Data(); 132 | if (joint == kNullEntity) { 133 | gzerr << "Joint not found."<< std::endl; 134 | } 135 | sensor->SetJoint(joint); 136 | 137 | // set sensor parent 138 | std::string parentName = _ecm.Component(_parent->Data())->Data(); 139 | sensor->SetParent(parentName); 140 | this->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); 141 | this->newSensors.insert(_entity); 142 | } 143 | 144 | void EncoderSystemPrivate::RemoveEncoderEntities(const gz::sim::EntityComponentManager &_ecm) { 145 | GZ_PROFILE("EncoderSystemPrivate::RemoveEncoderEntities"); 146 | _ecm.EachRemoved( 147 | [&](const Entity &_entity, const gz::sim::components::CustomSensor *) -> bool { 148 | auto sensorId = this->entitySensorMap.find(_entity); 149 | if (sensorId == this->entitySensorMap.end()) { 150 | if (this->entitiesToSkip.find(_entity) == this->entitiesToSkip.end()) { 151 | gzerr << "Internal error, missing Encoder sensor for entity [" << _entity << "]" << std::endl; 152 | } 153 | return true; 154 | } 155 | this->entitySensorMap.erase(sensorId); 156 | return true; 157 | } 158 | ); 159 | _ecm.EachRemoved( 160 | [&](const Entity &_entity, const gz::sim::components::CustomSensor *) -> bool { 161 | auto sensorId = this->entitiesToSkip.find(_entity); 162 | if (sensorId == this->entitiesToSkip.end()) { 163 | return true; 164 | } 165 | this->entitiesToSkip.erase(sensorId); 166 | return true; 167 | } 168 | ); 169 | } 170 | 171 | EncoderSystem::EncoderSystem() 172 | : gz::sim::System(), dataPtr(std::make_unique()) 173 | { 174 | } 175 | 176 | EncoderSystem::~EncoderSystem() = default; 177 | 178 | void EncoderSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm) { 179 | GZ_PROFILE("EncoderSystem::PreUpdate"); 180 | 181 | // Create components 182 | for (auto entity : this->dataPtr->newSensors) { 183 | auto it = this->dataPtr->entitySensorMap.find(entity); 184 | if (it == this->dataPtr->entitySensorMap.end()) { 185 | gzerr << "Entity [" << entity << "] isn't in sensor map, this shouldn't happen." << std::endl; 186 | continue; 187 | } 188 | // Set topic 189 | _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); 190 | 191 | // Set joint 192 | auto jointPosition = _ecm.ComponentData(it->second->Joint()); 193 | if (!jointPosition && _ecm.HasEntity(it->second->Joint())) { 194 | _ecm.CreateComponent(it->second->Joint(), components::JointPosition()); 195 | } 196 | } 197 | this->dataPtr->newSensors.clear(); 198 | } 199 | 200 | void EncoderSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { 201 | GZ_PROFILE("EncoderSystem::PostUpdate"); 202 | 203 | if (_info.dt < std::chrono::steady_clock::duration::zero()) { 204 | gzwarn << "Detected jump back in time [" << std::chrono::duration(_info.dt).count() << "s]. System may not work properly." << std::endl; 205 | } 206 | 207 | this->dataPtr->CreateSensors(_ecm); 208 | 209 | if (!_info.paused) { 210 | bool needsUpdate = false; 211 | for (auto &it : this->dataPtr->entitySensorMap) { 212 | auto &sensor = it.second; 213 | if (sensor->NextDataUpdateTime() <= _info.simTime && sensor->HasConnections()) { 214 | needsUpdate = true; 215 | break; 216 | } 217 | } 218 | if (!needsUpdate) { 219 | return; 220 | } 221 | 222 | this->dataPtr->Update(_ecm); 223 | 224 | for (auto &it : this->dataPtr->entitySensorMap) { 225 | auto &sensor = it.second; 226 | // Update measurement time 227 | sensor->Update(_info.simTime, false); 228 | } 229 | } 230 | 231 | this->dataPtr->RemoveEncoderEntities(_ecm); 232 | } 233 | 234 | GZ_ADD_PLUGIN( 235 | EncoderSystem, 236 | gz::sim::System, 237 | EncoderSystem::ISystemPreUpdate, 238 | EncoderSystem::ISystemPostUpdate 239 | ) 240 | 241 | GZ_ADD_PLUGIN_ALIAS(EncoderSystem, "custom::EncoderSystem") 242 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/solver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/solver.hpp" 19 | #include 20 | #include 21 | 22 | #define X 16 23 | #define Y 16 24 | 25 | StateTransition stateFromTo(State from, State to) 26 | { 27 | int x1 = from.position.x; 28 | int y1 = from.position.y; 29 | Direction r1 = from.orientation; 30 | int x2 = to.position.x; 31 | int y2 = to.position.y; 32 | Direction r2 = to.orientation; 33 | if (x1 != x2 && y1 == y2 && r1 == r2) 34 | { 35 | Direction r = r1; 36 | if ((r == Direction::left && x2 == x1 - 1) || (r == Direction::right && x2 == x1 + 1)) 37 | { 38 | return StateTransition::forward; 39 | } 40 | } 41 | else if (x1 == x2 && y1 != y2 && r1 == r2) 42 | { 43 | Direction r = r1; 44 | if ((r == Direction::down && y2 == y1 - 1) || (r == Direction::up && y2 == y1 + 1)) 45 | { 46 | return StateTransition::forward; 47 | } 48 | } 49 | else if (x1 == x2 && y1 == y2 && r1 != r2) 50 | { 51 | if (r2 == ccwDirection(r1)) return StateTransition::turnLeft; 52 | if (r2 == cwDirection(r1)) return StateTransition::turnRight; 53 | } 54 | 55 | return StateTransition::turnLeft; 56 | }; 57 | 58 | bool operator==(const State& lhs, const State& rhs) 59 | { 60 | return lhs.position == rhs.position && lhs.orientation == rhs.orientation; 61 | } 62 | 63 | FloodFillAdvSolver::FloodFillAdvSolver(const IMaze& maze) 64 | : _maze(maze), _seeds{false} 65 | { 66 | } 67 | 68 | void FloodFillAdvSolver::addGoal(Vector2Int position) 69 | { 70 | for (int i = 0; i < 4; ++i) 71 | { 72 | _seeds[position.x][position.y][i] = true; 73 | } 74 | } 75 | 76 | void FloodFillAdvSolver::resetGoals() 77 | { 78 | std::memset(_seeds, 0, sizeof(_seeds)); 79 | } 80 | 81 | static int indexFromDirection(Direction direction) 82 | { 83 | int ret = -1; 84 | switch(direction) 85 | { 86 | case Direction::left: 87 | ret = 0; 88 | break; 89 | case Direction::down: 90 | ret = 1; 91 | break; 92 | case Direction::right: 93 | ret = 2; 94 | break; 95 | case Direction::up: 96 | ret = 3; 97 | break; 98 | } 99 | return ret; 100 | } 101 | 102 | static Direction direction2FromInt(int orientation) 103 | { 104 | switch(orientation) 105 | { 106 | case 0: 107 | return Direction::left; 108 | break; 109 | case 1: 110 | return Direction::down; 111 | break; 112 | case 2: 113 | return Direction::right; 114 | break; 115 | case 3: 116 | return Direction::up; 117 | break; 118 | } 119 | return Direction::left; //not supposed to happen 120 | } 121 | 122 | static State stateFromVars(int x, int y, int r) 123 | { 124 | State s; 125 | s.position.x = x; 126 | s.position.y = y; 127 | s.orientation = direction2FromInt(r); 128 | return s; 129 | } 130 | 131 | bool FloodFillAdvSolver::isSeed(State state) const 132 | { 133 | int x = state.position.x; 134 | int y = state.position.y; 135 | int r = indexFromDirection(state.orientation); 136 | return _seeds[x][y][r]; 137 | } 138 | 139 | int FloodFillAdvSolver::cost(State state) const 140 | { 141 | int x = state.position.x; 142 | int y = state.position.y; 143 | int r = indexFromDirection(state.orientation); 144 | return _costs[x][y][r]; 145 | } 146 | 147 | void FloodFillAdvSolver::calculateCosts() 148 | { 149 | //resets cost array; 150 | int initialCost = std::numeric_limits::max(); 151 | for (int i = 0; i < X; ++i) 152 | { 153 | for(int j = 0; j < Y; ++j) 154 | { 155 | for(int k = 0; k < 4; ++k) 156 | { 157 | _costs[i][j][k] = initialCost; 158 | } 159 | } 160 | } 161 | 162 | //assigns seeds costs 0; enqueues their backward neighbors; 163 | for (int i = 0; i < X; ++i) 164 | { 165 | for (int j = 0; j < Y; ++j) 166 | { 167 | for (int k = 0; k < 4; ++k) 168 | { 169 | if (_seeds[i][j][k]) 170 | { 171 | _costs[i][j][k] = 0; 172 | State s = stateFromVars(i, j, k); 173 | std::vector bwdNeighbors = backwardNeighbors(s); 174 | for (const State& s : bwdNeighbors) { 175 | if (!isSeed(s)) { 176 | q.emplace(s); 177 | } 178 | } 179 | } 180 | } 181 | } 182 | } 183 | 184 | // processes all 185 | while (!q.empty()) 186 | { 187 | State elem = q.front(); 188 | q.pop(); 189 | std::vector fwdNeighs = forwardNeighbors(elem); 190 | 191 | State& first = fwdNeighs[0]; 192 | auto fCost = cost(first); 193 | for(auto n : fwdNeighs) 194 | { 195 | auto nc = cost(n); 196 | if (nc < fCost) 197 | { 198 | first = n; 199 | fCost = nc; 200 | } 201 | } 202 | int lowestForwardCost = fCost; 203 | /* 204 | auto lowestCostNeighbor = etl::min_element( 205 | fwdNeighs.begin(), 206 | fwdNeighs.end(), 207 | [this] (const State& a, const State& b) -> bool { return cost(a) < cost(b); } 208 | ); 209 | */ 210 | //int lowestForwardCost = cost(*lowestCostNeighbor); 211 | int candidateSelfCost = lowestForwardCost + 1; 212 | int currentSelfCost = cost(elem); 213 | if (candidateSelfCost >= currentSelfCost) 214 | { 215 | continue; 216 | } 217 | 218 | setCost(elem, candidateSelfCost); 219 | std::vector bwdNeighs = backwardNeighbors(elem); 220 | for (const State& state : bwdNeighs) { 221 | q.emplace(state); 222 | } 223 | } 224 | } 225 | 226 | void FloodFillAdvSolver::setCost(State s, int cost) 227 | { 228 | int x = s.position.x; 229 | int y = s.position.y; 230 | int r = indexFromDirection(s.orientation); 231 | _costs[x][y][r] = cost; 232 | } 233 | 234 | void FloodFillAdvSolver::calculateCostsOptimized(State seedState) 235 | { 236 | (void)seedState; 237 | calculateCosts(); 238 | } 239 | 240 | State FloodFillAdvSolver::bestMovement(State current) const 241 | { 242 | std::vector fwdNeighbors = forwardNeighbors(current); 243 | State& first = fwdNeighbors[0]; 244 | auto fCost = cost(first); 245 | for(auto n : fwdNeighbors) { 246 | auto nc = cost(n); 247 | if (nc < fCost) { 248 | first = n; 249 | fCost = nc; 250 | } 251 | } 252 | State& lowestCostNeighbor = first; 253 | int lowestForwardCost = fCost; 254 | (void)lowestForwardCost; 255 | return lowestCostNeighbor; 256 | } 257 | 258 | std::vector FloodFillAdvSolver::forwardNeighbors(State s) const 259 | { 260 | std::vector neighbors; 261 | State ccwNeighbor; 262 | ccwNeighbor.position = s.position; 263 | ccwNeighbor.orientation = ccwDirection(s.orientation); 264 | 265 | State cwNeighbor; 266 | cwNeighbor.position = s.position; 267 | cwNeighbor.orientation = cwDirection(s.orientation); 268 | if (!_maze.hasWall(s.position, s.orientation)) 269 | { 270 | State fwdNeighbor = s; 271 | switch(s.orientation) 272 | { 273 | case Direction::left: 274 | fwdNeighbor.position.x -= 1; 275 | break; 276 | case Direction::down: 277 | fwdNeighbor.position.y -= 1; 278 | break; 279 | case Direction::right: 280 | fwdNeighbor.position.x += 1; 281 | break; 282 | case Direction::up: 283 | fwdNeighbor.position.y += 1; 284 | break; 285 | } 286 | neighbors.push_back(fwdNeighbor); 287 | } 288 | neighbors.push_back(ccwNeighbor); 289 | neighbors.push_back(cwNeighbor); 290 | 291 | return neighbors; 292 | } 293 | 294 | std::vector FloodFillAdvSolver::backwardNeighbors(State s) const 295 | { 296 | std::vector neighbors; 297 | State ccwNeighbor; 298 | ccwNeighbor.position = s.position; 299 | ccwNeighbor.orientation = ccwDirection(s.orientation); 300 | 301 | State cwNeighbor; 302 | cwNeighbor.position = s.position; 303 | cwNeighbor.orientation = cwDirection(s.orientation); 304 | 305 | neighbors.push_back(ccwNeighbor); 306 | neighbors.push_back(cwNeighbor); 307 | 308 | if (!_maze.hasWall(s.position, oppositeDirection(s.orientation))) 309 | { 310 | State bwdNeighbor = s; 311 | switch(s.orientation) 312 | { 313 | case Direction::left: 314 | bwdNeighbor.position.x += 1; 315 | break; 316 | case Direction::down: 317 | bwdNeighbor.position.y += 1; 318 | break; 319 | case Direction::right: 320 | bwdNeighbor.position.x -= 1; 321 | break; 322 | case Direction::up: 323 | bwdNeighbor.position.y -= 1; 324 | break; 325 | } 326 | neighbors.push_back(bwdNeighbor); 327 | } 328 | 329 | return neighbors; 330 | } 331 | 332 | void FloodFillAdvSolver::setGoalToHome() 333 | { 334 | resetGoals(); 335 | addGoal(_maze.homePosition()); 336 | } 337 | 338 | void FloodFillAdvSolver::setGoalToMazeGoal() 339 | { 340 | resetGoals(); 341 | for(const Vector2Int& goal : _maze.goalPositions()) { 342 | addGoal(goal); 343 | } 344 | } 345 | 346 | // PATH MANAGER ----------- 347 | 348 | PathManager::PathManager(const IMaze& maze) 349 | : _solver(maze), _maze(maze), _goingToCenter{false}, _robotState{{0, 0}, Direction::up} 350 | { 351 | } 352 | 353 | StateTransition PathManager::nextMovement() 354 | { 355 | if (_goingToCenter && isGoal(_robotState)) 356 | { 357 | _goingToCenter = false; 358 | _solver.setGoalToHome(); 359 | // m_solver.calculateCosts(); 360 | } 361 | else if (!_goingToCenter && isHome(_robotState)) 362 | { 363 | _goingToCenter = true; 364 | _solver.setGoalToMazeGoal(); 365 | // m_solver.calculateCosts(); 366 | } 367 | _solver.calculateCosts(); 368 | State nextState = _solver.bestMovement(_robotState); 369 | StateTransition nextMovement = stateFromTo(_robotState, nextState); 370 | _robotState = nextState; 371 | // p(nextMovement); 372 | return nextMovement; 373 | } 374 | 375 | bool PathManager::isGoal(State s) const 376 | { 377 | return _solver.isSeed(s); 378 | } 379 | 380 | bool PathManager::isHome(State s) const 381 | { 382 | static Vector2Int home; 383 | home.x = 0; 384 | home.y = 0; 385 | return s.position == home; 386 | } 387 | 388 | void PathManager::updatedWalls() 389 | { 390 | } 391 | 392 | State PathManager::getRobotState() const 393 | { 394 | return _robotState; 395 | } 396 | -------------------------------------------------------------------------------- /ros2_ws/src/micromouse/src/micromouse.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "micromouse/micromouse.hpp" 19 | 20 | using namespace std::chrono_literals; 21 | 22 | #include "micromouse/robot_specs.hpp" 23 | #include "micromouse/robot.hpp" 24 | #include "micromouse/maze.hpp" 25 | #include "micromouse/maze_utils.hpp" 26 | #include "micromouse/solver.hpp" 27 | #include "micromouse/solver_utils.hpp" 28 | #include "micromouse/angle_utils.hpp" 29 | #include 30 | #include 31 | 32 | static double constexpr MAZE_XSIZE = 0.18; 33 | static double constexpr MAZE_YSIZE = 0.18; 34 | static double constexpr WALL_THICKNESS = 0.012; 35 | 36 | static double constexpr ROBOT_YZISE = 0.096; 37 | static double constexpr SENSOR_DISTANCE (ROBOT_YZISE / 2); 38 | 39 | //assumes sensors at 45deg 40 | static double constexpr BETWEEN_WALLS_IDEAL_MEASUREMENT = ((((MAZE_YSIZE - WALL_THICKNESS) / 2) - SENSOR_DISTANCE) * M_SQRT2); 41 | static double constexpr MAX_THEO_ERROR = (((MAZE_YSIZE - 2*WALL_THICKNESS) - ROBOT_YZISE) * M_SQRT2); 42 | 43 | static double constexpr LIN_BASE = 0.15; 44 | static double constexpr ANG_BASE = 1.0; 45 | 46 | static double constexpr TURN_EXTRA_HEADROOM = 0; 47 | 48 | static RobotConstructionSpecification specs; 49 | static SimpleRobotPoseManager* poseManager; 50 | 51 | static LogicMaze maze {classicMicromouseMaze()}; 52 | static PathManager pm(maze); 53 | 54 | static Pose2 startingPose; 55 | 56 | static bool waitingForNextMovement = true; 57 | static StateTransition movement; 58 | 59 | static double k = 50; 60 | 61 | static bool didRead = false; 62 | 63 | static double leftMeasurement; 64 | static double rightMeasurement; 65 | static double frontMeasurement; 66 | 67 | static Pose2 currentPose; 68 | 69 | static State currentMouseState; 70 | static Pose2 currentIdealPose; 71 | 72 | static State goalMouseState; 73 | static Pose2 goalIdealPose; 74 | 75 | static bool wasMovingForward = false; 76 | static bool shouldFixPosition = false; 77 | 78 | int main(int argc, char **argv) 79 | { 80 | specs.leftWheelPPR = 29.86 * 12; 81 | specs.rightWheelPPR = 29.86 * 12; 82 | specs.leftWheelRadius = 0.016; 83 | specs.rightWheelRadius = 0.016; 84 | specs.wheelBaseDistance = ROBOT_YZISE; 85 | 86 | poseManager = new SimpleRobotPoseManager(specs); 87 | poseManager->setPose({0, 0, M_PI_2}); 88 | startingPose = poseManager->getPose(); 89 | 90 | rclcpp::init(argc, argv); 91 | rclcpp::spin(std::make_shared()); 92 | rclcpp::shutdown(); 93 | } 94 | 95 | MicromouseNode::MicromouseNode() 96 | : Node("micromouse_node"), 97 | cmdVel(*this, "cmd_vel"), 98 | motor_left(*this, "left_motor"), 99 | motor_right(*this, "right_motor"), 100 | range_center(*this,"lidar_center"), 101 | range_right(*this,"lidar_right"), 102 | range_left(*this,"lidar_left"), 103 | imu(*this, "imu"), 104 | encoder_left(*this, "left_wheel_encoder"), 105 | encoder_right(*this, "right_wheel_encoder") 106 | { 107 | auto cb_micromouse = [this](std_msgs::msg::Bool::UniquePtr msg) -> void 108 | { 109 | bool data = msg.get()->data; 110 | micromouseStarted = data; 111 | }; 112 | micromoue_sub = this->create_subscription("micromouse", 10, cb_micromouse); 113 | update_timer_ = this->create_timer(10ms, std::bind(&MicromouseNode::update_callback, this)); 114 | RCLCPP_INFO(this->get_logger(), "Micromouse simulation node has been initialised"); 115 | } 116 | 117 | MicromouseNode::~MicromouseNode() 118 | { 119 | RCLCPP_INFO(this->get_logger(), "Micromouse simulation node has been terminated"); 120 | } 121 | #ifdef RCLCPP_DEBUG 122 | #undef RCLCPP_DEBUG 123 | #define RCLCPP_DEBUG RCLCPP_INFO 124 | #endif 125 | 126 | 127 | void MicromouseNode::update_callback() 128 | { 129 | // Do not do anything until the start signal has been received 130 | if (!micromouseStarted) { 131 | return; 132 | } 133 | 134 | int64_t leftWheelPulses = this->encoder_left.TicksDelta(); 135 | int64_t rightWheelPulses = this->encoder_right.TicksDelta(); 136 | poseManager->updatePose(leftWheelPulses, rightWheelPulses); 137 | 138 | if (waitingForNextMovement) { 139 | // Current mouse position 140 | currentMouseState = pm.getRobotState(); 141 | currentIdealPose = stateToPose(currentMouseState, MAZE_XSIZE, MAZE_YSIZE); 142 | if (movement == StateTransition::forward) { 143 | wasMovingForward = true; 144 | shouldFixPosition = true; 145 | } 146 | movement = pm.nextMovement(); 147 | switch (movement) { 148 | case StateTransition::forward: 149 | RCLCPP_INFO(this->get_logger(), "Next Movement: StateTransition::forward"); 150 | break; 151 | case StateTransition::turnRight: 152 | RCLCPP_INFO(this->get_logger(), "Next Movement: StateTransition::turnRight:"); 153 | break; 154 | case StateTransition::turnLeft: 155 | RCLCPP_INFO(this->get_logger(), "Next Movement: StateTransition::turnLeft"); 156 | break; 157 | case StateTransition::invalid: 158 | RCLCPP_INFO(this->get_logger(), "Next Movement: StateTransition::invalid"); 159 | break; 160 | } 161 | 162 | goalMouseState = pm.getRobotState(); 163 | goalIdealPose = stateToPose(goalMouseState, MAZE_XSIZE, MAZE_YSIZE); 164 | 165 | waitingForNextMovement = false; 166 | didRead = false; 167 | //Correct one axis of pose 168 | if (shouldFixPosition && wasMovingForward) 169 | { 170 | shouldFixPosition = false; 171 | currentPose = poseManager->getPose(); 172 | switch (currentMouseState.orientation) 173 | { 174 | case Direction::up: 175 | case Direction::down: 176 | currentPose.position.x = currentIdealPose.position.x; 177 | break; 178 | case Direction::right: 179 | case Direction::left: 180 | currentPose.position.y = currentIdealPose.position.y; 181 | break; 182 | } 183 | currentPose.angle = currentIdealPose.angle; 184 | poseManager->setPose(currentPose); 185 | } 186 | } 187 | 188 | currentPose = poseManager->getPose(); 189 | Pose2 deltaPoseEnd = goalIdealPose - currentPose; 190 | double dap = signedAngleDifference(currentIdealPose.normalizedAngle(), currentPose.normalizedAngle()); 191 | 192 | leftMeasurement = this->range_left.Read(); 193 | rightMeasurement = this->range_right.Read(); 194 | frontMeasurement = this->range_center.Read(); 195 | 196 | double error = 0; 197 | double measu_limit = BETWEEN_WALLS_IDEAL_MEASUREMENT + 0.01; 198 | 199 | // Has two parallel walls on either side 200 | if(leftMeasurement < measu_limit && rightMeasurement < measu_limit) { 201 | error = leftMeasurement - rightMeasurement; 202 | } else { 203 | if (leftMeasurement < measu_limit) { 204 | error = leftMeasurement - BETWEEN_WALLS_IDEAL_MEASUREMENT; 205 | } else if (rightMeasurement < measu_limit) { 206 | error = BETWEEN_WALLS_IDEAL_MEASUREMENT - rightMeasurement; 207 | } 208 | } 209 | 210 | double dist = 0; 211 | 212 | switch (movement) 213 | { 214 | case StateTransition::forward: 215 | switch(currentMouseState.orientation) 216 | { 217 | case Direction::up: 218 | case Direction::down: 219 | dist = abs(deltaPoseEnd.position.y); 220 | break; 221 | case Direction::right: 222 | case Direction::left: 223 | dist = abs(deltaPoseEnd.position.x); 224 | break; 225 | } 226 | { 227 | double ang = k * error; 228 | double ang_limit = 1.0; 229 | if (ang > ang_limit) ang = ang_limit; 230 | if (ang < -ang_limit) ang = -ang_limit; 231 | 232 | this->update_cmd_vel(LIN_BASE, ang); 233 | } 234 | 235 | if(dist <= 0.01) { 236 | waitingForNextMovement = true; 237 | } 238 | 239 | if (frontMeasurement <= 0.036) { 240 | waitingForNextMovement = true; 241 | } 242 | 243 | //Detect walls when entering the cell 244 | if (!didRead && dist >= 0.10 && dist <= .12) { 245 | didRead = true; 246 | if(leftMeasurement < .1) { 247 | maze.placeWall(goalMouseState.position, localToGlobalDirection(goalMouseState.orientation, Direction::left)); 248 | RCLCPP_INFO(this->get_logger(), "+Wall %d %d left", goalMouseState.position.x, goalMouseState.position.y); 249 | } 250 | 251 | if (rightMeasurement < .1) { 252 | maze.placeWall(goalMouseState.position, localToGlobalDirection(goalMouseState.orientation, Direction::right)); 253 | 254 | RCLCPP_INFO(this->get_logger(), "+Wall %d %d right", goalMouseState.position.x, goalMouseState.position.y); 255 | } 256 | 257 | if(frontMeasurement < .25) { 258 | maze.placeWall(goalMouseState.position, localToGlobalDirection(goalMouseState.orientation, Direction::up)); 259 | RCLCPP_INFO(this->get_logger(), "+Wall %d %d up", goalMouseState.position.x, goalMouseState.position.y); 260 | } 261 | } 262 | break; 263 | 264 | case StateTransition::turnLeft: 265 | this->update_cmd_vel(0.0, ANG_BASE); 266 | if (dap >= M_PI_2 - TURN_EXTRA_HEADROOM) { 267 | waitingForNextMovement = true; 268 | } 269 | break; 270 | 271 | case StateTransition::turnRight: 272 | this->update_cmd_vel(0.0, -ANG_BASE); 273 | if (dap <= - M_PI_2 + TURN_EXTRA_HEADROOM) { 274 | waitingForNextMovement = true; 275 | } 276 | break; 277 | 278 | default: 279 | this->update_cmd_vel(0.0, 0.0); 280 | RCLCPP_INFO(this->get_logger(), "defaulting"); 281 | break; 282 | } 283 | } 284 | 285 | void MicromouseNode::update_cmd_vel(double linear, double angular) 286 | { 287 | if (micromouseStarted) 288 | { 289 | this->cmdVel.SetVel(linear, angular); 290 | } 291 | } 292 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /gz_ws/src/micromouse_system/MicromouseSystem.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024 Pedro Fontoura Zawadniak 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "MicromouseSystem.hh" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | using namespace gz; 49 | using namespace custom; 50 | using maze_coordinate = int; 51 | 52 | enum class CompetitionState 53 | { 54 | configuring, 55 | running, 56 | over 57 | }; 58 | 59 | enum class MazeThing 60 | { 61 | pole, 62 | hwall, 63 | vwall, 64 | hspace, 65 | vspace, 66 | cspace 67 | }; 68 | 69 | struct MazeCoordinate 70 | { 71 | MazeCoordinate(maze_coordinate x_, maze_coordinate y_); 72 | maze_coordinate x; 73 | maze_coordinate y; 74 | }; 75 | 76 | MazeCoordinate::MazeCoordinate(maze_coordinate x_, maze_coordinate y_) 77 | : x(x_), y(y_) 78 | { 79 | } 80 | 81 | class PhysicalDims 82 | { 83 | public: 84 | double maze_height; 85 | double thickness; 86 | double wall_length; 87 | bool validate() const; 88 | }; 89 | 90 | bool PhysicalDims::validate() const 91 | { 92 | return maze_height > 0 && thickness > 0 && wall_length > 0; 93 | } 94 | 95 | class Maze 96 | { 97 | public: 98 | static Maze fromMicromouseOnline(std::string maze_string); 99 | bool validate() const; 100 | std::string toSdf() const; 101 | 102 | std::vector> things; 103 | 104 | static const std::string goal_link_name; 105 | static const std::string start_link_name; 106 | std::vector goals; 107 | std::vector starts; 108 | PhysicalDims descr; 109 | }; 110 | 111 | const std::string Maze::goal_link_name = "goal_link"; 112 | const std::string Maze::start_link_name = "start_link"; 113 | 114 | std::string reverse_string(std::string in) 115 | { 116 | std::vector lines; 117 | std::stringstream iss(in); 118 | for (std::string line; std::getline(iss, line);) { 119 | lines.push_back(line); 120 | } 121 | std::reverse(lines.begin(), lines.end()); 122 | std::ostringstream os; 123 | for (const auto& line : lines) { 124 | os << line << "\n"; 125 | } 126 | return os.str(); 127 | } 128 | 129 | std::string area_obj(std::string name, int id, double pos_x, double pos_y, double pos_z, double size_x, double size_y, double size_z); 130 | 131 | std::string start_obj(int id, double pos_x, double pos_y, double pos_z, double size_x, double size_y, double size_z) 132 | { 133 | std::ostringstream ss; 134 | ss << area_obj("start", id, pos_x, pos_y, pos_z, size_x, size_y, size_z); 135 | ss << R"()" << "\n"; 136 | ss << R"(1)" << "\n"; 137 | ss << "" << "\n"; 138 | return ss.str(); 139 | } 140 | 141 | std::string goal_obj(int id, double pos_x, double pos_y, double pos_z, double size_x, double size_y, double size_z) 142 | { 143 | std::ostringstream ss; 144 | ss << area_obj("goal", id, pos_x, pos_y, pos_z, size_x, size_y, size_z); 145 | ss << R"()" << "\n"; 146 | ss << R"(2)" << "\n"; 147 | ss << "" << "\n"; 148 | return ss.str(); 149 | } 150 | 151 | std::string area_obj(std::string name, int id, double pos_x, double pos_y, double pos_z, double size_x, double size_y, double size_z) { 152 | std::ostringstream ss; 153 | // Area visual 154 | ss << R"()" << "\n"; 155 | ss << "0" << "\n"; 156 | ss << "" << pos_x << " " << pos_y << " " << pos_z << " 0 0 0" << "\n"; 157 | ss << "" << size_x << " " << size_y << " " << size_z << "" << "\n"; 158 | ss << "1 1 1 0.151 1 1 0.151 1 1 0.15" << "\n"; 159 | ss << "" << "\n"; 160 | 161 | // Area collision 162 | ss << R"()" << "\n"; 163 | ss << "" << "\n"; 164 | ss << "0" << "\n"; // Area doesn't collide with anything, but still generates contacts (I assume) 165 | ss << "" << "\n"; 166 | ss << "" << pos_x << " " << pos_y << " " << pos_z << " 0 0 0" << "\n"; 167 | ss << "" << size_x << " " << size_y << " " << size_z << "" << "\n"; 168 | ss << "" << "\n"; 169 | 170 | return ss.str(); 171 | } 172 | 173 | std::string maze_obj(double id, double pos_x, double pos_y, double pos_z, double size_x, double size_y, double size_z) { 174 | static double constexpr MAZE_RED_TAPE_VISUAL_OFFSET = 0.00005; 175 | std::ostringstream ss; 176 | // Wall visual 177 | ss << R"()" << "\n"; 178 | ss << "" << pos_x << " " << pos_y << " " << pos_z << " 0 0 0" << "\n"; 179 | ss << "" << size_x << " " << size_y << " " << size_z << "" << "\n"; 180 | ss << "1 1 1 11 1 1 11 1 1 1" << "\n"; 181 | ss << "" << "\n"; 182 | 183 | // Red wall top visual 184 | ss << R"()" << "\n"; 185 | ss << "" << pos_x << " " << pos_y << " " << (pos_z + size_z / 2) + MAZE_RED_TAPE_VISUAL_OFFSET << " 0 0 0" << "\n"; 186 | ss << "0 0 1" << size_x << " " << size_y << " " << size_z << "" << "\n"; 187 | ss << "1 0 0 11 0 0 11 0 0 1" << "\n"; 188 | ss << "" << "\n"; 189 | 190 | // Wall collision 191 | ss << R"()" << "\n"; 192 | ss << "" << pos_x << " " << pos_y << " " << pos_z << " 0 0 0" << "\n"; 193 | ss << "" << size_x << " " << size_y << " " << size_z << "" << "\n"; 194 | ss << "" << "\n"; 195 | return ss.str(); 196 | } 197 | 198 | Maze Maze::fromMicromouseOnline(std::string maze_string) 199 | { 200 | Maze maze; 201 | maze.descr.maze_height = 0.05; 202 | maze.descr.thickness = 0.012; 203 | maze.descr.wall_length = 0.168; 204 | std::istringstream iss(reverse_string(maze_string)); 205 | size_t line_idx = 0; 206 | maze_coordinate pos_x = 0; 207 | maze_coordinate pos_y = 0; 208 | for (std::string line; std::getline(iss, line);) { 209 | std::vector things; 210 | pos_x = 0; 211 | if (line_idx % 2 == 0) { // Horizontal wall line 212 | size_t idx_in_line = 0; 213 | while (idx_in_line < line.length()) { 214 | const char* a = line.c_str() + idx_in_line; 215 | if (strncmp(a, "o", 1) == 0) { 216 | idx_in_line += 1; 217 | things.push_back(MazeThing::pole); 218 | } else if (strncmp(a, " ", 3) == 0) { 219 | idx_in_line += 3; 220 | things.push_back(MazeThing::hspace); 221 | } else if (strncmp(a, "---", 3) == 0) { 222 | idx_in_line += 3; 223 | things.push_back(MazeThing::hwall); 224 | } 225 | ++pos_x; 226 | } 227 | } else { // Vertical wall line 228 | size_t idx_in_line = 0; 229 | while (idx_in_line < line.length()) { 230 | const char* a = line.c_str() + idx_in_line; 231 | if (idx_in_line % 4 == 0) { 232 | size_t char_sz = 1; 233 | if (strncmp(a, " ", char_sz) == 0) { 234 | idx_in_line += char_sz; 235 | things.push_back(MazeThing::vspace); 236 | } else if (strncmp(a, "|", char_sz) == 0) { 237 | idx_in_line += char_sz; 238 | things.push_back(MazeThing::vwall); 239 | } 240 | } else { 241 | size_t char_sz = 3; 242 | if (strncmp(a, " ", char_sz) == 0) { 243 | idx_in_line += char_sz; 244 | things.push_back(MazeThing::cspace); 245 | } else if (strncmp(a, " G ", char_sz) == 0) { 246 | maze.goals.push_back({pos_x, pos_y}); 247 | idx_in_line += char_sz; 248 | things.push_back(MazeThing::cspace); 249 | } else if (strncmp(a, " S ", char_sz) == 0) { 250 | maze.starts.push_back({pos_x, pos_y}); 251 | idx_in_line += char_sz; 252 | things.push_back(MazeThing::cspace); 253 | } 254 | ++pos_x; 255 | } 256 | } 257 | ++pos_y; 258 | } 259 | ++line_idx; 260 | maze.things.push_back(things); 261 | } 262 | return maze; 263 | } 264 | 265 | bool Maze::validate() const 266 | { 267 | return goals.size() >= 1 && starts.size() == 1 && descr.validate(); 268 | } 269 | 270 | std::string Maze::toSdf() const 271 | { 272 | // Object to build the sdf string on 273 | std::stringstream ss; 274 | 275 | // Add xml tag 276 | std::string xml_version = "1.0"; 277 | ss << R"()" << "\n"; 278 | 279 | // Open sdf tag 280 | std::string sdf_version = "1.11"; 281 | ss << R"()" << "\n"; 282 | 283 | // Open model tag 284 | std::string model_name = "micromouse_maze"; 285 | ss << R"()" << "\n"; 286 | 287 | ss << "true" << "\n"; 288 | 289 | // Add maze wall link (Every wall as single link has better performance) 290 | std::string link_name = "micromouse_maze_link"; 291 | ss << R"()" << "\n"; 292 | ss << "0 0 0 0 0 0" << "\n"; 293 | 294 | const double thickness = descr.thickness; 295 | const double half_thickness = thickness / 2; 296 | const double wall_length = descr.wall_length; 297 | const double half_wall_length = wall_length / 2; 298 | const double pole_lenth = descr.thickness; 299 | const double half_pole_length = pole_lenth / 2; 300 | const double wall_height = descr.maze_height; 301 | const double half_wall_height = wall_height / 2; 302 | const double cell_length = descr.wall_length + descr.thickness; 303 | const double half_cell_length = cell_length / 2; 304 | 305 | double x_pos = half_thickness; 306 | double y_pos = -half_thickness; 307 | const double z_pos = half_wall_height; 308 | 309 | size_t thing_id = 0; 310 | size_t max_i = 0; 311 | for (auto& vec : things) { 312 | if (vec.size() > max_i) max_i = vec.size(); 313 | } 314 | 315 | for(size_t i = 0; i < max_i; ++i) { 316 | double x_pos = 0; 317 | bool is_there_x = false; 318 | double start_x = x_pos; 319 | double end_x = x_pos; 320 | for (size_t j = 0; j < this->things.size(); ++j){ 321 | double my_x; 322 | if (i >= this->things[j].size()) { 323 | continue; 324 | } 325 | auto thing = this->things[j][i]; 326 | switch(thing) { 327 | case MazeThing::vspace: 328 | x_pos += wall_length; 329 | if (is_there_x) { 330 | double l = end_x - start_x; 331 | my_x = (start_x + end_x) / 2; 332 | ss << maze_obj(thing_id, my_x, y_pos, z_pos, l, thickness, wall_height); 333 | thing_id++; 334 | is_there_x = false; 335 | } 336 | start_x = x_pos; 337 | break; 338 | case MazeThing::vwall: 339 | x_pos += wall_length; 340 | is_there_x = true; 341 | end_x = x_pos; 342 | break; 343 | case MazeThing::pole: 344 | x_pos += pole_lenth; 345 | end_x = x_pos; 346 | break; 347 | case MazeThing::hwall: // fallthrough 348 | case MazeThing::hspace: // fallthrough 349 | case MazeThing::cspace: 350 | // Do nothing 351 | break; 352 | } 353 | } 354 | if (is_there_x) { 355 | double l = end_x - start_x; 356 | double my_x = (start_x + end_x) / 2; 357 | ss << maze_obj(thing_id, my_x, y_pos, z_pos, l, thickness, wall_height); 358 | thing_id++; 359 | is_there_x = false; 360 | } 361 | y_pos -= half_wall_length + half_pole_length; 362 | } 363 | x_pos = half_thickness; 364 | for (auto& line : this->things) { 365 | double y_pos = 0; 366 | bool is_there_y = false; 367 | double start_y = y_pos; 368 | double end_y = y_pos; 369 | for (auto thing : line) { 370 | double my_y; 371 | switch(thing) { 372 | case MazeThing::hspace: 373 | y_pos -= wall_length; 374 | if (is_there_y) { 375 | double l = -(end_y - start_y); 376 | my_y = (start_y + end_y) / 2; 377 | ss << maze_obj(thing_id, x_pos, my_y, z_pos, thickness, l, wall_height); 378 | thing_id++; 379 | is_there_y = false; 380 | } 381 | start_y = y_pos; 382 | break; 383 | case MazeThing::hwall: 384 | y_pos -= wall_length; 385 | is_there_y = true; 386 | end_y = y_pos; 387 | break; 388 | case MazeThing::pole: 389 | y_pos -= pole_lenth; 390 | end_y = y_pos; 391 | break; 392 | case MazeThing::vwall: // fallthrough 393 | case MazeThing::vspace: // fallthrough 394 | case MazeThing::cspace: 395 | // Do nothing 396 | break; 397 | } 398 | } 399 | if (is_there_y) { 400 | double l = -(end_y - start_y); 401 | double my_y = (start_y + end_y) / 2; 402 | ss << maze_obj(thing_id, x_pos, my_y, z_pos, thickness, l, wall_height); 403 | thing_id++; 404 | is_there_y = false; 405 | } 406 | x_pos += half_wall_length + half_pole_length; 407 | } 408 | 409 | // Find lone poles 410 | x_pos = half_thickness; 411 | y_pos = 0; 412 | for (size_t i = 0; i < things.size(); ++i) { 413 | y_pos = 0; 414 | for (size_t j = 0; j < things[i].size(); ++j) { 415 | MazeThing thing = things[i][j]; 416 | bool was_put = false; 417 | if (thing == MazeThing::pole) { 418 | if (i != 0) { 419 | MazeThing thingthing = this->things[i - 1][j]; 420 | if (thingthing == MazeThing::hwall || thingthing == MazeThing::vwall) { 421 | was_put = true; 422 | } 423 | } 424 | if (i != things.size() - 1) { 425 | MazeThing thingthing = this->things[i + 1][j]; 426 | if (thingthing == MazeThing::hwall || thingthing == MazeThing::vwall) { 427 | was_put = true; 428 | } 429 | } 430 | if (j != 0) { 431 | MazeThing thingthing = this->things[i][j - 1]; 432 | if (thingthing == MazeThing::hwall || thingthing == MazeThing::vwall) { 433 | was_put = true; 434 | } 435 | } 436 | if (j != things.size() - 1) { 437 | MazeThing thingthing = this->things[i][j + 1]; 438 | if (thingthing == MazeThing::hwall || thingthing == MazeThing::vwall) { 439 | was_put = true; 440 | } 441 | } 442 | if (!was_put) { 443 | double my_y = y_pos -= half_thickness; 444 | ss << maze_obj(thing_id, x_pos, my_y, z_pos, thickness, thickness, wall_height); 445 | thing_id++; 446 | } 447 | } 448 | switch (thing) { 449 | case MazeThing::cspace: 450 | case MazeThing::hspace: 451 | case MazeThing::hwall: 452 | y_pos -= wall_length; 453 | break; 454 | case MazeThing::vspace: 455 | case MazeThing::vwall: 456 | case MazeThing::pole: 457 | y_pos -= thickness; 458 | break; 459 | } 460 | } 461 | x_pos += half_wall_length + half_pole_length; 462 | } 463 | ss << "" << "\n"; 464 | 465 | ss << R"(start_link_name << R"(">)" << "\n"; 466 | int start_id = 0; 467 | for (auto start : this->starts) { 468 | double x = half_pole_length + half_cell_length + cell_length * start.y; 469 | double y = -(half_pole_length + half_cell_length + cell_length * start.x); 470 | ss << area_obj("start", start_id++, x, y, descr.wall_length / 2, cell_length, cell_length, cell_length); 471 | } 472 | ss << "" << "\n"; 473 | 474 | ss << R"(goal_link_name << R"(">)" << "\n"; 475 | int goal_id = 0; 476 | for (auto goal : this->goals) { 477 | double x = half_pole_length + half_cell_length + cell_length * goal.y; 478 | double y = -(half_pole_length + half_cell_length + cell_length * goal.x); 479 | ss << area_obj("goal", goal_id++, x, y, descr.wall_length / 2, cell_length, cell_length, cell_length); 480 | } 481 | 482 | ss << "" << "\n"; 483 | ss << "" << "\n"; 484 | ss << "" << "\n"; 485 | return ss.str(); 486 | } 487 | 488 | class custom::MicromouseSystemPrivate 489 | { 490 | public: 491 | transport::Node node; 492 | transport::Node::Publisher pub; 493 | transport::Node::Publisher time_pub; 494 | sim::Entity world_entity {sim::kNullEntity}; 495 | bool SetMazeService(const msgs::StringMsg &_req, msgs::Boolean &_res); 496 | bool SetRobotService(const msgs::StringMsg &_req, msgs::Boolean &_res); 497 | bool SetRobotPoseService(const msgs::Pose &_req, msgs::Boolean &_res); 498 | bool StartCompService(const msgs::Empty &_req, msgs::Boolean &_res); 499 | bool ResetService(const msgs::StringMsg &_req, msgs::StringMsg &_res); 500 | bool ClearService(const msgs::Empty &_req, msgs::Empty &_res); 501 | bool RepositionService(const msgs::Empty &_req, msgs::Empty &_res); 502 | 503 | public: std::unique_ptr creator{nullptr}; 504 | 505 | std::vector goal_col_entities; 506 | std::vector start_col_entities; 507 | 508 | CompetitionState state = CompetitionState::configuring; 509 | 510 | void ProcessRobotRequest(sim::EntityComponentManager &_ecm); 511 | void HandleResetRequest(sim::EntityComponentManager &_ecm); 512 | std::mutex resetMutex; 513 | bool resetPending = false; 514 | 515 | void SetRobotPose(sim::EntityComponentManager &_ecm); 516 | std::mutex robotRequestMutex; 517 | std::string lastRobotRequest; 518 | gz::math::Pose3d lastRobotPoseRequest; 519 | 520 | bool isRobotRequestProcessed = false; 521 | sim::Entity robot_entity {sim::kNullEntity}; 522 | 523 | void HandleMazeRequests(sim::EntityComponentManager &_ecm); 524 | std::mutex mazeRequestMutex; 525 | std::unique_ptr lastMazeRequest; 526 | bool isMazeRequestProcessed = false; 527 | sim::Entity currentMazeEntity {sim::kNullEntity}; 528 | 529 | bool timerRunning = false; 530 | bool prevStartContains = false; 531 | bool prevGoalContains = false; 532 | std::chrono::steady_clock::duration best_time = std::chrono::steady_clock::duration::max(); 533 | std::chrono::steady_clock::duration start_time = std::chrono::steady_clock::duration::zero(); 534 | }; 535 | 536 | bool MicromouseSystemPrivate::SetMazeService(const msgs::StringMsg &_req, msgs::Boolean &_res) 537 | { 538 | std::lock_guard lock(this->mazeRequestMutex); 539 | this->lastMazeRequest = std::make_unique(Maze::fromMicromouseOnline(_req.data())); 540 | if (nullptr == this->lastMazeRequest) { 541 | _res.set_data(false); 542 | return false; 543 | } 544 | _res.set_data(this->lastMazeRequest->validate()); 545 | this->isMazeRequestProcessed = false; 546 | return true; 547 | } 548 | 549 | bool MicromouseSystemPrivate::SetRobotService(const msgs::StringMsg &_req, msgs::Boolean &_res) 550 | { 551 | std::lock_guard lock(this->robotRequestMutex); 552 | this->lastRobotRequest = _req.data(); 553 | if (this->lastRobotRequest.empty()) { 554 | _res.set_data(false); 555 | return true; 556 | } 557 | this->isRobotRequestProcessed = false; 558 | _res.set_data(true); 559 | return true; 560 | } 561 | 562 | bool MicromouseSystemPrivate::SetRobotPoseService(const msgs::Pose &_req, msgs::Boolean &_res) 563 | { 564 | std::lock_guard lock(this->robotRequestMutex); 565 | this->lastRobotPoseRequest.Pos().Set(_req.position().x(), _req.position().y(), _req.position().z()); 566 | this->lastRobotPoseRequest.Rot().Set(_req.orientation().w(), _req.orientation().x(), _req.orientation().y(), _req.orientation().z()); 567 | _res.set_data(true); 568 | return true; 569 | } 570 | 571 | bool MicromouseSystemPrivate::ResetService(const msgs::StringMsg &_req, msgs::StringMsg &_res) 572 | { 573 | (void)_req; 574 | (void)_res; 575 | std::lock_guard lock(this->resetMutex); 576 | this->resetPending = true; 577 | return true; 578 | } 579 | 580 | bool MicromouseSystemPrivate::ClearService(const msgs::Empty &_req, msgs::Empty &_res) 581 | { 582 | (void)_req; 583 | (void)_res; 584 | if (this->state != CompetitionState::configuring) { 585 | return false; 586 | } 587 | return true; 588 | } 589 | 590 | bool MicromouseSystemPrivate::RepositionService(const msgs::Empty &_req, msgs::Empty &_res) 591 | { 592 | (void)_req; 593 | (void)_res; 594 | if (this->state != CompetitionState::running) { 595 | return false; 596 | } 597 | return true; 598 | } 599 | 600 | bool MicromouseSystemPrivate::StartCompService(const msgs::Empty &_req, msgs::Boolean &_res) 601 | { 602 | (void)_req; 603 | std::lock_guard lock(this->mazeRequestMutex); 604 | if (nullptr == this->lastMazeRequest) { 605 | _res.set_data(false); 606 | return true; 607 | } 608 | 609 | gz::msgs::Boolean msg; 610 | msg.set_data(true); 611 | bool pub_result = this->pub.Publish(msg); 612 | if (!pub_result) { 613 | gzerr << "Error publishing. Result: " << pub_result << std::endl; 614 | _res.set_data(false); 615 | } else { 616 | gzdbg << "Publishing. Result: " << pub_result << std::endl; 617 | this->state = CompetitionState::running; 618 | _res.set_data(true); 619 | } 620 | 621 | return true; 622 | } 623 | 624 | MicromouseSystem::MicromouseSystem() 625 | : dataPtr(std::make_unique()) 626 | { 627 | // Advertise services 628 | std::string setMaze = "micromouse/set_maze"; 629 | this->dataPtr->node.Advertise(setMaze, &MicromouseSystemPrivate::SetMazeService, this->dataPtr.get()); 630 | 631 | std::string setRobot = "micromouse/set_robot"; 632 | this->dataPtr->node.Advertise(setRobot, &MicromouseSystemPrivate::SetRobotService, this->dataPtr.get()); 633 | 634 | std::string setRobotPose = "micromouse/set_robot_pose"; 635 | this->dataPtr->node.Advertise(setRobotPose, &MicromouseSystemPrivate::SetRobotPoseService, this->dataPtr.get()); 636 | 637 | std::string startComp = "micromouse/start_comp"; 638 | this->dataPtr->node.Advertise(startComp, &MicromouseSystemPrivate::StartCompService, this->dataPtr.get()); 639 | 640 | std::string reset = "micromouse/reset"; 641 | this->dataPtr->node.Advertise(reset, &MicromouseSystemPrivate::ResetService, this->dataPtr.get()); 642 | 643 | std::string reposition = "micromouse/reposition"; 644 | this->dataPtr->node.Advertise(reposition, &MicromouseSystemPrivate::RepositionService, this->dataPtr.get()); 645 | 646 | std::string clear = "micromouse/clear"; 647 | this->dataPtr->node.Advertise(reposition, &MicromouseSystemPrivate::ClearService, this->dataPtr.get()); 648 | 649 | std::string timetopic = "/micromouse/lap"; 650 | this->dataPtr->time_pub = this->dataPtr->node.Advertise(timetopic); 651 | 652 | std::string statustopic = "/micromouse/status"; 653 | this->dataPtr->pub = this->dataPtr->node.Advertise(statustopic); 654 | if (!this->dataPtr->pub) { 655 | gzerr << "Error advertising topic [" << statustopic << "]" << std::endl; 656 | } else { 657 | gzdbg << "Advertising topic [" << statustopic << "]" << std::endl; 658 | } 659 | } 660 | 661 | MicromouseSystem::~MicromouseSystem() 662 | { 663 | } 664 | 665 | void MicromouseSystem::Configure( 666 | const sim::Entity &_entity, 667 | const std::shared_ptr &_sdf, 668 | sim::EntityComponentManager &_ecm, 669 | sim::EventManager &_eventMgr) 670 | { 671 | (void)_sdf; 672 | 673 | this->dataPtr->world_entity = _entity; 674 | this->dataPtr->creator = std::make_unique(_ecm, _eventMgr); 675 | } 676 | 677 | 678 | void MicromouseSystemPrivate::HandleMazeRequests(sim::EntityComponentManager &_ecm) 679 | { 680 | { 681 | std::lock_guard lock(this->mazeRequestMutex); 682 | if (this->isMazeRequestProcessed || !this->lastMazeRequest) { 683 | return; 684 | } 685 | } 686 | 687 | // Process the request 688 | if (this->currentMazeEntity != sim::kNullEntity) { 689 | this->creator->RequestRemoveEntity(this->currentMazeEntity); 690 | this->currentMazeEntity = sim::kNullEntity; 691 | return; 692 | } 693 | 694 | std::lock_guard lock(this->mazeRequestMutex); 695 | std::string maze_sdf = this->lastMazeRequest->toSdf(); 696 | sdf::Root root; 697 | auto errors = root.LoadSdfString(maze_sdf); 698 | if (!errors.empty()) { 699 | gzerr << "Maze creation failed: failed to parse maze string" << std::endl; 700 | } 701 | auto model = root.Model(); 702 | this->currentMazeEntity = creator->CreateEntities(model); 703 | if (this->currentMazeEntity == sim::kNullEntity) { 704 | gzerr << "Maze creation failed: falied to to create entity" << std::endl; 705 | } else { 706 | this->creator->SetParent(this->currentMazeEntity, this->world_entity); 707 | } 708 | 709 | _ecm.Each( 710 | [&](const sim::Entity &entity, sim::components::Name *name) -> bool 711 | { 712 | if (name->Data() == Maze::goal_link_name) { 713 | this->goal_col_entities = _ecm.ChildrenByComponents(entity, sim::components::Collision()); 714 | } else if (name->Data() == Maze::start_link_name) { 715 | this->start_col_entities = _ecm.ChildrenByComponents(entity, sim::components::Collision()); 716 | } 717 | return true; 718 | }); 719 | 720 | this->isMazeRequestProcessed = true; 721 | } 722 | 723 | 724 | void MicromouseSystemPrivate::SetRobotPose(gz::sim::EntityComponentManager &_ecm) 725 | { 726 | if (this->robot_entity == sim::kNullEntity) { 727 | gzerr << "There is no robot to be reset [robot = kNullEntity]" << std::endl; 728 | return; 729 | } 730 | 731 | double y = this->lastMazeRequest->descr.thickness + this->lastMazeRequest->descr.wall_length/2; 732 | auto state = _ecm.SetComponentData( 733 | this->robot_entity, 734 | math::Pose3d(y+this->lastRobotPoseRequest.X(), -y + this->lastRobotPoseRequest.Y(), this->lastRobotPoseRequest.Z(), 735 | this->lastRobotPoseRequest.Roll(), this->lastRobotPoseRequest.Pitch(), this->lastRobotPoseRequest.Yaw())) ? sim::ComponentState::OneTimeChange : sim::ComponentState::NoChange; 736 | _ecm.SetChanged(this->robot_entity, sim::components::WorldPoseCmd::typeId, state); 737 | } 738 | 739 | void MicromouseSystemPrivate::HandleResetRequest(gz::sim::EntityComponentManager &_ecm) 740 | { 741 | std::lock_guard lock(this->resetMutex); 742 | if (this->resetPending) { 743 | this->SetRobotPose(_ecm); 744 | this->resetPending = false; 745 | } 746 | } 747 | void MicromouseSystemPrivate::ProcessRobotRequest(gz::sim::EntityComponentManager &_ecm) 748 | { 749 | { 750 | std::lock_guard lock(this->robotRequestMutex); 751 | if (this->isRobotRequestProcessed || this->lastRobotRequest.empty()) { 752 | return; 753 | } 754 | } 755 | std::lock_guard lock1(this->robotRequestMutex); 756 | std::lock_guard lock2(this->mazeRequestMutex); 757 | if (this->robot_entity != sim::kNullEntity) { 758 | _ecm.RequestRemoveEntity(this->robot_entity); 759 | this->robot_entity = sim::kNullEntity; 760 | return; 761 | } 762 | sdf::Root root; 763 | auto errors = root.LoadSdfString(this->lastRobotRequest); 764 | auto model = root.Model(); 765 | this->robot_entity = this->creator->CreateEntities(model); 766 | this->SetRobotPose(_ecm); 767 | _ecm.CreateComponent(this->robot_entity, sim::components::AxisAlignedBox()); 768 | this->creator->SetParent(this->robot_entity, this->world_entity); 769 | this->isRobotRequestProcessed = true; 770 | } 771 | 772 | void MicromouseSystem::PreUpdate( 773 | const gz::sim::UpdateInfo &_info, 774 | gz::sim::EntityComponentManager &_ecm) 775 | { 776 | GZ_PROFILE("MicromouseSystem::PreUpdate"); 777 | (void)_info; 778 | this->dataPtr->HandleMazeRequests(_ecm); 779 | this->dataPtr->ProcessRobotRequest(_ecm); 780 | this->dataPtr->HandleResetRequest(_ecm); 781 | } 782 | 783 | void MicromouseSystem::PostUpdate( 784 | const gz::sim::UpdateInfo &_info, 785 | const gz::sim::EntityComponentManager &_ecm) 786 | { 787 | (void)_info; 788 | GZ_PROFILE("MicromouseSystem::PostUpdate"); 789 | 790 | if (!this->dataPtr->isMazeRequestProcessed) { 791 | return; 792 | } 793 | 794 | if (this->dataPtr->lastMazeRequest == nullptr) { 795 | gzerr << "Uai" << std::endl; 796 | return; 797 | } 798 | 799 | auto aabb = _ecm.ComponentData(this->dataPtr->robot_entity); 800 | bool goal_contains = false; 801 | for (sim::Entity goal : this->dataPtr->goal_col_entities) { 802 | // Check if robot position is within limits, disregarding robot's Z position 803 | auto pose = _ecm.ComponentData(goal); 804 | auto pos = pose->Pos(); 805 | pos.Z(aabb->Center().Z()); 806 | if (aabb->Contains(pos)) { 807 | goal_contains = true; 808 | } 809 | } 810 | 811 | bool start_contains = false; 812 | for (const auto& start : this->dataPtr->start_col_entities) { 813 | // Check if robot position is within limits, disregarding robot's Z position 814 | auto pose = _ecm.ComponentData(start); 815 | auto pos = pose->Pos(); 816 | pos.Z(aabb->Center().Z()); 817 | if (aabb->Contains(pos)) { 818 | start_contains = true; 819 | } 820 | } 821 | if (!start_contains && this->dataPtr->prevStartContains) { 822 | // start timer 823 | this->dataPtr->start_time = _info.simTime; 824 | this->dataPtr->timerRunning = true; 825 | gzmsg << "Starting run at " << this->dataPtr->start_time.count() << std::endl; 826 | } 827 | 828 | if (goal_contains && !this->dataPtr->prevGoalContains && this->dataPtr->timerRunning) { 829 | auto finish_time = _info.simTime; 830 | auto diff = finish_time - this->dataPtr->start_time; 831 | 832 | msgs::Time time_msg; 833 | std::chrono::seconds sec = std::chrono::duration_cast(diff); 834 | std::chrono::nanoseconds total_nanoseconds = std::chrono::duration_cast(diff); 835 | std::chrono::nanoseconds nsec = total_nanoseconds - sec; 836 | time_msg.set_sec(sec.count()); 837 | time_msg.set_nsec(nsec.count()); 838 | this->dataPtr->time_pub.Publish(time_msg); 839 | 840 | if (diff.count() < this->dataPtr->best_time.count()) { 841 | this->dataPtr->best_time = diff; 842 | } 843 | this->dataPtr->timerRunning = false; 844 | } 845 | this->dataPtr->prevStartContains = start_contains; 846 | this->dataPtr->prevGoalContains = goal_contains; 847 | } 848 | 849 | GZ_ADD_PLUGIN( 850 | custom::MicromouseSystem, 851 | sim::System, 852 | sim::ISystemConfigure, 853 | sim::ISystemPreUpdate, 854 | sim::ISystemPostUpdate 855 | ) 856 | 857 | GZ_ADD_PLUGIN_ALIAS(custom::MicromouseSystem, "MicromouseSystem") 858 | --------------------------------------------------------------------------------