Unable to load Position Trajectory Controller

edit

I want to control and drive a real 6-DOF robotic arm.

I have setup my Moveit configuration files using joint_trajectory_controller and MoveItSimpleControllerManager. Here is my moveit controllers.yaml file:

controller_list:
   - name: /nymblebot/position_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - base_swivel
      - arm_joint
      - elbow_joint
      - wrist_pitch
      - wrist_yaw
      - wrist_roll

Then I have setup my ros_control files along with hardware interfaces. Here is my ros_control hardware_controllers.yaml file:

generic_hw_control_loop:
  loop_hz: 30
  cycle_time_error_threshold: 0.01

# Settings for ros_control hardware interface
nymble_hw_interface:
   joints:
      - base_swivel
      - arm_joint
      - elbow_joint
      - wrist_pitch
      - wrist_yaw
      - wrist_roll

# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
   type: position_controllers/JointTrajectoryController
# These joints can likely just be copied from the hardware_interface list above

joints:     
      - base_swivel
      - arm_joint
      - elbow_joint
      - wrist_pitch
      - wrist_yaw
      - wrist_roll

# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type:         joint_state_controller/JointStateController
  publish_rate: 50

My control interface is based on Adolfo's ROSCON'14 talk. In my main executable, I instantiate a hardware interface class of my robot. Then I instantiate a control_loop class which has read(), write() as member functions and pulls info out from ROS parameter server.

Also I form an instance of the controller manager. Following this I have my main control loop which proceeds as: 1. read() 2.update() 3.write()

Unfortunately the process crashes soon after the 2.update() command. And only the joint_state_controller gets loaded.

Started ['joint_state_controller'] successfully
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/controller_manager", line 65, in <module>
    controller_manager_interface.start_controller(c)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/controller_manager/controller_manager_interface.py", line 86, in start_controller
    return start_stop_controllers([name], True)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/controller_manager/controller_manager_interface.py", line 107, in start_stop_controllers
    resp = s.call(SwitchControllerRequest(start, stop, strictness))
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))

rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details
[nymblebot/nymblebot_hardware_interface-8] process has died [pid 8763, exit code -11, cmd /home/rohin/catkin_ws/devel/lib/hardware/nymble_hw_control_loop __name:=nymblebot_hardware_interface __log:=/home/rohin/.ros/log/4856bd5a-41c5-11e6-b1dc-20689d39eac5/nymblebot-nymblebot_hardware_interface-8.log].
log file: /home/rohin/.ros/log/4856bd5a-41c5-11e6-b1dc-20689d39eac5/nymblebot-nymblebot_hardware_interface-8*.log
[nymblebot/ros_control_controller_manager-7] process has died [pid 8755, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/controller_manager spawn joint_state_controller position_trajectory_controller __name:=ros_control_controller_manager __log:=/home/rohin/.ros/log/4856bd5a-41c5-11e6-b1dc-20689d39eac5/nymblebot-ros_control_controller_manager-7.log].
log file: /home/rohin/.ros/log/4856bd5a-41c5-11e6-b1dc-20689d39eac5/nymblebot-ros_control_controller_manager-7*.log

For reference here is my main control loop in my executable:

int main(int argc, char **argv)
{
    // Setup ROS
    ros::init(argc, argv, "nymblebot_hw_interface");
    ros::NodeHandle n;
// NOTE: We run the ROS loop in a separate thread as external calls such
// as service callbacks to load controllers can block the (main) control loop
ros::AsyncSpinner spinner(2);
spinner.start();

// Create the hardware interface specific to your robot
boost::shared_ptr<NymbleHW> nymble_hw_interface (new NymbleHW(n));
//nymble_hw_interface->init();

// Start the control loop
Nymble_HW_Loop control_loop(n, nymble_hw_interface);
control_loop.init();

boost::shared_ptr<controller_manager::ControllerManager> controller_manager_ ...