X-Git-Url: https://defiant.homedns.org/gitweb/?p=wt_open_manipulator.git;a=blobdiff_plain;f=src%2Fwt_open_manipulator_node.cpp;h=e07ec0de193199d20a1cd8a22e0c6f05f8fd03dc;hp=7200361bba557538e7c58c898a1d1893f7105e93;hb=ea3762901592d6a1f24f4aec027a88e3f0f454b7;hpb=d9e47da9f541587654d760f467cff1262087cb70

diff --git a/src/wt_open_manipulator_node.cpp b/src/wt_open_manipulator_node.cpp
index 7200361..e07ec0d 100644
--- 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);