[RobotHardware/robot.cpp] send correct emergency signal when servo alarm

[Naoki-Hiraoka]

RobotHardwareからemergencySignalとしてEMG_SERVO_ERRORが出ていて、このif文によって全身がservo offするにも関わらず、

hrpsys-base/rtc/RobotHardware/RobotHardware.cpp

Lines 242 to 243 in 2336d26

	if (reason == robot::EMG_SERVO_ERROR || reason == robot::EMG_POWER_OFF){
	m_robot->servo("all", false);

実際にはEMG_SERVO_ALARMが発生していて、EMG_SERVO_ERRORは発生していない、というバグがあります。

原因は、変数reasonはenumの初期値としてEMG_SERVO_ERRORが入っていて、

hrpsys-base/rtc/RobotHardware/robot.h

Line 254 in 2336d26

typedef enum {EMG_SERVO_ERROR, EMG_FZ, EMG_SERVO_ALARM, EMG_POWER_OFF} emg_reason;

SS_EMERGENCYが発生していてm_reportedEmergencyがtrueの場合には変数reasonが初期値のまま変更されずに返るためでした。

hrpsys-base/rtc/RobotHardware/robot.cpp

Lines 708 to 717 in 2336d26

	for (unsigned int i=0; i<numJoints(); i++){
	if (!read_servo_alarm(i, &alarm)) continue;
	if (alarm & SS_EMERGENCY) {
	if (!m_reportedEmergency) {
	m_reportedEmergency = true;
	o_reason = EMG_SERVO_ALARM;
	o_id = i;
	}
	return true;
	}

このバグがあるとemergencyの原因をデバッグすることが困難になってしまうので、変数reasonにEMG_SERVO_ALARMを入れてから返るように修正しました。

(関連)
2d088ad
#556
#564