From a263ea724fed7955ead4ee5dbeb98cd282f51dfd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 26 Nov 2024 00:03:00 +0100 Subject: [PATCH] Moved Python and C++ tutorials directory to root level --- definitions/jackal.vehicle.xml | 2 +- docs/mvsim-cli.rst | 2 +- docs/teleoperation.rst | 2 +- .../CMakeLists.txt | 0 .../mvsim-service-caller-example.cpp | 0 .../mvsim-subscriber-example/CMakeLists.txt | 0 .../mvsim-subscriber-example.cpp | 0 .../call-shutdown.py | 0 .../move-object-example.py | 0 .../mvsim-teleop.py | 0 examples_python/plot-log-files-4-wheels.py | 125 ++++++++++++++++++ .../simple-obstacle-avoidance.py | 6 +- .../subscriber-example.py | 0 formatter.sh | 2 +- 14 files changed, 132 insertions(+), 7 deletions(-) rename {mvsim_tutorial/cpp => examples_cpp}/mvsim-service-caller-example/CMakeLists.txt (100%) rename {mvsim_tutorial/cpp => examples_cpp}/mvsim-service-caller-example/mvsim-service-caller-example.cpp (100%) rename {mvsim_tutorial/cpp => examples_cpp}/mvsim-subscriber-example/CMakeLists.txt (100%) rename {mvsim_tutorial/cpp => examples_cpp}/mvsim-subscriber-example/mvsim-subscriber-example.cpp (100%) rename {mvsim_tutorial/python => examples_python}/call-shutdown.py (100%) rename {mvsim_tutorial/python => examples_python}/move-object-example.py (100%) rename {mvsim_tutorial/python => examples_python}/mvsim-teleop.py (100%) create mode 100755 examples_python/plot-log-files-4-wheels.py rename {mvsim_tutorial/python => examples_python}/simple-obstacle-avoidance.py (96%) rename {mvsim_tutorial/python => examples_python}/subscriber-example.py (100%) diff --git a/definitions/jackal.vehicle.xml b/definitions/jackal.vehicle.xml index 0725be9f..d603d8c9 100644 --- a/definitions/jackal.vehicle.xml +++ b/definitions/jackal.vehicle.xml @@ -28,7 +28,7 @@ - + diff --git a/docs/mvsim-cli.rst b/docs/mvsim-cli.rst index 12e52a2b..46253df4 100644 --- a/docs/mvsim-cli.rst +++ b/docs/mvsim-cli.rst @@ -105,4 +105,4 @@ Command ``mvsim topic`` mvsim topic hz Estimate topic publication rate (in Hz) Can be used to list or inspect the publication of MVSim (not ROS!) topics with sensor and pose data. -These topics are accessible via the provided Python API, refer to examples: https://github.com/MRPT/mvsim/tree/develop/mvsim_tutorial/python +These topics are accessible via the provided Python API, refer to examples: https://github.com/MRPT/mvsim/tree/develop/examples_python diff --git a/docs/teleoperation.rst b/docs/teleoperation.rst index e025c326..ff238ae4 100644 --- a/docs/teleoperation.rst +++ b/docs/teleoperation.rst @@ -12,7 +12,7 @@ If no data is being published to ``/cmd_vel``, or if you run MVSim without ROS s three alternative methods can be used to move the robots: - Via ZMQ messages using the MVSim built-in messaging protocol (Write me!). - See a `Python code example `_. + See a `Python code example `_. - Joystick, if present (see below). - Keyboard, from the MVSim GUI (see below). diff --git a/mvsim_tutorial/cpp/mvsim-service-caller-example/CMakeLists.txt b/examples_cpp/mvsim-service-caller-example/CMakeLists.txt similarity index 100% rename from mvsim_tutorial/cpp/mvsim-service-caller-example/CMakeLists.txt rename to examples_cpp/mvsim-service-caller-example/CMakeLists.txt diff --git a/mvsim_tutorial/cpp/mvsim-service-caller-example/mvsim-service-caller-example.cpp b/examples_cpp/mvsim-service-caller-example/mvsim-service-caller-example.cpp similarity index 100% rename from mvsim_tutorial/cpp/mvsim-service-caller-example/mvsim-service-caller-example.cpp rename to examples_cpp/mvsim-service-caller-example/mvsim-service-caller-example.cpp diff --git a/mvsim_tutorial/cpp/mvsim-subscriber-example/CMakeLists.txt b/examples_cpp/mvsim-subscriber-example/CMakeLists.txt similarity index 100% rename from mvsim_tutorial/cpp/mvsim-subscriber-example/CMakeLists.txt rename to examples_cpp/mvsim-subscriber-example/CMakeLists.txt diff --git a/mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp b/examples_cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp similarity index 100% rename from mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp rename to examples_cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp diff --git a/mvsim_tutorial/python/call-shutdown.py b/examples_python/call-shutdown.py similarity index 100% rename from mvsim_tutorial/python/call-shutdown.py rename to examples_python/call-shutdown.py diff --git a/mvsim_tutorial/python/move-object-example.py b/examples_python/move-object-example.py similarity index 100% rename from mvsim_tutorial/python/move-object-example.py rename to examples_python/move-object-example.py diff --git a/mvsim_tutorial/python/mvsim-teleop.py b/examples_python/mvsim-teleop.py similarity index 100% rename from mvsim_tutorial/python/mvsim-teleop.py rename to examples_python/mvsim-teleop.py diff --git a/examples_python/plot-log-files-4-wheels.py b/examples_python/plot-log-files-4-wheels.py new file mode 100755 index 00000000..f9297e14 --- /dev/null +++ b/examples_python/plot-log-files-4-wheels.py @@ -0,0 +1,125 @@ +#!/bin/env python3 + +# Usage: +# plot-log-files.py session1-mvsim_r1_logger_pose.log + +# Requirements: +# pip install pandas matplotlib + +import pandas as pd +import matplotlib.pyplot as plt +import sys +import os + + +def main(file_path): + # Read the main file + try: + main_data = pd.read_csv(file_path, skipinitialspace=True) + print(f"Main data loaded from {file_path}") + except Exception as e: + print(f"Error reading {file_path}: {e}") + return + + # Define the wheel files based on the main file name + base_name = file_path.replace("pose.log", "") + wheel_files = [f"{base_name}wheel{i}.log" for i in range(1, 5)] + + # Load wheel data + wheel_data = {} + for i, wheel_file in enumerate(wheel_files, start=1): + try: + wheel_data[i] = pd.read_csv(wheel_file, skipinitialspace=True) + print(f"Wheel {i} data loaded from {wheel_file}") + except Exception as e: + print(f"Error reading {wheel_file}: {e}") + return + + # Create plots for the wheel files (non-blocking) + plot_wheel_data(wheel_data) + + # Create plots for the main file (blocking) + plot_main_data(main_data) + + +def plot_main_data(data): + """Create plots for the main file data.""" + plt.figure(figsize=(12, 8)) + + # print(data.columns) + ts = data['Timestamp'].to_numpy() + + # Plot 1: qx, qy, qz over time + plt.subplot(3, 1, 1) + plt.plot(ts, data['qx'].to_numpy(), label='qx') + plt.plot(ts, data['qy'].to_numpy(), label='qy') + plt.plot(ts, data['qz'].to_numpy(), label='qz') + plt.xlabel('Timestamp') + plt.ylabel('Position') + plt.title('Position (qx, qy, qz) vs Time') + plt.grid() + plt.legend() + + # Plot 2: Orientation angles (qpitch, qroll, qyaw) over time + plt.subplot(3, 1, 2) + plt.plot(ts, + data['qpitch'].to_numpy(), label='qpitch') + plt.plot(ts, + data['qroll'].to_numpy(), label='qroll') + plt.plot(ts, + data['qyaw'].to_numpy(), label='qyaw') + plt.xlabel('Timestamp') + plt.ylabel('Orientation') + plt.title('Orientation Angles vs Time') + plt.grid() + plt.legend() + + # Plot 3: dqz over time + plt.subplot(3, 1, 3) + plt.plot(ts, data['dqx'].to_numpy(), label='dqx', color='red') + plt.plot(ts, data['dqy'].to_numpy(), label='dqy', color='green') + plt.plot(ts, data['dqz'].to_numpy(), label='dqz', color='blue') + plt.xlabel('Timestamp') + plt.ylabel('Velocity') + plt.title('Velocity components') + plt.grid() + plt.legend() + + plt.tight_layout() + plt.show() + + +def plot_wheel_data(wheel_data): + """Create plots for the wheel data.""" + plt.figure(figsize=(12, 12)) + + # Create one plot per variable group across all wheels + variables = ['friction_x', 'friction_y', + 'velocity_x', 'velocity_y', + 'torque', 'weight'] + wheel_names = ['LR', 'RR', 'LF', 'RF'] + + for i, var in enumerate(variables, start=1): + plt.subplot(3, 2, i) + for wheel, data in wheel_data.items(): + ts = data['Timestamp'].to_numpy() + plt.plot(ts, + data[var].to_numpy(), label=f'{wheel_names[wheel-1]} wheel') + plt.xlabel('Timestamp') + plt.ylabel(var) + plt.title(f'{var.capitalize()} vs Time') + plt.grid() + plt.legend() + + plt.tight_layout() + plt.show(block=False) + + +if __name__ == "__main__": + # Check if the path to the main file is provided + if len(sys.argv) != 2: + print("Usage: python3 script.py ") + sys.exit(1) + + file_path = sys.argv[1] + main(file_path) diff --git a/mvsim_tutorial/python/simple-obstacle-avoidance.py b/examples_python/simple-obstacle-avoidance.py similarity index 96% rename from mvsim_tutorial/python/simple-obstacle-avoidance.py rename to examples_python/simple-obstacle-avoidance.py index de3f7aaf..332796f8 100755 --- a/mvsim_tutorial/python/simple-obstacle-avoidance.py +++ b/examples_python/simple-obstacle-avoidance.py @@ -11,7 +11,7 @@ # export PYTHONPATH=$HOME/code/mvsim/build/:$PYTHONPATH # # Demo with N robots: -# for i in $(seq 1 25); do bash -c "mvsim_tutorial/python/simple-obstacle-avoidance.py --vehicle veh${i} &"; done +# for i in $(seq 1 25); do bash -c "examples_python/simple-obstacle-avoidance.py --vehicle veh${i} &"; done # # --------------------------------------------------------------------- @@ -81,7 +81,7 @@ def evalObstacleAvoidance(obs: ObservationLidar2D_pb2.ObservationLidar2D): # Force vectors resultantForce = [0, 0] - #obsWeight = 20.0 / len(obs.scanRanges) + # obsWeight = 20.0 / len(obs.scanRanges) obsWeight = 1 for idx, r in enumerate(obs.scanRanges): @@ -138,7 +138,7 @@ def onLidar2DMessage(msgType, msg): global prevLidarMsgTimestamp global prevGlobalGoalTimestamp, prevGlobalGoal - assert(msgType == "mvsim_msgs.ObservationLidar2D") + assert (msgType == "mvsim_msgs.ObservationLidar2D") p = ObservationLidar2D_pb2.ObservationLidar2D() p.ParseFromString(bytes(msg)) # print("[lidar callback] received:\n ranges=\n" + str(p.scanRanges) + "\n validRanges=" + str(p.validRanges)) diff --git a/mvsim_tutorial/python/subscriber-example.py b/examples_python/subscriber-example.py similarity index 100% rename from mvsim_tutorial/python/subscriber-example.py rename to examples_python/subscriber-example.py diff --git a/formatter.sh b/formatter.sh index dfa34ec9..ecbc20d3 100755 --- a/formatter.sh +++ b/formatter.sh @@ -1,2 +1,2 @@ # formatter.sh -find modules/ mvsim_node_src/ mvsim_tutorial/cpp/ mvsim-cli/ tests/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i +find modules/ mvsim_node_src/ examples_cpp/ mvsim-cli/ tests/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i