From 055e9bc3b2f95a757f8eae5e75615933896f943b Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Mon, 25 Feb 2019 16:12:38 +0900 Subject: [PATCH 01/16] Retry libypspur initialization --- src/ypspur_ros.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 5d64b0c..ac073f2 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -656,11 +656,16 @@ class YpspurRosNode ROS_ERROR("failed to start ypspur-coordinator"); throw(std::string("failed to start ypspur-coordinator")); } - sleep(2); - if (YP::YPSpur_initex(key_) < 0) + for (int i = 10; i >= 0; i--) { - ROS_ERROR("failed to init libypspur"); - throw(std::string("failed to init libypspur")); + if (i == 0) + { + ROS_ERROR("failed to init libypspur"); + throw(std::string("failed to init libypspur")); + } + ros::Duration(1).sleep(); + if (YP::YPSpur_initex(key_) >= 0) + break; } } double test_v, test_w; From 63d95fa6047f709f47cd537def81a67ea3988316 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 14:15:01 +0900 Subject: [PATCH 02/16] Fix subprocess argument --- src/ypspur_ros.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index ac073f2..a186a8b 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -647,12 +647,16 @@ class YpspurRosNode args.push_back(param_file_); } - const char **argv = new const char *[args.size() + 1]; + char **argv = new char *[args.size() + 1]; for (unsigned int i = 0; i < args.size(); i++) - argv[i] = args[i].c_str(); + { + argv[i] = new char[args[i].size() + 1]; + memcpy(argv[i], args[i].c_str(), args[i].size()); + argv[i][args[i].size()] = 0; + } argv[args.size()] = nullptr; - execvp(ypspur_bin_.c_str(), const_cast(argv)); + execvp(ypspur_bin_.c_str(), argv); ROS_ERROR("failed to start ypspur-coordinator"); throw(std::string("failed to start ypspur-coordinator")); } From 1a24b3bfcf01820114085304d2caa6cb844abc38 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 15:15:06 +0900 Subject: [PATCH 03/16] Clean-up ypspur-coordinator existence check --- src/ypspur_ros.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index a186a8b..86902b8 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -667,22 +667,21 @@ class YpspurRosNode ROS_ERROR("failed to init libypspur"); throw(std::string("failed to init libypspur")); } - ros::Duration(1).sleep(); + ros::WallDuration(1).sleep(); if (YP::YPSpur_initex(key_) >= 0) break; } } - double test_v, test_w; double ret; boost::atomic done(false); - auto get_vel_thread = [&test_v, &test_w, &ret, &done] + auto get_vel_thread = [&ret, &done] { + double test_v, test_w; ret = YP::YPSpur_get_vel(&test_v, &test_w); done = true; }; boost::thread spur_test = boost::thread(get_vel_thread); - boost::chrono::milliseconds span(100); - boost::this_thread::sleep_for(span); + ros::WallDuration(0.1).sleep(); if (!done) { // There is no way to kill thread safely in C++11 From 194bb98284a5288540ce97d4b77da3bc6e0d74e2 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 15:18:45 +0900 Subject: [PATCH 04/16] Remove msq before ypspur-coordinator init --- src/ypspur_ros.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 86902b8..733d67c 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -47,6 +47,8 @@ #include #include +#include +#include #include #include @@ -647,6 +649,10 @@ class YpspurRosNode args.push_back(param_file_); } + int msq; + msq = msgget(key_, 0666 | IPC_CREAT); + msgctl(msq, IPC_RMID, nullptr); + char **argv = new char *[args.size() + 1]; for (unsigned int i = 0; i < args.size(); i++) { From 2b5e2f3fc9d9de245d45fed941d903b96cd0a2e0 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 17:09:20 +0900 Subject: [PATCH 05/16] Fix exit code --- src/ypspur_ros.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 733d67c..db0b52c 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -1246,6 +1246,7 @@ int main(int argc, char *argv[]) } catch (std::string e) { + return -1; } return 0; From 2507a9914b514e9bd39ead60d8f914e2b16ac958 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 18:22:31 +0900 Subject: [PATCH 06/16] Add debug output --- src/ypspur_ros.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index db0b52c..82fd19d 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -661,6 +661,7 @@ class YpspurRosNode argv[i][args[i].size()] = 0; } argv[args.size()] = nullptr; + ROS_INFO("execute %s", ypspur_bin_.c_str()); execvp(ypspur_bin_.c_str(), argv); ROS_ERROR("failed to start ypspur-coordinator"); From 149c06d0693bc82ea212788327b08091c4741055 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 18:52:39 +0900 Subject: [PATCH 07/16] Check fork error --- src/ypspur_ros.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 82fd19d..74355ab 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -629,7 +630,13 @@ class YpspurRosNode { ROS_WARN("launching ypspur-coordinator"); pid_ = fork(); - if (pid_ == 0) + if (pid_ == -1) + { + const int err = errno; + ROS_ERROR("failed to fork process: %s", strerror(err)); + throw(std::string("failed to fork process")); + } + else if (pid_ == 0) { std::vector args; args.push_back(ypspur_bin_); From 2b1c3e01e33a542390981e8f0b528a2522f44f70 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 18:53:07 +0900 Subject: [PATCH 08/16] Fix temporary variable init --- src/ypspur_ros.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 74355ab..402d0f7 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -656,8 +656,7 @@ class YpspurRosNode args.push_back(param_file_); } - int msq; - msq = msgget(key_, 0666 | IPC_CREAT); + int msq = msgget(key_, 0666 | IPC_CREAT); msgctl(msq, IPC_RMID, nullptr); char **argv = new char *[args.size() + 1]; From a64cfc1d5943fb164b1e94669d45960f92cd7268 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 19:09:46 +0900 Subject: [PATCH 09/16] Generate arg list before fork --- src/ypspur_ros.cpp | 61 +++++++++++++++++++++++----------------------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 402d0f7..113aef4 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -628,6 +628,36 @@ class YpspurRosNode { if (i > 0 || YP::YPSpur_initex(key_) < 0) { + std::vector args; + args.push_back(ypspur_bin_); + args.push_back(std::string("-d")); + args.push_back(port_); + args.push_back(std::string("--admask")); + args.push_back(ad_mask); + args.push_back(std::string("--msq-key")); + args.push_back(std::to_string(key_)); + if (digital_input_enable_) + args.push_back(std::string("--enable-get-digital-io")); + if (simulate_) + args.push_back(std::string("--without-device")); + if (param_file_.size() > 0) + { + args.push_back(std::string("-p")); + args.push_back(param_file_); + } + + char **argv = new char *[args.size() + 1]; + for (unsigned int i = 0; i < args.size(); i++) + { + argv[i] = new char[args[i].size() + 1]; + memcpy(argv[i], args[i].c_str(), args[i].size()); + argv[i][args[i].size()] = 0; + } + argv[args.size()] = nullptr; + + int msq = msgget(key_, 0666 | IPC_CREAT); + msgctl(msq, IPC_RMID, nullptr); + ROS_WARN("launching ypspur-coordinator"); pid_ = fork(); if (pid_ == -1) @@ -638,37 +668,6 @@ class YpspurRosNode } else if (pid_ == 0) { - std::vector args; - args.push_back(ypspur_bin_); - args.push_back(std::string("-d")); - args.push_back(port_); - args.push_back(std::string("--admask")); - args.push_back(ad_mask); - args.push_back(std::string("--msq-key")); - args.push_back(std::to_string(key_)); - if (digital_input_enable_) - args.push_back(std::string("--enable-get-digital-io")); - if (simulate_) - args.push_back(std::string("--without-device")); - if (param_file_.size() > 0) - { - args.push_back(std::string("-p")); - args.push_back(param_file_); - } - - int msq = msgget(key_, 0666 | IPC_CREAT); - msgctl(msq, IPC_RMID, nullptr); - - char **argv = new char *[args.size() + 1]; - for (unsigned int i = 0; i < args.size(); i++) - { - argv[i] = new char[args[i].size() + 1]; - memcpy(argv[i], args[i].c_str(), args[i].size()); - argv[i][args[i].size()] = 0; - } - argv[args.size()] = nullptr; - ROS_INFO("execute %s", ypspur_bin_.c_str()); - execvp(ypspur_bin_.c_str(), argv); ROS_ERROR("failed to start ypspur-coordinator"); throw(std::string("failed to start ypspur-coordinator")); From 80ac5d477764a07b18f501946ca6623145a47ce7 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 19:31:14 +0900 Subject: [PATCH 10/16] Wait child process at exit --- src/ypspur_ros.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 113aef4..40debb6 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -1230,14 +1230,16 @@ class YpspurRosNode } } ROS_INFO("ypspur_ros main loop terminated"); - ros::shutdown(); - ros::spin(); if (pid_ > 0) { ROS_INFO("killing ypspur-coordinator (%d)", (int)pid_); kill(pid_, SIGINT); - sleep(2); + int status; + waitpid(pid_, &status, WNOHANG); + ROS_INFO("ypspur-coordinator is killed (status: %d)", status); } + ros::shutdown(); + ros::spin(); } }; From c3aaf0a3b860f7e7750e88f1606c0acf3139c70a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 5 Mar 2019 19:33:59 +0900 Subject: [PATCH 11/16] Move child process finalization to destructor --- src/ypspur_ros.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 40debb6..ec7813f 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -744,6 +744,15 @@ class YpspurRosNode } ~YpspurRosNode() { + if (pid_ > 0) + { + ROS_INFO("killing ypspur-coordinator (%d)", (int)pid_); + kill(pid_, SIGINT); + int status; + waitpid(pid_, &status, WNOHANG); + ROS_INFO("ypspur-coordinator is killed (status: %d)", status); + } + ros::shutdown(); } void spin() { @@ -1230,16 +1239,6 @@ class YpspurRosNode } } ROS_INFO("ypspur_ros main loop terminated"); - if (pid_ > 0) - { - ROS_INFO("killing ypspur-coordinator (%d)", (int)pid_); - kill(pid_, SIGINT); - int status; - waitpid(pid_, &status, WNOHANG); - ROS_INFO("ypspur-coordinator is killed (status: %d)", status); - } - ros::shutdown(); - ros::spin(); } }; From 350cb237c391df7c2ed908561056af3c555af3a5 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 6 Mar 2019 11:34:54 +0900 Subject: [PATCH 12/16] Reduce retry limit --- src/ypspur_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index ec7813f..9b5e132 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -672,7 +672,7 @@ class YpspurRosNode ROS_ERROR("failed to start ypspur-coordinator"); throw(std::string("failed to start ypspur-coordinator")); } - for (int i = 10; i >= 0; i--) + for (int i = 4; i >= 0; i--) { if (i == 0) { From 0ff47fe39b1deaf3261593e3c0fd0d13ab5c87e8 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 6 Mar 2019 11:38:31 +0900 Subject: [PATCH 13/16] Check process existence before init libypspur --- src/ypspur_ros.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 9b5e132..3bdb3af 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -674,7 +674,8 @@ class YpspurRosNode } for (int i = 4; i >= 0; i--) { - if (i == 0) + int status; + if (i == 0 || waitpid(pid_, &status, WNOHANG) == pid_) { ROS_ERROR("failed to init libypspur"); throw(std::string("failed to init libypspur")); From 22079d21b6ea0b8343748e6b4226868c72d2e0a0 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 6 Mar 2019 11:43:02 +0900 Subject: [PATCH 14/16] Split error messages --- src/ypspur_ros.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 3bdb3af..1f89e16 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -675,7 +675,12 @@ class YpspurRosNode for (int i = 4; i >= 0; i--) { int status; - if (i == 0 || waitpid(pid_, &status, WNOHANG) == pid_) + if (waitpid(pid_, &status, WNOHANG) == pid_) + { + ROS_ERROR("ypspur-coordinator dead immediately"); + throw(std::string("ypspur-coordinator dead immediately")); + } + else if (i == 0) { ROS_ERROR("failed to init libypspur"); throw(std::string("failed to init libypspur")); From 86e1bcb171e5292ca57add978b959f4bbb246959 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 7 Mar 2019 17:39:05 +0900 Subject: [PATCH 15/16] Address review comments --- src/ypspur_ros.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 1f89e16..086d662 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -628,14 +628,13 @@ class YpspurRosNode { if (i > 0 || YP::YPSpur_initex(key_) < 0) { - std::vector args; - args.push_back(ypspur_bin_); - args.push_back(std::string("-d")); - args.push_back(port_); - args.push_back(std::string("--admask")); - args.push_back(ad_mask); - args.push_back(std::string("--msq-key")); - args.push_back(std::to_string(key_)); + std::vector args = + { + ypspur_bin_, + "-d", port_, + "--admask", ad_mask, + "--msq-key", std::to_string(key_) + }; if (digital_input_enable_) args.push_back(std::string("--enable-get-digital-io")); if (simulate_) @@ -672,6 +671,11 @@ class YpspurRosNode ROS_ERROR("failed to start ypspur-coordinator"); throw(std::string("failed to start ypspur-coordinator")); } + + for (unsigned int i = 0; i < args.size(); i++) + delete argv[i]; + delete argv; + for (int i = 4; i >= 0; i--) { int status; From b8477a2402c50cc89b01b272df42c982bca94b5e Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 7 Mar 2019 17:39:38 +0900 Subject: [PATCH 16/16] Fix to properly wait child --- src/ypspur_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index 086d662..ba89bcb 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -759,7 +759,7 @@ class YpspurRosNode ROS_INFO("killing ypspur-coordinator (%d)", (int)pid_); kill(pid_, SIGINT); int status; - waitpid(pid_, &status, WNOHANG); + waitpid(pid_, &status, 0); ROS_INFO("ypspur-coordinator is killed (status: %d)", status); } ros::shutdown();