ros_control: Difficulty getting custom hardware interface to run

edit

Hello,

I am trying to implement a custom hardware interface for a 3-joint manipulator with ros_control. I was following the tutorial on the matter pretty closely. The problem is that while the controller_manager apparently gets started, service calls like list_controllers just hang. I am also unable to load any controllers, as this call also hangs.

To narrow down the possible causes, I created a test node (called test_iface) which contains almost exactly the code from the tutorial mentioned above, i.e., two joints, "A" and "B", and the interface implements a JointStateInterface as well as a PositionJointInterface. The read() and write() methods simply write a message to stdout (ROS_INFO).

Now, when I start this test node, the read/write loop gets executed properly, as I get the ROS_INFO messages from them. Also, the output of rosservice list is:

user@ubuntu:~$ rosservice list
/controller_manager/list_controller_types
/controller_manager/list_controllers
/controller_manager/load_controller
/controller_manager/reload_controller_libraries
/controller_manager/switch_controller
/controller_manager/unload_controller
/joint_state_publisher/get_loggers
/joint_state_publisher/set_logger_level
/rosout/get_loggers
/rosout/set_logger_level
/test_iface_node/get_loggers
/test_iface_node/set_logger_level

So the controller_manager seems to start up okay. However, when I try to call the list_controllers service (via rqt; expecting an empty list, since no controllers have been loaded yet), the request just hangs and nothing happens.

When I use gazebo instead of my custom HW interface, the service call to list_controllers returns an empty list, as expected. (on a side note, if I run "rosrun controller_manager controller_manager list", that always seems to hang).

What can I do to debug this? Do you have any suggestions as to what I might be doing wrong? The test_iface code is the MyRobot class from the tutorial, extended by a read() and write() that only print debug messages, and this main method:

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_iface_node");
  ros::NodeHandle nh;

  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  ros::Time ts = ros::Time::now();
  while (true)
  {
     ros::Duration d = ts - ros::Time::now(); 
     ts = ros::Time::now();
     robot.read();
     cm.update(ts, d);
     robot.write();
     sleep(1);
  }

  return 0;
}

Any hints or suggestions are welcome. Thanks!