Automatically power up/down

[wt_open_manipulator.git] / src / wt_open_manipulator_node.cpp

diff --git a/src/wt_open_manipulator_node.cpp b/src/wt_open_manipulator_node.cpp
index b20f13ccdc6100f5d52b9050c3ab521fd19b0781..e07ec0de193199d20a1cd8a22e0c6f05f8fd03dc 100644 (file)
--- a/src/wt_open_manipulator_node.cpp
+++ b/src/wt_open_manipulator_node.cpp
@@ -1,13 +1,42 @@
+#include <signal.h>
 #include "ros/ros.h"
 #include "open_manipulator_i2c.h"
 #include "controller_manager/controller_manager.h"
+#include "std_srvs/SetBool.h"
+
+static ros::ServiceClient enableManip;
+
+void enableManippulator(bool enable) {
+       std_srvs::SetBool srv;
+       srv.request.data = enable;
+
+       if (enableManip.call(srv)) {
+               if (enable) {
+                       ROS_INFO("Manipulator enabled");
+               } else {
+                       ROS_INFO("Manipulator disabled");
+               }
+       } else {
+               ROS_ERROR("Failed to call service manipulator_enable");
+       }
+}
+
+void handleSigint(int sig) {
+       enableManippulator(false);
+       ros::shutdown();
+}
 
 int main(int argc, char **argv) {
-       ros::init(argc, argv, "wt_open_manipulator_i2c");
+       ros::init(argc, argv, "wt_open_manipulator_i2c", ros::init_options::NoSigintHandler);
+
+       ros::NodeHandle nh("~");
 
-       ros::NodeHandle n;
+       enableManip = nh.serviceClient<std_srvs::SetBool>("/wild_thumper/manipulator_enable");
+       enableManippulator(true);
+       sleep(1); // wait to power up
+       signal(SIGINT, handleSigint);
 
-       WtOpenManipulatorI2C robot(n);
+       WtOpenManipulatorI2C robot(nh);
        controller_manager::ControllerManager cm(&robot);
 
        ros::AsyncSpinner spinner(1);