diff --git a/README.md b/README.md index 30879a2..a4666fb 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,7 @@ Please read through [this](https://www.overleaf.com/read/dmgrrcmpkbkq#211e69) do - docker -- Where the Dockerfile lives. - scripts -- Where necessary external scripts live. - workspace -- Where all the packages live. +- plots -- Where the generated simulation data plots are stored. ``` ### Build the simulator @@ -19,17 +20,26 @@ Please read through [this](https://www.overleaf.com/read/dmgrrcmpkbkq#211e69) do ```bash ./scripts/deploy/devel.sh # To enter the docker container +./scripts/deploy/start.sh # To start the simulation and control task +./scripts/deploy/start.sh --plot # To start the simulation and control task, and generate data plots of the simulation ros2 launch limo_simulation limo.launch.py # To launch the simulator ``` +When running ./scripts/deploy/start.sh for the first time on the system, you might need to run the following to make app.sh executable: + +```bash +chmod +x ./scripts/deploy/app.sh +``` + ### What do I edit? 1. Modify the package `limo_control` in the workspace directory for adding your c++ controller program. -2. Make a launch file that can launch everything (Controller and Simualation). +2. Make a launch file that can launch everything (Controller and Simulation). 3. Modify `scripts/deploy/app.sh` such that, when `scripts/deploy/start.sh` is run, the task is executed automatically. ### Known Issues 1. This will not work with docker desktop, please do not use it, use the default engine. +2. pandas and matplotlib will need to be installed in order to plot the simulation data. -Feel free to modify anything else if it does not work as expected. \ No newline at end of file +Feel free to modify anything else if it does not work as expected. diff --git a/plots/robot_path.png b/plots/robot_path.png new file mode 100644 index 0000000..9917ebd Binary files /dev/null and b/plots/robot_path.png differ diff --git a/plots/x_error.png b/plots/x_error.png new file mode 100644 index 0000000..e15ad39 Binary files /dev/null and b/plots/x_error.png differ diff --git a/plots/y_error.png b/plots/y_error.png new file mode 100644 index 0000000..4e5b8e8 Binary files /dev/null and b/plots/y_error.png differ diff --git a/scripts/deploy/app.sh b/scripts/deploy/app.sh old mode 100644 new mode 100755 index e69de29..632d307 --- a/scripts/deploy/app.sh +++ b/scripts/deploy/app.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +# build workspace + limo control and sim packages +cd /root/workspace +colcon build --packages-select limo_control limo_simulation + +#source setup +source /root/workspace/install/setup.bash + +#launch both the sim and the control nodes +ros2 launch limo_control limo_control.launch.py \ No newline at end of file diff --git a/scripts/deploy/start.sh b/scripts/deploy/start.sh index b785228..3674469 100755 --- a/scripts/deploy/start.sh +++ b/scripts/deploy/start.sh @@ -22,14 +22,25 @@ while getopts 'ch' opt; do ;; esac done + shift "$(($OPTIND -1))" +HOST_WS=$(dirname "$0")/../../workspace/ + if [ "$mode" == "gpu" ]; then run_docker --runtime=nvidia \ - -v $(dirname "$0")/../../workspace/:/root/workspace/src \ + -v $HOST_WS:/root/workspace \ limo_bot:sim "/root/app.sh" else run_docker \ - -v $(dirname "$0")/../../workspace/:/root/workspace/src \ + -v $HOST_WS:/root/workspace \ limo_bot:sim "/root/app.sh" +fi + +sleep 1 #wait a little bit + +#plot data after exiting the container if the --plot flag is given +if [ "$1" == "--plot" ]; then + echo "Generating plots..." + python3 ./workspace/plot_data.py fi \ No newline at end of file diff --git a/workspace/controller_log.csv b/workspace/controller_log.csv new file mode 100644 index 0000000..9bbd2de --- /dev/null +++ b/workspace/controller_log.csv @@ -0,0 +1,306 @@ +Time,Target_X,Target_Y,Target_Theta,Current_X,Current_Y,Current_Theta +2.45018,5,5,1.5708,-5.67513e-25,2.7102e-59,-9.55175e-35 +2.50008,5,5,1.5708,-5.71693e-25,5.32206e-24,0.0267035 +2.55013,5,5,1.5708,-5.71693e-25,5.32206e-24,0.0527662 +2.60012,5,5,1.5708,-5.71693e-25,5.32206e-24,0.103185 +2.65006,5,5,1.5708,-5.71693e-25,5.32206e-24,0.126481 +2.70006,5,5,1.5708,4.62434e-08,6.23327e-09,0.148551 +2.75006,5,5,1.5708,4.62439e-08,6.23336e-09,0.192453 +2.80009,5,5,1.5708,4.62442e-08,6.23341e-09,0.21292 +2.85009,5,5,1.5708,4.62445e-08,6.23347e-09,0.232814 +2.90006,5,5,1.5708,4.6245e-08,6.23361e-09,0.271086 +2.95004,5,5,1.5708,4.62452e-08,6.23369e-09,0.289147 +3.0002,5,5,1.5708,4.62457e-08,6.23383e-09,0.323416 +3.05011,5,5,1.5708,4.62459e-08,6.23391e-09,0.339466 +3.10007,5,5,1.5708,4.62464e-08,6.23408e-09,0.370174 +3.15008,5,5,1.5708,4.62466e-08,6.23417e-09,0.384446 +3.20007,5,5,1.5708,4.62468e-08,6.23426e-09,0.398349 +3.25009,5,5,1.5708,4.62473e-08,6.23445e-09,0.425127 +3.30017,5,5,1.5708,4.62475e-08,6.23455e-09,0.437751 +3.35012,5,5,1.5708,4.62479e-08,6.23476e-09,0.461757 +3.40007,5,5,1.5708,4.62481e-08,6.23487e-09,0.472977 +3.45018,5,5,1.5708,4.62486e-08,6.23509e-09,0.49448 +3.50017,5,5,1.5708,4.62488e-08,6.23521e-09,0.504457 +3.55007,5,5,1.5708,4.6249e-08,6.23533e-09,0.514189 +3.60011,5,5,1.5708,4.62494e-08,6.23557e-09,0.532942 +3.65006,5,5,1.5708,4.62496e-08,6.23569e-09,0.54177 +3.70007,5,5,1.5708,4.625e-08,6.23594e-09,0.558575 +3.75013,5,5,1.5708,4.62502e-08,6.23607e-09,0.566404 +3.80006,5,5,1.5708,4.62506e-08,6.23633e-09,0.58146 +3.85011,5,5,1.5708,4.62508e-08,6.23646e-09,0.588424 +3.90012,5,5,1.5708,4.6251e-08,6.2366e-09,0.595233 +3.95011,5,5,1.5708,4.62514e-08,6.23687e-09,0.608375 +4.00014,5,5,1.5708,4.62516e-08,6.23701e-09,0.614525 +4.05012,5,5,1.5708,4.6252e-08,6.23729e-09,0.626292 +4.10013,5,5,1.5708,4.62522e-08,6.23743e-09,0.631749 +4.15008,5,5,1.5708,4.62524e-08,6.23757e-09,0.637071 +4.20007,5,5,1.5708,4.62528e-08,6.23786e-09,0.647328 +4.25009,5,5,1.5708,4.6253e-08,6.23801e-09,0.652145 +4.3001,5,5,1.5708,4.62534e-08,6.2383e-09,0.661337 +4.35009,5,5,1.5708,4.62536e-08,6.23845e-09,0.665619 +4.40008,5,5,1.5708,4.62539e-08,6.23875e-09,0.673858 +4.45012,5,5,1.5708,4.62541e-08,6.2389e-09,0.677667 +4.50007,5,5,1.5708,4.62543e-08,6.23905e-09,0.681391 +4.5501,5,5,1.5708,4.62547e-08,6.23935e-09,0.688575 +4.60007,5,5,1.5708,4.62549e-08,6.23951e-09,0.691946 +4.65009,5,5,1.5708,4.62552e-08,6.23981e-09,0.698385 +4.70008,5,5,1.5708,4.62554e-08,6.23997e-09,0.701376 +4.75003,5,5,1.5708,4.62558e-08,6.24028e-09,0.707149 +4.80006,5,5,1.5708,4.6256e-08,6.24044e-09,0.709821 +4.85013,5,5,1.5708,4.62562e-08,6.24059e-09,0.712433 +4.90007,5,5,1.5708,4.62565e-08,6.24091e-09,0.717473 +4.95011,5,5,1.5708,4.62567e-08,6.24107e-09,0.719833 +5.00015,5,5,1.5708,4.62571e-08,6.24139e-09,0.724348 +5.05005,5,5,1.5708,4.62572e-08,6.24155e-09,0.726447 +5.10007,5,5,1.5708,4.62574e-08,6.24171e-09,0.728491 +5.15009,5,5,1.5708,4.62578e-08,6.24203e-09,0.732428 +5.20007,5,5,1.5708,4.6258e-08,6.24219e-09,0.734284 +5.25006,5,5,1.5708,4.62583e-08,6.24251e-09,0.737814 +5.30016,5,5,1.5708,0.130714,0.119012,0.738869 +5.35016,5,5,1.5708,0.480161,0.437879,0.74049 +5.40011,5,5,1.5708,0.642665,0.586529,0.741328 +5.4501,5,5,1.5708,0.727907,0.664606,0.742582 +5.5001,5,5,1.5708,0.727907,0.664606,0.746003 +5.55038,5,5,1.5708,0.82617,0.755573,0.74712 +5.60012,5,5,1.5708,1.12558,1.03333,0.748707 +5.65012,5,5,1.5708,1.26513,1.1631,0.749527 +5.70006,5,5,1.5708,1.40099,1.28966,0.750357 +5.75013,5,5,1.5708,1.52627,1.40655,0.752956 +5.80012,5,5,1.5708,1.60644,1.4818,0.754103 +5.85012,5,5,1.5708,1.8506,1.71153,0.75578 +5.90009,5,5,1.5708,1.87194,1.73165,0.757362 +5.95001,5,5,1.5708,1.91463,1.7721,0.758763 +6.00009,5,5,1.5708,2.13629,1.98262,0.760449 +6.05012,5,5,1.5708,2.18805,2.03187,0.761762 +6.10014,5,5,1.5708,2.18805,2.03187,0.765219 +6.15012,5,5,1.5708,2.25877,2.0999,0.766292 +6.20014,5,5,1.5708,2.45582,2.28987,0.767898 +6.25009,5,5,1.5708,2.54749,2.37846,0.768728 +6.30006,5,5,1.5708,2.63683,2.46494,0.76957 +6.35009,5,5,1.5708,2.72188,2.54741,0.772176 +6.40012,5,5,1.5708,2.72188,2.54741,0.77389 +6.45008,5,5,1.5708,2.81986,2.64351,0.776234 +6.50017,5,5,1.5708,2.8984,2.72069,0.777051 +6.55011,5,5,1.5708,3.05032,2.87033,0.778704 +6.60009,5,5,1.5708,3.05473,2.87469,0.780378 +6.6501,5,5,1.5708,3.09155,2.91121,0.781634 +6.70011,5,5,1.5708,3.22909,3.04795,0.7833 +6.75012,5,5,1.5708,3.24914,3.06792,0.784761 +6.80007,5,5,1.5708,3.33017,3.14913,0.787074 +6.85007,5,5,1.5708,3.39038,3.2096,0.787919 +6.90008,5,5,1.5708,3.42378,3.24318,0.78915 +6.95013,5,5,1.5708,3.42378,3.24318,0.792584 +7.00014,5,5,1.5708,3.45357,3.27347,0.793812 +7.05014,5,5,1.5708,3.56488,3.38684,0.795401 +7.10016,5,5,1.5708,3.61702,3.44008,0.796217 +7.15009,5,5,1.5708,3.71723,3.54265,0.797881 +7.20008,5,5,1.5708,3.7216,3.54713,0.79954 +7.25007,5,5,1.5708,3.74316,3.56936,0.800858 +7.3001,5,5,1.5708,3.83391,3.66311,0.802536 +7.35011,5,5,1.5708,3.84978,3.67955,0.803953 +7.40012,5,5,1.5708,3.84978,3.67955,0.80739 +7.45012,5,5,1.5708,3.88245,3.71373,0.808373 +7.50005,5,5,1.5708,3.92313,3.75636,0.809171 +7.55007,5,5,1.5708,4.00169,3.83887,0.810786 +7.60081,5,5,1.5708,4.03872,3.87786,0.81162 +7.65009,5,5,1.5708,4.07032,3.9112,0.814299 +7.70006,5,5,1.5708,4.09389,3.93621,0.815405 +7.75007,5,5,1.5708,4.15954,4.00604,0.817099 +7.80013,5,5,1.5708,4.16432,4.01113,0.818726 +7.8502,5,5,1.5708,4.16432,4.01113,0.820466 +7.90013,5,5,1.5708,4.19427,4.0434,0.823009 +7.95017,5,5,1.5708,4.22359,4.07504,0.823827 +8.00013,5,5,1.5708,4.28018,4.13625,0.825469 +8.05014,5,5,1.5708,4.28755,4.14424,0.826953 +8.1001,5,5,1.5708,4.32217,4.18199,0.829205 +8.15017,5,5,1.5708,4.34666,4.20875,0.830049 +8.20008,5,5,1.5708,4.35956,4.22287,0.831304 +8.25012,5,5,1.5708,4.35956,4.22287,0.834729 +8.30017,5,5,1.5708,4.37442,4.2393,0.835847 +8.35008,5,5,1.5708,4.41968,4.28945,0.837436 +8.40009,5,5,1.5708,4.44082,4.31293,0.838256 +8.45008,5,5,1.5708,4.4614,4.33582,0.839086 +8.50011,5,5,1.5708,4.4821,4.3589,0.841612 +8.55008,5,5,1.5708,4.49252,4.37059,0.842839 +8.60007,5,5,1.5708,4.52937,4.412,0.844523 +8.6501,5,5,1.5708,4.53474,4.41805,0.846001 +8.70017,5,5,1.5708,4.53474,4.41805,0.849448 +8.75007,5,5,1.5708,4.549,4.43428,0.850381 +8.80012,5,5,1.5708,4.56549,4.45308,0.851182 +8.8501,5,5,1.5708,4.59731,4.48945,0.852808 +8.90009,5,5,1.5708,4.60374,4.49681,0.854149 +8.95008,5,5,1.5708,4.62053,4.51616,0.856545 +9.0001,5,5,1.5708,4.63436,4.53212,0.857382 +9.05004,5,5,1.5708,4.64361,4.54282,0.8602 +9.10007,5,5,1.5708,4.65494,4.55601,0.861127 +9.15007,5,5,1.5708,4.66761,4.57076,0.861955 +9.20018,5,5,1.5708,4.69205,4.59931,0.863637 +9.2501,5,5,1.5708,4.69698,4.60509,0.865024 +9.30008,5,5,1.5708,4.69698,4.60509,0.868529 +9.35012,5,5,1.5708,4.70534,4.61497,0.86956 +9.40011,5,5,1.5708,4.72677,4.6404,0.87119 +9.4501,5,5,1.5708,4.7367,4.6522,0.872036 +9.50009,5,5,1.5708,4.74134,4.65773,0.873348 +9.55007,5,5,1.5708,4.74134,4.65773,0.876782 +9.60011,5,5,1.5708,4.74765,4.66532,0.877874 +9.65013,5,5,1.5708,4.76599,4.68746,0.879467 +9.70011,5,5,1.5708,4.77455,4.6978,0.88029 +9.75019,5,5,1.5708,4.78288,4.70789,0.881123 +9.80007,5,5,1.5708,4.79079,4.71749,0.883707 +9.8501,5,5,1.5708,4.79569,4.72348,0.884859 +9.90007,5,5,1.5708,4.81061,4.74174,0.886547 +9.95007,5,5,1.5708,4.81213,4.7436,0.888111 +10.0001,5,5,1.5708,4.81213,4.7436,0.89156 +10.0502,5,5,1.5708,4.81832,4.75127,0.892438 +10.1001,5,5,1.5708,4.825,4.75957,0.89324 +10.1502,5,5,1.5708,4.83787,4.7756,0.89487 +10.2001,5,5,1.5708,4.84029,4.77861,0.89624 +10.2501,5,5,1.5708,4.84727,4.78737,0.898615 +10.3001,5,5,1.5708,4.85286,4.7944,0.899454 +10.3501,5,5,1.5708,4.85644,4.79891,0.902305 +10.4001,5,5,1.5708,4.86134,4.80512,0.903181 +10.4501,5,5,1.5708,4.86645,4.81161,0.904012 +10.5001,5,5,1.5708,4.87089,4.81725,0.906654 +10.5502,5,5,1.5708,4.87406,4.8213,0.90777 +10.6001,5,5,1.5708,4.88327,4.83313,0.909438 +10.6501,5,5,1.5708,4.88395,4.83399,0.911039 +10.7001,5,5,1.5708,4.88581,4.83639,0.91238 +10.7501,5,5,1.5708,4.89416,4.84721,0.914057 +10.8001,5,5,1.5708,4.89586,4.84942,0.915419 +10.8501,5,5,1.5708,4.89586,4.84942,0.918857 +10.9001,5,5,1.5708,4.89864,4.85307,0.919896 +10.9501,5,5,1.5708,4.90605,4.86281,0.921493 +11.0002,5,5,1.5708,4.90949,4.86734,0.922321 +11.0501,5,5,1.5708,4.91285,4.87178,0.923158 +11.1002,5,5,1.5708,4.91594,4.87588,0.925777 +11.1502,5,5,1.5708,4.91783,4.8784,0.926958 +11.2004,5,5,1.5708,4.92387,4.88645,0.928647 +11.2501,5,5,1.5708,4.92431,4.88704,0.930265 +11.3002,5,5,1.5708,4.92431,4.88704,0.93199 +11.3502,5,5,1.5708,4.92762,4.89152,0.934338 +11.4003,5,5,1.5708,4.93027,4.89511,0.935156 +11.4501,5,5,1.5708,4.93286,4.89862,0.935981 +11.5001,5,5,1.5708,4.93784,4.90539,0.937662 +11.5501,5,5,1.5708,4.93863,4.90647,0.939129 +11.6001,5,5,1.5708,4.93863,4.90647,0.942617 +11.6501,5,5,1.5708,4.94053,4.90909,0.943561 +11.7001,5,5,1.5708,4.94273,4.91213,0.944372 +11.7501,5,5,1.5708,4.94698,4.91801,0.946018 +11.8001,5,5,1.5708,4.94772,4.91903,0.947428 +11.8501,5,5,1.5708,4.94772,4.91903,0.950849 +11.9001,5,5,1.5708,4.94934,4.92131,0.951775 +11.9501,5,5,1.5708,4.95122,4.92394,0.95257 +12.0001,5,5,1.5708,4.95484,4.92904,0.954185 +12.0501,5,5,1.5708,4.95654,4.93144,0.95502 +12.1001,5,5,1.5708,4.9579,4.93336,0.957748 +12.1502,5,5,1.5708,4.95907,4.93504,0.958798 +12.2001,5,5,1.5708,4.9621,4.93935,0.960488 +12.2501,5,5,1.5708,4.96218,4.93948,0.962193 +12.3002,5,5,1.5708,4.96218,4.93948,0.963922 +12.3501,5,5,1.5708,4.96373,4.94171,0.966345 +12.4002,5,5,1.5708,4.96507,4.94365,0.967161 +12.4501,5,5,1.5708,4.96766,4.94741,0.968805 +12.5001,5,5,1.5708,4.96781,4.94763,0.970419 +12.5501,5,5,1.5708,4.96833,4.94839,0.971748 +12.6001,5,5,1.5708,4.97066,4.95182,0.973409 +12.6501,5,5,1.5708,4.9711,4.95247,0.974784 +12.7002,5,5,1.5708,4.97238,4.95436,0.977165 +12.7501,5,5,1.5708,4.9734,4.95588,0.978006 +12.8001,5,5,1.5708,4.97406,4.95685,0.980864 +12.8501,5,5,1.5708,4.97496,4.95819,0.981743 +12.9001,5,5,1.5708,4.97589,4.95959,0.982576 +12.9501,5,5,1.5708,4.97667,4.96077,0.985251 +13.0001,5,5,1.5708,4.9773,4.96172,0.986317 +13.0501,5,5,1.5708,4.97899,4.96427,0.987991 +13.1001,5,5,1.5708,4.97909,4.96442,0.989625 +13.1502,5,5,1.5708,4.97942,4.96494,0.990971 +13.2001,5,5,1.5708,4.98095,4.96727,0.992653 +13.2501,5,5,1.5708,4.98126,4.96774,0.994018 +13.3001,5,5,1.5708,4.98126,4.96774,0.997465 +13.3502,5,5,1.5708,4.98176,4.96853,0.998507 +13.4001,5,5,1.5708,4.98311,4.97063,1.00011 +13.4501,5,5,1.5708,4.98374,4.9716,1.00094 +13.5001,5,5,1.5708,4.98435,4.97256,1.00178 +13.5505,5,5,1.5708,4.98493,4.97347,1.00438 +13.6001,5,5,1.5708,4.98528,4.97401,1.00556 +13.6501,5,5,1.5708,4.98637,4.97574,1.00726 +13.7001,5,5,1.5708,4.9864,4.97579,1.00896 +13.7502,5,5,1.5708,4.9864,4.97579,1.01069 +13.8001,5,5,1.5708,4.98698,4.97671,1.01309 +13.8502,5,5,1.5708,4.98746,4.97749,1.01391 +13.9001,5,5,1.5708,4.9884,4.97899,1.01556 +13.9502,5,5,1.5708,4.98844,4.97906,1.0172 +14.0001,5,5,1.5708,4.98865,4.9794,1.01848 +14.0501,5,5,1.5708,4.9895,4.98078,1.02015 +14.1002,5,5,1.5708,4.98965,4.98102,1.02155 +14.1501,5,5,1.5708,4.99011,4.98178,1.02394 +14.2001,5,5,1.5708,4.99048,4.98238,1.02478 +14.2501,5,5,1.5708,4.99071,4.98277,1.02765 +14.3001,5,5,1.5708,4.99104,4.98331,1.02853 +14.3502,5,5,1.5708,4.99137,4.98387,1.02936 +14.4001,5,5,1.5708,4.99168,4.98437,1.03199 +14.4501,5,5,1.5708,4.99189,4.98472,1.03311 +14.5002,5,5,1.5708,4.99249,4.98574,1.03479 +14.5501,5,5,1.5708,4.99255,4.98583,1.03638 +14.6001,5,5,1.5708,4.99255,4.98583,1.03811 +14.6502,5,5,1.5708,4.99282,4.9863,1.04063 +14.7002,5,5,1.5708,4.99309,4.98676,1.04144 +14.7501,5,5,1.5708,4.99361,4.98765,1.04307 +14.8001,5,5,1.5708,4.99369,4.98778,1.04452 +14.8501,5,5,1.5708,4.99399,4.98831,1.04681 +14.9003,5,5,1.5708,4.99421,4.9887,1.04764 +14.9503,5,5,1.5708,4.99434,4.98891,1.04886 +15.0002,5,5,1.5708,4.99456,4.98929,1.05139 +15.0501,5,5,1.5708,4.99476,4.98965,1.05223 +15.1001,5,5,1.5708,4.99516,4.99035,1.05391 +15.1502,5,5,1.5708,4.9952,4.99043,1.05546 +15.2001,5,5,1.5708,4.9952,4.99043,1.05894 +15.2502,5,5,1.5708,4.99537,4.99072,1.05983 +15.3002,5,5,1.5708,4.99554,4.99103,1.06064 +15.3501,5,5,1.5708,4.99588,4.99163,1.06228 +15.4001,5,5,1.5708,4.99593,4.99173,1.07425 +15.4502,5,5,1.5708,4.99593,4.99173,1.10831 +15.5001,5,5,1.5708,4.99593,4.99173,1.12424 +15.5501,5,5,1.5708,4.99593,4.99173,1.13974 +15.6001,5,5,1.5708,4.99593,4.99173,1.1696 +15.6501,5,5,1.5708,4.99593,4.99173,1.18372 +15.7001,5,5,1.5708,4.99593,4.99173,1.21046 +15.7501,5,5,1.5708,4.99593,4.99173,1.22301 +15.8001,5,5,1.5708,4.99593,4.99173,1.24697 +15.8502,5,5,1.5708,4.99593,4.99173,1.2581 +15.9001,5,5,1.5708,4.99593,4.99173,1.26895 +15.9501,5,5,1.5708,4.99593,4.99173,1.28985 +16.0001,5,5,1.5708,4.99593,4.99173,1.29969 +16.0501,5,5,1.5708,4.99593,4.99173,1.31841 +16.1002,5,5,1.5708,4.99593,4.99173,1.32716 +16.1501,5,5,1.5708,4.99593,4.99173,1.34393 +16.2001,5,5,1.5708,4.99593,4.99173,1.3517 +16.2502,5,5,1.5708,4.99593,4.99173,1.35928 +16.3001,5,5,1.5708,4.99593,4.99173,1.37389 +16.3504,5,5,1.5708,4.99593,4.99173,1.38075 +16.4002,5,5,1.5708,4.99593,4.99173,1.39385 +16.4501,5,5,1.5708,4.99593,4.99173,1.39995 +16.5001,5,5,1.5708,4.99593,4.99173,1.40588 +16.5501,5,5,1.5708,4.99593,4.99173,1.4173 +16.6002,5,5,1.5708,4.99593,4.99173,1.42269 +16.6501,5,5,1.5708,4.99593,4.99173,1.43292 +16.7001,5,5,1.5708,4.99593,4.99173,1.43771 +16.7502,5,5,1.5708,4.99593,4.99173,1.44687 +16.8001,5,5,1.5708,4.99593,4.99173,1.45113 +16.8502,5,5,1.5708,4.99593,4.99173,1.45528 +16.9002,5,5,1.5708,4.99593,4.99173,1.46328 +16.9502,5,5,1.5708,4.99593,4.99173,1.46704 +17.0001,5,5,1.5708,4.99593,4.99173,1.47421 +17.0501,5,5,1.5708,4.99593,4.99173,1.47755 +17.1001,5,5,1.5708,4.99593,4.99173,1.48396 +17.1501,5,5,1.5708,4.99593,4.99173,1.48694 +17.2001,5,5,1.5708,4.99593,4.99173,1.48984 +17.2502,5,5,1.5708,4.99593,4.99173,1.49544 +17.3001,5,5,1.5708,4.99593,4.99173,1.49807 +17.3502,5,5,1.5708,4.99593,4.99173,1.50309 +17.4001,5,5,1.5708,4.99593,4.99173,1.50543 +17.4501,5,5,1.5708,4.99593,4.99173,1.50993 +17.5001,5,5,1.5708,4.99593,4.99173,1.51201 +17.5502,5,5,1.5708,4.99593,4.99173,1.51405 +17.6001,5,5,1.5708,4.99593,4.99173,1.51797 +17.6501,5,5,1.5708,4.99593,4.99173,1.51981 diff --git a/workspace/limo_control/CMakeLists.txt b/workspace/limo_control/CMakeLists.txt index fb2bb23..5b47a13 100644 --- a/workspace/limo_control/CMakeLists.txt +++ b/workspace/limo_control/CMakeLists.txt @@ -10,4 +10,16 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} #install the controller launch file +) + +add_executable(limo_control_node src/p_controller.cpp) +ament_target_dependencies(limo_control_node rclcpp geometry_msgs nav_msgs) + +install(TARGETS + limo_control_node + DESTINATION lib/${PROJECT_NAME} +) + ament_package() \ No newline at end of file diff --git a/workspace/limo_control/launch/limo_control.launch.py b/workspace/limo_control/launch/limo_control.launch.py new file mode 100644 index 0000000..778184b --- /dev/null +++ b/workspace/limo_control/launch/limo_control.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + +import math +import os + +def generate_launch_description(): + + # Path to limo_simulation's launch file + limo_simulation_launch = os.path.join( + get_package_share_directory('limo_simulation'), + 'launch', + 'limo.launch.py' + ) + + # Include that launch file so that we can launch it from this file + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(limo_simulation_launch) + ) + + #launch the controller node + controller_node = Node( + package='limo_control', + executable='limo_control_node', + name='P_Controller_Node', + output='screen', + + parameters=[{ + #set target pose + 'target_x': 5.0, 'target_y': 5.0, 'target_theta': math.pi / 2, + #set controller gains + 'Kp_linear' : 1.0, 'Kp_angular' : 1.0, 'Kp_angle_trim' : 0.5, + #set tolerances + 'target_tolerance' : 0.01, 'target_angle_tolerance': 0.05 + }] + ) + + return LaunchDescription([ + simulation_launch, + controller_node + ]) \ No newline at end of file diff --git a/workspace/limo_control/src/p_controller.cpp b/workspace/limo_control/src/p_controller.cpp new file mode 100644 index 0000000..facbd85 --- /dev/null +++ b/workspace/limo_control/src/p_controller.cpp @@ -0,0 +1,230 @@ +#include +#include +#include +#include +#include +#include +#include + +//helper function to convert angles to [-pi, pi] +void convertAngle(double &angle){ + double converted_angle = std::fmod(angle, 2.0 * M_PI); + + if (converted_angle > M_PI){ + converted_angle -= 2 * M_PI; + } + else if(converted_angle < -M_PI){ + converted_angle += 2 * M_PI; + } + + angle = converted_angle; +} + +class P_Controller_Node : public rclcpp::Node +{ + public: + P_Controller_Node() : Node("P_Controller_Node") + { + //create publisher for cmd_vel + cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + //create subscriber for odometry + odom_sub_ = this->create_subscription("/odom", 10, std::bind(&P_Controller_Node::odomCallback, this, std::placeholders::_1)); + //create timer to call control loop + timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&P_Controller_Node::controllerLoop, this)); + + //initialize parameters with default values + this->declare_parameter("Kp_linear", 1.0); + this->declare_parameter("Kp_angular", 1.0); + this->declare_parameter("Kp_angle_trim", 0.5); + this->declare_parameter("target_x", 5.0); + this->declare_parameter("target_y", 5.0); + this->declare_parameter("target_theta", M_PI / 2); + this->declare_parameter("target_tolerance", 0.01); + this->declare_parameter("target_angle_tolerance", 0.05); + + //get the parameters from the launch file + Kp_linear_ = this->get_parameter("Kp_linear").as_double(); + Kp_angular_ = this->get_parameter("Kp_angular").as_double(); + Kp_angle_trim_ = this->get_parameter("Kp_angle_trim").as_double(); + + target_x_ = this->get_parameter("target_x").as_double(); + target_y_ = this->get_parameter("target_y").as_double(); + target_theta_ = this->get_parameter("target_theta").as_double(); + + target_tolerance_ = this->get_parameter("target_tolerance").as_double(); + target_angle_tolerance_ = this->get_parameter("target_angle_tolerance").as_double(); + + if(log_data){ + //open log file + log_file_.open("/root/workspace/controller_log.csv"); //directory to log file + + if (!log_file_.fail()){ + log_file_ << "Time,Target_X,Target_Y,Target_Theta,Current_X,Current_Y,Current_Theta\n"; + file_opened_successfully = true; + start_time_ = this->get_clock()->now().seconds(); + RCLCPP_INFO(this->get_logger(), "Log file opened successfully"); + } + else{ + RCLCPP_ERROR(this->get_logger(), "Failed to open log file"); + } + } + } + + ~P_Controller_Node(){ + if (log_file_.is_open()) { + //write log data to file + for (const auto& entry : log_data_vector_){ + log_file_ << entry[0] << "," << entry[1] << "," << entry[2] << "," << entry[3] << "," + << entry[4] << "," << entry[5] << "," << entry[6] << "\n"; + } + log_file_.close(); + } + } + + private: + /* FUNCTIONS */ + + //extracts the current x, y, and theta from odometry message + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg){ + current_x_ = msg->pose.pose.position.x; + current_y_ = msg->pose.pose.position.y; + + //extract quaternion + double x = msg->pose.pose.orientation.x; + double y = msg->pose.pose.orientation.y; + double z = msg->pose.pose.orientation.z; + double w = msg->pose.pose.orientation.w; + + //get yaw (theta) from quaternion + current_theta_ = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + + //update odom received flag + if (!odom_received_){ + odom_received_ = true; + } + } + + void controllerLoop(){ + + if (!odom_received_){//only run the loop once the robot has odom data + RCLCPP_INFO(this->get_logger(), "Waiting for odometry msg"); + return; + } + + if (target_pose_reached_){//if target already reached, do nothing + return; + } + + //set up message to be filled and published + geometry_msgs::msg::Twist cmd_vel_msg; + double vel_linear; + double vel_angular; + + //calculate distance and angle errors + double error_x = target_x_ - current_x_; + double error_y = target_y_ - current_y_; + double distance_error = std::sqrt(error_x * error_x + error_y * error_y); + + double angle_to_target = std::atan2(error_y, error_x); //required angle to turn to in order to head in right direction to target position + + //make sure that the angle to target is in [-pi, pi] to avoid large rotations + if(angle_to_target > M_PI || angle_to_target < -M_PI){ + convertAngle(angle_to_target); + } + + if (distance_error <= target_tolerance_){//robot is at correct x and y position + vel_linear = 0.0; //stop linear movement + + if (std::abs(current_theta_ - target_theta_) > target_angle_tolerance_){ //not at correct orientation yet, need to rotate + vel_angular = Kp_angular_ * (target_theta_ - current_theta_); + RCLCPP_INFO(this->get_logger(), "Final Rotation Phase: Target Theta: %.2f, Angular Velocity: %.2f", target_theta_, vel_angular); + } + + else { //robot is at correct pose, finished + vel_angular = 0.0; //stop angular movement + RCLCPP_INFO(this->get_logger(), "Target Reached"); + target_pose_reached_ = true; + } + } + + else{ //robot is NOT at correct x and y position + + if (std::abs(current_theta_ - angle_to_target) > target_angle_tolerance_){ //robot is not at the right spot AND not facing right direction, perform rotation to get robot on the correct direction to start moving towards target + vel_linear = 0.0; //robot should only rotate + vel_angular = Kp_angular_ * (angle_to_target - current_theta_); + + RCLCPP_INFO(this->get_logger(), "Initial Rotation Phase: Target Theta: %.2f, Angular Velocity: %.2f", angle_to_target, vel_angular); + } + + else{//robot is not at the right spot but IS facing right direction, drive towards target coords + vel_linear = Kp_linear_ * distance_error; + vel_angular = Kp_angle_trim_ * (angle_to_target - current_theta_); //small angle corrections while driving forward + + RCLCPP_INFO(this->get_logger(), "Driving Phase: Target X: %.2f, Target Y: %.2f, Linear Velocity: %.2f, Angular Velocity: %.2f", target_x_, target_y_, vel_linear, vel_angular); + } + } + + //publish twist message + cmd_vel_msg.linear.x = vel_linear; + cmd_vel_msg.angular.z = vel_angular; + cmd_vel_pub_->publish(cmd_vel_msg); + + //show current stats for logging + RCLCPP_INFO(this->get_logger(), "Current Position: X= %.2f, Y= %.2f, Theta= %.2f", current_x_, current_y_, current_theta_); + + if (log_data && file_opened_successfully && !target_pose_reached_){ + //store data in log data vector + double time_sec = this->get_clock()->now().seconds(); + double time_elapsed = time_sec - start_time_; + log_data_vector_.push_back({time_elapsed, target_x_, target_y_, target_theta_, current_x_, current_y_, current_theta_}); + } + } + + /* VARIABLES */ + + // Publishers and Subscribers + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + //timer + rclcpp::TimerBase::SharedPtr timer_; + + // Control parameters + double Kp_linear_ = 1.0; + double Kp_angular_ = 1.0; //for large rotations + double Kp_angle_trim_ = 0.5; //for small angle corrections while driving forward + + //target pose + double target_x_ = 5.0; + double target_y_ = 5.0; + double target_theta_ = M_PI / 2; + + //current pose + double current_x_ = 0; + double current_y_ = 0; + double current_theta_ = 0; //dummy values, these will be updated as soon as odom messages are received + + //tolerances + double target_tolerance_ = 0.01; //tolerance for distance to target + double target_angle_tolerance_ = 0.05; //tolerance for target angle + + //status flags + bool odom_received_ = false; + bool target_pose_reached_ = false; + bool log_data = true; + bool file_opened_successfully = false; + + //logging variables + std::ofstream log_file_; + double start_time_; + std::vector> log_data_vector_; + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/workspace/plot_data.py b/workspace/plot_data.py new file mode 100644 index 0000000..50e2f87 --- /dev/null +++ b/workspace/plot_data.py @@ -0,0 +1,63 @@ +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +import os + +# Load csv file +file_path = "workspace/controller_log.csv" +df = pd.read_csv(file_path) + +# compute errors +df['x_error'] = df['Target_X'] - df['Current_X'] +df['y_error'] = df['Target_Y'] - df['Current_Y'] + +# Create a folder to save plots +save_folder = "plots" +os.makedirs(save_folder, exist_ok=True) + +# plot x error as a function of time elapsed +plt.figure(figsize=(10, 5)) +plt.plot(df['Time'], df['x_error'], label='X Error', color='r') +plt.xlabel('Time (s)') +plt.ylabel('X Error (m)') +plt.title('X Error as a Function of Time') +plt.grid(True) +plt.tight_layout() +plt.savefig(os.path.join(save_folder, "x_error.png")) +plt.close() + +# plot y error as a function of time elapsed +plt.figure(figsize=(10, 5)) +plt.plot(df['Time'], df['y_error'], label='Y Error', color='b') +plt.xlabel('Time (s)') +plt.ylabel('Y Error (m)') +plt.title('Y Error as a Function of Time') +plt.grid(True) +plt.tight_layout() +plt.savefig(os.path.join(save_folder, "y_error.png")) +plt.close() + +# Calculate heading vector components for arrows +arrow_length = 0.2 # adjust for visibility +dx = arrow_length * np.cos(df['Current_Theta']) +dy = arrow_length * np.sin(df['Current_Theta']) + +# plot trajectory with heading arrows +# Create plot +plt.figure(figsize=(10, 10)) +plt.plot(df['Current_X'], df['Current_Y'], 'bo-', label='Robot Path') # line with markers +plt.quiver(df['Current_X'], df['Current_Y'], dx, dy, angles='xy', scale_units='xy', scale=1, color='r', label='Heading') + +plt.xlabel('X Position (m)') +plt.ylabel('Y Position (m)') +plt.title('Robot Path with Orientation') +plt.legend() +plt.grid(True) +plt.axis('equal') # keeps x and y scales equal + +# Save plot +plt.savefig(os.path.join(save_folder, "robot_path.png")) +plt.close() + +print(f"plots saved in folder: {save_folder}") +print("Done plotting")