[DRAFT] A PID grapher for the current arm codebase (position target vs position real)

.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.*
+upgrade_pkg.tar.*
+src/kindr

requirements.txt

@@ -19,4 +19,5 @@ onnx
 onnxruntime
 onnxslim
 odrive
-smbus2
+smbus2
+matplotlib

src/Cameras/video_streaming/CMakeLists.txt

@@ -28,8 +28,6 @@ add_library(${PROJECT_NAME} SHARED
   src/webrtc_node.cpp
   src/srt_node.cpp
   src/srt_node_client.cpp
-  src/rtp_node.cpp
-  src/rtp_client_node.cpp
 )
 
 include_directories(
@@ -62,8 +60,6 @@ rclcpp_components_register_nodes(${PROJECT_NAME}
   "WebRtcNode"
   "video_streaming::SrtNode"
   "video_streaming::SrtClientNode"
-  "video_streaming::RtpNode"
-  "video_streaming::RtpClientNode"
 )
 
 # ---- Install

src/Cameras/video_streaming/include/video_streaming/input_node.hpp

@@ -43,5 +43,4 @@ class InputNode : public BaseVideoNode {
 private:
   std::map<std::string, size_t> source_map_;
   rclcpp::Service<interfaces::srv::VideoOut>::SharedPtr video_service_;
-  std::shared_ptr<interfaces::srv::VideoOut::Request> current_video_request_;
 };

src/Cameras/video_streaming/launch/rtp_client.launch.py → src/Cameras/video_streaming/launch/srt_client.launch.py

@@ -5,21 +5,21 @@
 
 def generate_launch_description():
 
-    rtp_client_node = ComposableNode(
+    srt_client_node = ComposableNode(
         package="video_streaming",
-        plugin="video_streaming::RtpClientNode",
-        name="rtp_client_node",
+        plugin="video_streaming::SrtClientNode",
+        name="srt_client_node",
         namespace="",
-        parameters=[{"port": 5004, "latency_ms": 40}],
+        parameters=[{"srt_uri": "srt://192.168.0.55:7001?mode=caller"}],
     )
 
     # Container_mt
     container = ComposableNodeContainer(
-        name="video_client_container",
+        name="test_client_container",
         namespace="",
         package="rclcpp_components",
         executable="component_container",
-        composable_node_descriptions=[rtp_client_node],
+        composable_node_descriptions=[srt_client_node],
         output="screen",
     )
 

src/Cameras/video_streaming/launch/video_streaming.launch.py

@@ -34,10 +34,10 @@ def generate_launch_description():
         parameters=[],
     )
 
-    rtp_node = ComposableNode(
+    srt_node = ComposableNode(
         package="video_streaming",
-        plugin="video_streaming::RtpNode",
-        name="rtp_node",
+        plugin="video_streaming::SrtNode",
+        name="srt_node",
         namespace="",
         parameters=[{"dest_ip": "192.168.0.20", "dest_port": 5004}],
     )
@@ -52,7 +52,7 @@ def generate_launch_description():
             input_node,
             detect_node,
             # streaming_node,
-            rtp_node,
+            srt_node,
         ],
         output="screen",
     )

src/Cameras/video_streaming/src/input_node.cpp

@@ -89,10 +89,6 @@ bool InputNode::create_pipeline() {
   }
 
   pipeline_ = p;
-  if (current_video_request_) {
-    video_cb(current_video_request_,
-             std::make_shared<interfaces::srv::VideoOut::Response>());
-  }
   return true;
 }
 
@@ -171,9 +167,6 @@ void InputNode::video_cb(
     RCLCPP_ERROR(this->get_logger(), "Failed to start pipeline");
     response->success = false;
   }
-  if (response->success) {
-    current_video_request_ = request;
-  }
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(InputNode)