Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Running NetFT example controller

Before you start

Go through tutorial on using netft_ethercat_hardware plugin and make sure you execute all the steps in that tutorial in your account on the robot.

Introduction

NetFT plugin for pr2_etherCAT publishes data to

• AnalogIn structure. The AnalogIn structure contains a std::vector<double> with 6 elements. The six elements each contain 3 force and three force values. The order of the elements are

   0 Force in direction of X axis
   1 Force in direction of Y axis
   2 Force in direction of Z axis
   3 Torque in around X axis
   4 Torque in around Y axis
   5 Torque in around Z axis 

Running controller

First compile controller

• rosmake netft_example_controller

Next, run pr2_etherCAT with netft plugin. Use directions here

Use the example_controller.launch script contains a launch file loading a the example controller.

• roslaunch netft_example_controller example_controller.launch

Try viewing controller output

• rostopic echo /example_controller/force_torque_stats

Troubleshooting

Nothing coming from force_torque_stats topic

Did controller load properly? Try checking with

• rosrun pr2_controller_manager pr2_controller_manager list  | grep example_controller

You should see something like

• example_controller ( running )

If controller is running use rxconsole to view any messages controller may be producing. If controller is not running use rxconsole while starting controller and look for any error messages.

Output message : Cannot find AnalogIn named "XXXX"

Make sure NetFT driver is loaded by looking for device data in diagnostics. Also and make sure "XXXX" matches name provided to netft_ethercat_hardware driver. The diagnostics for the NetFT device lists this name.

Code breakdown

Getting pointer to AnalogIn structure containing force-torque data

The following code attempts to locate AnalogIn with name analog_in_name which was loaded from rosparam in previous part of code. If the AnalogIn is not found, it was print list of currently available AnalogIn structures.

From netft_example_controller.cpp

•   pr2_hardware_interface::HardwareInterface* hw = robot->model_->hw_;  
    analog_in_ = hw->getAnalogIn(analog_in_name);
    if (analog_in_ == NULL)
    {
      ROS_ERROR("NetFTExampleController: Cannot find AnalogIn named \"%s\"",
                analog_in_name.c_str());
      BOOST_FOREACH(const pr2_hardware_interface::AnalogInMap::value_type &v, hw->analog_ins_)
      {
        ROS_INFO("AnalogIn : %s", v.first.c_str());
      }
      return false;
    }
  }

Using AnalogIn data

The following code gets values from AnalogIn structure and assigns them to temporary variables. Note fx stands for force along X axis and tx for torque around X axis.

From netft_example_controller.cpp

•   double fx = analog_in_->state_.state_[0];
    double fy = analog_in_->state_.state_[1];
    double fz = analog_in_->state_.state_[2];
    double tx = analog_in_->state_.state_[3];
    double ty = analog_in_->state_.state_[4];
    double tz = analog_in_->state_.state_[5];