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

4
wpb_home_bringup/src/wpb_home_calibrate_velocity.cpp
View File

@ -71,8 +71,8 @@ public:
{ {
std::cout << "输入如下指令并回车执行：\n" std::cout << "输入如下指令并回车执行：\n"
"W - 前进1米 \nS - 后退1米\n" "W - 前进1米 \nS - 后退1米\n"
"A - 左平移1米\nD - 右平移1米 \n" "A - 左平移1米\nD - 右平移1米 \n"
"Q - 逆时针转360度\nE - 顺时针转360度\nX - 退出\n"; "Q - 逆时针转360度\nE - 顺时针转360度\nX - 退出\n";
//we will be sending commands of type "twist" //we will be sending commands of type "twist"
geometry_msgs::Twist base_cmd; geometry_msgs::Twist base_cmd;

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; fVy = (fPosDiff[0] + fPosDiff[1]) - (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVy;
fVz = (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVz; fVz = (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVz;
double fTimeDur = current_time.toSec() - last_time.toSec(); double fTimeDur = current_time.toSec() - last_time.toSec();
fVx = fVx/(fTimeDur*9700); fVx = fVx/(fTimeDur*9100);
fVy = fVy/(fTimeDur*9700); fVy = fVy/(fTimeDur*9100);
fVz = fVz/(fTimeDur*1925); fVz = fVz/(fTimeDur*1840);
double dx = (lastVel.linear.x*cos(lastPose.theta) - lastVel.linear.y*sin(lastPose.theta))*fTimeDur; 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; double dy = (lastVel.linear.x*sin(lastPose.theta) + lastVel.linear.y*cos(lastPose.theta))*fTimeDur;