theta_rad from orientation in update_odometry()

[dsryzhov]

Hi Jack,

In update_odometry theta_rad is set incorrect https://github.com/hadabot/hadabot_main/blob/master/content/p7/hadabot_ws/src/hadabot_driver/src/hadabot_controller.cpp

auto thetarad = pose->pose.pose.orientation.z;

Correct procedure is tf2::Quaternion q0(pose->pose.pose.orientation.x, pose->pose.pose.orientation.y, pose->pose.pose.orientation.z, pose->pose.pose.orientation.w); tf2::Matrix3x3 m(q0); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); double theta_rad = yaw;

In constructor initial orientation should be initialized from Euler angles tf2::Quaternion q; q.setEuler(0,0,0); pose->pose.pose.orientation.x = q.getX(); pose->pose.pose.orientation.y = q.getY(); pose->pose.pose.orientation.z = q.getZ(); pose->pose.pose.orientation.w = q.getW();

Regards, Dmitry

[jackpien]

Good catch @dsryzhov !

Also, like with Issue #9, the d_left_m and d_right_m is incorrectly dividing by PI. I will fix that along with the theta_rad fix.

BTW - if you'd like, you can fork this repo, make changes to fix issues, then submit a Pull Request. This way you don't have to cut-n-paste code into a new Issue. Happy to walk you through process if you'd like, just drop me email. Thx!! Jack