From 4bb46dcdc3734c0b753452295270e438696db3ae Mon Sep 17 00:00:00 2001
From: brsbilgin <bbilgin@outlook.com>
Date: Sat, 29 Apr 2023 13:35:06 +0300
Subject: [PATCH] Fix missed ModeParameter msg ros to ff converison

---
 free_fleet_server_ros2/src/utilities.cpp | 16 ++++++++++++++++
 free_fleet_server_ros2/src/utilities.hpp |  3 +++
 2 files changed, 19 insertions(+)

diff --git a/free_fleet_server_ros2/src/utilities.cpp b/free_fleet_server_ros2/src/utilities.cpp
index ad62a0ff..54d71f79 100644
--- a/free_fleet_server_ros2/src/utilities.cpp
+++ b/free_fleet_server_ros2/src/utilities.cpp
@@ -33,6 +33,13 @@ void to_ff_message(
   _out_msg.level_name = _in_msg.level_name;
 }
 
+void to_ff_message(
+    const rmf_fleet_msgs::msg::ModeParameter& _in_msg, messages::ModeParameter& _out_msg)
+{
+  _out_msg.name = _in_msg.name;
+  _out_msg.value = _in_msg.value;
+}
+
 void to_ff_message(
     const rmf_fleet_msgs::msg::ModeRequest& _in_msg, 
     messages::ModeRequest& _out_msg)
@@ -41,6 +48,15 @@ void to_ff_message(
   _out_msg.robot_name = _in_msg.robot_name;
   _out_msg.mode.mode = _in_msg.mode.mode;
   _out_msg.task_id = _in_msg.task_id;
+
+  _out_msg.parameters.clear();
+  for (size_t i = 0; i < _in_msg.parameters.size(); ++i)
+  {
+    messages::ModeParameter tmp_loc_msg;
+    to_ff_message(_in_msg.parameters[i], tmp_loc_msg);
+    _out_msg.parameters.push_back(tmp_loc_msg);
+  }
+
 }
 
 void to_ff_message(
diff --git a/free_fleet_server_ros2/src/utilities.hpp b/free_fleet_server_ros2/src/utilities.hpp
index 99b13573..fccc9c3c 100644
--- a/free_fleet_server_ros2/src/utilities.hpp
+++ b/free_fleet_server_ros2/src/utilities.hpp
@@ -38,6 +38,9 @@ namespace ros2
 void to_ff_message(
     const rmf_fleet_msgs::msg::Location& in_msg, messages::Location& out_msg);
 
+void to_ff_message(
+    const rmf_fleet_msgs::msg::ModeParameter& in_msg, messages::ModeParameter& out_msg);
+
 void to_ff_message(
     const rmf_fleet_msgs::msg::ModeRequest& in_msg, 
     messages::ModeRequest& out_msg);