diff --git a/data/kansei.json b/data/kansei.json index d228d7b..d8639df 100755 --- a/data/kansei.json +++ b/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 } } diff --git a/include/kansei/fallen/node/fallen_determinant.hpp b/include/kansei/fallen/node/fallen_determinant.hpp index cc8dddd..d9d16a3 100644 --- a/include/kansei/fallen/node/fallen_determinant.hpp +++ b/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 diff --git a/src/check_rpy_main.cpp b/src/check_rpy_main.cpp index 0002b05..7fb3fd2 100755 --- a/src/check_rpy_main.cpp +++ b/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"; diff --git a/src/kansei/fallen/node/fallen_determinant.cpp b/src/kansei/fallen/node/fallen_determinant.cpp index bbd0147..e0b319a 100755 --- a/src/kansei/fallen/node/fallen_determinant.cpp +++ b/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 & 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; } } diff --git a/src/kansei/fallen/node/fallen_node.cpp b/src/kansei/fallen/node/fallen_node.cpp index d4712bd..5747733 100755 --- a/src/kansei/fallen/node/fallen_node.cpp +++ b/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 +// #include #include #include #include diff --git a/src/kansei_main.cpp b/src/kansei_main.cpp index f2c9a6e..6d5e71d 100755 --- a/src/kansei_main.cpp +++ b/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(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("kansei_node"); auto kansei_node = std::make_shared(node); - auto fallen = std::make_shared( - kansei::fallen::DeterminantType::ACCELERO); + auto fallen = std::make_shared(determinant_type); + fallen->load_config(path); kansei_node->set_measurement_unit(mpu); kansei_node->set_fallen_determinant(fallen); diff --git a/test/fallen/fallen_test.cpp b/test/fallen/fallen_test.cpp index c26143a..5f39495 100644 --- a/test/fallen/fallen_test.cpp +++ b/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)) { } diff --git a/test/node/kansei_node_test.cpp b/test/node/kansei_node_test.cpp index c18228f..a5f987c 100644 --- a/test/node/kansei_node_test.cpp +++ b/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_node.set_fallen_determinant( - std::make_shared( - kansei::fallen::DeterminantType::ACCELERO)); + std::make_shared(kansei::fallen::DeterminantType::ACCELERO)); } catch (...) { } }