From: Erik Andresen <erik@vontaene.de>
Date: Fri, 31 Jan 2020 06:10:21 +0000 (+0100)
Subject: Automatically power up/down
X-Git-Url: https://defiant.homedns.org/gitweb/?p=wt_open_manipulator.git;a=commitdiff_plain;h=ea3762901592d6a1f24f4aec027a88e3f0f454b7

Automatically power up/down
---

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);