Merge pull request #35 from ichiro-its/enhancement/add-fallen-determi…

…nant-options

[Enhancement] - Add Fallent Determinant Options

data/kansei.json

@@ -1,13 +1,14 @@
 {
-  "fallen_limit": {
-    "fallen_back_limit": 620.0,
-    "fallen_front_limit": 445.0,
-    "fallen_right_limit": 570.0,
-    "fallen_left_limit": 400.0
+  "accel_limit": {
+    "accel_back_limit": 620.0,
+    "accel_front_limit": 445.0,
+    "accel_right_limit": 570.0,
+    "accel_left_limit": 400.0
   },
-  "filter": {
-    "gyro_mux_x": 3.22,
-    "gyro_mux_y": 3.22,
-    "gyro_mux_z": 3.22
+  "orientation_limit": {
+    "pitch_back_limit": 0.0,
+    "pitch_front_limit": 0.0,
+    "roll_right_limit": 0.0,
+    "roll_left_limit": 0.0
   }
 }

include/kansei/fallen/node/fallen_determinant.hpp

@@ -51,11 +51,16 @@ class FallenDeterminant
 
   DeterminantType determinant_type;
 
-  // fallen raw variables
-  float fallen_back_raw_limit;
-  float fallen_front_raw_limit;
-  float fallen_right_raw_limit;
-  float fallen_left_raw_limit;
+  // fallen variables
+  double accel_back_limit;
+  double accel_front_limit;
+  double accel_right_limit;
+  double accel_left_limit;
+
+  double pitch_back_limit;
+  double pitch_front_limit;
+  double roll_right_limit;
+  double roll_left_limit;
 };
 
 }  // namespace kansei::fallen

src/check_rpy_main.cpp

@@ -35,12 +35,10 @@ int main(int argc, char * argv[])
     port_name = argv[1];
   }
 
-  std::cout << "set the port name as " << port_name << "\n";
   kansei::measurement::MPU mpu(port_name);
 
