diff --git a/.gitignore b/.gitignore
index b965222e..4a58af14 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,7 @@
 install*/
 log/
 build*/
+src/kindr
 *.vscode
 raman/
 
@@ -15,4 +16,5 @@ camera_setup/images/*
 # macOS Directory Information
 .DS_Store
 
-upgrade_pkg.tar.*
\ No newline at end of file
+upgrade_pkg.tar.*
+src/kindr
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_15-46-17.png b/pid_grapher/plots/joint_plot_2025-11-01_15-46-17.png
new file mode 100644
index 00000000..cbb175f8
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_15-46-17.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_15-46-24.png b/pid_grapher/plots/joint_plot_2025-11-01_15-46-24.png
new file mode 100644
index 00000000..4a3ae732
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_15-46-24.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_15-54-06.png b/pid_grapher/plots/joint_plot_2025-11-01_15-54-06.png
new file mode 100644
index 00000000..bc192de9
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_15-54-06.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_15-58-38.png b/pid_grapher/plots/joint_plot_2025-11-01_15-58-38.png
new file mode 100644
index 00000000..9c43a984
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_15-58-38.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_15-58-58.png b/pid_grapher/plots/joint_plot_2025-11-01_15-58-58.png
new file mode 100644
index 00000000..39210a1c
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_15-58-58.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_16-00-24.png b/pid_grapher/plots/joint_plot_2025-11-01_16-00-24.png
new file mode 100644
index 00000000..3fab72a0
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_16-00-24.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_16-02-30.png b/pid_grapher/plots/joint_plot_2025-11-01_16-02-30.png
new file mode 100644
index 00000000..39f2b6f8
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_16-02-30.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_16-07-57.png b/pid_grapher/plots/joint_plot_2025-11-01_16-07-57.png
new file mode 100644
index 00000000..836f5a8c
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_16-07-57.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-01_16-08-12.png b/pid_grapher/plots/joint_plot_2025-11-01_16-08-12.png
new file mode 100644
index 00000000..1043b7c5
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-01_16-08-12.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-06_01-23-51.png b/pid_grapher/plots/joint_plot_2025-11-06_01-23-51.png
new file mode 100644
index 00000000..6e27b096
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-06_01-23-51.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-06_01-24-10.png b/pid_grapher/plots/joint_plot_2025-11-06_01-24-10.png
new file mode 100644
index 00000000..ff90aeba
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-06_01-24-10.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-06_01-25-27.png b/pid_grapher/plots/joint_plot_2025-11-06_01-25-27.png
new file mode 100644
index 00000000..5370c9a7
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-06_01-25-27.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-22_16-31-43.png b/pid_grapher/plots/joint_plot_2025-11-22_16-31-43.png
new file mode 100644
index 00000000..0c80cbef
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-22_16-31-43.png differ
diff --git a/pid_grapher/plots/joint_plot_2025-11-22_16-31-54.png b/pid_grapher/plots/joint_plot_2025-11-22_16-31-54.png
new file mode 100644
index 00000000..823e345f
Binary files /dev/null and b/pid_grapher/plots/joint_plot_2025-11-22_16-31-54.png differ
diff --git a/requirements.txt b/requirements.txt
index 580be294..78be7bb0 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -14,7 +14,8 @@ RPi.GPIO
 rpi-hardware-pwm
 Jetson.GPIO
 python-can
+matplotlib
 ultralytics
 onnx
 onnxruntime
-odrive
\ No newline at end of file
+odrive
diff --git a/rosbag2_2025_10_16-00_10_00/metadata.yaml b/rosbag2_2025_10_16-00_10_00/metadata.yaml
new file mode 100644
index 00000000..d6cfe7d2
--- /dev/null
+++ b/rosbag2_2025_10_16-00_10_00/metadata.yaml
@@ -0,0 +1,32 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 14330097927
+  starting_time:
+    nanoseconds_since_epoch: 1760573403749726640
+  message_count: 1611
+  topics_with_message_count:
+    - topic_metadata:
+        name: /rover_arm_controller/joint_trajectory
+        type: trajectory_msgs/msg/JointTrajectory
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 1\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 177
+    - topic_metadata:
+        name: /joint_states
+        type: sensor_msgs/msg/JointState
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 1\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 1434
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_2025_10_16-00_10_00_0.db3
+  files:
+    - path: rosbag2_2025_10_16-00_10_00_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1760573403749726640
+      duration:
+        nanoseconds: 14330097927
+      message_count: 1611
\ No newline at end of file
diff --git a/rosbag2_2025_10_16-00_10_00/rosbag2_2025_10_16-00_10_00_0.db3 b/rosbag2_2025_10_16-00_10_00/rosbag2_2025_10_16-00_10_00_0.db3
new file mode 100644
index 00000000..343be2e0
Binary files /dev/null and b/rosbag2_2025_10_16-00_10_00/rosbag2_2025_10_16-00_10_00_0.db3 differ
diff --git a/src/kindr b/src/kindr
new file mode 160000
index 00000000..22a19ddd
--- /dev/null
+++ b/src/kindr
@@ -0,0 +1 @@
+Subproject commit 22a19ddd30ccfeba72bb2c35361e090bca7d16ec
diff --git a/src/pid_grapher/launch/pid_grapher.launch.py b/src/pid_grapher/launch/pid_grapher.launch.py
new file mode 100644
index 00000000..5d96f1a2
--- /dev/null
+++ b/src/pid_grapher/launch/pid_grapher.launch.py
@@ -0,0 +1,19 @@
+import launch
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return launch.LaunchDescription(
+        [
+            launch_ros.actions.Node(
+                package="pid_grapher",
+                executable="base_pid_grapher",
+                name="base_pid_grapher_node",
+            ),
+            launch_ros.actions.Node(
+                package="pid_grapher",
+                executable="base_pid_grapher",
+                name="base_pid_grapher_node",
+            ),
+        ]
+    )
diff --git a/src/pid_grapher/package.xml b/src/pid_grapher/package.xml
new file mode 100644
index 00000000..b4c33911
--- /dev/null
+++ b/src/pid_grapher/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pid_grapher</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="PSPuttaguna@gmail.com">vscode</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>python3-matplotlib</exec_depend>
+
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/src/pid_grapher/pid_grapher/__init__.py b/src/pid_grapher/pid_grapher/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/pid_grapher/pid_grapher/base_pid_grapher.py b/src/pid_grapher/pid_grapher/base_pid_grapher.py
new file mode 100644
index 00000000..05011607
--- /dev/null
+++ b/src/pid_grapher/pid_grapher/base_pid_grapher.py
@@ -0,0 +1,121 @@
+import math
+from datetime import datetime
+from .joint_data import JointData
+import rclpy
+from rclpy.node import Node
+import rclpy.time as time
+import matplotlib
+
+matplotlib.use("TkAgg")
+import matplotlib.pyplot as plt
+from sensor_msgs.msg import Joy
+from sensor_msgs.msg import JointState
+from trajectory_msgs.msg import JointTrajectory
+import os
+
+plt.rcParams["toolbar"] = "none"
+
+
+class BasePIDGrapher(Node):
+    def update_plots(self):
+        rate = 10
+        period = 1.0 / rate
+        for j in self.Joints.values():
+            j.plotGraph()
+        self.fig.canvas.draw_idle()
+        plt.pause(0.000000001)
+
+    def __init__(self):
+        super().__init__("base_pid_grapher")
+        self.subscription = self.create_subscription(
+            JointState,
+            "/joint_states",
+            self.plotter_callback,
+            10,
+        )
+        self.subscription_targets = self.create_subscription(
+            JointTrajectory,
+            "/arm_controller/joint_trajectory",
+            self.trajectory_callback,
+            10,
+        )
+
+        plt.ion()
+
+        joint_names = ["Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"]
+
+        n_plots = len(joint_names)
+        cols = 2
+        rows = math.ceil(n_plots / cols)
+
+        self.fig, self.axs = plt.subplots(rows, cols, figsize=(12, rows * 3))
+
+        # self.fig, self.axs = plt.subplots(3, 2, figsize=(12, 8))  # 3 rows × 2 columns
+        self.fig.subplots_adjust(
+            left=0.07, right=0.95, top=0.95, bottom=0.07, hspace=0.3, wspace=0.3
+        )
+        self.axs = self.axs.flatten()  # make it easy to index as a list
+        self.Joints = {}
+        for name, ax in zip(joint_names, self.axs):
+            self.Joints[name] = JointData(name, ax)
+
+        plt.show(block=False)
+
+        self.timer = self.create_timer(0.1, self.update_plots)
+
+        self.subscription
+
+        self.beginning = self.get_clock().now()
+
+        self.fig.canvas.mpl_connect("key_press_event", self.on_key)
+
+    def plotter_callback(self, msg):
+        now = self.get_clock().now()
+
+        delta = now - self.beginning
+        x = delta.nanoseconds / 1e9
+        for i in range(0, len(msg.name)):
+            if msg.name[i] in self.Joints:
+                Joint = self.Joints[msg.name[i]]
+                Joint.time = x
+                Joint.state = msg.position[i]
+
+    def _show_plot(self):
+        plt.show(block=True)
+
+    def trajectory_callback(self, msg):
+        if len(msg.points) > 0:
+            point = msg.points[-1]  # Get the last point in the trajectory
+            for i in range(0, len(msg.joint_names)):
+                jointName = msg.joint_names[i]
+                if jointName in self.Joints:
+                    self.Joints[jointName].target = point.positions[i]
+
+    def on_key(self, event):
+        if event.key == "p":
+            script_dir = os.path.join(os.getcwd(), "pid_grapher")
+            plots_dir = os.path.join(script_dir, "plots")
+            os.makedirs(plots_dir, exist_ok=True)
+
+            timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
+            save_path = os.path.join(plots_dir, f"joint_plot_{timestamp}.png")
+
+            plt.savefig(save_path, bbox_inches="tight", dpi=300)
+            print(f"Plot saved to {save_path}")
+        elif event.key == "r":
+            for j in self.Joints.values():
+                j.reset()
+            self.beginning = self.get_clock().now()
+            print("Reset data")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BasePIDGrapher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/pid_grapher/pid_grapher/joint_data.py b/src/pid_grapher/pid_grapher/joint_data.py
new file mode 100644
index 00000000..7b481747
--- /dev/null
+++ b/src/pid_grapher/pid_grapher/joint_data.py
@@ -0,0 +1,50 @@
+class JointData:
+    def __init__(self, name, ax):
+        self.name = name
+        self.time = 0
+        self.target = 0
+        self.state = 0
+        self.times = []
+        self.states = []
+        self.targetsHistory = []
+        self.ax = ax
+        self.ax.set_xlabel("Time (s)")
+        self.ax.set_ylabel("State")
+        self.ax.set_title(name)
+        self.ax.legend(["State", "Target"])
+        self.ax.grid()
+        self.line1 = self.ax.plot([], [], "-", label="State")[0]
+        self.line2 = self.ax.plot([], [], "--", label="Target")[0]
+
+        seconds = 30  # Number of seconds to display on the graph
+
+        self.MAX_POINTS = (
+            seconds * 10
+        ) - 50  # Maximum number of points to display on the graph (gotten from seconds)
+
+        self.ax.legend()
+
+
+    def plotGraph(self):
+        self.targetsHistory.append(round(self.target, 2))
+        self.times.append(round(self.time, 2))
+        self.states.append(round(self.state, 2))
+
+        if len(self.times) > self.MAX_POINTS:
+            self.times.pop(0)
+            self.states.pop(0)
+            self.targetsHistory.pop(0)
+
+        self.line1.set_xdata(self.times)
+        self.line2.set_xdata(self.times)
+
+        self.line1.set_ydata(self.states)
+        self.line2.set_ydata(self.targetsHistory)
+
+        self.ax.relim()
+        self.ax.autoscale_view()
+
+    def reset(self):
+        self.times = []
+        self.states = []
+        self.targetsHistory = []
diff --git a/src/pid_grapher/plots/joint_plot.png b/src/pid_grapher/plots/joint_plot.png
new file mode 100644
index 00000000..c64fa91b
Binary files /dev/null and b/src/pid_grapher/plots/joint_plot.png differ
diff --git a/src/pid_grapher/resource/pid_grapher b/src/pid_grapher/resource/pid_grapher
new file mode 100644
index 00000000..e69de29b
diff --git a/src/pid_grapher/setup.cfg b/src/pid_grapher/setup.cfg
new file mode 100644
index 00000000..c60af230
--- /dev/null
+++ b/src/pid_grapher/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pid_grapher
+[install]
+install_scripts=$base/lib/pid_grapher
diff --git a/src/pid_grapher/setup.py b/src/pid_grapher/setup.py
new file mode 100644
index 00000000..1da83986
--- /dev/null
+++ b/src/pid_grapher/setup.py
@@ -0,0 +1,31 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = "pid_grapher"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        (
+            os.path.join("share", package_name, "launch"),
+            glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+        ),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools", "matplotlib"],
+    zip_safe=True,
+    maintainer="vscode",
+    maintainer_email="PSPuttaguna@gmail.com",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "pid_grapher=pid_grapher.base_pid_grapher:main",
+        ],
+    },
+)
diff --git a/src/pid_grapher/test/test_copyright.py b/src/pid_grapher/test/test_copyright.py
new file mode 100644
index 00000000..ceffe896
--- /dev/null
+++ b/src/pid_grapher/test/test_copyright.py
@@ -0,0 +1,27 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/pid_grapher/test/test_flake8.py b/src/pid_grapher/test/test_flake8.py
new file mode 100644
index 00000000..ee79f31a
--- /dev/null
+++ b/src/pid_grapher/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/pid_grapher/test/test_pep257.py b/src/pid_grapher/test/test_pep257.py
new file mode 100644
index 00000000..a2c3deb8
--- /dev/null
+++ b/src/pid_grapher/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"