Added pioneer_id parameter

.env

@@ -0,0 +1 @@
+PIONEER_ID=5

compose.ros2aria.yaml

@@ -2,4 +2,4 @@ version: "3"
 services:
   ros2aria:
    build: .
-   command: ros2 run ros2aria ros2aria
+   command: ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=${PIONEER_ID}

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();

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.header.frame_id = "odom";
-    odom_msg.child_frame_id = "child_frame_id";    // TODO: use correct child_frame_id
+    odom_msg.child_frame_id = "base_link";
     odom_msg.header.stamp = stamp;
 
     return odom_msg;

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()

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();
 

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;