diff --git a/include/create/create.h b/include/create/create.h index e4662f8..cf51442 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -83,6 +83,8 @@ namespace create { Matrix poseCovar; + float measuredLeftVel; + float measuredRightVel; float requestedLeftVel; float requestedRightVel; @@ -560,6 +562,18 @@ namespace create { */ float getRightWheelDistance() const; + /** + * \brief Get the measured velocity of the left wheel. + * \return velocity in m/s + */ + float getMeasuredLeftWheelVel() const; + + /** + * \brief Get the measured velocity of the right wheel. + * \return velocity in m/s + */ + float getMeasuredRightWheelVel() const; + /** * \brief Get the requested velocity of the left wheel. * This value is bounded at the maximum velocity of the robot model. diff --git a/src/create.cpp b/src/create.cpp index 950c33e..2e00cf1 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -170,6 +170,9 @@ namespace create { deltaYaw = wheelDistDiff / model.getAxleLength(); } + measuredLeftVel = leftWheelDist / dt; + measuredRightVel = rightWheelDist / dt; + // Moving straight if (fabs(wheelDistDiff) < util::EPS) { deltaX = deltaDist * cos(pose.yaw); @@ -975,6 +978,14 @@ namespace create { return totalRightDist; } + float Create::getMeasuredLeftWheelVel() const { + return measuredLeftVel; + } + + float Create::getMeasuredRightWheelVel() const { + return measuredRightVel; + } + float Create::getRequestedLeftWheelVel() const { return requestedLeftVel; }