From 4c6f618a1ab7d00a68677b813b5063598ada007a Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 17 Aug 2023 10:14:43 +0200 Subject: [PATCH] fixed id Signed-off-by: Jakub Delicat --- .vscode/settings.json | 3 ++- src/ros2aria/include/ros2aria/ros2aria.hpp | 4 +++- src/ros2aria/src/ros2aria.cpp | 23 +++++++++++++++++++++- src/ros2aria/src/state.cpp | 6 ++---- 4 files changed, 29 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index bd2a2e3..2b5ea4c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -80,7 +80,8 @@ "stop_token": "cpp", "valarray": "cpp", "variant": "cpp", - "regex": "cpp" + "regex": "cpp", + "ranges": "cpp" }, "cSpell.words": [ "rclcpp" diff --git a/src/ros2aria/include/ros2aria/ros2aria.hpp b/src/ros2aria/include/ros2aria/ros2aria.hpp index 836516a..d6b937c 100644 --- a/src/ros2aria/include/ros2aria/ros2aria.hpp +++ b/src/ros2aria/include/ros2aria/ros2aria.hpp @@ -41,6 +41,7 @@ public: ~Ros2Aria(); private: + std::size_t id; RAIIBot::SharedPtr robot; ArFunctorC 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); diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 60dc9a1..5896cad 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/src/ros2aria/src/ros2aria.cpp @@ -1,5 +1,7 @@ #include "ros2aria/ros2aria.hpp" - +#include +#include +#include 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(this, "/dev/ttyS0"); handle_parameters(); RCLCPP_INFO(this->get_logger(), "starting subscribers and services"); + restrictions_sub_ = this->create_subscription( "/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(){ diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp index 30fb425..4af80d1 100644 --- a/src/ros2aria/src/state.cpp +++ b/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;