From ba021e78d476c58134252fbe7adbabb08f25551f Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 17:42:34 +0700 Subject: [PATCH 1/9] refactor: get port_name from argv --- src/check_rpy_main.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/check_rpy_main.cpp b/src/check_rpy_main.cpp index 0002b05..817f04c 100755 --- a/src/check_rpy_main.cpp +++ b/src/check_rpy_main.cpp @@ -29,22 +29,26 @@ int main(int argc, char * argv[]) { - std::string port_name = "/dev/ttyUSB1"; + std::string port_name = "/dev/ttyUSB0"; if (argc > 1) { 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"; - return 0; + port_name = "/dev/ttyUSB1"; + mpu.set_port_name(port_name); + if (mpu.connect()) { + std::cout << "succeeded to connect to mpu " << port_name << "!\n"; + } else { + std::cout << "failed to connect to mpu!\n" << + "try again!\n"; + return 0; + } } while (true) { From 22ba1517352180afa4c6d7ea52af135748eb3f5e Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 17:43:45 +0700 Subject: [PATCH 2/9] feat: fallen determinant using imu --- data/kansei.json | 19 ++++--- .../kansei/fallen/node/fallen_determinant.hpp | 17 ++++-- src/kansei/fallen/node/fallen_determinant.cpp | 57 ++++++++++++++----- src/kansei/fallen/node/fallen_node.cpp | 2 +- 4 files changed, 65 insertions(+), 30 deletions(-) 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..9357eac 100644 --- a/include/kansei/fallen/node/fallen_determinant.hpp +++ b/include/kansei/fallen/node/fallen_determinant.hpp @@ -34,7 +34,7 @@ namespace kansei::fallen class FallenDeterminant { public: - explicit FallenDeterminant(const DeterminantType & type); + explicit FallenDeterminant(const std::string & fallen_type); void load_config(const std::string & path); @@ -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 + float accel_back_limit; + float accel_front_limit; + float accel_right_limit; + float accel_left_limit; + + float pitch_back_limit; + float pitch_front_limit; + float roll_right_limit; + float roll_left_limit; }; } // namespace kansei::fallen diff --git a/src/kansei/fallen/node/fallen_determinant.cpp b/src/kansei/fallen/node/fallen_determinant.cpp index bbd0147..39e21aa 100755 --- a/src/kansei/fallen/node/fallen_determinant.cpp +++ b/src/kansei/fallen/node/fallen_determinant.cpp @@ -30,26 +30,45 @@ 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) +FallenDeterminant::FallenDeterminant(const std::string &fallen_type) +: fallen_status(FallenStatus::STANDUP) { + if (fallen_type == "orientation") { + determinant_type = DeterminantType::ORIENTATION; + } else if (fallen_type == "accelero") { + determinant_type = DeterminantType::ACCELERO; + } else { + throw std::runtime_error("Invalid fallen type: " + fallen_type); + } } 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; + } + } 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; } @@ -60,19 +79,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 From 33a4d510582e58d290bf6490c4e5658649be18f7 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 17:44:02 +0700 Subject: [PATCH 3/9] refactor: get port_name and type from argv --- src/kansei_main.cpp | 58 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 48 insertions(+), 10 deletions(-) diff --git a/src/kansei_main.cpp b/src/kansei_main.cpp index f2c9a6e..e75b04a 100755 --- a/src/kansei_main.cpp +++ b/src/kansei_main.cpp @@ -35,29 +35,67 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - std::string port_name = "/dev/ttyUSB1"; + std::string port_name = "/dev/ttyUSB0"; + std::string fallen_type = ""; + std::string path = ""; + + 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) { + fallen_type = argv[i + 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"; - return 0; + port_name = "/dev/ttyUSB1"; + mpu->set_port_name(port_name); + if (mpu->connect()) { + std::cout << "succeeded to connect to mpu " << port_name << "!\n"; + } else { + std::cout << "failed to connect to mpu!\n" << + "try again!\n"; + return 0; + } } 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(fallen_type); + fallen->load_config(path); kansei_node->set_measurement_unit(mpu); kansei_node->set_fallen_determinant(fallen); From 0c44db8799cc404e4cd0458aea4e000aed100768 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 17:44:33 +0700 Subject: [PATCH 4/9] refactor: follow changes --- test/fallen/determinant_test.cpp | 2 +- test/fallen/fallen_test.cpp | 3 +-- test/node/kansei_node_test.cpp | 5 +++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/test/fallen/determinant_test.cpp b/test/fallen/determinant_test.cpp index 423d4f2..f4ae372 100644 --- a/test/fallen/determinant_test.cpp +++ b/test/fallen/determinant_test.cpp @@ -23,7 +23,7 @@ #include "kansei/fallen/fallen.hpp" TEST(DeterminantTest, DeterminantType) { - kansei::fallen::FallenDeterminant fallen_determinant(kansei::fallen::DeterminantType::ACCELERO); + kansei::fallen::FallenDeterminant fallen_determinant("accelero"); ASSERT_EQ(kansei::fallen::DeterminantType::ACCELERO, fallen_determinant.get_determinant_type()); } diff --git a/test/fallen/fallen_test.cpp b/test/fallen/fallen_test.cpp index c26143a..ab61c82 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("accelero")) { } diff --git a/test/node/kansei_node_test.cpp b/test/node/kansei_node_test.cpp index c18228f..299a242 100644 --- a/test/node/kansei_node_test.cpp +++ b/test/node/kansei_node_test.cpp @@ -34,9 +34,10 @@ 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("accelero")); + std::cout << "Fallen determinant" << std::endl; } catch (...) { } } From bce6d9ae8130c3e49a9e38a18b49c5d9a767cc68 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 19:24:42 +0700 Subject: [PATCH 5/9] refactor: keeping DeterminantType as parameter constructor --- include/kansei/fallen/node/fallen_determinant.hpp | 2 +- src/kansei/fallen/node/fallen_determinant.cpp | 11 ++--------- src/kansei_main.cpp | 14 +++++++++++--- test/fallen/determinant_test.cpp | 2 +- test/fallen/fallen_test.cpp | 2 +- test/node/kansei_node_test.cpp | 2 +- 6 files changed, 17 insertions(+), 16 deletions(-) diff --git a/include/kansei/fallen/node/fallen_determinant.hpp b/include/kansei/fallen/node/fallen_determinant.hpp index 9357eac..d8a7442 100644 --- a/include/kansei/fallen/node/fallen_determinant.hpp +++ b/include/kansei/fallen/node/fallen_determinant.hpp @@ -34,7 +34,7 @@ namespace kansei::fallen class FallenDeterminant { public: - explicit FallenDeterminant(const std::string & fallen_type); + explicit FallenDeterminant(const DeterminantType & type); void load_config(const std::string & path); diff --git a/src/kansei/fallen/node/fallen_determinant.cpp b/src/kansei/fallen/node/fallen_determinant.cpp index 39e21aa..4e154ae 100755 --- a/src/kansei/fallen/node/fallen_determinant.cpp +++ b/src/kansei/fallen/node/fallen_determinant.cpp @@ -30,16 +30,9 @@ namespace kansei::fallen { -FallenDeterminant::FallenDeterminant(const std::string &fallen_type) -: fallen_status(FallenStatus::STANDUP) +FallenDeterminant::FallenDeterminant(const DeterminantType & type) +: fallen_status(FallenStatus::STANDUP), determinant_type(type) { - if (fallen_type == "orientation") { - determinant_type = DeterminantType::ORIENTATION; - } else if (fallen_type == "accelero") { - determinant_type = DeterminantType::ACCELERO; - } else { - throw std::runtime_error("Invalid fallen type: " + fallen_type); - } } void FallenDeterminant::load_config(const std::string & path) diff --git a/src/kansei_main.cpp b/src/kansei_main.cpp index e75b04a..59636b1 100755 --- a/src/kansei_main.cpp +++ b/src/kansei_main.cpp @@ -36,8 +36,8 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); std::string port_name = "/dev/ttyUSB0"; - std::string fallen_type = ""; std::string path = ""; + kansei::fallen::DeterminantType determinant_type; const char * help_message = "Usage: ros2 run kansei main --path [config_path] --type [fallen_type]\n" @@ -62,7 +62,15 @@ int main(int argc, char * argv[]) } } else if (arg == "--type") { if (i + 1 < argc) { - fallen_type = argv[i + 1]; + 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; @@ -94,7 +102,7 @@ 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(fallen_type); + auto fallen = std::make_shared(determinant_type); fallen->load_config(path); kansei_node->set_measurement_unit(mpu); diff --git a/test/fallen/determinant_test.cpp b/test/fallen/determinant_test.cpp index f4ae372..423d4f2 100644 --- a/test/fallen/determinant_test.cpp +++ b/test/fallen/determinant_test.cpp @@ -23,7 +23,7 @@ #include "kansei/fallen/fallen.hpp" TEST(DeterminantTest, DeterminantType) { - kansei::fallen::FallenDeterminant fallen_determinant("accelero"); + kansei::fallen::FallenDeterminant fallen_determinant(kansei::fallen::DeterminantType::ACCELERO); ASSERT_EQ(kansei::fallen::DeterminantType::ACCELERO, fallen_determinant.get_determinant_type()); } diff --git a/test/fallen/fallen_test.cpp b/test/fallen/fallen_test.cpp index ab61c82..5f39495 100644 --- a/test/fallen/fallen_test.cpp +++ b/test/fallen/fallen_test.cpp @@ -27,7 +27,7 @@ class FallenTest : public testing::Test { protected: FallenTest() - : fallen_determinant(kansei::fallen::FallenDeterminant("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 299a242..295a076 100644 --- a/test/node/kansei_node_test.cpp +++ b/test/node/kansei_node_test.cpp @@ -36,7 +36,7 @@ TEST(KanseiNodeTest, CompileProcess) { kansei_node.set_measurement_unit(std::make_shared()); kansei_node.set_fallen_determinant( - std::make_shared("accelero")); + std::make_shared(kansei::fallen::DeterminantType::ACCELERO)); std::cout << "Fallen determinant" << std::endl; } catch (...) { } From bb25b0a7e00cfc69b5586677a621de98463a1763 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 21:41:03 +0700 Subject: [PATCH 6/9] refactor: initialize vars and throw exception --- src/kansei/fallen/node/fallen_determinant.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/kansei/fallen/node/fallen_determinant.cpp b/src/kansei/fallen/node/fallen_determinant.cpp index 4e154ae..e0b319a 100755 --- a/src/kansei/fallen/node/fallen_determinant.cpp +++ b/src/kansei/fallen/node/fallen_determinant.cpp @@ -31,7 +31,9 @@ namespace kansei::fallen { FallenDeterminant::FallenDeterminant(const DeterminantType & type) -: fallen_status(FallenStatus::STANDUP), determinant_type(type) +: 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) { } @@ -55,6 +57,7 @@ void FallenDeterminant::load_config(const std::string & path) 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 { @@ -64,6 +67,7 @@ void FallenDeterminant::load_config(const std::string & path) 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; } } } From 831bea6801c9dccf838d81bd268fbf1c2ae72f76 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 3 Mar 2024 21:41:14 +0700 Subject: [PATCH 7/9] fix: remove unused line --- test/node/kansei_node_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/test/node/kansei_node_test.cpp b/test/node/kansei_node_test.cpp index 295a076..a5f987c 100644 --- a/test/node/kansei_node_test.cpp +++ b/test/node/kansei_node_test.cpp @@ -37,7 +37,6 @@ TEST(KanseiNodeTest, CompileProcess) { kansei_node.set_fallen_determinant( std::make_shared(kansei::fallen::DeterminantType::ACCELERO)); - std::cout << "Fallen determinant" << std::endl; } catch (...) { } } From aa6df3d8ac6f15b3779450b0aaa95f2e1187c427 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Mon, 4 Mar 2024 17:18:42 +0700 Subject: [PATCH 8/9] refactor: use double for fallen variables --- .../kansei/fallen/node/fallen_determinant.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/kansei/fallen/node/fallen_determinant.hpp b/include/kansei/fallen/node/fallen_determinant.hpp index d8a7442..d9d16a3 100644 --- a/include/kansei/fallen/node/fallen_determinant.hpp +++ b/include/kansei/fallen/node/fallen_determinant.hpp @@ -52,15 +52,15 @@ class FallenDeterminant DeterminantType determinant_type; // fallen variables - float accel_back_limit; - float accel_front_limit; - float accel_right_limit; - float accel_left_limit; + double accel_back_limit; + double accel_front_limit; + double accel_right_limit; + double accel_left_limit; - float pitch_back_limit; - float pitch_front_limit; - float roll_right_limit; - float roll_left_limit; + double pitch_back_limit; + double pitch_front_limit; + double roll_right_limit; + double roll_left_limit; }; } // namespace kansei::fallen From 6c0446ce7750b40d68bce8dfc5cd26718f3c36f6 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Mon, 4 Mar 2024 19:37:56 +0700 Subject: [PATCH 9/9] refactor: fix use ttyusb1 --- src/check_rpy_main.cpp | 14 ++++---------- src/kansei_main.cpp | 14 ++++---------- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/src/check_rpy_main.cpp b/src/check_rpy_main.cpp index 817f04c..7fb3fd2 100755 --- a/src/check_rpy_main.cpp +++ b/src/check_rpy_main.cpp @@ -29,7 +29,7 @@ int main(int argc, char * argv[]) { - std::string port_name = "/dev/ttyUSB0"; + std::string port_name = "/dev/ttyUSB1"; if (argc > 1) { port_name = argv[1]; @@ -40,15 +40,9 @@ int main(int argc, char * argv[]) if (mpu.connect()) { std::cout << "succeeded to connect to mpu " << port_name << "!\n"; } else { - port_name = "/dev/ttyUSB1"; - mpu.set_port_name(port_name); - if (mpu.connect()) { - std::cout << "succeeded to connect to mpu " << port_name << "!\n"; - } else { - std::cout << "failed to connect to mpu!\n" << - "try again!\n"; - return 0; - } + std::cout << "failed to connect to mpu!\n" << + "try again!\n"; + return 0; } while (true) { diff --git a/src/kansei_main.cpp b/src/kansei_main.cpp index 59636b1..6d5e71d 100755 --- a/src/kansei_main.cpp +++ b/src/kansei_main.cpp @@ -35,7 +35,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - std::string port_name = "/dev/ttyUSB0"; + std::string port_name = "/dev/ttyUSB1"; std::string path = ""; kansei::fallen::DeterminantType determinant_type; @@ -88,15 +88,9 @@ int main(int argc, char * argv[]) if (mpu->connect()) { std::cout << "succeeded to connect to mpu " << port_name << "!\n"; } else { - port_name = "/dev/ttyUSB1"; - mpu->set_port_name(port_name); - if (mpu->connect()) { - std::cout << "succeeded to connect to mpu " << port_name << "!\n"; - } else { - std::cout << "failed to connect to mpu!\n" << - "try again!\n"; - return 0; - } + std::cout << "failed to connect to mpu!\n" << + "try again!\n"; + return 0; } auto node = std::make_shared("kansei_node");