Automatically power up/down

diff --git a/src/wt_open_manipulator_node.cpp b/src/wt_open_manipulator_node.cpp
index 7200361bba557538e7c58c898a1d1893f7105e93..e07ec0de193199d20a1cd8a22e0c6f05f8fd03dc 100644 (file)
--- a/src/wt_open_manipulator_node.cpp
+++ b/src/wt_open_manipulator_node.cpp
@@ -1,12 +1,41 @@
+#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("~");
 
+       enableManip = nh.serviceClient<std_srvs::SetBool>("/wild_thumper/manipulator_enable");
+       enableManippulator(true);
+       sleep(1); // wait to power up
+       signal(SIGINT, handleSigint);
+
        WtOpenManipulatorI2C robot(nh);
        controller_manager::ControllerManager cm(&robot);