-  std::cout << "connect to mpu\n";
   if (mpu.connect()) {
-    std::cout << "succeeded to connect to mpu!\n";
+    std::cout << "succeeded to connect to mpu " << port_name << "!\n";
   } else {
     std::cout << "failed to connect to mpu!\n" <<
       "try again!\n";

src/kansei/fallen/node/fallen_determinant.cpp

@@ -31,27 +31,43 @@ namespace kansei::fallen
 {
 
 FallenDeterminant::FallenDeterminant(const DeterminantType & type)
-: fallen_status(FallenStatus::STANDUP), determinant_type(type), fallen_back_raw_limit(491.0),
-  fallen_front_raw_limit(458.0), fallen_right_raw_limit(519.0), fallen_left_raw_limit(498.0)
+: fallen_status(FallenStatus::STANDUP), determinant_type(type),
+  accel_back_limit(1000.0), accel_front_limit(0.0), accel_right_limit(1000.0), accel_left_limit(0.0),
+  pitch_back_limit(100.0), pitch_front_limit(-100.0), roll_right_limit(-100.0), roll_left_limit(100.0)
 {
 }
 
 void FallenDeterminant::load_config(const std::string & path)
 {
-  std::string file_name =
-    path + "imu/" + "kansei.json";
+  std::string file_name = path + "kansei.json";
   std::ifstream file(file_name);
+
+  if (!file.is_open()) {
+    throw std::runtime_error("Failed to open file: " + file_name);
+  }
+
   nlohmann::json imu_data = nlohmann::json::parse(file);
 
   for (const auto &[key, val] : imu_data.items()) {
-    if (key == "fallen_limit") {
+    if (key == "accel_limit") {
+      try {
+        val.at("accel_back_limit").get_to(accel_back_limit);
+        val.at("accel_front_limit").get_to(accel_front_limit);
+        val.at("accel_right_limit").get_to(accel_right_limit);
+        val.at("accel_left_limit").get_to(accel_left_limit);
+      } catch (nlohmann::json::parse_error & ex) {
+        std::cerr << "parse error at byte " << ex.byte << std::endl;
+        throw ex;
+      }
+    } else if (key == "orientation_limit") {
       try {
-        val.at("fallen_back_limit").get_to(fallen_back_raw_limit);
-        val.at("fallen_front_limit").get_to(fallen_front_raw_limit);
-        val.at("fallen_right_limit").get_to(fallen_right_raw_limit);
-        val.at("fallen_left_limit").get_to(fallen_left_raw_limit);
+        val.at("pitch_back_limit").get_to(pitch_back_limit);
+        val.at("pitch_front_limit").get_to(pitch_front_limit);
+        val.at("roll_right_limit").get_to(roll_right_limit);
+        val.at("roll_left_limit").get_to(roll_left_limit);
       } catch (nlohmann::json::parse_error & ex) {
         std::cerr << "parse error at byte " << ex.byte << std::endl;
+        throw ex;
       }
     }
   }
@@ -60,19 +76,29 @@ void FallenDeterminant::load_config(const std::string & path)
 void FallenDeterminant::update_fallen_status(const keisan::Euler<double> & rpy)
 {
   fallen_status = FallenStatus::STANDUP;
+
+  if (rpy.pitch.degree() < pitch_front_limit) {
+    fallen_status = FallenStatus::FORWARD;
+  } else if (rpy.pitch.degree() > pitch_back_limit) {
+    fallen_status = FallenStatus::BACKWARD;
+  } else if (rpy.roll.degree() > roll_right_limit) {
+    fallen_status = FallenStatus::RIGHT;
+  } else if (rpy.roll.degree() < roll_left_limit) {
+    fallen_status = FallenStatus::LEFT;
+  }
 }
 
 void FallenDeterminant::update_fallen_status(const keisan::Vector<3> & acc)
 {
   fallen_status = FallenStatus::STANDUP;
 
-  if (acc[1] < fallen_front_raw_limit) {
+  if (acc[1] < accel_front_limit) {
     fallen_status = FallenStatus::FORWARD;
-  } else if (acc[1] > fallen_back_raw_limit) {
+  } else if (acc[1] > accel_back_limit) {
     fallen_status = FallenStatus::BACKWARD;
-  } else if (acc[0] > fallen_right_raw_limit) {
+  } else if (acc[0] < accel_right_limit) {
     fallen_status = FallenStatus::RIGHT;
-  } else if (acc[0] < fallen_left_raw_limit) {
+  } else if (acc[0] > accel_left_limit) {
     fallen_status = FallenStatus::LEFT;
   }
 }

src/kansei/fallen/node/fallen_node.cpp

@@ -18,7 +18,7 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 
-#include <kansei_interfaces/msg/fallen.hpp>
+// #include <kansei_interfaces/msg/fallen.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <string>

src/kansei_main.cpp

@@ -36,17 +36,57 @@ int main(int argc, char * argv[])
   rclcpp::init(argc, argv);
 
   std::string port_name = "/dev/ttyUSB1";
+  std::string path = "";
+  kansei::fallen::DeterminantType determinant_type;
+
+  const char * help_message = 
+    "Usage: ros2 run kansei main --path [config_path] --type [fallen_type]\n"
+    "[config_path]:       path to the configuration file\n"
+    "[fallen_type]:       fallen type to be used (orientation / accelero)\n"
+    "Optional:\n"
+    "-h, --help           show this help message and exit\n";
 
   if (argc > 1) {
-    port_name = argv[1];
+    for (int i = 1; i < argc; i++) {
+      std::string arg = argv[i];
+      if (arg == "-h" || arg == "--help") {
+        std::cout << help_message << std::endl;
+        return 1;
+      } else if (arg == "--path") {
+        if (i + 1 < argc) {
+          path = argv[i + 1];
+          i++;
+        } else {
+          std::cerr << "Error: --path requires a path argument" << std::endl;
+          return 1;
+        }
+      } else if (arg == "--type") {
+        if (i + 1 < argc) {
+          std::string fallen_type = argv[i + 1];
+          if (fallen_type == "orientation") {
+            determinant_type = kansei::fallen::DeterminantType::ORIENTATION;
+          } else if (fallen_type == "accelero") {
+            determinant_type = kansei::fallen::DeterminantType::ACCELERO;
+          } else {
+            std::cerr << "Error: invalid fallen type argument\n";
+            return 1;
+          }
+          i++;
+        } else {
+          std::cerr << "Error: --type requires a fallen type argument" << std::endl;
+          return 1;
+        }
+      }
+    }
+  } else {
+    std::cout << "Invalid arguments!\n\n" << help_message << "\n";
+    return 0;
   }
 
-  std::cout << "set the port name as " << port_name << "\n";
   auto mpu = std::make_shared<kansei::measurement::MPU>(port_name);
 
-  std::cout << "connect to mpu\n";
   if (mpu->connect()) {
-    std::cout << "succeeded to connect to mpu!\n";
+    std::cout << "succeeded to connect to mpu " << port_name << "!\n";
   } else {
     std::cout << "failed to connect to mpu!\n" <<
       "try again!\n";
@@ -56,8 +96,8 @@ int main(int argc, char * argv[])
   auto node = std::make_shared<rclcpp::Node>("kansei_node");
   auto kansei_node = std::make_shared<kansei::KanseiNode>(node);
 
-  auto fallen = std::make_shared<kansei::fallen::FallenDeterminant>(
-    kansei::fallen::DeterminantType::ACCELERO);
+  auto fallen = std::make_shared<kansei::fallen::FallenDeterminant>(determinant_type);
+  fallen->load_config(path);
 
   kansei_node->set_measurement_unit(mpu);
   kansei_node->set_fallen_determinant(fallen);

test/fallen/fallen_test.cpp

@@ -27,8 +27,7 @@ class FallenTest : public testing::Test
 {
 protected:
   FallenTest()
-  : fallen_determinant(kansei::fallen::FallenDeterminant(
-        kansei::fallen::DeterminantType::ACCELERO))
+  : fallen_determinant(kansei::fallen::FallenDeterminant(kansei::fallen::DeterminantType::ACCELERO))
   {
   }
 

test/node/kansei_node_test.cpp

@@ -34,9 +34,9 @@ TEST(KanseiNodeTest, CompileProcess) {
     kansei::KanseiNode kansei_node(node);
 
     kansei_node.set_measurement_unit(std::make_shared<kansei::measurement::Filter>());
+
     kansei_node.set_fallen_determinant(
-      std::make_shared<kansei::fallen::FallenDeterminant>(
-        kansei::fallen::DeterminantType::ACCELERO));
+      std::make_shared<kansei::fallen::FallenDeterminant>(kansei::fallen::DeterminantType::ACCELERO));
   } catch (...) {
   }
 }