fixed id

Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>

.vscode/settings.json

@@ -80,7 +80,8 @@
         "stop_token": "cpp",
         "valarray": "cpp",
         "variant": "cpp",
-        "regex": "cpp"
+        "regex": "cpp",
+        "ranges": "cpp"
     },
     "cSpell.words": [
         "rclcpp"

src/ros2aria/include/ros2aria/ros2aria.hpp

@@ -41,6 +41,7 @@ public:
     ~Ros2Aria();
 
 private:
+    std::size_t id;
     RAIIBot::SharedPtr robot;
     ArFunctorC<Ros2Aria> sensorCb;
     ros2aria_msgs::msg::RestrictionsMsg restrictions;
@@ -51,8 +52,9 @@ private:
     bool use_sonar = true;
     uint8_t num_of_sonars = 0;
 
-    void handle_parameters();
+    std::size_t extract_number_from_string(const std::string& str);
 
+    void handle_parameters();
     void publish();
 
     sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);

src/ros2aria/src/ros2aria.cpp

@@ -1,5 +1,7 @@
 #include "ros2aria/ros2aria.hpp"
-
+#include <iostream>
+#include <string>
+#include <sstream>
 using std::placeholders::_1;
 using std::placeholders::_2;
 
@@ -7,10 +9,13 @@ Ros2Aria::Ros2Aria()
     : Node("ros2aria"),
       sensorCb(this, &Ros2Aria::publish)
 {
+    id = extract_number_from_string(std::string(get_namespace()));
+
     this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
     handle_parameters();
 
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
+
     restrictions_sub_ = this->create_subscription<ros2aria_msgs::msg::RestrictionsMsg>(
         "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1));
     init_restrictions();
@@ -55,6 +60,22 @@ Ros2Aria::Ros2Aria()
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
 
     RCLCPP_INFO(get_logger(), "NAMESPACE = %s", get_namespace());
+    RCLCPP_INFO(get_logger(), "ID = %d", id);
+
+}
+
+std::size_t Ros2Aria::extract_number_from_string(const std::string& str) {
+    std::string numericPart;
+    for (char c : str) {
+        if (isdigit(c)) {
+            numericPart += c;
+        } else if (!numericPart.empty()) {
+            break; 
+        }
+    }
+    int number;
+    std::istringstream(numericPart) >> number;
+    return number;
 }
 
 void Ros2Aria::handle_parameters(){

src/ros2aria/src/state.cpp

@@ -17,10 +17,8 @@ void Ros2Aria::publishState()
     if (this->robot_info_pub_->get_subscription_count() > 0)
     {
         ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
-        auto ns = get_namespace();
-        // TODO: id
-        
-        robot_info_msg.robot_id.data =  ns[sizeof(ns) - 1] - '0';
+
+        robot_info_msg.robot_id.data =  id;
         robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
         robot_info_msg.minimal_distance.data = minimal_distance;
         robot_info_msg.twist.linear.x = r->getVel() / 1000;