Merge pull request #14 from FrankVanris2/velocity_commandFix

Updated Code 2.0

Author: j03hoang

FollowBotAROS2/.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/jazzy/include/**",
+        "/home/ubuntu/myProjects/ROS2_FollowBot/FollowBotAROS2/src/arduino_serial/include/**",
+        "/home/ubuntu/myProjects/ROS2_FollowBot/FollowBotAROS2/src/ldlidar_sl_ros2/include/**",
+        "/home/ubuntu/myProjects/ROS2_FollowBot/FollowBotAROS2/src/mapping/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-arm64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

FollowBotAROS2/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/ubuntu/myProjects/ROS2_FollowBot/FollowBotAROS2/install/path_making/lib/python3.12/site-packages",
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/ubuntu/myProjects/ROS2_FollowBot/FollowBotAROS2/install/path_making/lib/python3.12/site-packages",
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ]
+}

FollowBotAROS2/src/arduino_serial/src/cmd_vel_serial_node.cpp

@@ -21,27 +21,33 @@ class cmd_velSerialNode : public rclcpp::Node {
 			"/cmd_vel", 10,
 			std::bind(&cmd_velSerialNode::cmd_velCallback, this, std::placeholders::_1)
 		);
-
+		
 		RCLCPP_INFO(this->get_logger(), "Started cmd_velSerialNode");
 	}
 
  private:
 	//TODO: send as JSON
 	void cmd_velCallback(geometry_msgs::msg::Twist::SharedPtr msg) {
-		nlohmann::json js; 
+		RCLCPP_INFO(this->get_logger(), "cmd_velCallback: linear.x = %f, angular.z = %f", msg->linear.x, msg->angular.z);
+		nlohmann::json js;
+		
+		js["sensor_type"] = "cmd_vel"; // Add sensor_type field
 
-		js["sensor_type"] = "cmd_vel";
+		// Creating a data object
+		nlohmann::json data_obj;
 
-		js["linear"]["x"] = msg->linear.x;
-		js["linear"]["y"] = msg->linear.y;
-		js["linear"]["z"] = msg->linear.z;
+		data_obj["linear"]["x"] = msg->linear.x;
+		data_obj["linear"]["y"] = msg->linear.y;
+		data_obj["linear"]["z"] = msg->linear.z;
 
 		// Add angular velocity components
-		js["angular"]["x"] = msg->angular.x;
-		js["angular"]["y"] = msg->angular.y;
-		js["angular"]["z"] = msg->angular.z;
+		data_obj["angular"]["x"] = msg->angular.x;
+		data_obj["angular"]["y"] = msg->angular.y;
+		data_obj["angular"]["z"] = msg->angular.z;
 
-		std::string json_str = js.dump(4);
+		// Assign the populated "data" object to the main JSON
+		js["data"] = data_obj;
+		std::string json_str = js.dump(-1); // Use 4 for pretty printing if desired, or -1 for compact
 
 		SerialManager::get().write_line(json_str.c_str());
 		RCLCPP_INFO(this->get_logger(), "published json: \n\t%s", json_str.c_str());

FollowBotAROS2/src/arduino_serial/src/include/serial_manager.hpp

@@ -1,14 +1,18 @@
 #pragma once
 #include <libserial/SerialPort.h>
+#include <libserial/SerialStream.h>
 #include <nlohmann/json.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <iostream>
 #include <thread>
 #include <atomic>
 #include <queue>
 #include <mutex>
 #include <optional>
 #include <string>
+#include <algorithm> // For std::remove_if, std::isspace
+#include <cctype> 	 // For std::isspace
 
 class SerialManager {
  public:
@@ -50,13 +54,26 @@ class SerialManager {
 	 /**
 	  * Sends a string through serial port
 	  */
-	 void write_line(std::string out) {
+	 void write_line(const std::string& line_to_write) {
+		RCLCPP_INFO(rclcpp::get_logger("serial_manager"), 
+					"Attempting to write to serial: %s", line_to_write.c_str()); 
 		std::lock_guard<std::mutex> lock(port_mutex_);
-		RCLCPP_INFO(rclcpp::get_logger("serial_manager"), "In WriteLine()");
-		port_.Write(out);
+		if (port_.IsOpen()) {
+			try {
+				port_.Write(line_to_write);
+				port_.Write("\n"); // Ensure a newline is sent if arduino expects it for each JSON Message
+				RCLCPP_INFO(rclcpp::get_logger("serial_manager"), 
+							"Serial write successful");
+			} catch (const std::exception& e) {
+				RCLCPP_ERROR(rclcpp::get_logger("serial_manager"), 
+							"Serial write error: %s", e.what());
+			}
+		} else {
+			RCLCPP_WARN(rclcpp::get_logger("serial_manager"), 
+						"Serial port not open. Cannot write.");
+		}
      }
 
-	
 	 /**
 	 * pop one message for each message type
 	 * return std::nullopt if queue empty
@@ -90,16 +107,50 @@ class SerialManager {
 		std::lock_guard<std::mutex> lock(port_mutex_);
         if (port_.IsDataAvailable()) {
             port_.ReadLine(out);
+			//RCLCPP_INFO(rclcpp::get_logger("serial_manager"), "Received: %s", out.c_str()); // NEED TO REMOVE
             return true;
         }
         return false;
      }
 
+	 /**
+	 * trim lines to remove leading/trailing whitespace
+	 * If there is whitespace, remove it
+	 * @param line the line to trim
+	 */
+	 std::string trim(const std::string& line) {
+		size_t first = line.find_first_not_of(" \t\n\r");
+		if (std::string::npos == first) {
+			return line; // No non-whitespace characters found.
+		}
+		size_t last = line.find_last_not_of(" \t\n\r");
+		return line.substr(first, (last - first + 1));
+	 }
+
 	 bool read_and_demux() {
 		std::string line;
         if (!read_line(line)) 
 			return false;
 
+		// --- THE CRITICAL STEP: TRIM THE LINE ---
+		std::string trimmed_line = trim(line); // only needed for debugging purposes.
+		
+		// FOR DEBUGGING PURPOSES
+		// Check if error occurs on arduino side for Json Deserialization
+		if (!trimmed_line.empty() && trimmed_line == "JSON_ERROR_ACK") {
+			RCLCPP_INFO(rclcpp::get_logger("serial_manager"), 
+				"Arduino acknowledged JSON error");
+			return true;
+		}
+
+		// FOR DEBUGGING PURPOSES
+		// Check for specific confirmation message first
+		if (!trimmed_line.empty() && trimmed_line.find("ARDUINO_CMD_VEL_ACK") == 0) {
+			RCLCPP_INFO(rclcpp::get_logger("serial_manager"),
+				"Arduino acknowledged cmd_vel receipt ACKNOWLEDGED");
+			return true; // Successfully handled this line
+		}
+
         // message validation
         if (line.empty() || line.front() != '{') 
             return false;

FollowBotAROS2/src/arduino_serial/src/serial_manager_node.cpp

@@ -16,6 +16,7 @@ class SerialManagerNode : public rclcpp::Node {
 		RCLCPP_INFO(get_logger(), 
 			"SerialManager initialized on %s.", port_name.c_str());
 	 }
+
 };
 
 #include <rclcpp_components/register_node_macro.hpp>

README.md

@@ -84,10 +84,6 @@ sudo apt install libserial-dev
 ```
 ```bash
 sudo apt update
-sudo apt install libbluetooth-dev
-```
-```bash
-sudo apt update
 sudo apt install libgeographiclib-dev
 ```
 ```bash