diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h index 492edfe9476a56f8deb9ae8cc5e08da3f8c3f200..9c5064ccf8154d23a83c2be8ace0db066cc6e08a 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -35,21 +35,6 @@ namespace algorithm_base { -/** - * \brief Abstract Class for Ctrl+C management - * - */ -class AbstractAlgorithmNode -{ - protected: - - static void hupCalled(int sig) - { - ROS_WARN("Unexpected SIGHUP caught. Ignoring it."); - } - -}; - /** * \brief IRI ROS Algorithm Base Node Class * @@ -75,7 +60,7 @@ class AbstractAlgorithmNode * mainNodeThread() function defined in the inherit node class is called. */ template <class Algorithm> -class IriBaseAlgorithm : public AbstractAlgorithmNode +class IriBaseAlgorithm { public: /** @@ -159,7 +144,7 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode * loop_rate_ and the diagnostic updater. It also instantiates the main * thread of the class and the dynamic reconfigure callback. */ - IriBaseAlgorithm(void); + IriBaseAlgorithm(const ros::NodeHandle &nh = ros::NodeHandle("~")); /** * \brief destructor @@ -239,13 +224,6 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode */ virtual void addNodeDiagnostics(void) = 0; -// void addOpenedTests(void); -// void addStoppedTests(void); -// void addRunningTests(void); -// virtual void addNodeOpenedTests(void) = 0; -// virtual void addNodeStoppedTests(void) = 0; -// virtual void addNodeRunningTests(void) = 0; - /** * \brief main node thread * @@ -268,12 +246,14 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode * have to be detailed. */ virtual void mainNodeThread(void) = 0; + + static void hupCalled(int sig); }; template <class Algorithm> -IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : - public_node_handle_(ros::this_node::getName()), +IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(const ros::NodeHandle &nh) : + public_node_handle_(nh), private_node_handle_("~"), loop_rate_(DEFAULT_RATE), diagnostic_(), @@ -281,9 +261,8 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : { ROS_DEBUG("IriBaseAlgorithm::Constructor"); -// // assign callback to dynamic reconfigure server -// dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2)); - + // allow Ctrl+C management + signal(SIGHUP, &IriBaseAlgorithm<Algorithm>::hupCalled); } template <class Algorithm> @@ -330,6 +309,12 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) pthread_exit(NULL); } +template <class Algorithm> +void IriBaseAlgorithm<Algorithm>::hupCalled(int sig) +{ + ROS_WARN("Unexpected SIGHUP caught. Ignoring it."); +} + template <class Algorithm> int IriBaseAlgorithm<Algorithm>::spin(void) { @@ -390,9 +375,6 @@ int main(int argc, char **argv, std::string node_name) // ROS initialization ros::init(argc, argv, node_name); - // allow Ctrl+C management - signal(SIGHUP, &AbstractAlgorithmNode::hupCalled); - // define and launch generic algorithm implementation object AlgImplTempl algImpl; algImpl.spin();