//---------------------------------------------------------------------------------------------- /// 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); } }