From a1a54f58e4e1b911a1a62e3fc52d28d63fa2abe0 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Thu, 29 Sep 2022 12:22:30 +0200
Subject: [PATCH] Added pioneer_id parameter

---
 .env                                       | 1 +
 compose.ros2aria.yaml                      | 2 +-
 src/ros2aria/include/ros2aria/ros2aria.hpp | 2 +-
 src/ros2aria/src/pose.cpp                  | 4 ++--
 src/ros2aria/src/ros2aria.cpp              | 4 +++-
 src/ros2aria/src/sonar.cpp                 | 2 +-
 src/ros2aria/src/state.cpp                 | 9 +--------
 7 files changed, 10 insertions(+), 14 deletions(-)
 create mode 100644 .env

diff --git a/.env b/.env
new file mode 100644
index 0000000..06bc912
--- /dev/null
+++ b/.env
@@ -0,0 +1 @@
+PIONEER_ID=5
\ No newline at end of file
diff --git a/compose.ros2aria.yaml b/compose.ros2aria.yaml
index 244fed5..fb30dc2 100644
--- a/compose.ros2aria.yaml
+++ b/compose.ros2aria.yaml
@@ -2,4 +2,4 @@ version: "3"
 services:
   ros2aria:
    build: .
-   command: ros2 run ros2aria ros2aria
\ No newline at end of file
+   command: ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=${PIONEER_ID}
\ No newline at end of file
diff --git a/src/ros2aria/include/ros2aria/ros2aria.hpp b/src/ros2aria/include/ros2aria/ros2aria.hpp
index 04d45e1..7b3285a 100644
--- a/src/ros2aria/include/ros2aria/ros2aria.hpp
+++ b/src/ros2aria/include/ros2aria/ros2aria.hpp
@@ -28,7 +28,7 @@ private:
     ArFunctorC<Ros2Aria> sensorCb;
 
     // config
-    std::string sonar_frame_id;
+    std::size_t pioneer_id;
 
     // publishers
     void publish();
diff --git a/src/ros2aria/src/pose.cpp b/src/ros2aria/src/pose.cpp
index fba29a9..355cda3 100644
--- a/src/ros2aria/src/pose.cpp
+++ b/src/ros2aria/src/pose.cpp
@@ -19,8 +19,8 @@ nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp) {
     odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
     odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
 
-    odom_msg.header.frame_id = "header.frame_id";  // TODO: use correct frame_id
-    odom_msg.child_frame_id = "child_frame_id";    // TODO: use correct child_frame_id
+    odom_msg.header.frame_id = "odom";
+    odom_msg.child_frame_id = "base_link";
     odom_msg.header.stamp = stamp;
 
     return odom_msg;
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 27702d4..4e6b5f6 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -37,7 +37,9 @@ Ros2Aria::Ros2Aria()
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
 
-    sonar_frame_id = "pionier6"; // TODO: use args
+    declare_parameter<uint8_t>("pioneer_id");
+    pioneer_id = get_parameter("pioneer_id").as_int();
+    RCLCPP_INFO(get_logger(), "PIONEER_ID = %d", pioneer_id);
 }
 
 Ros2Aria::~Ros2Aria()
diff --git a/src/ros2aria/src/sonar.cpp b/src/ros2aria/src/sonar.cpp
index 3be0065..66ed18c 100644
--- a/src/ros2aria/src/sonar.cpp
+++ b/src/ros2aria/src/sonar.cpp
@@ -6,7 +6,7 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
     sensor_msgs::msg::PointCloud cloud; //sonar readings.
     cloud.header.stamp = stamp;
     // sonar sensors relative to base_link
-    cloud.header.frame_id = this->sonar_frame_id;
+    cloud.header.frame_id = "sonar_frame";
 
     auto r = robot->getRobot();
 
diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp
index 9f28f3a..eca3365 100644
--- a/src/ros2aria/src/state.cpp
+++ b/src/ros2aria/src/state.cpp
@@ -13,20 +13,13 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
         this->battery_recharge_state_pub_->publish(recharge_state);
     }
 
-    // TODO: our robots don't have this, cannot test
-    if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0)
-    {
-        std_msgs::msg::Float32 soc;
-        soc.data = r->getStateOfCharge() / 100.0;
-        this->battery_state_of_charge_pub_->publish(soc);
-    }
 
     if (this->robot_info_pub_->get_subscription_count() > 0)
     {
         ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
 
         // TODO: allow setting the robot_id
-        robot_info_msg.robot_id.data = 6;
+        robot_info_msg.robot_id.data = pioneer_id;
         robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
         robot_info_msg.twist.linear.x = r->getVel() / 1000;
         robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;