Closed dsryzhov closed 3 years ago
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
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