diff --git a/.vscode/launch.json b/.vscode/launch.json index 060245a..e5ddfb9 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,21 +1,26 @@ { - // For information on how to use the debug with ROS see - // https://github.com/ms-iot/vscode-ros/blob/master/doc/debug-support.md "version": "0.2.0", "configurations": [ { - // Note: Install GDB to use this configuration - "name": "ROS: Attach", - "type": "ros", - "request": "attach" + "name": "ROS Launch", + "request": "launch", + "type": "cppdbg", + "preLaunchTask": "ROS Launch Micras Simulation", + "serverLaunchTimeout": 10000, + "miDebuggerServerAddress": "localhost:3000", + "miDebuggerPath": "gdb", + "miDebuggerArgs": "-x ${workspaceFolder}/scripts/pause_gazebo.py", + "cwd": "${workspaceFolder}", + "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", }, { - "name": "ROS: Launch", - "type": "ros", + "name": "ROS Attach", "request": "launch", - "target": "${workspaceFolder}/launch/include/node.launch.xml", - "preLaunchTask": "ROS Launch World", - "arguments": ["name:=${input:args}"] + "type": "cppdbg", + "miDebuggerPath": "gdb", + "miDebuggerServerAddress": "localhost:3000", + "cwd": "${workspaceFolder}", + "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", } ], "inputs": [ diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 694909a..9d10116 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -2,15 +2,15 @@ "version": "2.0.0", "tasks": [ { - "label": "ROS Launch World", + "label": "ROS Launch Micras Simulation", "type": "ros", - "command": "ros2 launch micras_simulation world.launch.xml", + "command": "bash -c 'source /opt/ros/jazzy/setup.bash && source ${workspaceFolder}/../../install/setup.bash && ros2 launch micras_simulation micras.launch.xml'", "isBackground": true, "problemMatcher": [ { "pattern": [ { - "regexp": ".", + "regexp": "", "file": 1, "location": 2, "message": 3 @@ -18,11 +18,11 @@ ], "background": { "activeOnStart": true, - "beginsPattern": ".", - "endsPattern": ".", + "beginsPattern": "\\[INFO\\] \\[launch\\]", + "endsPattern": "Listening on port" } } ] - }, + } ] } diff --git a/CMakeLists.txt b/CMakeLists.txt index 4410350..303a6f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,9 @@ -# Name: CMakeLists.txt -# Micras Team -# Brief: CMake configuration file for the project -# 04/2024 - cmake_minimum_required(VERSION 3.16.3) +############################################################################### +## CMake Configuration +############################################################################### + project(micras_simulation) if(NOT CMAKE_CXX_STANDARD) @@ -26,6 +25,10 @@ endif() add_subdirectory(gazebo/micras_plugin) +############################################################################### +## Find ROS2 Dependencies +############################################################################### + find_package(ament_cmake REQUIRED) set(ROS_CODE_DEPS @@ -34,6 +37,7 @@ set(ROS_CODE_DEPS sensor_msgs geometry_msgs nav_msgs + example_interfaces ament_index_cpp ) @@ -41,12 +45,39 @@ foreach(dependency ${ROS_CODE_DEPS}) find_package(${dependency} REQUIRED) endforeach() +############################################################################### +## Find WebSocketpp +############################################################################### + +find_package(Boost REQUIRED COMPONENTS system thread) + +include(FetchContent) + +FetchContent_Declare( + websocketpp + GIT_REPOSITORY https://github.com/zaphoyd/websocketpp.git + GIT_TAG b9aeec6eaf3d5610503439b4fae3581d9aff08e8 +) + +# Suppress CMake deprecation warnings from websocketpp +set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "Whether to issue deprecation warnings." FORCE) +FetchContent_MakeAvailable(websocketpp) +set(CMAKE_WARN_DEPRECATED ON CACHE BOOL "Whether to issue deprecation warnings." FORCE) + +############################################################################### +## Input Files +############################################################################### + file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/**/*.c*" "MicrasFirmware/src/*.c*") -file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "MicrasFirmware/micras_nav/src/*.c*" "MicrasFirmware/micras_core/src/*.c*") +file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "MicrasFirmware/micras_nav/src/*.c*" "MicrasFirmware/micras_comm/src/*.c*" "MicrasFirmware/micras_core/src/*.c*") file(GLOB_RECURSE SIMULATION_PROXY_SOURCES CONFIGURE_DEPENDS "src/proxy/*.c*") file(GLOB_RECURSE PROJECT_TESTS CONFIGURE_DEPENDS "MicrasFirmware/tests/src/*/*.c*") list(REMOVE_ITEM PROJECT_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/src/main.cpp") +############################################################################### +## Add Micras Simulation Library +############################################################################### + add_library(${PROJECT_NAME}_lib ${LIB_SOURCES} ${SIMULATION_PROXY_SOURCES} @@ -61,9 +92,20 @@ target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ $ + $ $ + $ +) + +target_link_libraries(${PROJECT_NAME}_lib + Boost::system + Boost::thread ) +############################################################################### +## Add main executable +############################################################################### + add_executable(micras_node src/micras_node.cpp ${PROJECT_SOURCES} @@ -77,6 +119,10 @@ target_link_libraries(micras_node add_dependencies(micras_node MicrasPlugin) +############################################################################### +## Install targets and directories +############################################################################### + install(DIRECTORY include DESTINATION include ) @@ -86,6 +132,10 @@ install(DIRECTORY gazebo PATTERN gazebo/micras_plugin EXCLUDE ) +install(DIRECTORY mujoco + DESTINATION share/${PROJECT_NAME} +) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) @@ -106,6 +156,10 @@ install(TARGETS MicrasPlugin DESTINATION share/${PROJECT_NAME}/micras_plugin ) +############################################################################### +## Add Tests +############################################################################### + if(COMPILE_TESTS) foreach(TEST_FILE ${PROJECT_TESTS}) @@ -123,7 +177,9 @@ if(COMPILE_TESTS) ${CMAKE_CURRENT_SOURCE_DIR}/tests/include ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/include ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_nav/include + ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_comm/include ${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware/micras_core/include + ${websocketpp_SOURCE_DIR} ) target_link_libraries(${TEST_NAME} diff --git a/MicrasFirmware b/MicrasFirmware index 4ec774f..ad34254 160000 --- a/MicrasFirmware +++ b/MicrasFirmware @@ -1 +1 @@ -Subproject commit 4ec774f1731b21c3567d50950207498f33d783af +Subproject commit ad342541267bb3c8db263630f27fc7edafba5b09 diff --git a/config/constants.hpp b/config/constants.hpp index d75e318..ef6d49a 100644 --- a/config/constants.hpp +++ b/config/constants.hpp @@ -15,19 +15,20 @@ #include "micras/nav/speed_controller.hpp" namespace micras { -/***************************************** +/*************** * Constants - *****************************************/ + ***************/ +constexpr bool debug_mode{false}; constexpr uint8_t maze_width{16}; constexpr uint8_t maze_height{16}; constexpr float cell_size{0.18}; constexpr uint32_t loop_time_us{1042}; -constexpr float wall_thickness{0.0126F}; -constexpr float start_offset{0.04F + wall_thickness / 2.0F}; -constexpr float exploration_speed{0.5F}; -constexpr float max_linear_acceleration{1.0F}; -constexpr float max_angular_acceleration{200.0F}; +constexpr float wall_thickness{0.012F}; +constexpr float start_offset{0.05F}; +constexpr float max_angular_acceleration{400.0F}; +constexpr float crash_acceleration{1000000.0F}; +constexpr float fan_speed{100.0F}; constexpr core::WallSensorsIndex wall_sensors_index{ .left_front = 0, @@ -36,37 +37,36 @@ constexpr core::WallSensorsIndex wall_sensors_index{ .right_front = 3, }; -/***************************************** +/*************** * Template Instantiations - *****************************************/ + ***************/ namespace nav { using Maze = TMaze; } // namespace nav -/***************************************** +/*************** * Configurations - *****************************************/ + ***************/ const nav::ActionQueuer::Config action_queuer_config{ .cell_size = cell_size, .start_offset = start_offset, + .curve_safety_margin = 0.053F, .exploring = { - .max_linear_speed = exploration_speed, - .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, - .max_centrifugal_acceleration = 2.78F, - .max_angular_acceleration = max_angular_acceleration, + .max_linear_speed = 0.3F, + .max_linear_acceleration = 5.0F, + .max_linear_deceleration = 5.0F, + .max_centrifugal_acceleration = 1.0F, + .max_angular_acceleration = 200.0F, }, .solving = { - .max_linear_speed = exploration_speed, - .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, - .max_centrifugal_acceleration = 1.0F, + .max_linear_speed = 5.0F, + .max_linear_acceleration = 12.0F, + .max_linear_deceleration = 20.0F, + .max_centrifugal_acceleration = 40.0F, .max_angular_acceleration = max_angular_acceleration, }, }; @@ -74,17 +74,19 @@ const nav::ActionQueuer::Config action_queuer_config{ const nav::FollowWall::Config follow_wall_config{ .pid = { - .kp = 0.5F, + .kp = 30.0F, .ki = 0.0F, - .kd = 0.0F, + .kd = 0.008F, .setpoint = 0.0F, .saturation = 1.0F, .max_integral = -1.0F, }, - .max_linear_speed = 0.1F, - .post_threshold = 16.5F, + .wall_sensor_index = wall_sensors_index, + .max_angular_acceleration = max_angular_acceleration, .cell_size = cell_size, - .post_clearance = 0.2F * cell_size, + .post_threshold = 400.0F, + .post_reference = 0.44F * cell_size, + .post_clearance = 0.035F, }; const nav::Maze::Config maze_config{ @@ -95,48 +97,48 @@ const nav::Maze::Config maze_config{ {maze_width / 2, (maze_height - 1) / 2}, {(maze_width - 1) / 2, (maze_height - 1) / 2}, }}, + .cost_margin = 1.2F, + .action_queuer_config = action_queuer_config, }; const nav::Odometry::Config odometry_config{ - .linear_cutoff_frequency = 5.0F, - .wheel_radius = 0.0112F, - .initial_pose = {{0.0F, 0.0F}, 0.0F}, + .linear_cutoff_frequency = 10.0F, + .wheel_radius = 0.011F, + .initial_pose = {{cell_size / 2.0f, start_offset}, std::numbers::pi_v / 2.0f}, }; const nav::SpeedController::Config speed_controller_config{ - .max_linear_acceleration = max_linear_acceleration, - .max_angular_acceleration = max_angular_acceleration, .linear_pid = { - .kp = 10.0F, - .ki = 1.0F, + .kp = 0.0F, + .ki = 10.0F, .kd = 0.0F, .setpoint = 0.0F, - .saturation = 40.0F, + .saturation = 20.0F, .max_integral = -1.0F, }, .angular_pid = { - .kp = 2.0F, - .ki = 1.0F, + .kp = 0.0F, + .ki = 5.0F, .kd = 0.0F, .setpoint = 0.0F, - .saturation = 40.0F, + .saturation = 20.0F, .max_integral = -1.0F, }, .left_feed_forward = { - .linear_speed = 12.706F, - .linear_acceleration = 2.796F, - .angular_speed = -0.971F, - .angular_acceleration = -0.0258F, + .linear_speed = 102.0F, + .linear_acceleration = 0.3F, + .angular_speed = -4.9F, + .angular_acceleration = -0.1F, }, .right_feed_forward = { - .linear_speed = 13.319F, - .linear_acceleration = 2.878F, - .angular_speed = 0.901F, - .angular_acceleration = -0.0244F, + .linear_speed = 102.0F, + .linear_acceleration = 0.3F, + .angular_speed = +4.9F, + .angular_acceleration = +0.1F, }, }; } // namespace micras diff --git a/config/target.hpp b/config/target.hpp index 1890f19..10ac689 100644 --- a/config/target.hpp +++ b/config/target.hpp @@ -25,6 +25,7 @@ #include "micras/proxy/rotary_sensor.hpp" #include "micras/proxy/storage.hpp" #include "micras/proxy/torque_sensors.hpp" +#include "micras/proxy/bluetooth_serial.hpp" // clang-format off namespace micras { @@ -55,36 +56,42 @@ const proxy::Storage::Config maze_storage_config { *****************************************/ const proxy::Led::Config led_config { - micras_node, // node - "led" // topic + .node = micras_node, + .topic = "led" }; const proxy::Argb::Config argb_config { - micras_node, // node - { + .node = micras_node, + .topic_array = { "rgb_0", "rgb_1" - }, // topic_array + } }; const proxy::Button::Config button_config { - micras_node, // node - "button" // topic + .node = micras_node, + .topic = "button", + .long_press_delay = 4000U, + .extra_long_press_delay = 4001U }; const proxy::DipSwitch::Config dip_switch_config { - micras_node, // node - { + .node = micras_node, + .topic_array = { "dip_switch_0", "dip_switch_1", "dip_switch_2", "dip_switch_3" - }, // topic_array + }, }; const proxy::Buzzer::Config buzzer_config { - micras_node, // node - "buzzer" // topic + .node = micras_node, + .topic = "buzzer" +}; + +const proxy::BluetoothSerial::Config bluetooth_config = { + .port = 8080, }; /***************************************** @@ -92,18 +99,18 @@ const proxy::Buzzer::Config buzzer_config { *****************************************/ const proxy::RotarySensor::Config rotary_sensor_left_config { - micras_node, // node - "rotary_sensor_left" // topic + .node = micras_node, + .topic = "sensors/encoder_left" }; const proxy::RotarySensor::Config rotary_sensor_right_config { - micras_node, // node - "rotary_sensor_right" // topic + .node = micras_node, + .topic = "sensors/encoder_right" }; const proxy::TorqueSensors::Config torque_sensors_config { - micras_node, // node - { + .node = micras_node, + .wheel_pairs_topics = { { { "torque_flw", @@ -114,42 +121,44 @@ const proxy::TorqueSensors::Config torque_sensors_config { "torque_rrw" } // front_topic, rear_topic } - }, // wheel_pairs_topics - 0.04F * 20, // shunt_resistor - 0.5F, // max_torque - 3.3F // reference_voltage + }, + .shunt_resistor = 0.04F * 20, + .max_torque = 3.0F, + .reference_voltage = 3.3F }; const proxy::WallSensors::Config wall_sensors_config { - micras_node, // node - { - "wall_sensor_0", - "wall_sensor_1", - "wall_sensor_2", - "wall_sensor_3" - }, // topic_array - 0.3F, // max_distance - 4095, // max_reading - 5.0F, // filter_cutoff - { - 0.413F, - 0.161F, - 0.177F, - 0.230F, - }, // base_readings - 0.5F // uncertainty + .node = micras_node, + .topic_array = { + "sensors/lidar_0", + "sensors/lidar_1", + "sensors/lidar_2", + "sensors/lidar_3" + }, + .uncertainty = 0.5F, + .base_readings = { + 0.0696F, + 0.1090F, + 0.1090F, + 0.0696F, + }, + .max_sensor_reading = 0.6F, + .min_sensor_reading = 0.01F, + .max_sensor_distance = 0.18F * 2, + .filter_cutoff = 20.0F }; const proxy::Imu::Config imu_config { - micras_node, // node - "imu" // topic + .node = micras_node, + .gyro_topic = "sensors/gyro", + .accelerometer_topic = "sensors/accelerometer" }; const proxy::Battery::Config battery_config { - micras_node, // node - "battery", // topic - 9.9F, // max_voltage - 4095 // max_reading + .node = micras_node, + .topic = "battery", + .max_voltage = 9.9F, + .max_reading = 4095 }; @@ -158,13 +167,19 @@ const proxy::Battery::Config battery_config { *****************************************/ const proxy::Fan::Config fan_config { - micras_node, // node - "fan" // topic + .node = micras_node, + .topic = "actuators/fan/command" }; const proxy::Locomotion::Config locomotion_config { - micras_node, // node - "cmd_vel" // topic + .left_motor = { + .node = micras_node, + .topic = "actuators/motor_left/command", + }, + .right_motor = { + .node = micras_node, + .topic = "actuators/motor_right/command" + } }; } // namespace micras diff --git a/gazebo/models/robot.sdf b/gazebo/models/robot.sdf index 86b69e4..82b1604 100644 --- a/gazebo/models/robot.sdf +++ b/gazebo/models/robot.sdf @@ -92,7 +92,7 @@ true imu - " + 0 0 0 0 0 0 lidar_0 100 @@ -120,7 +120,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_1 100 @@ -148,7 +148,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_2 100 @@ -176,7 +176,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_3 100 diff --git a/include/micras/proxy/argb.hpp b/include/micras/proxy/argb.hpp index ebbba0e..a06c7f5 100644 --- a/include/micras/proxy/argb.hpp +++ b/include/micras/proxy/argb.hpp @@ -21,6 +21,7 @@ class TArgb { * @brief Configuration struct for the addressable RGB LED. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::array topic_array; }; diff --git a/include/micras/proxy/battery.hpp b/include/micras/proxy/battery.hpp index 45f057f..6343f75 100644 --- a/include/micras/proxy/battery.hpp +++ b/include/micras/proxy/battery.hpp @@ -19,6 +19,7 @@ class Battery { * @brief Configuration struct for the battery. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; float max_voltage; diff --git a/include/micras/proxy/bluetooth_serial.hpp b/include/micras/proxy/bluetooth_serial.hpp new file mode 100644 index 0000000..d1fe6d4 --- /dev/null +++ b/include/micras/proxy/bluetooth_serial.hpp @@ -0,0 +1,150 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BLUETOOTH_SERIAL_HPP +#define MICRAS_PROXY_BLUETOOTH_SERIAL_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace micras::proxy { + +/** + * @brief Mock class for bluetooth serial that communicates through WebSockets instead of UART + */ +class BluetoothSerial { +public: + using WebSocketServer = websocketpp::server; + using ConnectionPtr = WebSocketServer::connection_ptr; + using ConnectionHdl = websocketpp::connection_hdl; + using MessagePtr = WebSocketServer::message_ptr; + + /** + * @brief Configuration struct for the BluetoothSerial. + */ + struct Config { + uint16_t port = 9002; + }; + + /** + * @brief Construct a new BluetoothSerial object. + * + * @param config Configuration for the BluetoothSerial. + */ + explicit BluetoothSerial(const Config& config); + + /** + * @brief Destructor for BluetoothSerial + */ + ~BluetoothSerial(); + + /** + * @brief Process all received messages and send queued data. + * + * This method handles the actual sending and receiving of data via WebSocket. + * It should be called regularly in the main loop. + */ + void update(); + + /** + * @brief Queue data to be sent during the next update call. + * + * @param data Bytes to be sent over WebSocket. + */ + void send_data(std::vector data); + + /** + * @brief Get data that has been received since the last call. + * + * @return Vector containing received bytes (will be empty if no new data) + */ + std::vector get_data(); + +private: + /** + * @brief Initialize the WebSocket server + */ + void init_websocket_server(); + + /** + * @brief Handle a new WebSocket connection + * + * @param hdl Handle to the connection + */ + void on_open(ConnectionHdl hdl); + + /** + * @brief Handle a closed WebSocket connection + * + * @param hdl Handle to the connection + */ + void on_close(ConnectionHdl hdl); + + /** + * @brief Handle a message received from a WebSocket connection + * + * @param hdl Handle to the connection + * @param msg The received message + */ + void on_message(ConnectionHdl hdl, MessagePtr msg); + + /** + * @brief WebSocket server instance + */ + WebSocketServer server; + + /** + * @brief Thread for the WebSocket server + */ + std::thread server_thread; + + /** + * @brief Mutex to protect the received data + */ + std::mutex rx_mutex; + + /** + * @brief Mutex to protect the data to be sent + */ + std::mutex tx_mutex; + + /** + * @brief Buffer for received data waiting to be retrieved + */ + std::vector received_data; + + /** + * @brief Buffer for data waiting to be sent in the next update + */ + std::vector tx_queue; + + /** + * @brief Set of active connections + */ + std::set> connections; + + /** + * @brief Mutex to protect the connections set + */ + std::mutex connection_mutex; + + /** + * @brief Port on which the WebSocket server listens + */ + uint16_t port; +}; + +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BLUETOOTH_SERIAL_HPP diff --git a/include/micras/proxy/button.hpp b/include/micras/proxy/button.hpp index b1bc3e0..ba8ea43 100644 --- a/include/micras/proxy/button.hpp +++ b/include/micras/proxy/button.hpp @@ -39,6 +39,7 @@ class Button { * @brief Configuration structure for button. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; uint16_t long_press_delay{500}; diff --git a/include/micras/proxy/buzzer.hpp b/include/micras/proxy/buzzer.hpp index 2fbd271..5990e78 100644 --- a/include/micras/proxy/buzzer.hpp +++ b/include/micras/proxy/buzzer.hpp @@ -21,6 +21,7 @@ class Buzzer { * @brief Configuration struct for the buzzer. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; }; diff --git a/include/micras/proxy/dip_switch.hpp b/include/micras/proxy/dip_switch.hpp index 4851e4e..b20e620 100644 --- a/include/micras/proxy/dip_switch.hpp +++ b/include/micras/proxy/dip_switch.hpp @@ -21,6 +21,7 @@ class TDipSwitch { * @brief Configuration struct for the Dip Switch. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::array topic_array; }; diff --git a/include/micras/proxy/fan.hpp b/include/micras/proxy/fan.hpp index 07de347..e49c1a3 100644 --- a/include/micras/proxy/fan.hpp +++ b/include/micras/proxy/fan.hpp @@ -7,9 +7,7 @@ #include #include -#include - -#include "micras/proxy/stopwatch.hpp" +#include namespace micras::proxy { /** @@ -21,6 +19,7 @@ class Fan { * @brief Configuration struct for the fan. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; }; @@ -65,12 +64,12 @@ class Fan { /** * @brief Publisher para controlar a velocidade do ventilador. */ - rclcpp::Publisher::SharedPtr publisher; + rclcpp::Publisher::SharedPtr publisher; /** * @brief Mensagem contendo a velocidade do ventilador. */ - std_msgs::msg::Float32 message; + example_interfaces::msg::Float64 message; /** * @brief Flag para verificar se o ventilador está habilitado. diff --git a/include/micras/proxy/imu.hpp b/include/micras/proxy/imu.hpp index a11c9f5..47647af 100644 --- a/include/micras/proxy/imu.hpp +++ b/include/micras/proxy/imu.hpp @@ -7,9 +7,9 @@ #include #include +#include #include #include -#include namespace micras::proxy { /** @@ -21,8 +21,10 @@ class Imu { * @brief IMU configuration struct. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; - std::string topic; + std::string gyro_topic; + std::string accelerometer_topic; }; /** @@ -78,12 +80,14 @@ class Imu { /** * @brief Subscriber para os dados do IMU. */ - rclcpp::Subscription::SharedPtr subscriber; + rclcpp::Subscription::SharedPtr gyro_subscriber; + rclcpp::Subscription::SharedPtr accelerometer_subscriber; /** * @brief Dados do IMU recebidos. */ - sensor_msgs::msg::Imu data; + example_interfaces::msg::Float64MultiArray gyro_data; + example_interfaces::msg::Float64MultiArray accelerometer_data; /** * @brief Current angular velocity on each axis. diff --git a/include/micras/proxy/led.hpp b/include/micras/proxy/led.hpp index 549a4a6..6d44382 100644 --- a/include/micras/proxy/led.hpp +++ b/include/micras/proxy/led.hpp @@ -18,6 +18,7 @@ class Led { * @brief Configuration struct for LED. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; }; diff --git a/include/micras/proxy/locomotion.hpp b/include/micras/proxy/locomotion.hpp index 8687cda..0c8bc6d 100644 --- a/include/micras/proxy/locomotion.hpp +++ b/include/micras/proxy/locomotion.hpp @@ -9,6 +9,8 @@ #include #include +#include "micras/proxy/motor.hpp" + namespace micras::proxy { /** * @brief Class for controlling the locomotion driver. @@ -19,8 +21,9 @@ class Locomotion { * @brief Configuration struct for the locomotion. */ struct Config { - std::shared_ptr& node; - std::string topic; + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) + Motor::Config left_motor; + Motor::Config right_motor; }; /** @@ -63,17 +66,17 @@ class Locomotion { private: /** - * @brief Publisher para enviar comandos de movimento. + * @brief Left motor of the robot. */ - rclcpp::Publisher::SharedPtr publisher; + Motor left_motor; /** - * @brief Mensagem twist para controle de movimento. + * @brief Right motor of the robot. */ - geometry_msgs::msg::Twist twist; + Motor right_motor; /** - * @brief Estado de habilitação dos motores. + * @brief Flag to indicate if the locomotion is enabled. */ bool enabled{false}; }; diff --git a/include/micras/proxy/motor.hpp b/include/micras/proxy/motor.hpp index ce1aec2..b90b898 100644 --- a/include/micras/proxy/motor.hpp +++ b/include/micras/proxy/motor.hpp @@ -5,7 +5,8 @@ #ifndef MICRAS_PROXY_MOTOR_HPP #define MICRAS_PROXY_MOTOR_HPP -#include "micras/hal/pwm.hpp" +#include +#include namespace micras::proxy { /** @@ -17,10 +18,9 @@ class Motor { * @brief Configuration struct for the motor. */ struct Config { - hal::Pwm::Config backwards_pwm; - hal::Pwm::Config forward_pwm; - float max_stopped_command; - float deadzone; + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) + std::shared_ptr& node; + std::string topic; }; /** @@ -39,24 +39,14 @@ class Motor { private: /** - * @brief PWM object for controlling the motor in the backwards direction. + * @brief Publisher for the motor command. */ - hal::Pwm backwards_pwm; + rclcpp::Publisher::SharedPtr publisher; /** - * @brief PWM object for controlling the motor in the forward direction. + * @brief Command message for the motor. */ - hal::Pwm forward_pwm; - - /** - * @brief Maximum command value for the motor to be considered stopped. - */ - float max_stopped_command; - - /** - * @brief Deadzone for the motor. - */ - float deadzone; + example_interfaces::msg::Float64 command; }; } // namespace micras::proxy diff --git a/include/micras/proxy/rotary_sensor.hpp b/include/micras/proxy/rotary_sensor.hpp index 36d23df..79f4b16 100644 --- a/include/micras/proxy/rotary_sensor.hpp +++ b/include/micras/proxy/rotary_sensor.hpp @@ -6,8 +6,8 @@ #define MICRAS_PROXY_ROTARY_SENSOR_HPP #include +#include #include -#include namespace micras::proxy { /** @@ -19,6 +19,7 @@ class RotarySensor { * @brief Rotary sensor configuration struct. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::string topic; }; @@ -39,14 +40,14 @@ class RotarySensor { private: /** - * @brief Subscriber para os dados do sensor rotativo. + * @brief Subscription to the rotary sensor topic. */ - rclcpp::Subscription::SharedPtr subscriber; + rclcpp::Subscription::SharedPtr subscriber; /** - * @brief Dados do sensor rotativo recebidos. + * @brief Received data from the rotary sensor. */ - sensor_msgs::msg::JointState data; + mutable double data{0.0}; }; } // namespace micras::proxy diff --git a/include/micras/proxy/stopwatch.hpp b/include/micras/proxy/stopwatch.hpp index b9022ea..a6fe970 100644 --- a/include/micras/proxy/stopwatch.hpp +++ b/include/micras/proxy/stopwatch.hpp @@ -6,6 +6,7 @@ #define MICRAS_PROXY_STOPWATCH_HPP #include +#include #include namespace micras::proxy { @@ -17,9 +18,7 @@ class Stopwatch { /** * @brief Stopwatch configuration struct. */ - struct Config { - // Configurações específicas para ROS 2 poderiam ser adicionadas aqui - }; + struct Config { }; /** * @brief Construct a new Stopwatch object. @@ -90,6 +89,16 @@ class Stopwatch { * @brief Stopwatch counter. */ uint32_t counter{}; + + /** + * @brief Subscription to the clock topic. + */ + rclcpp::Subscription::SharedPtr clock_subscriber; + + /** + * @brief Last clock message received (shared between all instances). + */ + static inline builtin_interfaces::msg::Time clock_msg{}; }; } // namespace micras::proxy diff --git a/include/micras/proxy/torque_sensors.hpp b/include/micras/proxy/torque_sensors.hpp index 6668e05..53b6539 100644 --- a/include/micras/proxy/torque_sensors.hpp +++ b/include/micras/proxy/torque_sensors.hpp @@ -21,10 +21,12 @@ class TTorqueSensors { * @brief Estrutura para armazenar dados de um par de rodas. */ struct WheelPair { + // NOLINTBEGIN(cppcoreguidelines-avoid-const-or-ref-data-members) rclcpp::Subscription::SharedPtr front_subscriber; rclcpp::Subscription::SharedPtr rear_subscriber; - float front_torque{0.0f}; - float rear_torque{0.0f}; + // NOLINTEND(cppcoreguidelines-avoid-const-or-ref-data-members) + float front_torque{0.0f}; + float rear_torque{0.0f}; }; /** diff --git a/include/micras/proxy/wall_sensors.hpp b/include/micras/proxy/wall_sensors.hpp index 999ced3..dfadc2b 100644 --- a/include/micras/proxy/wall_sensors.hpp +++ b/include/micras/proxy/wall_sensors.hpp @@ -7,8 +7,8 @@ #include #include +#include #include -#include #include "micras/core/butterworth_filter.hpp" @@ -23,13 +23,15 @@ class TWallSensors { * @brief Configuration struct for wall sensors. */ struct Config { + // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members) std::shared_ptr& node; std::array topic_array; - float max_distance; - uint16_t max_reading; - float filter_cutoff; - std::array base_readings; float uncertainty; + std::array base_readings; + float max_sensor_reading; + float min_sensor_reading; + float max_sensor_distance; + float filter_cutoff; }; /** @@ -96,33 +98,23 @@ class TWallSensors { /** * @brief Array of subscribers for distance sensors. */ - std::array::SharedPtr, num_of_sensors> subscribers; + std::array::SharedPtr, num_of_sensors> subscribers; /** * @brief Array of distances measured by the sensors. */ - std::array readings; + mutable std::array readings; /** * @brief Maximum distance detectable by the sensor. */ - const float max_distance; - - /** - * @brief Maximum ADC reading. - */ - const uint16_t max_reading; + const float max_sensor_reading; /** * @brief LED state. */ bool leds_on{true}; - // /** - // * @brief Uncertainty of the wall sensors. - // */ - // float uncertainty{0.1f}; - /** * @brief Measured wall value during calibration. */ @@ -145,10 +137,6 @@ class TWallSensors { /** * @brief Buffer to store the ADC values. - * - * - * - * */ std::array buffer; @@ -166,6 +154,11 @@ class TWallSensors { * @brief Ratio of the base reading to still consider as seeing a wall. */ float uncertainty; + + /** + * @brief Constant value for the wall sensor. + */ + float constant; }; } // namespace micras::proxy diff --git a/launch/include/mujoco_world.launch.xml b/launch/include/mujoco_world.launch.xml new file mode 100644 index 0000000..28826c7 --- /dev/null +++ b/launch/include/mujoco_world.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/launch/include/node.launch.xml b/launch/include/node.launch.xml index 38c8db1..8520521 100644 --- a/launch/include/node.launch.xml +++ b/launch/include/node.launch.xml @@ -1,5 +1,10 @@ + - + + + diff --git a/launch/micras.launch.xml b/launch/micras.launch.xml index 08720f8..7875bdf 100644 --- a/launch/micras.launch.xml +++ b/launch/micras.launch.xml @@ -1,9 +1,11 @@ + + diff --git a/launch/mujoco_micras.launch.xml b/launch/mujoco_micras.launch.xml new file mode 100644 index 0000000..9709ec8 --- /dev/null +++ b/launch/mujoco_micras.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mujoco/models/gen_maze.py b/mujoco/models/gen_maze.py new file mode 100755 index 0000000..cd73827 --- /dev/null +++ b/mujoco/models/gen_maze.py @@ -0,0 +1,276 @@ +#!/usr/bin/env python3 + +import sys +import argparse +from enum import StrEnum +from pathlib import Path + +CELL_CHAR_WIDTH = 4 +CELL_CHAR_HEIGHT = 2 + +class MazeSymbol(StrEnum): + POST = 'o' + WALL_HORIZONTAL = '---' + WALL_HORIZONTAL_EMPTY = ' ' + WALL_VERTICAL = '|' + WALL_VERTICAL_EMPTY = '' + START = 'S' + GOAL = 'G' + +class MazeParser: + def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): + self.cell_size = cell_size + self.wall_thickness = wall_thickness + self.wall_height = wall_height + self.width = 0 + self.height = 0 + self.start = None + self.goals = [] + + def parse_maze_file(self, maze_file: str) -> list[str]: + with open(maze_file, 'r') as f: + lines = [line.rstrip() for line in f.readlines()] + + if not lines: + raise ValueError("Empty maze file") + + self.maze_width = (len(lines[0]) - 1) // CELL_CHAR_WIDTH + self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT + + print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") + return lines + + def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: + horizontal_walls = [] + for i in range(0, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j + 1:j + 4] == MazeSymbol.WALL_HORIZONTAL: + row.append(True) + else: + row.append(False) + horizontal_walls.append(row) + return horizontal_walls + + + def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: + vertical_walls = [] + for i in range(1, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j] == MazeSymbol.WALL_VERTICAL: + row.append(True) + else: + row.append(False) + vertical_walls.append(row) + return vertical_walls + + def find_start_and_goals(self, lines: list[str]) -> tuple[tuple[int, int], list[tuple[int, int]]]: + start = None + goals = [] + + row = 0 + col = 0 + + inverted_lines = lines[::-1] + + for i in range(1, len(inverted_lines), 2): + for j in range(2, len(inverted_lines[i]), 4): + if inverted_lines[i][j] == MazeSymbol.START: + if start is not None: + raise ValueError("Multiple start points found") + start = (row, col) + elif inverted_lines[i][j] == MazeSymbol.GOAL: + goals.append((row, col)) + col += 1 + row += 1 + col = 0 + + self.start = start + self.goals = goals + + return start, goals + + def is_start_or_goal(self, row: int, col: int) -> bool: + if (row, col) == self.start: + return True + if (row, col) in self.goals: + return True + return False + + def is_post_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row, col) or \ + self.is_start_or_goal(row - 1, col) or \ + self.is_start_or_goal(row, col - 1) or \ + self.is_start_or_goal(row - 1, col - 1): + return True + + return False + + def is_horizontal_wall_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row - 1, col) or \ + self.is_start_or_goal(row, col): + return True + + return False + + def is_vertical_wall_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row, col) or \ + self.is_start_or_goal(row, col - 1): + return True + + return False + + def generate_posts_xml(self) -> list[str]: + xml = [] + + for row in range(self.maze_height + 1): + for col in range(self.maze_width + 1): + x = col * self.cell_size + y = row * self.cell_size + + class_name = 'post_w' if self.is_post_on_start_cell(row, col) else 'post' + xml.append( + f' ' + ) + return xml + + def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: + xml = [] + inverted_horizontal_walls = horizontal_walls[::-1] + + for row in range(len(inverted_horizontal_walls)): + for col in range(len(inverted_horizontal_walls[row])): + if inverted_horizontal_walls[row][col]: + x = col * self.cell_size + self.cell_size / 2 + y = row * self.cell_size + + class_name = 'hwall_w' if self.is_horizontal_wall_on_start_cell(row, col) else 'hwall' + xml.append( + f' ' + ) + return xml + + def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: + xml = [] + inverted_vertical_walls = vertical_walls[::-1] + + for row in range(len(inverted_vertical_walls)): + for col in range(len(inverted_vertical_walls[row])): + if inverted_vertical_walls[row][col]: + x = col * self.cell_size + y = row * self.cell_size + self.cell_size / 2 + + class_name = 'vwall_w' if self.is_vertical_wall_on_start_cell(row, col) else 'vwall' + xml.append( + f' ' + ) + return xml + + def add_header(self) -> str: + xml = [] + xml.append('') + xml.append(f'') + xml.append(f'\n') + return '\n'.join(xml) + + def generate_assets(self) -> str: + xml = [] + xml.append(' ') + xml.append(' ') + xml.append(' ') + xml.append(' \n') + return '\n'.join(xml) + + def generate_default_class(self) -> str: + xml = [] + red_wall_parameters = 'type="box" zaxis="0 1 0" material="walls"' + white_wall_parameters = 'type="box" zaxis="0 1 0" rgba="1 1 1 1"' + length = (self.cell_size - self.wall_thickness) / 2 + height = self.wall_height / 2 + width = self.wall_thickness / 2 + + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' \n') + return '\n'.join(xml) + + def generate_top_camera_view(self) -> str: + xml = [] + maze_center = (self.maze_width * self.cell_size / 2, self.maze_height * self.cell_size / 2) + xml.append(f' ') + return '\n'.join(xml) + + def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: + xml_lines = [] + xml_lines.append(self.add_header()) + xml_lines.append('') + xml_lines.append(self.generate_assets()) + xml_lines.append(self.generate_default_class()) + xml_lines.append(' ') + xml_lines.append(self.generate_top_camera_view()) + xml_lines.extend(self.generate_posts_xml()) + xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) + xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) + xml_lines.append(' ') + xml_lines.append('') + return '\n'.join(xml_lines) + +def main(): + parser = argparse.ArgumentParser(description="Generate MuJoCo XML maze walls from text maze file") + parser.add_argument("maze_file", help="Path to the maze text file") + parser.add_argument("-o", "--output", help="Output XML file (default: maze.xml)") + parser.add_argument("--cell-size", type=float, default=0.18, help="Cell size in meters (default: 0.18)") + parser.add_argument("--wall-thickness", type=float, default=0.0126, help="Wall thickness in meters (default: 0.0126)") + parser.add_argument("--wall-height", type=float, default=0.05, help="Wall height in meters (default: 0.05)") + + args = parser.parse_args() + + if not Path(args.maze_file).exists(): + print(f"Error: Maze file '{args.maze_file}' not found") + sys.exit(1) + + maze_file_path = Path(args.maze_file) + output_file = args.output or (maze_file_path.parent / "maze.xml") + + try: + maze_parser = MazeParser(args.cell_size, args.wall_thickness) + lines = maze_parser.parse_maze_file(args.maze_file) + horizontal_walls = maze_parser.extract_horizontal_walls(lines) + vertical_walls = maze_parser.extract_vertical_walls(lines) + start, goals = maze_parser.find_start_and_goals(lines) + print(f"Start point: {start}, Goals: {goals}") + xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) + + with open(output_file, 'w') as f: + f.write(xml_content) + + print(f"Generated XML maze walls saved to '{output_file}'") + + except ValueError as e: + print(f"Error parsing maze file: {e}") + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/mujoco/models/maze.txt b/mujoco/models/maze.txt new file mode 100644 index 0000000..85890fb --- /dev/null +++ b/mujoco/models/maze.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o---o---o---o o---o o---o---o---o---o o---o o o o +| | | | | | | +o o---o---o o---o---o o o o---o o o o---o---o o +| | | | | | | +o o---o o---o---o---o o o---o---o o o---o o o---o +| | | | | +o o---o---o---o---o---o o---o---o---o o o---o---o---o o +| | | | | +o o---o---o---o---o o---o o---o o o o---o---o o o +| | | | | | | | +o o o---o---o---o---o o---o o---o---o o o---o o o +| | | | | | | | | +o o o o---o---o---o o---o---o---o o o o---o o o +| | | | | G G | | | | | | +o o o---o---o---o o o o o o o o---o---o o o +| | | | G G | | | | +o o---o---o---o---o---o o---o---o o o o---o o---o o +| | | | | +o o---o---o---o o o---o---o---o---o---o---o o o o o +| | | | | | | | | | +o o o---o---o o o o o o---o---o o o o o o +| | | | | | | | | | +o---o---o---o o o o o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o o o o o o o---o o o o o o o +| | | | | | | | | +o o o---o---o o o---o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o---o o o o o---o---o o o o o o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/mujoco/models/maze.xml b/mujoco/models/maze.xml new file mode 100644 index 0000000..35bca6f --- /dev/null +++ b/mujoco/models/maze.xml @@ -0,0 +1,582 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/mujoco/models/robot.xml b/mujoco/models/robot.xml new file mode 100644 index 0000000..6fef509 --- /dev/null +++ b/mujoco/models/robot.xml @@ -0,0 +1,106 @@ + + diff --git a/package.xml b/package.xml index fb8cf6b..177b60d 100644 --- a/package.xml +++ b/package.xml @@ -6,7 +6,7 @@ TODO: Package description santi - + cosme TODO: License declaration @@ -17,6 +17,7 @@ sensor_msgs geometry_msgs nav_msgs + example_interfaces ament_lint_auto ament_lint_common diff --git a/plot_config.xml b/plot_config.xml new file mode 100644 index 0000000..2dd66b0 --- /dev/null +++ b/plot_config.xml @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/pause_gazebo.py b/scripts/pause_gazebo.py new file mode 100644 index 0000000..d377d2c --- /dev/null +++ b/scripts/pause_gazebo.py @@ -0,0 +1,20 @@ +import gdb +import os +# gz service -s /world/maze/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --req 'pause: true' + +def set_gazebo_paused(pause): + pause_command = "true" if pause else "false" + command = f"gz service -s /world/maze/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --req 'pause: {pause_command}'" + print(f"Executing command: {command}") + os.system(command) + +def stop(ev): + print("Stopping Gazebo...") + set_gazebo_paused(True) + +def cont(ev): + print("Continuing Gazebo...") + set_gazebo_paused(False) + +gdb.events.stop.connect(stop) +gdb.events.cont.connect(cont) diff --git a/src/micras_node.cpp b/src/micras_node.cpp index 9604756..2d113bc 100644 --- a/src/micras_node.cpp +++ b/src/micras_node.cpp @@ -20,7 +20,7 @@ int main(int argc, char** argv) { micras::Micras micras; try { - auto timer = main_node->create_wall_timer(10ms, [&micras]() { micras.update(); }); + auto timer = main_node->create_wall_timer(1ms, [&micras]() { micras.update(); }); executor.add_node(main_node); executor.add_node(micras::micras_node); diff --git a/src/proxy/bluetooth_serial.cpp b/src/proxy/bluetooth_serial.cpp new file mode 100644 index 0000000..ad5b484 --- /dev/null +++ b/src/proxy/bluetooth_serial.cpp @@ -0,0 +1,147 @@ +/** + * @file + */ + +#include "micras/proxy/bluetooth_serial.hpp" + +#include +#include +#include + +namespace micras::proxy { + +BluetoothSerial::BluetoothSerial(const Config& config) : port{config.port} { + init_websocket_server(); +} + +BluetoothSerial::~BluetoothSerial() { + this->server.stop(); + + if (this->server.is_listening()) { + this->server.stop_listening(); + } + + { + std::lock_guard lock(this->connection_mutex); + for (auto& hdl : this->connections) { + try { + this->server.close(hdl, websocketpp::close::status::going_away, "Server shutting down"); + } catch (const std::exception& e) { + std::cerr << "Error closing connection: " << e.what() << std::endl; + } + } + this->connections.clear(); + } + + if (this->server_thread.joinable()) { + this->server_thread.join(); + } +} + +void BluetoothSerial::init_websocket_server() { + try { + this->server.clear_access_channels(websocketpp::log::alevel::all); + this->server.set_error_channels(websocketpp::log::elevel::fatal); + + this->server.init_asio(); + + this->server.set_reuse_addr(true); + + this->server.set_open_handler(std::bind(&BluetoothSerial::on_open, this, std::placeholders::_1)); + this->server.set_close_handler(std::bind(&BluetoothSerial::on_close, this, std::placeholders::_1)); + this->server.set_message_handler( + std::bind(&BluetoothSerial::on_message, this, std::placeholders::_1, std::placeholders::_2) + ); + + this->server.listen(this->port); + this->server.start_accept(); + + this->server_thread = std::thread([this]() { + try { + this->server.run(); + } catch (const std::exception& e) { + std::cerr << "WebSocket server error: " << e.what() << std::endl; + } + }); + + std::cout << "WebSocket server started on port " << this->port; + std::cout << " -> ws://localhost:" << this->port << std::endl; + } catch (const websocketpp::exception& e) { + std::cerr << "WebSocket initialization error: " << e.what() << std::endl; + throw; + } catch (const std::exception& e) { + std::cerr << "General error during WebSocket initialization: " << e.what() << std::endl; + throw; + } +} + +void BluetoothSerial::update() { + std::vector bytes_to_send; + + { + std::lock_guard lock(this->tx_mutex); + if (!this->tx_queue.empty()) { + bytes_to_send = std::move(this->tx_queue); + this->tx_queue.clear(); + } + } + + if (!bytes_to_send.empty()) { + std::lock_guard lock(this->connection_mutex); + for (auto& hdl : this->connections) { + try { + auto connection = this->server.get_con_from_hdl(hdl); + if (connection->get_state() == websocketpp::session::state::open) { + this->server.send( + hdl, bytes_to_send.data(), bytes_to_send.size(), websocketpp::frame::opcode::binary + ); + } + } catch (const std::exception& e) { + std::cerr << "Error sending data: " << e.what() << std::endl; + } + } + } +} + +void BluetoothSerial::send_data(std::vector data) { + std::lock_guard lock(this->tx_mutex); + this->tx_queue.insert(this->tx_queue.end(), data.begin(), data.end()); +} + +std::vector BluetoothSerial::get_data() { + std::lock_guard lock(this->rx_mutex); + std::vector data = std::move(this->received_data); + this->received_data.clear(); + return data; +} + +void BluetoothSerial::on_open(ConnectionHdl hdl) { + std::lock_guard lock(this->connection_mutex); + this->connections.insert(hdl); + std::cout << "WebSocket connection opened. Total connections: " << this->connections.size() << std::endl; +} + +void BluetoothSerial::on_close(ConnectionHdl hdl) { + std::lock_guard lock(this->connection_mutex); + this->connections.erase(hdl); + std::cout << "WebSocket connection closed. Total connections: " << this->connections.size() << std::endl; +} + +void BluetoothSerial::on_message(ConnectionHdl /*hdl*/, MessagePtr msg) { + auto payload = msg->get_payload(); + + { + std::lock_guard lock(rx_mutex); + + if (msg->get_opcode() == websocketpp::frame::opcode::binary) { + this->received_data.insert( + this->received_data.end(), reinterpret_cast(payload.data()), + reinterpret_cast(payload.data() + payload.size()) + ); + } else { + this->received_data.insert(this->received_data.end(), payload.begin(), payload.end()); + } + } +} + +} // namespace micras::proxy diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index 1e61e68..c00ffe9 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -6,7 +6,7 @@ namespace micras::proxy { Fan::Fan(const Config& config) { - this->publisher = config.node->create_publisher(config.topic, 1); + this->publisher = config.node->create_publisher(config.topic, 1); this->stop(); this->enable(); } @@ -19,7 +19,7 @@ void Fan::enable() { void Fan::disable() { this->enabled = false; - std_msgs::msg::Float32 stopped_message{}; + example_interfaces::msg::Float64 stopped_message{}; stopped_message.data = 0.0f; this->publisher->publish(stopped_message); diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index bb1d555..7cc2a60 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -8,18 +8,33 @@ namespace micras::proxy { Imu::Imu(const Config& config) { - this->subscriber = config.node->create_subscription( - config.topic, 1, [this](const sensor_msgs::msg::Imu& msg) { this->data = msg; } + this->gyro_subscriber = config.node->create_subscription( + config.gyro_topic, 1, [](const example_interfaces::msg::Float64MultiArray& /*msg*/) {} ); + + this->accelerometer_subscriber = config.node->create_subscription( + config.accelerometer_topic, 1, [](const example_interfaces::msg::Float64MultiArray& /*msg*/) {} + ); + + this->angular_velocity.fill(0.0F); + this->linear_acceleration.fill(0.0F); } void Imu::update() { - this->angular_velocity[0] = this->data.angular_velocity.x; - this->angular_velocity[1] = this->data.angular_velocity.y; - this->angular_velocity[2] = this->data.angular_velocity.z; - this->linear_acceleration[0] = this->data.linear_acceleration.x; - this->linear_acceleration[1] = this->data.linear_acceleration.y; - this->linear_acceleration[2] = this->data.linear_acceleration.z; + example_interfaces::msg::Float64MultiArray msg; + rclcpp::MessageInfo info; + + if (this->gyro_subscriber->take(msg, info)) { + this->angular_velocity[0] = msg.data[0]; + this->angular_velocity[1] = msg.data[1]; + this->angular_velocity[2] = msg.data[2]; + } + + if (this->accelerometer_subscriber->take(msg, info)) { + this->linear_acceleration[0] = msg.data[0]; + this->linear_acceleration[1] = msg.data[1]; + this->linear_acceleration[2] = msg.data[2]; + } } float Imu::get_angular_velocity(Axis axis) const { diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 751b009..b4ee714 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -5,8 +5,7 @@ #include "micras/proxy/locomotion.hpp" namespace micras::proxy { -Locomotion::Locomotion(const Config& config) { - this->publisher = config.node->create_publisher(config.topic, 1); +Locomotion::Locomotion(const Config& config) : left_motor{config.left_motor}, right_motor{config.right_motor} { this->stop(); this->disable(); } @@ -20,26 +19,28 @@ void Locomotion::disable() { } void Locomotion::set_wheel_command(float left_command, float right_command) { - this->twist.linear.x = (left_command + right_command) / 2; - this->twist.angular.z = (right_command - left_command) / 2; - - if (this->enabled) { - this->publisher->publish(this->twist); - } + this->left_motor.set_command(left_command); + this->right_motor.set_command(right_command); } void Locomotion::set_command(float linear, float angular) { - this->twist.linear.x = linear; - this->twist.angular.z = angular; + float left_command = linear - angular; + float right_command = linear + angular; - if (this->enabled) { - this->publisher->publish(this->twist); + if (std::abs(left_command) > 100.0F) { + left_command *= 100.0F / std::abs(left_command); + right_command *= 100.0F / std::abs(left_command); } + + if (std::abs(right_command) > 100.0F) { + left_command *= 100.0F / std::abs(right_command); + right_command *= 100.0F / std::abs(right_command); + } + + this->set_wheel_command(left_command, right_command); } void Locomotion::stop() { - this->twist.linear.x = 0.0; - this->twist.angular.z = 0.0; - this->publisher->publish(this->twist); + this->set_wheel_command(0.0F, 0.0F); } } // namespace micras::proxy diff --git a/src/proxy/motor.cpp b/src/proxy/motor.cpp new file mode 100644 index 0000000..6b8f5cd --- /dev/null +++ b/src/proxy/motor.cpp @@ -0,0 +1,20 @@ +/** + * @file + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/proxy/motor.hpp" + +namespace micras::proxy { +Motor::Motor(const Config& config) { + this->publisher = config.node->create_publisher(config.topic, 1); + this->set_command(0.0F); +} + +void Motor::set_command(float command) { + this->command.data = static_cast(std::clamp(command, -100.0F, 100.0F)); + this->publisher->publish(this->command); +} +} // namespace micras::proxy diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index adef0cf..3d858ad 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -6,19 +6,19 @@ namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) { - this->subscriber = config.node->create_subscription( - config.topic, 1, - [this](const sensor_msgs::msg::JointState& msg) { - if (!msg.position.empty()) { - this->data = msg; - } - } + this->subscriber = config.node->create_subscription( + config.topic, 1, [](const example_interfaces::msg::Float64& /*msg*/) {} ); - - this->data.position.resize(1, 0.0); } float RotarySensor::get_position() const { - return this->data.position[0]; + example_interfaces::msg::Float64 msg; + rclcpp::MessageInfo info; + + if (this->subscriber->take(msg, info)) { + this->data = msg.data; + } + + return static_cast(this->data); } } // namespace micras::proxy diff --git a/src/proxy/stopwatch.cpp b/src/proxy/stopwatch.cpp index 5aa928b..2deb4ce 100644 --- a/src/proxy/stopwatch.cpp +++ b/src/proxy/stopwatch.cpp @@ -3,16 +3,22 @@ */ #include "micras/proxy/stopwatch.hpp" - +#include "target.hpp" #include #include namespace micras::proxy { + Stopwatch::Stopwatch() { + this->clock_subscriber = micras_node->create_subscription( + "clock", 1, [](const builtin_interfaces::msg::Time& /*msg*/) {} + ); + clock_msg.sec = 0; + clock_msg.nanosec = 0; this->reset_ms(); } -Stopwatch::Stopwatch(const Config& /*config*/) { +Stopwatch::Stopwatch(const Config& /*config*/) : Stopwatch() { this->reset_us(); } @@ -39,18 +45,28 @@ uint32_t Stopwatch::elapsed_time_us() const { } void Stopwatch::sleep_ms(uint32_t time) { - rclcpp::sleep_for(std::chrono::milliseconds(time)); + // rclcpp::sleep_for(std::chrono::milliseconds(time)); } void Stopwatch::sleep_us(uint32_t time) const { - rclcpp::sleep_for(std::chrono::microseconds(time)); + // rclcpp::sleep_for(std::chrono::microseconds(time)); } uint32_t Stopwatch::get_counter_ms() const { - return static_cast(rclcpp::Clock().now().nanoseconds() / 1000000); + builtin_interfaces::msg::Time msg; + rclcpp::MessageInfo info; + if (this->clock_subscriber->take(msg, info)) { + clock_msg = msg; + } + return static_cast(clock_msg.sec * 1000 + clock_msg.nanosec / 1000000); } uint32_t Stopwatch::get_counter_us() const { - return static_cast(rclcpp::Clock().now().nanoseconds() / 1000); + builtin_interfaces::msg::Time msg; + rclcpp::MessageInfo info; + if (this->clock_subscriber->take(msg, info)) { + clock_msg = msg; + } + return static_cast(clock_msg.sec * 1000000 + clock_msg.nanosec / 1000); } } // namespace micras::proxy diff --git a/src/proxy/wall_sensors.cpp b/src/proxy/wall_sensors.cpp index 67bcf6a..d6adc7c 100644 --- a/src/proxy/wall_sensors.cpp +++ b/src/proxy/wall_sensors.cpp @@ -11,22 +11,20 @@ namespace micras::proxy { template TWallSensors::TWallSensors(const Config& config) : - max_distance{config.max_distance}, - max_reading{config.max_reading}, + max_sensor_reading{config.max_sensor_reading}, leds_on{false}, filters{core::make_array(config.filter_cutoff)}, base_readings{config.base_readings}, - uncertainty{config.uncertainty} { + uncertainty{config.uncertainty}, + constant{static_cast( + -std::pow(config.max_sensor_distance, 2) * + std::log(1.0f - config.min_sensor_reading / config.max_sensor_reading) + )} { this->turn_off(); for (uint8_t i = 0; i < num_of_sensors; i++) { - this->subscribers[i] = config.node->template create_subscription( - config.topic_array[i], 1, - [this, i](const sensor_msgs::msg::LaserScan& msg) { - if (!msg.ranges.empty()) { - this->readings[i] = this->leds_on ? msg.ranges[0] : 0; - } - } + this->subscribers[i] = config.node->template create_subscription( + config.topic_array[i], 1, [](const example_interfaces::msg::Float64& /*msg*/) {} ); } } @@ -38,7 +36,7 @@ void TWallSensors::turn_on() { template void TWallSensors::turn_off() { - this->leds_on = false; + this->leds_on = true; } template @@ -61,12 +59,22 @@ float TWallSensors::get_reading(uint8_t sensor_index) const { template float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - return this->readings.at(sensor_index) / this->max_distance * this->max_reading; + example_interfaces::msg::Float64 msg; + rclcpp::MessageInfo info; + + if (this->subscribers[sensor_index]->take(msg, info)) { + this->readings.at(sensor_index) = msg.data; + } + float raw_reading = this->readings.at(sensor_index); + + if (raw_reading < 0.0F) { + return 0.0F; + } + + float intensity = 1 / std::pow(raw_reading, 2.0F); + float reading = this->max_sensor_reading * (1 - std::exp(-this->constant * intensity)); - // return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + - // num_of_sensors)) - // ) / - // this->adc.get_max_reading(); + return reading; } template