ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update odom

Browse Source
This commit is contained in:
zhangwanjie 2020-10-16 11:42:57 +08:00
parent 4887b5af8d
commit 6477148f33
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
wpb_home_bringup/src/wpb_home_core.cpp
View File

@ -209,9 +209,9 @@ int main(int argc, char** argv)
fVy = (fPosDiff[0] + fPosDiff[1]) - (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVy;
fVz = (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVz;
double fTimeDur = current_time.toSec() - last_time.toSec();
fVx = fVx/(fTimeDur*9700);
fVy = fVy/(fTimeDur*9700);
fVz = fVz/(fTimeDur*1925);
fVx = fVx/(fTimeDur*9100);
fVy = fVy/(fTimeDur*9100);
fVz = fVz/(fTimeDur*1840);
double dx = (lastVel.linear.x*cos(lastPose.theta) - lastVel.linear.y*sin(lastPose.theta))*fTimeDur;
double dy = (lastVel.linear.x*sin(lastPose.theta) + lastVel.linear.y*cos(lastPose.theta))*fTimeDur;