From bfbf2bbf996c120be28ee3ac088103692feb1cd8 Mon Sep 17 00:00:00 2001 From: anurag Date: Mon, 25 Nov 2024 19:40:08 +0530 Subject: [PATCH 1/2] Fixed only one motor rotating now both motor rotating following diff drive --- roboteq_controller/CMakeLists.txt | 4 +- .../src/roboteq_controller_node.cpp | 77 +++++++++++-------- 2 files changed, 50 insertions(+), 31 deletions(-) diff --git a/roboteq_controller/CMakeLists.txt b/roboteq_controller/CMakeLists.txt index e82037c..fa02915 100755 --- a/roboteq_controller/CMakeLists.txt +++ b/roboteq_controller/CMakeLists.txt @@ -26,6 +26,8 @@ find_package(rcutils REQUIRED) find_package(roboteq_interfaces REQUIRED) find_package(serial REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + include_directories(include) include_directories(/tmp/usr/local/include/) @@ -33,7 +35,7 @@ include_directories(/tmp/usr/local/include/) set(node_plugins "") add_executable(roboteq_controller_node src/roboteq_controller_node.cpp) -ament_target_dependencies(roboteq_controller_node rclcpp std_msgs roboteq_interfaces serial geometry_msgs) +ament_target_dependencies(roboteq_controller_node rclcpp std_msgs roboteq_interfaces serial geometry_msgs nav_msgs) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/roboteq_controller/src/roboteq_controller_node.cpp b/roboteq_controller/src/roboteq_controller_node.cpp index 7d2c75f..1b7814e 100755 --- a/roboteq_controller/src/roboteq_controller_node.cpp +++ b/roboteq_controller/src/roboteq_controller_node.cpp @@ -216,40 +216,57 @@ void RoboteqDriver::powerCmdCallback(const geometry_msgs::msg::Twist::SharedPtr } -void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg){ - // wheel speed (m/s) - float right_speed = msg->linear.x + track_width_ * msg->angular.z / 2.0; - float left_speed = msg->linear.x - track_width_ * msg->angular.z / 2.0; - - std::stringstream cmd_str; - - if (!closed_loop_){ - // motor power (scale 0-1000) - float right_power = right_speed *1000.0 *60.0/ (wheel_circumference_ * max_rpm_); - float left_power = left_speed *1000.0 *60.0/ (wheel_circumference_ * max_rpm_); - RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9d right: %9d", (int)left_power, (int)right_power); - cmd_str << "!G 1" - << " " << (int)left_power << "_" - << "!G 2" - << " " << (int)right_power << "_"; - } - else{ - // motor speed (rpm) - int32_t right_rpm = gear_reduction_ * right_speed * 60.0 / wheel_circumference_; - int32_t left_rpm = gear_reduction_ * left_speed * 60.0 / wheel_circumference_; - RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9d right: %9d", left_rpm, right_rpm); - cmd_str << "!S 1" - << " " << left_rpm << "_" - << "!S 2" - << " " << right_rpm << "_"; - } - - ser_.write(cmd_str.str()); - ser_.flush(); +void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + std::stringstream cmd_str; + + // Calculate right and left speed based on linear and angular velocities + float right_speed = msg->linear.x + track_width_ * msg->angular.z / 2.0; + float left_speed = msg->linear.x - track_width_ * msg->angular.z / 2.0; + + if (!closed_loop_) { + // Calculate motor power in open-loop (scale 0-1000) + float right_power = right_speed * 1000.0 * 60.0 / (wheel_circumference_ * max_rpm_); + float left_power = left_speed * 1000.0 * 60.0 / (wheel_circumference_ * max_rpm_); + + RCLCPP_INFO(this->get_logger(), "[ROBOTEQ] left: %9d right: %9d", (int)left_power, (int)right_power); + + // For linear velocity (move forward/backward), command motor 1 (G1) and stop motor 2 (G2) + if (msg->linear.x != 0) { + cmd_str << "!G 1" << " " << (int)left_power << "_"; // Command for motor 1 (linear velocity) + cmd_str << "!G 2" << " 0\r"; // Stop motor 2 (angular velocity set to 0) + } + // For angular velocity (turning), command motor 2 (G2) and stop motor 1 (G1) + else if (msg->angular.z != 0) { + cmd_str << "!G 1" << " 0\r"; // Stop motor 1 (linear velocity set to 0) + cmd_str << "!G 2" << " " << (int)right_power << "_"; // Command for motor 2 (angular velocity) + } + } else { + // Calculate motor RPM in closed-loop (rpm) + int32_t right_rpm = gear_reduction_ * right_speed * 60.0 / wheel_circumference_; + int32_t left_rpm = gear_reduction_ * left_speed * 60.0 / wheel_circumference_; + + RCLCPP_INFO(this->get_logger(), "[ROBOTEQ] left: %9d right: %9d", left_rpm, right_rpm); + + // For linear velocity (move forward/backward), command motor 1 (G1) and stop motor 2 (G2) + if (msg->linear.x != 0) { + cmd_str << "!G 1" << " " << left_rpm << "_"; // Command for motor 1 (linear velocity) + cmd_str << "!G 2" << " 0\r"; // Stop motor 2 (angular velocity set to 0) + } + // For angular velocity (turning), command motor 2 (G2) and stop motor 1 (G1) + else if (msg->angular.z != 0) { + cmd_str << "!G 1" << " 0\r"; // Stop motor 1 (linear velocity set to 0) + cmd_str << "!G 2" << " " << right_rpm << "_"; // Command for motor 2 (angular velocity) + } + } + + // Send the constructed command string to the serial port + ser_.write(cmd_str.str()); + ser_.flush(); } + void RoboteqDriver::configService(const std::shared_ptr request, std::shared_ptr response){ std::stringstream str; str << "^" << request->user_input << " " << request->channel << " " << request->value << "_ " << "%%clsav321654987"; From 93dba974a1562568565ff6e748f0754f657307a3 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 28 Nov 2024 12:34:28 +0530 Subject: [PATCH 2/2] Fixed:opposite direction rotation in diff-driver and Added : odom --- .vscode/settings.json | 3 + roboteq_controller/CMakeLists.txt | 1 + roboteq_controller/launch/driver.py | 8 + roboteq_controller/scripts/diff_tf.py | 188 +++++++++++++ .../src/roboteq_controller_node.cpp | 252 ++++++++++-------- 5 files changed, 337 insertions(+), 115 deletions(-) create mode 100644 .vscode/settings.json create mode 100755 roboteq_controller/scripts/diff_tf.py diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5d7d730 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "ros.distro": "noetic" +} \ No newline at end of file diff --git a/roboteq_controller/CMakeLists.txt b/roboteq_controller/CMakeLists.txt index fa02915..607f5b3 100755 --- a/roboteq_controller/CMakeLists.txt +++ b/roboteq_controller/CMakeLists.txt @@ -59,6 +59,7 @@ install(DIRECTORY install(PROGRAMS scripts/roboteq_proc_node.py + scripts/diff_tf.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/roboteq_controller/launch/driver.py b/roboteq_controller/launch/driver.py index 530b4d2..0480e16 100755 --- a/roboteq_controller/launch/driver.py +++ b/roboteq_controller/launch/driver.py @@ -27,7 +27,15 @@ def generate_launch_description(): executable = 'roboteq_proc_node.py', namespace= 'roboteq' ) + node3=Node( + package = 'roboteq_controller', + name = 'diff_odom', + executable = 'diff_tf.py', + namespace= 'roboteq' + ) + ld.add_action(node1) ld.add_action(node2) + ld.add_action(node3) return ld \ No newline at end of file diff --git a/roboteq_controller/scripts/diff_tf.py b/roboteq_controller/scripts/diff_tf.py new file mode 100755 index 0000000..975d6bf --- /dev/null +++ b/roboteq_controller/scripts/diff_tf.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from math import sin, cos, pi + +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TransformStamped +from nav_msgs.msg import Odometry +from tf2_ros import TransformBroadcaster +from std_msgs.msg import Int32 +from roboteq_interfaces.msg import ChannelValues +NS_TO_SEC = 1000000000 + + +class DiffTf(Node): + """ + diff_tf.py - follows the output of a wheel encoder and + creates tf and odometry messages. + some code borrowed from the arbotix diff_controller script + A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ + """ + + def __init__(self): + super().__init__("diff_tf") + self.nodename = "diff_tf" + self.get_logger().info(f"-I- {self.nodename} started") + + #### parameters ####### + self.rate_hz = self.declare_parameter("rate_hz", 10.0).value # the rate at which to publish the transform + self.create_timer(1.0 / self.rate_hz, self.update) + + self.ticks_meter = float( + self.declare_parameter('ticks_meter', 537).value) # The number of wheel encoder ticks per meter of travel + self.base_width = float(self.declare_parameter('base_width', 0.56).value) # The wheel base width in meters + + self.base_frame_id = self.declare_parameter('base_frame_id', + 'base_link').value # the name of the base frame of the robot + self.odom_frame_id = self.declare_parameter('odom_frame_id', + 'odom').value # the name of the odometry reference frame + + self.encoder_min = self.declare_parameter('encoder_min', -2147483648).value + self.encoder_max = self.declare_parameter('encoder_max', 2147483648).value + self.encoder_low_wrap = self.declare_parameter('wheel_low_wrap', ( + self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min).value + self.encoder_high_wrap = self.declare_parameter('wheel_high_wrap', ( + self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min).value + + # internal data + self.enc_left = None # wheel encoder readings + self.enc_right = None + self.left = 0.0 # actual values coming back from robot + self.right = 0.0 + self.lmult = 0.0 + self.rmult = 0.0 + self.prev_lencoder = 0 + self.prev_rencoder = 0 + self.x = 0.0 # position in xy plane + self.y = 0.0 + self.th = 0.0 + self.dx = 0.0 # speeds in x/rotation + self.dr = 0.0 + self.then = self.get_clock().now() + + # subscriptions + self.create_subscription(ChannelValues, "/roboteq/hall_count", self.lwheel_callback, 10) + self.create_subscription(ChannelValues, "/roboteq/hall_count", self.rwheel_callback, 10) + self.odom_pub = self.create_publisher(Odometry, "odom", 10) + self.odom_broadcaster = TransformBroadcaster(self) + + self.get_logger().info("Subscribed to /roboteq/hall_count topics") + + def update(self): + now = self.get_clock().now() + elapsed = now - self.then + self.then = now + elapsed = elapsed.nanoseconds / NS_TO_SEC + + if elapsed <= 0: + self.get_logger().warn("Elapsed time is zero or negative.") + return + + if self.enc_left is None: + d_left = 0 + d_right = 0 + else: + d_left = (self.left - self.enc_left) / self.ticks_meter + d_right = (self.right - self.enc_right) / self.ticks_meter + + self.enc_left = self.left + self.enc_right = self.right + d = (d_left + d_right) / 2 + print(d_left) + th = -(d_right - d_left) / self.base_width + + self.dx = d / elapsed + self.dr = th / elapsed + + if d != 0: + x = cos(th) * d + y = -sin(th) * d + self.x += (cos(self.th) * x - sin(self.th) * y) + self.y += (sin(self.th) * x + cos(self.th) * y) + if th != 0: + self.th += th + + quaternion = Quaternion() + quaternion.x = 0.0 + quaternion.y = 0.0 + quaternion.z = sin(self.th / 2) + quaternion.w = cos(self.th / 2) + + transform_stamped_msg = TransformStamped() + transform_stamped_msg.header.stamp = now.to_msg() + transform_stamped_msg.header.frame_id = self.odom_frame_id # Should be "odom" + transform_stamped_msg.child_frame_id = self.base_frame_id # Should be "base_link" + transform_stamped_msg.transform.translation.x = self.x + transform_stamped_msg.transform.translation.y = self.y + transform_stamped_msg.transform.translation.z = 0.0 + transform_stamped_msg.transform.rotation.x = quaternion.x + transform_stamped_msg.transform.rotation.y = quaternion.y + transform_stamped_msg.transform.rotation.z = quaternion.z + transform_stamped_msg.transform.rotation.w = quaternion.w + + self.odom_broadcaster.sendTransform(transform_stamped_msg) + + odom = Odometry() + odom.header.stamp = now.to_msg() + odom.header.frame_id = self.odom_frame_id + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation = quaternion + odom.child_frame_id = self.base_frame_id + odom.twist.twist.linear.x = self.dx + odom.twist.twist.linear.y = 0.0 + odom.twist.twist.angular.z = self.dr + self.odom_pub.publish(odom) + + # Debugging logs + # self.get_logger().info(f"Left Encoder: {self.left}, Right Encoder: {self.right}") + # self.get_logger().info(f"Distance Left: {d_left}, Distance Right: {d_right}, Distance: {d}, Theta: {th}") + # self.get_logger().info(f"Position - x: {self.x}, y: {self.y}, theta: {self.th}") + # self.get_logger().info(f"Velocities - linear: {self.dx}, angular: {self.dr}") + + + + + def lwheel_callback(self, msg): + enc = msg.value[1] + + # self.get_logger().info(f"Left encoder ticks received: {enc}") + if enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap: + self.lmult = self.lmult + 1 + + if enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap: + self.lmult = self.lmult - 1 + + self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) + self.prev_lencoder = enc + + def rwheel_callback(self, msg): + enc = msg.value[0] + # self.get_logger().info(f"Right encoder ticks received: {enc}") + if enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap: + self.rmult = self.rmult + 1 + + if enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap: + self.rmult = self.rmult - 1 + + self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min)) + self.prev_rencoder = enc + + +def main(args=None): + rclpy.init(args=args) + try: + diff_tf = DiffTf() + rclpy.spin(diff_tf) + except rclpy.exceptions.ROSInterruptException: + pass + + diff_tf.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/roboteq_controller/src/roboteq_controller_node.cpp b/roboteq_controller/src/roboteq_controller_node.cpp index 1b7814e..602b5d2 100755 --- a/roboteq_controller/src/roboteq_controller_node.cpp +++ b/roboteq_controller/src/roboteq_controller_node.cpp @@ -143,36 +143,47 @@ RoboteqDriver::RoboteqDriver(const rclcpp::NodeOptions &options): Node("roboteq_ void RoboteqDriver::cmdSetup(){ - // stop motors - ser_.write("!G 1 0\r"); - ser_.write("!G 2 0\r"); - ser_.write("!S 1 0\r"); - ser_.write("!S 2 0\r"); - ser_.flush(); - - // // disable echo - // ser.write("^ECHOF 1\r"); - // ser.flush(); - - // enable watchdog timer (1000 ms) to stop if no connection - ser_.write("^RWD 1000\r"); + // stop motors + std::string stop_motor_1 = "!G 1 0\r"; + std::string stop_motor_2 = "!G 2 0\r"; + std::string stop_motor_s1 = "!S 1 0\r"; + std::string stop_motor_s2 = "!S 2 0\r"; + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", stop_motor_1.c_str()); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", stop_motor_2.c_str()); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", stop_motor_s1.c_str()); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", stop_motor_s2.c_str()); + ser_.write(stop_motor_1); + ser_.write(stop_motor_2); + ser_.write(stop_motor_s1); + ser_.write(stop_motor_s2); + ser_.flush(); - // set motor operating mode (1 for closed-loop speed) - if (!closed_loop_){ - // open-loop speed mode - ser_.write("^MMOD 1 0\r"); - ser_.write("^MMOD 2 0\r"); - ser_.flush(); - } - else{ - // closed-loop speed mode - ser_.write("^MMOD 1 1\r"); - ser_.write("^MMOD 2 1\r"); - ser_.flush(); - } + // enable watchdog timer (1000 ms) to stop if no connection + std::string watchdog = "^RWD 1000\r"; + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", watchdog.c_str()); + ser_.write(watchdog); + + // set motor operating mode (1 for closed-loop speed) + if (!closed_loop_){ + std::string open_loop_motor_1 = "^MMOD 1 0\r"; + std::string open_loop_motor_2 = "^MMOD 2 0\r"; + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", open_loop_motor_1.c_str()); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", open_loop_motor_2.c_str()); + ser_.write(open_loop_motor_1); + ser_.write(open_loop_motor_2); + } else { + std::string closed_loop_motor_1 = "^MMOD 1 1\r"; + std::string closed_loop_motor_2 = "^MMOD 2 1\r"; + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", closed_loop_motor_1.c_str()); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", closed_loop_motor_2.c_str()); + ser_.write(closed_loop_motor_1); + ser_.write(closed_loop_motor_2); + } + ser_.flush(); } + void RoboteqDriver::run(){ initializeServices(); std::stringstream ss0, ss1; @@ -197,24 +208,26 @@ void RoboteqDriver::run(){ void RoboteqDriver::powerCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg){ - std::stringstream cmd_str; - if (closed_loop_){ - cmd_str << "!S 1" - << " " << msg->linear.x << "_" - << "!S 2" - << " " << msg->angular.z << "_"; - } - else{ - cmd_str << "!G 1" - << " " << msg->linear.x << "_" - << "!G 2" - << " " << msg->angular.z << "_"; - } - ser_.write(cmd_str.str()); - ser_.flush(); - RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9.3f right: %9.3f", msg->linear.x, msg->angular.z); -} + std::stringstream cmd_str; + if (closed_loop_){ + cmd_str << "!G 1" + << " " << msg->linear.x << "_" + << "!G 2" + << " " << msg->angular.z << "_"; + } + else{ + cmd_str << "!G 1" + << " " << msg->linear.x << "_" + << "!G 2" + << " " << msg->angular.z << "_"; + } + std::string cmd = cmd_str.str(); + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Writing to serial: %s", cmd.c_str()); + ser_.write(cmd); + ser_.flush(); + RCLCPP_INFO(this->get_logger(), "[ROBOTEQ] left: %9.3f right: %9.3f", msg->linear.x, msg->angular.z); +} void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { std::stringstream cmd_str; @@ -222,11 +235,19 @@ void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr ms // Calculate right and left speed based on linear and angular velocities float right_speed = msg->linear.x + track_width_ * msg->angular.z / 2.0; float left_speed = msg->linear.x - track_width_ * msg->angular.z / 2.0; - + // Check if both speeds are zero to stop the motor + if (msg->linear.x == 0.0 && msg->angular.z == 0.0) { + // Stop both motors + std::string stop_cmd = "!G 1 0\r!G 2 0\r"; + RCLCPP_INFO(this->get_logger(), "[RoboteQ] Stopping motors"); + ser_.write(stop_cmd); + ser_.flush(); + return; + } if (!closed_loop_) { // Calculate motor power in open-loop (scale 0-1000) - float right_power = right_speed * 1000.0 * 60.0 / (wheel_circumference_ * max_rpm_); - float left_power = left_speed * 1000.0 * 60.0 / (wheel_circumference_ * max_rpm_); + float right_power = right_speed * 500.0 * 60.0 / (wheel_circumference_ * max_rpm_); + float left_power = left_speed * 80.0 * 60.0 / (wheel_circumference_ * max_rpm_); RCLCPP_INFO(this->get_logger(), "[ROBOTEQ] left: %9d right: %9d", (int)left_power, (int)right_power); @@ -242,7 +263,7 @@ void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr ms } } else { // Calculate motor RPM in closed-loop (rpm) - int32_t right_rpm = gear_reduction_ * right_speed * 60.0 / wheel_circumference_; + int32_t right_rpm = - gear_reduction_ * right_speed * 60.0 / wheel_circumference_; int32_t left_rpm = gear_reduction_ * left_speed * 60.0 / wheel_circumference_; RCLCPP_INFO(this->get_logger(), "[ROBOTEQ] left: %9d right: %9d", left_rpm, right_rpm); @@ -266,7 +287,6 @@ void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr ms - void RoboteqDriver::configService(const std::shared_ptr request, std::shared_ptr response){ std::stringstream str; str << "^" << request->user_input << " " << request->channel << " " << request->value << "_ " << "%%clsav321654987"; @@ -308,77 +328,79 @@ void RoboteqDriver::initializeServices(){ void RoboteqDriver::queryCallback(){ - auto current_time = this->now(); - if (ser_.available()){ - std_msgs::msg::String result; - - std::lock_guard lock(locker); - - result.data = ser_.read(ser_.available()); - - // std::lock_guard unlock(locker); - - serial_read_pub_->publish(result); - - boost::replace_all(result.data, "\r", ""); - boost::replace_all(result.data, "+", ""); - - std::vector fields; - - boost::split(fields, result.data, boost::algorithm::is_any_of("D")); - if (fields.size() < 2){ + auto current_time = this->now(); + if (ser_.available()) { + std_msgs::msg::String result; + + // Lock to prevent concurrent access to serial communication + std::lock_guard lock(locker); + + // Read available data from the serial port + result.data = ser_.read(ser_.available()); + + // Check if result.data is not empty + if (result.data.empty()) { + RCLCPP_WARN_STREAM(this->get_logger(), tag << "No data received from serial port."); + return; // Skip processing if no data is received + } - RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Empty data:{" << result.data << "}"); - } - else if (fields.size() >= 2){ - std::vector fields_H; - for (int i = fields.size() - 1; i >= 0; i--){ - if (fields[i][0] == 'H'){ - try{ - fields_H.clear(); - boost::split(fields_H, fields[i], boost::algorithm::is_any_of("?")); - if ( fields_H.size() >= query_pub_.size() + 1){ - break; - } - } - catch (const std::exception &e){ - std::cerr << e.what() << '\n'; - RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Finding query output in :" << fields[i]); - continue; - } - } - } - - if (fields_H.size() > 0 && fields_H[0] == "H"){ - for (long unsigned int i = 0; i < fields_H.size() - 1; ++i){ - std::vector sub_fields_H; - boost::split(sub_fields_H, fields_H[i + 1], boost::algorithm::is_any_of(":")); - - roboteq_interfaces::msg::ChannelValues msg; - msg.header.stamp = current_time; - - for (long unsigned int j = 0; j < sub_fields_H.size(); j++){ - try{ - msg.value.push_back(boost::lexical_cast(sub_fields_H[j])); - } - catch (const std::exception &e){ - RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Garbage data on Serial"); - RCLCPP_ERROR_STREAM(this->get_logger(), result.data); - RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); - std::cerr << e.what() << '\n'; - } - } - query_pub_[i]->publish(msg); - } - } - } - else{ - RCLCPP_WARN_STREAM(this->get_logger(),tag << "Unknown:{" << result.data << "}"); - } - } + // Publish the raw serial data for debugging + serial_read_pub_->publish(result); + + // Clean the received data + boost::replace_all(result.data, "\r", ""); + boost::replace_all(result.data, "+", ""); + + // Split the data by the delimiter "D" + std::vector fields; + boost::split(fields, result.data, boost::algorithm::is_any_of("D")); + + // Check if the fields vector has at least 2 elements + if (fields.size() < 2) { + RCLCPP_ERROR_STREAM(this->get_logger(), tag << "Empty data:{" << result.data << "}"); + } else { + std::vector fields_H; + for (int i = fields.size() - 1; i >= 0; i--) { + if (fields[i][0] == 'H') { + try { + fields_H.clear(); + boost::split(fields_H, fields[i], boost::algorithm::is_any_of("?")); + if (fields_H.size() >= query_pub_.size() + 1) { + break; + } + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(this->get_logger(), tag << "Error parsing query output: " << fields[i]); + continue; + } + } + } + + if (!fields_H.empty() && fields_H[0] == "H") { + for (long unsigned int i = 0; i < fields_H.size() - 1; ++i) { + std::vector sub_fields_H; + boost::split(sub_fields_H, fields_H[i + 1], boost::algorithm::is_any_of(":")); + + roboteq_interfaces::msg::ChannelValues msg; + msg.header.stamp = current_time; + + // Convert sub_fields_H to integers and publish the message + for (long unsigned int j = 0; j < sub_fields_H.size(); j++) { + try { + msg.value.push_back(boost::lexical_cast(sub_fields_H[j])); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(this->get_logger(), tag << "Invalid data on Serial: " << result.data); + RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + } + } + query_pub_[i]->publish(msg); + } + } + } + } } + int main(int argc, char * argv[]) { rclcpp::init(argc, argv);