Update current_time_ (robot->getTime()) when EtherCAT resends dropped packet. (ros ticket #5003)

[ahendrix]

I believe there is a problem with the robot->getTime()

{{{ pr2_mechanismcontrollers/src/robot.cpp ros::Time Robot::getTime() { return hw->currenttime; } }}}

''ethercat_hardware/src/ethercat_hardware.cpp'' {{{ void EthercatHardware::update(bool reset, bool halt) { ros::Time update_starttime(ros::Time::now()); hw->currenttime = update_start_time;

... ...

bool success = txandrx_PD(buffersize, thisbuffer, max_pdretries); } }}}

The problem is that the hw->currenttime is the system time just before the first EtherCAT packet is sent. If the packet doesn't get dropped the currenttime will be about 0-300usec older that the encoder positions in the packet. However, if the packet gets dropped, it will be resent but the currenttime value is not required. If the packet timeout is set to 20ms, the currenttime will be 20msec older than the data in the packet.

There are two thinks that should be done:

1. I'll change ethercat_hardware set hw->currenttime to the time just before the realtime packet is sent.
2. The WG0X devices provide there own microsec precision timestamp. The FPGA firmware uses a signal (ReadDataLatchStrobe) that causes all realtime data to be moved to shadow registers just before it is read out of device and into buffer in EtherCAT ASIC. Thus the timestamp and encoder position in the data are taken from the exact same clock cycle on the FPGA. Ideally this value should be used for calculation velocity.

The velocity value from actuator state is actually calculated this way ''thercat_hardware/src/wg0x.cpp'' {{{ bool WG0X:unpackState(unsigned char this_buffer, unsigned char prev_buffer) { ... state.encodervelocity = double(int(this_status->encodercount - prev_status->encodercount)) / (thisstatus->timestamp - prevstatus->timestamp) * 1e+6; state.velocity_ = state.encodervelocity / actuatorinfo.pulses_perrevolution * 2 * M_PI; } }}}

The velocity_ value from actuator state should give a much better velocity measurement (even with packet loss)

trac data:

• Owner: dking
• Reporter: dking
• Reported at: Fri Feb 4 17:48:29 2011
• URL: https://code.ros.org/trac/wg-ros-pkg/ticket/5003

[ahendrix]

[dking] Putting update of hw->currenttime after txandrx_pd() should fix this problem.

{{{ void EthercatHardware::update(bool reset, bool halt) { ros::Time update_start_time(ros::Time::now());

... ...

bool success = txandrx_PD(buffersize, thisbuffer, max_pdretries); ros::Time txandrx_endtime(ros::Time::now());
hw->currenttime = txandrx_end_time; } }}}

[ahendrix]

[dking] Fixed by moving using the ros::Time::now() value taken after EtherCAT packets in successfully received as opposed to before first EtherCAT packet send. r48775