src/wt_open_manipulator_node.cpp

   1 #include <signal.h>
   2 #include "ros/ros.h"
   3 #include "open_manipulator_i2c.h"
   4 #include "controller_manager/controller_manager.h"
   5 #include "std_srvs/SetBool.h"
   6
   7 static ros::ServiceClient enableManip;
   8
   9 void enableManippulator(bool enable) {
  10         std_srvs::SetBool srv;
  11         srv.request.data = enable;
  12
  13         if (enableManip.call(srv)) {
  14                 if (enable) {
  15                         ROS_INFO("Manipulator enabled");
  16                 } else {
  17                         ROS_INFO("Manipulator disabled");
  18                 }
  19         } else {
  20                 ROS_ERROR("Failed to call service manipulator_enable");
  21         }
  22 }
  23
  24 void handleSigint(int sig) {
  25         enableManippulator(false);
  26         ros::shutdown();
  27 }
  28
  29 int main(int argc, char **argv) {
  30         ros::init(argc, argv, "wt_open_manipulator_i2c", ros::init_options::NoSigintHandler);
  31
  32         ros::NodeHandle nh("~");
  33
  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);
  38
  39         WtOpenManipulatorI2C robot(nh);
  40         controller_manager::ControllerManager cm(&robot);
  41
  42         ros::AsyncSpinner spinner(1);
  43         spinner.start();
  44
  45         ros::Time prev_time = ros::Time::now();
  46         ros::Rate loop_rate(10);
  47         while(ros::ok()) {
  48                 const ros::Time time = ros::Time::now();
  49                 const ros::Duration period = time - prev_time;
  50
  51                 robot.read(period);
  52                 cm.update(time, period);
  53                 robot.write();
  54
  55                 prev_time = time;
  56                 loop_rate.sleep();
  57         }
  58         spinner.stop();
  59
  60         return 0;
  61 }