diff --git a/README.md b/README.md index fcad3cc6..8ff81eef 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,12 @@ It provides different kind of exclusive applications, used for educational purpo - [The ZumoComSystem Shield](#the-zumocomsystem-shield) - [The simulation](#the-simulation) -- [Installation for simulation](#installation-for-simulation) + - [Installation for simulation](#installation-for-simulation) - [How to start?](#how-to-start) + - [Run](#run) + - [Run with Webots launcher (recommended)](#run-with-webots-launcher-recommended) + - [Run without Webots launcher](#run-without-webots-launcher) + - [Run via terminal](#run-via-terminal) - [The target](#the-target) - [Installation for target](#installation-for-target) - [Build and flash procedure](#build-and-flash-procedure) @@ -48,7 +52,7 @@ The simulation is based on the open source robot simulator *Webots*. The applica ![simulation-deployment](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/DroidControlShip/main/doc/architecture/uml/PhysicalView/SimulationDeployment.plantuml) -## Installation for simulation +### Installation for simulation 1. Install the native compiler toolchain: - Linux @@ -64,7 +68,7 @@ The simulation is based on the open source robot simulator *Webots*. The applica ### How to start? -After you built the application, you will find in in ```.pio/build//program.exe```. It provides several command line arguments to configure certain features. Use -h or --help to get a short user friendly overview about them. +After you built the application, you will find it in ```.pio/build//program[.exe]```. It provides several command line arguments to configure certain features. Use -h or --help to get a short user friendly overview about them. The applications are using a configuration file in JSON format to retrieve certain settings. In the very first run, such a configuration file will be automatically be created. If there exists already one, it will be loaded without modifications (regardless of any other program arguments). @@ -78,6 +82,42 @@ MQTT Broker --> Webots World --> RadonUlzer --> DroidControlShip In order to simplify this process, the [Launcher](https://github.com/gabryelreyes/Launcher) project is under active development. +### Run + +There are 3 ways how to run now the application. Choose according to your needs. + +#### Run with Webots launcher (recommended) + +The Webots launcher is recommended to connect to the simulation. + +It is mandatory if the simulation contains more than one robot. The robot is identified by its name. Adapt the robot name in the [platformio_override.ini](./platformio_override.ini), see *webots_robot_name*. + +The Webots documentation has more details about [Single Simulation and Multiple Local Extern Robot Controllers](https://cyberbotics.com/doc/guide/running-extern-robot-controllers?tab-os=windows#single-simulation-and-multiple-local-extern-robot-controllers). + +It is mandatory too if the simulation does not run locally (e.g. Webots runs on windows host and RadonUlzer in WSL). +Set the IP address and the port in [platformio_override.ini](./platformio_override.ini), see *webots_ip_address* and *webots_protocol*. + +Use *ipc* as *webots_protocol* for local connections and *tcp* for remote connections. + +| Key | Description | +| --- | ----------- | +| webots\_ip\_address | The IP address of the Webots simulation, which is used for TCP communication. | +| webots\_protocol | \[ipc\|tcp\] - ipc is faster but only works on the same machine, tcp works also over network. | +| webots\_robot\_name | The robot name used to identify the robot in the Webots world. See Webots world robot prototype. | + +PlatformIO project tasks --> <APP-NAME> --> Custom --> Launch + +#### Run without Webots launcher + +This can be choosen in case the simulation waits just for one robot. + +PlatformIO project tasks --> <APP-NAME> --> General --> Upload + +#### Run via terminal + +1. Open a command line (shell) and change to the folder with the built executable in ```.pio/build/```. This folder contains all necessary shared libraries as well. +2. Start the executable. + ## The target The main target of the firmware is the [ZumoComSystem](https://github.com/NewTec-GmbH/ZumoComSystem) from NewTec GmbH, which is a shield for the [Pololu 32U4 Zumo](https://www.pololu.com/product/2510) robot. diff --git a/doc/ROS2/README.md b/doc/ROS2/README.md index 8db4adc8..3a637375 100644 --- a/doc/ROS2/README.md +++ b/doc/ROS2/README.md @@ -71,11 +71,14 @@ The following diagram illustrates the component deployment of micro-ROS in the D ## Installation Steps +The installation procedure handles how the WSL in Windows is used to install the Ubuntu Linux distribution, the ROS2, Webots and micro-ROS parts. + 1. [WSL with Ubuntu](./setup/wsl.md) 2. [ROS2 Jazzy](./setup/ROS2_Jazzy.md) 3. [Webots](./setup/Webots.md) -4. [micro-ROS Native](./setup/microROS_Native.md) -5. [micro-ROS Agent](./setup/Agent.md) +4. [VSCode in WSL](./setup/vscode.md) +5. [micro-ROS Native](./setup/microROS_Native.md) +6. [micro-ROS Agent](./setup/Agent.md) ## Execution Steps diff --git a/doc/ROS2/setup/Agent.md b/doc/ROS2/setup/Agent.md index 165bc7d7..5a049191 100644 --- a/doc/ROS2/setup/Agent.md +++ b/doc/ROS2/setup/Agent.md @@ -2,17 +2,17 @@ Sources: [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/index.html) -* [Micro XRCE-DDS Agent](#micro-xrce-dds-agent) - * [Installation](#installation) - * [Using the agent with the serial interface](#using-the-agent-with-the-serial-interface) - * [Using the agent with the UDP interface](#using-the-agent-with-the-udp-interface) - * [Using the agent with the TCP interface](#using-the-agent-with-the-tcp-interface) - * [Troubleshooting on WSL environment](#troubleshooting-on-wsl-environment) - * [Testing the node](#testing-the-node) +- [Micro XRCE-DDS Agent](#micro-xrce-dds-agent) + - [Installation](#installation) + - [Using the agent with the serial interface](#using-the-agent-with-the-serial-interface) + - [Using the agent with the TCP interface (recommended)](#using-the-agent-with-the-tcp-interface-recommended) + - [Using the agent with the UDP interface](#using-the-agent-with-the-udp-interface) + - [Troubleshooting on WSL environment](#troubleshooting-on-wsl-environment) + - [Testing the node](#testing-the-node) ## Installation -Follow the instructions detailed [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html). It has only been tested as standalone executable (without Docker neither using Snap). +Follow the instructions detailed in [eProsima Micro XRCE-DDS documentation](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html). It has only been tested as standalone executable (without Docker neither using Snap). `cmake` and `make` are required to build the Agent. ## Using the agent with the serial interface @@ -38,7 +38,7 @@ Once the Agent and the Client are connected, the terminal should show something [1723186868.574254] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x64C59DFF, datawriter_id: 0x000(5), publisher_id: 0x000(3) ``` -## Using the agent with the TCP interface +## Using the agent with the TCP interface (recommended) Start the MicroXRCEAgent binary to listen to **TCP** connections: @@ -60,7 +60,7 @@ Start the MicroXRCEAgent binary to listen to **TCP** connections: > [!WARNING] > Note: UDP ports on WSL are not working properly if you need to access them outside of the WSL VM (Status 2024-10-31). Use TCP with WSL instead. > See [https://github.com/micro-ROS/micro-ROS-Agent/issues/194](https://github.com/micro-ROS/micro-ROS-Agent/issues/194) for further details. -> The mentioned ```netsh`` tool for port proxy forwarding only supports TCP. +> The mentioned ```netsh``` tool for port proxy forwarding only supports TCP. Start the MicroXRCEAgent binary to listen to UDP connections: @@ -87,6 +87,7 @@ Update: 2024-10-30: TCP transport is available in DroidControlShip, making UDP u In order to test your node, you can use `ros2 topic list` to list all topics used, or `ros2 topic echo ` to listen to incoming data in a specific topic. Publish some test messages (1Hz twist message): + ```bash ros2 topic pub -r 1.0 cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" ``` diff --git a/doc/ROS2/setup/Webots.md b/doc/ROS2/setup/Webots.md index 6ba4c9c8..c61a4a0c 100644 --- a/doc/ROS2/setup/Webots.md +++ b/doc/ROS2/setup/Webots.md @@ -1,9 +1,52 @@ -# Installation of Webots +# Installation of Webots -## Webots Setup Procedure +- [Windows](#windows) + - [Webots ROS2 Package (Webots on Windows)](#webots-ros2-package-webots-on-windows) + - [Running Webots ROS2 Universal Robot (on Windows)](#running-webots-ros2-universal-robot-on-windows) +- [Linux](#linux) + - [Webots Setup Procedure](#webots-setup-procedure) + - [Launching Webots](#launching-webots) + - [Webots ROS2 Package (Webots in WSL)](#webots-ros2-package-webots-in-wsl) + - [Running Webots ROS2 Universal Robot (on Linux)](#running-webots-ros2-universal-robot-on-linux) +- [Setting up a Webots Simulation with ROS2 controller](#setting-up-a-webots-simulation-with-ros2-controller) +- [Webots Supervisor](#webots-supervisor) -Follow the official installation guide for Webots on Linux with APT on the -[Webots installation Page](https://cyberbotics.com/doc/guide/installation-procedure#installing-the-debian-package-with-the-advanced-packaging-tool-apt). +## Windows + +Follow the official installation guide for Webots on Windows in the [Webots User Guide Installation Procedure](https://cyberbotics.com/doc/guide/installation-procedure#installation-on-windows). + +ROS2 requires Linux, therefore in this setup RadonUlzer and DroidControlShip are running inside the WSL. They will connect via TCP to Webots running on the host system. + +### Webots ROS2 Package (Webots on Windows) + +The setup procedure is described on [this page](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Windows.html) from the Jazzy documentation. If the listed package is not found by apt, you will need to install it by building from source. + +### Running Webots ROS2 Universal Robot (on Windows) + +See Task 2 from this [Jazzy documentaion page](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Ubuntu.html#launch-the-webots-ros2-universal-robot-example) + +Replace ```/``` in the following terminal commands: + +```bash + export WEBOTS_HOME=/mnt// + export WEBOTS_CONTROLLER_LIB_PATH=$WEBOTS_HOME/lib + export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$WEBOTS_CONTROLLER_LIB_PATH/controller + + cd ros2_webots_ws + source install/local_setup.bash + + ros2 launch webots_ros2_universal_robot multirobot_launch.py +``` + +You should get the following simulation on the screen: + +![Webots ROS2 Example](./img/Webots_ros2_example.png) + +## Linux + +### Webots Setup Procedure + +Follow the official installation guide for Webots on Linux with APT in the [Webots User Guide Installation Procedure](https://cyberbotics.com/doc/guide/installation-procedure#installing-the-debian-package-with-the-advanced-packaging-tool-apt). > **_NOTE:_** Some of the steps result in larger package downloads. @@ -17,7 +60,7 @@ Set environment variables for Webots home directory, the Webots controller libra source $HOME/.bashrc ``` -## Launching Webots +### Launching Webots Try @@ -36,31 +79,25 @@ If you get an error like "cannot open Display", try the following: - try running Webots again -## Webots ROS2 Package - -The setup procedure is described on [this page](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Ubuntu.html) -from the Jazzy documentation. The listed package is not found by apt, so -install it using building from source. +### Webots ROS2 Package (Webots in WSL) -Note: For the time being, we need to install a patched version from https://github.com/nhjschulz/webots_ros2.git. -The official one doesn't seem to support webots running inside WSL, but expects msys2/mingw. +The setup procedure is described on [this page](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Ubuntu.html) from the Jazzy documentation. If the listed package is not found by apt, you will need to install it by building from source. -transcript: +Because of [Webots Issue 6570 in webots-ros2-driver](https://github.com/cyberbotics/webots/issues/6570), running Webots inside the WSL requires a small local change. If it detects that the system is a WSL system, it will try to run ```webots.exe``` instead of ```webots```. ```bash - mkdir -p ros2_webots_ws/src - cd ros2_webots_ws - git clone --recurse-submodules https://github.com/nhjschulz/webots_ros2.git src/webots_ros2 - - sudo apt install python3-pip python3-rosdep python3-colcon-common-extensions - sudo rosdep init && rosdep update - rosdep install --from-paths src --ignore-src --rosdistro jazzy +sudo nano /opt/ros/jazzy/lib/python3.12/site-packages/webots_ros2_driver/utils.py +``` - colcon build +Required change: +```python +def is_wsl(): + # return 'microsoft-standard' in uname().release + return False ``` -## Running Webots ROS2 Universal Robot +### Running Webots ROS2 Universal Robot (on Linux) See Task 2 from this [Jazzy documentaion page](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Ubuntu.html#launch-the-webots-ros2-universal-robot-example) diff --git a/doc/ROS2/setup/vscode.md b/doc/ROS2/setup/vscode.md new file mode 100644 index 00000000..f0e342a2 --- /dev/null +++ b/doc/ROS2/setup/vscode.md @@ -0,0 +1,29 @@ +# VSCode in WSL + +All of the following steps are applied in the Ubuntu terminal. + +If DroidControlShip is not cloned yet, this will be the first step: + +```bash +cd ~ +git clone https://github.com/BlueAndi/DroidControlShip.git +``` + +In VSCode the PlatformIO extension is required. The PlatformIO extension requires Python with virtual environment capabilities. + +```bash +sudo apt install -y python3-venv +``` + +Start VSCode inside the DroidControlShip folder. + +```bash +cd DroidControlShip +vscode . +``` + +Install now the PlatformIO extension (extension id: platformio.platformio-ide) in VSCode and wait until installation is complete. + +After that a VSCode restart is required and VSCode is ready. + +Install additionally the C/C++ Extension Pack (extension id: ms-vscode.cpptools-extension-pack). diff --git a/doc/ROS2/setup/wsl.md b/doc/ROS2/setup/wsl.md index 1bd11755..649544ac 100644 --- a/doc/ROS2/setup/wsl.md +++ b/doc/ROS2/setup/wsl.md @@ -1,10 +1,10 @@ # Installation of WSL for ROS2 Jazzy -Each ROS2 distribution targets a specific Linux environment. -For Jazzy it is *Ubuntu 24.04*. To run it on Windows 10/11, +Each ROS2 distribution targets a specific Linux environment. +For Jazzy it is *Ubuntu 24.04*. To run it on Windows 10/11, the WSL2 feature will be used. -## Install WSL +## Install WSL Installing WSL (Linux on Windows) is officially supported by Microsoft. For details refer to the following @@ -18,9 +18,12 @@ But all you need is this from a windows power shell: ``` > **_NOTE:_** -A Windows message dialog may popup in the background, to ask for elevated rights. +A Windows message dialog may popup in the background, to ask for elevated rights. Check for such a window if the installation does not progress. +> **_NOTE:_** +If windows requests to restart after trying to install Ubuntu, do so and try to install again. + ## First time use A console window should appear with installation messages. It will ask you @@ -51,6 +54,8 @@ Configure Ubuntu to login by default with your user in the windows power shell: Check that you have the right distribution installed (Ubuntu 24.04 LTS): +In the Ubuntu terminal: + ```bash $ lsb_release -a No LSB modules are available. @@ -62,7 +67,7 @@ Check that you have the right distribution installed (Ubuntu 24.04 LTS): ## New Terminal Window -Is is recommended to install the new Microsoft Terminal Window +Is is recommended to install the new Microsoft Terminal Window if not done already. It is available from [Github Microsoft Terminal](https://github.com/microsoft/terminal). @@ -71,20 +76,31 @@ It offers tabs, history and embedds all kinds of shells (cmd, powershell, Ubuntu ![New Shell](./img/new_shell.png) -## Exposing WSL UDP Ports to the Network +## Exposing WSL Ports to the Network -To make UDP ports listening in WSL accessible from your local network, follow these steps: -Find your WSL IP address by running this command in WSL: +To make TCP/UDP ports listening in WSL accessible from your local network, follow these steps: + +Find your WSL IP address by running this command **in WSL**: ```bash ip addr show eth0 ``` +or this **in a windows powershell**: + +```bat +wsl hostname -I +``` + > **_NOTE:_** -UDP port forwarding to wsl is not supported without mirrored networking (WSL 2.0.0 on win11) +TCP/UDP port forwarding to WSL is not supported without mirrored networking (WSL 2.0.0 on win11) Open a terminal in an administrator shell on Windows. -Run the following command to forward the UDP port, replacing [PORT] with your desired port number (usually 8888) and [WSL_IP] with the IP address: +Run the following commands to forward the TCP/UDP port, replacing [PORT] with your desired port number and [WSL_IP] with the IP address. + +| Port | Description | +| ---- | ------------------ | +| 8888 | ROS2 network port | ```bat netsh interface portproxy add v4tov4 listenport=[PORT] listenaddress=0.0.0.0 connectport=[PORT] connectaddress=[WSL_IP] diff --git a/doc/ROS2/webots/webots.md b/doc/ROS2/webots/webots.md index 4da92f39..db79df5a 100644 --- a/doc/ROS2/webots/webots.md +++ b/doc/ROS2/webots/webots.md @@ -34,13 +34,3 @@ The `ros2_dcs_turtlesim` package is availabe from it's own GIT repository at Create a Webots ros2 workspace as described in [setup/Webots.md](../setup/Webots.md), then follow the instructions in `ros2_dcs_turtlesim` readme.md for building and launching. - -## Notes - -If the robot display shows an MCAL error, the calibration for the RadonUlzer is missing. Perform it first or manipulate the settings: - -```bash -nano .pio/build/RemoteControlSim/settings.json -``` - -Set the maxSpeed value to 4200. diff --git a/lib/APPTurtle/src/App.cpp b/lib/APPTurtle/src/App.cpp index 84025951..e654d6ba 100644 --- a/lib/APPTurtle/src/App.cpp +++ b/lib/APPTurtle/src/App.cpp @@ -296,7 +296,7 @@ void App::handleTurtle() RobotSpeed payload; const int32_t MILLI_CONVERSION_FACTOR = 1000; int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * MILLI_CONVERSION_FACTOR; /* Linear speed in mm/s */ - int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * MILLI_CONVERSION_FACTOR; /* Angular speed in mrad/s */ + int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * MILLI_CONVERSION_FACTOR; /* Angular speed in mrad/s */ payload.linearCenter = linearSpeed; payload.angular = angularSpeed; diff --git a/lib/APPTurtle/src/MicroRosClient.cpp b/lib/APPTurtle/src/MicroRosClient.cpp index 69bbac5d..1d519cf2 100644 --- a/lib/APPTurtle/src/MicroRosClient.cpp +++ b/lib/APPTurtle/src/MicroRosClient.cpp @@ -71,7 +71,12 @@ MicroRosClient::MicroRosClient() : m_executor(), m_allocator(), m_subscribers{nullptr}, - m_numberOfHandles(0U) + m_numberOfHandles(0U), + m_timer(), + m_numberOfConnectionRetries(0U), + m_isSupportInitialized(false), + m_isNodeInitialized(false), + m_isExecutorInitialized(false) { } @@ -198,17 +203,28 @@ bool MicroRosClient::createEntities() { LOG_ERROR("Failed to initialize support structure."); } - else if (RCL_RET_OK != rclc_node_init_default(&m_node, m_nodeName.c_str(), m_nodeNamespace.c_str(), &m_support)) - { - LOG_ERROR("Failed to initialize node."); - } - else if (RCL_RET_OK != rclc_executor_init(&m_executor, &m_support.context, m_numberOfHandles, &m_allocator)) - { - LOG_ERROR("Failed to initialize executor."); - } else { - isSuccessful = true; + m_isSupportInitialized = true; + + if (RCL_RET_OK != rclc_node_init_default(&m_node, m_nodeName.c_str(), m_nodeNamespace.c_str(), &m_support)) + { + LOG_ERROR("Failed to initialize node."); + } + else + { + m_isNodeInitialized = true; + + if (RCL_RET_OK != rclc_executor_init(&m_executor, &m_support.context, m_numberOfHandles, &m_allocator)) + { + LOG_ERROR("Failed to initialize executor."); + } + else + { + m_isExecutorInitialized = true; + isSuccessful = true; + } + } } /* If necessary, it will be cleaned-up. */ @@ -223,10 +239,26 @@ bool MicroRosClient::createEntities() void MicroRosClient::destroyEntities() { - rcl_ret_t ret = rclc_executor_fini(&m_executor); + rcl_ret_t ret = RCL_RET_OK; + + /* Clean up entities in reverse order of initialization. */ + if (true == m_isExecutorInitialized) + { + ret = rclc_executor_fini(&m_executor); + m_isExecutorInitialized = false; + } - ret += rcl_node_fini(&m_node); - ret += rclc_support_fini(&m_support); + if (true == m_isNodeInitialized) + { + ret += rcl_node_fini(&m_node); + m_isNodeInitialized = false; + } + + if (true == m_isSupportInitialized) + { + ret += rclc_support_fini(&m_support); + m_isSupportInitialized = false; + } if (RCL_RET_OK != ret) { @@ -292,6 +324,7 @@ void MicroRosClient::waitingForAgentState() { m_timer.stop(); m_state = STATE_CONNECTING; + m_numberOfConnectionRetries = MAX_CONNECTION_RETRIES; } else if (false == m_timer.isTimerRunning()) { @@ -314,7 +347,14 @@ void MicroRosClient::connectingState() if (false == createEntities()) { - m_state = STATE_WAIT_FOR_AGENT; + if (0U < m_numberOfConnectionRetries) + { + --m_numberOfConnectionRetries; + } + else + { + m_state = STATE_WAIT_FOR_AGENT; + } } else { @@ -352,6 +392,11 @@ void MicroRosClient::connectedState() m_timer.restart(); } } + else + { + /* Nothing to do. */ + ; + } /* Connection lost? */ if (true == isConnectionLost) diff --git a/lib/APPTurtle/src/MicroRosClient.h b/lib/APPTurtle/src/MicroRosClient.h index e91c036c..273ecc70 100644 --- a/lib/APPTurtle/src/MicroRosClient.h +++ b/lib/APPTurtle/src/MicroRosClient.h @@ -110,6 +110,7 @@ class MicroRosClient * This is the period time in ms. */ static const uint32_t MICRO_ROS_AGENT_PING_PERIOD_LONG = 500U; + /** * The Micro-ROS agent will be periodically pinged to detect connection loss. * This is the period time in ms. @@ -127,6 +128,11 @@ class MicroRosClient */ static const uint8_t MICRO_ROS_AGENT_PING_ATTEMPTS = 1U; + /** + * Maximum number of connection retries before going back to waiting state. + */ + static const uint8_t MAX_CONNECTION_RETRIES = 1U; + /** * DDS queue check timeout in ms. */ @@ -198,6 +204,26 @@ class MicroRosClient */ SimpleTimer m_timer; + /** + * Number of connection retries. + */ + uint8_t m_numberOfConnectionRetries; + + /** + * Tracks if support structure was successfully initialized. + */ + bool m_isSupportInitialized; + + /** + * Tracks if node was successfully initialized. + */ + bool m_isNodeInitialized; + + /** + * Tracks if executor was successfully initialized. + */ + bool m_isExecutorInitialized; + /** * Setup the custom transport protocol. * diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 27f0e955..19b48a24 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -171,7 +171,7 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui if (InputState::INPUT_STATE_FINISH == m_inputState) { - if (readBytes > size) + if (m_payloadLen > size) { /* Internal error, request buffer would be overrun. */ close(); @@ -262,7 +262,7 @@ bool CustomRosTransportTcp::readSizePrefix(int timeout, uint8_t* errorCode) case 2U: /* 2 byte prefix received */ m_payloadLen = static_cast(prefix[0]) + (static_cast(prefix[1]) << 8); m_received = 0U; - m_inputState = InputState::INPUT_STATE_PLAYLOAD; + m_inputState = InputState::INPUT_STATE_PAYLOAD; loop = true; break; @@ -293,7 +293,7 @@ bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCod case 1U: /* High byte received. */ m_payloadLen |= (static_cast(prefix[0]) << 8); m_received = 0U; - m_inputState = InputState::INPUT_STATE_PLAYLOAD; + m_inputState = InputState::INPUT_STATE_PAYLOAD; loop = true; break; diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index 56ca3b90..129389d8 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -222,7 +222,7 @@ class CustomRosTransportTcp : public CustomRosTransportBase { INPUT_STATE_INIT = 0, /**< Input buffer ready for new record. */ INPUT_STATE_PREFIX_1, /**< First byte of 2 -Byte size prefix received. */ - INPUT_STATE_PLAYLOAD, /**< Collecting payload bytes. */ + INPUT_STATE_PAYLOAD, /**< Collecting payload bytes. */ INPUT_STATE_FINISH, /**< Payload record is complete. */ INPUT_STATE_MAX /**< Enum Range value, not a true state. */ }; diff --git a/lib/MainNative/src/main.cpp b/lib/MainNative/src/main.cpp index 83942141..8f5db92f 100644 --- a/lib/MainNative/src/main.cpp +++ b/lib/MainNative/src/main.cpp @@ -382,7 +382,11 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char } else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "cwd")) { - chdir(optarg); + if (0 != chdir(optarg)) + { + printf("Failed to change working directory to %s.\n", optarg); + status = -1; + } } else { diff --git a/platformio.ini b/platformio.ini index 22cb9a80..f844ac54 100644 --- a/platformio.ini +++ b/platformio.ini @@ -102,6 +102,8 @@ extra_scripts = post:./scripts/copy_webots_shared_libs.py ; Custom options (https://docs.platformio.org/en/latest/scripting/examples/platformio_ini_custom_options.html) +webots_ip_address = 127.0.0.1 ; IP address of the Webots simulation, which is used for TCP communication +webots_protocol = ipc ; [ipc|tcp] - ipc is faster but only works on the same machine, tcp works also over network webots_robot_name = ZumoComSystem webots_robot_serial_rx_channel = 4 webots_robot_serial_tx_channel = 3 @@ -483,51 +485,7 @@ extends = target:Sim, app:Turtle, static_check_configuration build_flags = ${target:Sim.build_flags} ${app:Turtle.build_flags} - -I./lib/libmicroros/include/action_msgs - -I./lib/libmicroros/include/actionlib_msgs - -I./lib/libmicroros/include/builtin_interfaces - -I./lib/libmicroros/include/composition_interfaces - -I./lib/libmicroros/include/diagnostic_msgs - -I./lib/libmicroros/include/example_interfaces - -I./lib/libmicroros/include/geometry_msgs - -I./lib/libmicroros/include/lifecycle_msgs - -I./lib/libmicroros/include/micro_ros_msgs - -I./lib/libmicroros/include/micro_ros_utilities - -I./lib/libmicroros/include/nav_msgs - -I./lib/libmicroros/include/rcl - -I./lib/libmicroros/include/rcl_action - -I./lib/libmicroros/include/rcl_interfaces - -I./lib/libmicroros/include/rcl_lifecycle - -I./lib/libmicroros/include/rcl_logging_interface - -I./lib/libmicroros/include/rclc - -I./lib/libmicroros/include/rclc_lifecycle - -I./lib/libmicroros/include/rclc_parameter - -I./lib/libmicroros/include/rcutils - -I./lib/libmicroros/include/rmw - -I./lib/libmicroros/include/rmw_microros - -I./lib/libmicroros/include/rmw_microxrcedds_c - -I./lib/libmicroros/include/rosgraph_msgs - -I./lib/libmicroros/include/rosidl_dynamic_typesupport - -I./lib/libmicroros/include/rosidl_runtime_c - -I./lib/libmicroros/include/rosidl_typesupport_c - -I./lib/libmicroros/include/rosidl_typesupport_interface - -I./lib/libmicroros/include/rosidl_typesupport_introspection_c - -I./lib/libmicroros/include/rosidl_typesupport_microxrcedds_c - -I./lib/libmicroros/include/sensor_msgs - -I./lib/libmicroros/include/service_msgs - -I./lib/libmicroros/include/shape_msgs - -I./lib/libmicroros/include/statistics_msgs - -I./lib/libmicroros/include/std_msgs - -I./lib/libmicroros/include/std_srvs - -I./lib/libmicroros/include/stereo_msgs - -I./lib/libmicroros/include/test_msgs - -I./lib/libmicroros/include/tracetools - -I./lib/libmicroros/include/trajectory_msgs - -I./lib/libmicroros/include/type_description_interfaces - -I./lib/libmicroros/include/ucdr - -I./lib/libmicroros/include/unique_identifier_msgs - -I./lib/libmicroros/include/uxr - -I./lib/libmicroros/include/visualization_msgs + -I./lib/libmicroros/include -L./lib/libmicroros -llibmicroros lib_deps = diff --git a/platformio_override.ini b/platformio_override.ini index 20ec68c4..c313b487 100644 --- a/platformio_override.ini +++ b/platformio_override.ini @@ -19,6 +19,8 @@ ; Webots directory, e.g. WEBOTS_HOME=C:\Users\\AppData\Local\Programs\Webots ; ***************************************************************************** [target:Sim] +webots_ip_address = 127.0.0.1 ; IP address of the Webots simulation, which is used for TCP communication +webots_protocol = ipc ; [ipc|tcp] - ipc is faster but only works on the same machine, tcp works also over network webots_robot_name = ZumoComSystem webots_robot_serial_rx_channel = 4 webots_robot_serial_tx_channel = 3 diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index e836a84e..bf778844 100644 --- a/scripts/webots_launcher.py +++ b/scripts/webots_launcher.py @@ -29,6 +29,7 @@ import sys import platform +# pyright: reportUndefinedVariable=false Import("env") # pylint: disable=undefined-variable ################################################################################ @@ -39,22 +40,29 @@ OS_PLATFORM_TYPE_MACOS = "Darwin" OS_PLATFORM_TYPE = platform.system() -ROBOT_NAME = env.GetProjectOption( - "webots_robot_name") # pylint: disable=undefined-variable +WEBOTS_IP_ADDRESS = env.GetProjectOption("webots_ip_address") # pylint: disable=undefined-variable +WEBOTS_PROTOCOL = env.GetProjectOption("webots_protocol") # pylint: disable=undefined-variable -ROBOT_SERIAL_RX_CHANNEL = env.GetProjectOption( - "webots_robot_serial_rx_channel") # pylint: disable=undefined-variable -ROBOT_SERIAL_TX_CHANNEL = env.GetProjectOption( - "webots_robot_serial_tx_channel") # pylint: disable=undefined-variable +ROBOT_NAME = env.GetProjectOption("webots_robot_name") # pylint: disable=undefined-variable + +ROBOT_SERIAL_RX_CHANNEL = env.GetProjectOption("webots_robot_serial_rx_channel") # pylint: disable=undefined-variable +ROBOT_SERIAL_TX_CHANNEL = env.GetProjectOption("webots_robot_serial_tx_channel") # pylint: disable=undefined-variable PROGRAM_PATH = "$BUILD_DIR/" PROGRAM_OPTIONS = '--cfgFilePath "../../../data/config/config.json" ' \ + '--serialRxCh ' + ROBOT_SERIAL_RX_CHANNEL + ' ' \ + '--serialTxCh ' + ROBOT_SERIAL_TX_CHANNEL + ' ' \ + '-v' -WEBOTS_CONTROLLER_OPTIONS = '--robot-name=' + ROBOT_NAME + ' --stdout-redirect' +WEBOTS_CONTROLLER_OPTIONS = '--robot-name=' + ROBOT_NAME + ' ' \ + '--stdout-redirect' + ' ' \ + '--ip-address=' + WEBOTS_IP_ADDRESS + ' --protocol=' + WEBOTS_PROTOCOL WEBOTS_HOME = os.getenv('WEBOTS_HOME') +if WEBOTS_HOME is None: + print("WEBOTS_HOME environment variable is not set. " + "Please set it to the Webots installation directory.") + sys.exit(1) + if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: WEBOTS_HOME = WEBOTS_HOME.replace('\\', '/')