3 #include "open_manipulator_i2c.h"
4 #include "controller_manager/controller_manager.h"
5 #include "std_srvs/SetBool.h"
7 static ros::ServiceClient enableManip;
9 void enableManippulator(bool enable) {
10 std_srvs::SetBool srv;
11 srv.request.data = enable;
13 if (enableManip.call(srv)) {
15 ROS_INFO("Manipulator enabled");
17 ROS_INFO("Manipulator disabled");
20 ROS_ERROR("Failed to call service manipulator_enable");
24 void handleSigint(int sig) {
25 enableManippulator(false);
29 int main(int argc, char **argv) {
30 ros::init(argc, argv, "wt_open_manipulator_i2c", ros::init_options::NoSigintHandler);
32 ros::NodeHandle nh("~");
34 enableManip = nh.serviceClient<std_srvs::SetBool>("/wild_thumper/manipulator_enable");
35 enableManippulator(true);
36 sleep(1); // wait to power up
37 signal(SIGINT, handleSigint);
39 WtOpenManipulatorI2C robot(nh);
40 controller_manager::ControllerManager cm(&robot);
42 ros::AsyncSpinner spinner(1);
45 ros::Time prev_time = ros::Time::now();
46 ros::Rate loop_rate(10);
48 const ros::Time time = ros::Time::now();
49 const ros::Duration period = time - prev_time;
52 cm.update(time, period);