caster angle errors published by pr2_base_controller2 are wrong

[goretkin]

If you rostopic echo /base_controller/state after having moved the robot around in circles but keeping the same heading, the casters will have rotated around (they're continuous rotation joints), the first four numbers of joint_error: corresponding to the caster angles, will be multiples of pi, even though the true error is zero as calculated for the PID controller:

pr2_controllers/pr2_mechanism_controllers/src/pr2_base_controller2.cpp

Lines 407 to 413 in 02376d1

	steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired + M_PI);
	error_steer = angles::shortest_angular_distance(
	base_kinematics_.caster_[i].joint_->position_,
	steer_angle_desired);
	error_steer_m_pi = angles::shortest_angular_distance(
	base_kinematics_.caster_[i].joint_->position_,
	steer_angle_desired_m_pi);

The error calculation for publishing (here:

pr2_controllers/pr2_mechanism_controllers/src/pr2_base_controller2.cpp

Line 346 in 02376d1

state_publisher_->msg_.joint_effort_error[i] = base_kinematics_.caster_[i].joint_->measured_effort_ - base_kinematics_.caster_[i].joint_->commanded_effort_;

) should match the error calculation above.

[trainman419]

If you change this, be very careful in your testing. If the gains or the error term for any of the caster motors is wrong, the motors can fight each other and burn out.

I recommend lifting the robot slightly off the floor by putting it on wooden blocks while testing this.