From ea3762901592d6a1f24f4aec027a88e3f0f454b7 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 31 Jan 2020 07:10:21 +0100 Subject: [PATCH] Automatically power up/down --- src/wt_open_manipulator_node.cpp | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) 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 #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("/wild_thumper/manipulator_enable"); + enableManippulator(true); + sleep(1); // wait to power up + signal(SIGINT, handleSigint); + WtOpenManipulatorI2C robot(nh); controller_manager::ControllerManager cm(&robot); -- 2.39.5