diff --git a/include/create/create.h b/include/create/create.h index 03ef4cc..640bb88 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -454,11 +454,11 @@ namespace create { /* Get the estimated position of Create based on wheel encoders. */ - const create::Pose& getPose() const; + create::Pose getPose() const; /* Get the estimated velocity of Create based on wheel encoders. */ - const create::Vel& getVel() const; + create::Vel getVel() const; /* Get the number of corrupt serial packets since first connecting to Create. */ diff --git a/include/create/types.h b/include/create/types.h index 6c3601d..1495094 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -32,10 +32,12 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_TYPES_H #define CREATE_TYPES_H +#include + namespace create { enum RobotModel { CREATE_1 = 0, // Roomba 400 series - CREATE_2 // Roomba 600 series + CREATE_2 // Roomba 600 series }; enum SensorPacketID { @@ -226,7 +228,7 @@ namespace create { float x; float y; float yaw; - float covariance[9]; + std::vector covariance; }; typedef Pose Vel; diff --git a/src/create.cpp b/src/create.cpp index 3f8aa06..5b9ada8 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -32,10 +32,12 @@ namespace create { pose.x = 0; pose.y = 0; pose.yaw = 0; + pose.covariance = std::vector(9, 0.0); vel.x = 0; vel.y = 0; vel.yaw = 0; - poseCovar = Matrix(3, 3); + vel.covariance = std::vector(9, 0.0); + poseCovar = Matrix(3, 3, 0.0); data = boost::shared_ptr(new Data(model)); serial = boost::make_shared(data); } @@ -871,11 +873,11 @@ namespace create { } } - const Pose& Create::getPose() const { + Pose Create::getPose() const { return pose; } - const Vel& Create::getVel() const { + Vel Create::getVel() const { return vel; }