MlNdt::MlNdt(ros::NodeHandle & n,ros::NodeHandle & n_private):nh_(n),nh_private_(n_private){
// find tf_prefix if exists add it to tf_prefix_ variable
tf_prefix_="";
std::string tf_prefix_path;
if (nh_.searchParam("tf_prefix", tf_prefix_path))
{
nh_.getParam(tf_prefix_path, tf_prefix_);
}
robot_base_frame_ = nh_private_.param<std::string>("robot_base_frame","base_link");
world_farme_ = nh_private_.param<std::string>("world_farme","odom");
odom_topic_ = nh_private_.param<std::string>("odom_topic","/odom");
laser_topic_ = nh_private_.param<std::string>("laser_topic","/laser");
if(tf_prefix_ != ""){
robot_base_frame_=tf_prefix_+"/"+robot_base_frame_;
}
// subscribers to acceleromer and gyroscope
message_filters::Subscriber<nav_msgs::Odometry> odom_sub (nh_,odom_topic_, 1000);
message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub (nh_,laser_topic_, 1000);
// sync messages using approximate alghorithm
constexpr int sync_delay = 10;
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::LaserScan> ImuSyncPolicy;
message_filters::Synchronizer<ImuSyncPolicy> msg_sync (ImuSyncPolicy(sync_delay), odom_sub,laser_sub);
msg_sync.registerCallback(boost::bind(&MlNdt::data_cb,this,_1,_2));
}
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;
}