//----------------------------------------------------------------------------------------------
/// Execute the algorithm.
void EstimateFitParameters::execConcrete() {
auto costFunction = getCostFunctionInitialized();
auto func = costFunction->getFittingFunction();
// Use additional constraints on parameters tied in some way
// to the varied parameters to exculde unwanted results.
std::vector<std::unique_ptr<IConstraint>> constraints;
std::string constraintStr = getProperty("Constraints");
if (!constraintStr.empty()) {
Expression expr;
expr.parse(constraintStr);
expr.toList();
for (auto &term : expr.terms()) {
IConstraint *c =
ConstraintFactory::Instance().createInitialized(func.get(), term);
constraints.push_back(std::unique_ptr<IConstraint>(c));
}
}
// Ranges to use with random number generators: one for each free parameter.
std::vector<std::pair<double, double>> ranges;
ranges.reserve(costFunction->nParams());
for (size_t i = 0; i < func->nParams(); ++i) {
if (!func->isActive(i)) {
continue;
}
auto constraint = func->getConstraint(i);
if (constraint == nullptr) {
func->fix(i);
continue;
}
auto boundary = dynamic_cast<Constraints::BoundaryConstraint *>(constraint);
if (boundary == nullptr) {
throw std::runtime_error("Parameter " + func->parameterName(i) +
" must have a boundary constraint. ");
}
if (!boundary->hasLower()) {
throw std::runtime_error("Constraint of " + func->parameterName(i) +
" must have a lower bound.");
}
if (!boundary->hasUpper()) {
throw std::runtime_error("Constraint of " + func->parameterName(i) +
" must have an upper bound.");
}
// Use the lower and upper bounds of the constraint to set the range
// of a generator with uniform distribution.
ranges.push_back(std::make_pair(boundary->lower(), boundary->upper()));
}
// Number of parameters could have changed
costFunction->reset();
if (costFunction->nParams() == 0) {
throw std::runtime_error("No parameters are given for which to estimate "
"initial values. Set boundary constraints to "
"parameters that need to be estimated.");
}
size_t nSamples = static_cast<int>(getProperty("NSamples"));
unsigned int seed = static_cast<int>(getProperty("Seed"));
if (getPropertyValue("Type") == "Monte Carlo") {
int nOutput = getProperty("NOutputs");
auto outputWorkspaceProp = getPointerToProperty("OutputWorkspace");
if (outputWorkspaceProp->isDefault() || nOutput <= 0) {
nOutput = 1;
}
auto output = runMonteCarlo(*costFunction, ranges, constraints, nSamples,
static_cast<size_t>(nOutput), seed);
if (!outputWorkspaceProp->isDefault()) {
auto table = API::WorkspaceFactory::Instance().createTable();
auto column = table->addColumn("str", "Name");
column->setPlotType(6);
for (size_t i = 0; i < output.size(); ++i) {
column = table->addColumn("double", std::to_string(i + 1));
column->setPlotType(2);
}
for (size_t i = 0, ia = 0; i < m_function->nParams(); ++i) {
if (m_function->isActive(i)) {
TableRow row = table->appendRow();
row << m_function->parameterName(i);
for (auto &j : output) {
row << j[ia];
}
++ia;
}
}
setProperty("OutputWorkspace", table);
}
} else {
size_t nSelection = static_cast<int>(getProperty("Selection"));
size_t nIterations = static_cast<int>(getProperty("NIterations"));
runCrossEntropy(*costFunction, ranges, constraints, nSamples, nSelection,
nIterations, seed);
}
bool fixBad = getProperty("FixBadParameters");
if (fixBad) {
fixBadParameters(*costFunction, ranges);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mcl_node");
ros::NodeHandle n("/mcl_node");;
n.param<std::string>("map_frame", mapFrame, "map");
n.param<std::string>("robot_base_link", baseFrame, "base_link");
n.param<double>("map_offset_x", coords.x0, 0.0);
n.param<double>("map_offset_y", coords.y0, 0.0);
n.param<int>("map_occupied_min_threshold", minOccupied, 10);
double odom_a1, odom_a2, odom_a3, odom_a4;
n.param<std::string>("odometry_frame", odomFrame, "odom");
n.param<double>("odometry_model_a1", odom_a1, 0.05);
n.param<double>("odometry_model_a2", odom_a2, 0.01);
n.param<double>("odometry_model_a3", odom_a3, 0.02);
n.param<double>("odometry_model_a4", odom_a4, 0.01);
//TODO: setup these IR constants more properly.
double irZhit, irZrm;
n.param<double>("ir_model_sigma_hit", irSigma, 0.1);
n.param<double>("ir_model_z_hit", irZhit, 0.95);
n.param<double>("ir_model_z_random_over_z_max", irZrm, 0.05);
int nParticles, mcl_rate;
double minDelta, aslow, afast;
double crashRadius, crashYaw, stdXY, stdYaw, locStdXY, locStdYaw;
n.param<int>("mcl_particles", nParticles, 200);
n.param<int>("mcl_rate", mcl_rate, 5);
n.param<double>("mcl_init_cone_radius", initConeRadius, 0.2);
n.param<double>("mcl_init_yaw_variance", initYawVar, 0.3);
n.param<double>("mcl_min_position_delta", minDelta, 0.001);
n.param<double>("mcl_aslow", aslow, 0.01);
n.param<double>("mcl_afast", afast, 0.2);
n.param<double>("mcl_crash_radius", crashRadius, 0.1);
n.param<double>("mcl_crash_yaw", crashYaw, 0.2);
n.param<double>("mcl_good_std_xy", stdXY, 0.05);
n.param<double>("mcl_good_std_yaw", stdYaw, 0.6);
n.param<double>("mcl_localized_std_xy", locStdXY, 0.05);
n.param<double>("mcl_localized_std_yaw", locStdYaw, 0.6);
tf::TransformBroadcaster broadcaster_obj;
tf_broadcaster = &broadcaster_obj;
tf::TransformListener listener_obj;
tf_listener = &listener_obj;
odom2map = new tf::Stamped<tf::Pose>;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub(n, "/odom", 1);
message_filters::Subscriber<ir_sensors::RangeArray> range_sub(n,"/ir_publish/sensors", 1);
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), odom_sub, range_sub);
sync.registerCallback(boost::bind(&odomRangeUpdate, _1, _2));
ros::Subscriber map_version_sub = n.subscribe<std_msgs::Int32>("/map_node/version", 2, mapVersionCB);
ros::Publisher particle_pub_obj = n.advertise<geometry_msgs::PoseArray>("/mcl/particles", 5);
ros::Publisher localized_pub = n.advertise<std_msgs::Bool>("/mcl/is_localized", 5);
geometry_msgs::PoseArray poseArray;
poseArray.header.frame_id = mapFrame;
//Wait 8 s for the map service.
if(!ros::service::waitForService("/map_node/get_map", 8000)) {
ROS_ERROR("Map service unreachable.");
return -1;
}
mapInflated = new nav_msgs::OccupancyGrid();
mapDistance = new nav_msgs::OccupancyGrid();
ros::ServiceClient map_client_obj = n.serviceClient<map_tools::GetMap>("/map_node/get_map");
map_client = &map_client_obj;
if(!updateMap()) {
return -1;
}
struct PoseState pose, goodStd, locStd;
goodStd.set(stdXY);
goodStd.yaw = stdYaw;
locStd.set(locStdXY);
locStd.yaw = locStdYaw;
pose.set(0.0);
pose.x += coords.x0;
pose.y += coords.y0;
om = new OdometryModel(odom_a1, odom_a2, odom_a3, odom_a4);
mclEnabled = false;
coords.x = coords.x0;
coords.y = coords.y0;
coords.th = 0.0;
mc = new MonteCarlo(om, &isPointFree, nParticles, minDelta,
aslow, afast, crashRadius, crashYaw, goodStd);
irm = new RangeModel(&getDist, irZhit, irZrm);
mc->addSensor(irm);
double csz = mapInflated->info.resolution;
double wc = ((double)mapInflated->info.width);
double hc = ((double)mapInflated->info.height);
assert(csz > 0.00001);
mc->init(pose, initConeRadius, initYawVar, wc*csz, hc*csz);
//mc->init(wc*csz, hc*csz);
mclEnabled = true;
current_time = ros::Time::now();
int rate = 40, counter = 0;
ros::Rate loop_rate(rate);
if(!updateMap())
return -1;
*odom2map = mclTransform();
struct PoseState odom0;
bool firstMcl = true;
double dx = 0, dy = 0, dyaw = 0, dx1 = 0, dy1 = 0, dyaw1 = 0;
std_msgs::Bool isLocalizedMsg;
isLocalizedMsg.data = isLocalized;
ros::Subscriber init_mcl_sub = n.subscribe<geometry_msgs::Pose>("/mcl/initial_pose", 2, initMcl);
while (ros::ok())
{
if(!firstMcl) {
dx = odomState.x - odom0.x;
dy = odomState.y - odom0.y;
dyaw = odomState.yaw - odom0.yaw;
dx1 = dx;
dy1 = dy;
dyaw1 = dyaw;
}
if(counter % (rate/mcl_rate) == 0) {
if(mclEnabled) {
if(firstMcl) {
odom0 = odomState;
firstMcl = false;
}
odom0 = odomState;
if(runMonteCarlo()) {
dx = 0;
dy = 0;
dyaw = 0;
}
particles2PoseArray(mc->getParticles(), poseArray);
particle_pub_obj.publish(poseArray);
struct PoseState std = mc->getStd();
if(std.x > locStd.x || std.y > locStd.y || std.yaw > locStd.yaw) {
isLocalized = false;
} else {
isLocalized = true;
}
}
isLocalizedMsg.data = isLocalized;
localized_pub.publish(isLocalizedMsg);
counter = 0;
}
counter++;
if(!firstMcl) {
struct PoseState avg = mc->getState();
coords.x = avg.x + dx;
coords.y = avg.y + dy;
coords.th = avg.yaw + dyaw;
//Do not update transform when rotating quickly on spot.
if((std::sqrt(dx1*dx1 + dy1*dy1) > 0.03/(double)mcl_rate)
&& isLocalized)
*odom2map = mclTransform();
}
publishTransform(*odom2map);
ros::spinOnce();
loop_rate.sleep();
ros::Duration last_update = ros::Time::now() - current_time;
if(last_update > ros::Duration(1.2/(double)rate))
current_time = ros::Time::now();
}
return 0;
}
//----------------------------------------------------------------------------------------------
/// Execute the algorithm.
void EstimateFitParameters::execConcrete() {
auto costFunction = getCostFunctionProperty();
auto func = costFunction->getFittingFunction();
// Use additional constraints on parameters tied in some way
// to the varied parameters to exculde unwanted results.
std::vector<std::unique_ptr<IConstraint>> constraints;
std::string constraintStr = getProperty("Constraints");
if (!constraintStr.empty()) {
Expression expr;
expr.parse(constraintStr);
expr.toList();
for (auto &term : expr.terms()) {
IConstraint *c =
ConstraintFactory::Instance().createInitialized(func.get(), term);
constraints.push_back(std::unique_ptr<IConstraint>(c));
}
}
// Ranges to use with random number generators: one for each free parameter.
std::vector<std::pair<double, double>> ranges;
ranges.reserve(costFunction->nParams());
for (size_t i = 0; i < func->nParams(); ++i) {
if (func->isFixed(i)) {
continue;
}
auto constraint = func->getConstraint(i);
if (constraint == nullptr) {
func->fix(i);
continue;
}
auto boundary = dynamic_cast<Constraints::BoundaryConstraint *>(constraint);
if (boundary == nullptr) {
throw std::runtime_error("Parameter " + func->parameterName(i) +
" must have a boundary constraint. ");
}
if (!boundary->hasLower()) {
throw std::runtime_error("Constraint of " + func->parameterName(i) +
" must have a lower bound.");
}
if (!boundary->hasUpper()) {
throw std::runtime_error("Constraint of " + func->parameterName(i) +
" must have an upper bound.");
}
// Use the lower and upper bounds of the constraint to set the range
// of a generator with uniform distribution.
ranges.push_back(std::make_pair(boundary->lower(), boundary->upper()));
}
// Number of parameters could have changed
costFunction->reset();
if (costFunction->nParams() == 0) {
throw std::runtime_error("No parameters are given for which to estimate "
"initial values. Set boundary constraints to "
"parameters that need to be estimated.");
}
size_t nSamples = static_cast<int>(getProperty("NSamples"));
size_t seed = static_cast<int>(getProperty("Seed"));
if (getPropertyValue("Type") == "Monte Carlo") {
runMonteCarlo(*costFunction, ranges, constraints, nSamples, seed);
} else {
size_t nSelection = static_cast<int>(getProperty("Selection"));
size_t nIterations = static_cast<int>(getProperty("NIterations"));
runCrossEntropy(*costFunction, ranges, constraints, nSamples, nSelection,
nIterations, seed);
}
bool fixBad = getProperty("FixBadParameters");
if (fixBad) {
fixBadParameters(*costFunction, ranges);
}
}
