bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
QMutexLocker locker(mutex);
// YEP: OMNI-DIRECTIONAL ROBOTS: Abstract: All the robots introduced in chapter 7, with the exception of syncro-drive vehicles...
// NOPE: http://cdn.intechopen.com/pdfs-wm/465.pdf
R = QString::fromStdString(params["DelfosBase.WheelRadius"].value).toFloat();
l1 = QString::fromStdString(params["DelfosBase.DistAxes"].value ).toFloat();
l2 = QString::fromStdString(params["DelfosBase.AxesLength"].value ).toFloat();
printf("l1: %f\n", l1);
printf("l2: %f\n", l2);
printf("r: %f\n", R);
// inverse kinematics matrix
const float ill = 1. / (2.*(l1 + l2));
M_wheels_2_vels = QMat(3, 4);
M_wheels_2_vels(0,0) = +1./4.;
M_wheels_2_vels(0,1) = +1./4.;
M_wheels_2_vels(0,2) = +1./4.;
M_wheels_2_vels(0,3) = +1./4.;
M_wheels_2_vels(1,0) = +1./4.;
M_wheels_2_vels(1,1) = -1./4.;
M_wheels_2_vels(1,2) = -1./4.;
M_wheels_2_vels(1,3) = +1./4.;
M_wheels_2_vels(2,0) = +ill;
M_wheels_2_vels(2,1) = -ill;
M_wheels_2_vels(2,2) = +ill;
M_wheels_2_vels(2,3) = -ill;
M_wheels_2_vels = M_wheels_2_vels.operator*(R); // R instead of 2*pi*R because we use rads/s instead of rev/s
M_wheels_2_vels.print("M_wheels_2_vels");
// forward kinematics matrix
const float ll = (l1 + l2)/2.;
M_vels_2_wheels = QMat(4,3);
M_vels_2_wheels(0,0) = +1.;
M_vels_2_wheels(1,0) = +1.;
M_vels_2_wheels(2,0) = +1.;
M_vels_2_wheels(3,0) = +1.;
M_vels_2_wheels(0,1) = +1.;
M_vels_2_wheels(1,1) = -1.;
M_vels_2_wheels(2,1) = -1.;
M_vels_2_wheels(3,1) = +1.;
M_vels_2_wheels(0,2) = +ll; // In contrast with the paper this code is based on, the
M_vels_2_wheels(1,2) = -ll; // third column of the matrix is inverted because we use
M_vels_2_wheels(2,2) = +ll; // the left-hand rule for angles.
M_vels_2_wheels(3,2) = -ll;
M_vels_2_wheels = M_vels_2_wheels.operator*(1./(R)); // 1/R instead of 1/(2*pi*R) because we use rads/s instead of rev/s
M_vels_2_wheels.print("M_vels_2_wheels");
timer.start(Period);
return true;
}
// Initialization process
void EkfNode::init() {
/**************************************************************************
* Initialize firstRun, time, and frame names
**************************************************************************/
// Set first run to true for encoders. Once a message is received, this will
// be set to false.
firstRunEnc_ = true;
firstRunSys_ = true;
// Set Times
ros::Time currTime = ros::Time::now();
lastEncTime_ = currTime;
lastSysTime_ = currTime;
// Use the ROS parameter server to initilize parameters
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("odom_frame", base_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
/**************************************************************************
* Initialize state for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize state
double state_temp[] = {0, 0, 0, 0, 0, 0};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
// Check the parameter server and initialize state
if(!private_nh_.getParam("state", state)) {
ROS_WARN_STREAM("No state found. Using default.");
}
// Check to see if the size is not equal to 6
if (state.size() != 6) {
ROS_WARN_STREAM("state isn't 6 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 1> Vector6d;
Vector6d stateMat(state.data());
std::cout << "state_map = " << std::endl;
std::cout << stateMat << std::endl;
ekf_map_.initState(stateMat);
// Odom is always initialized at all zeros.
stateMat << 0, 0, 0, 0, 0, 0;
ekf_odom_.initState(stateMat);
std::cout << "state_odom = " << std::endl;
std::cout << stateMat << std::endl;
/**************************************************************************
* Initialize covariance for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize covariance
double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
// Check the parameter server and initialize cov
if(!private_nh_.getParam("covariance", cov)) {
ROS_WARN_STREAM("No covariance found. Using default.");
}
// Check to see if the size is not equal to 36
if (cov.size() != 36) {
ROS_WARN_STREAM("cov isn't 36 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 6, RowMajor> Matrix66;
Matrix66 covMat(cov.data());
std::cout << "covariance = " << std::endl;
std::cout << covMat << std::endl;
ekf_map_.initCov(covMat);
// Initialize odom covariance the same as the map covariance (this isn't
// correct. But, since it is all an estimate anyway, it should be fine.
ekf_odom_.initCov(covMat);
/**************************************************************************
* Initialize Q for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize Q
double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
// Check the parameter server and initialize Q
if(!private_nh_.getParam("Q", Q)) {
ROS_WARN_STREAM("No Q found. Using default.");
}
// Check to see if the size is not equal to 36
if (Q.size() != 36) {
ROS_WARN_STREAM("Q isn't 36 elements long!");
}
// And initialize the Matrix
Matrix66 QMat(Q.data());
std::cout << "Q = " << std::endl;
std::cout << QMat << std::endl;
ekf_map_.initSystem(QMat);
ekf_odom_.initSystem(QMat);
/**************************************************************************
* Initialize Decawave for ekf_map_ (not used in ekf_odom_)
**************************************************************************/
// Create temp array to initialize R for DecaWave
double RDW_temp[] = {0.01, 0, 0, 0,
0, 0.01, 0, 0,
0, 0, 0.01, 0,
0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}
