+#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();
+}