Revision history [back]

Instead of global_localization, I'm using publishing to /initialpose topic method in my custom package. Now I can set covariance matrix to disperse particles in specific area. X_DEVIATION_WEIGHT, Y_DEVIATION_WEIGHT and YAW_DEVIATION_WEIGHT parameters are globally declared;

bool publishToInitialPose(geometry_msgs::PoseWithCovarianceStamped msg, ros::Publisher publisher, bool last_publish = false)
{
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "/" + NAMESPACE + "/map";
    msg.pose.pose.position.x = POS_X;
    msg.pose.pose.position.y = POS_Y;

    msg.pose.pose.orientation.x = 0.0;
    msg.pose.pose.orientation.y = 0.0;
    msg.pose.pose.orientation.z = ORI_Z;
    msg.pose.pose.orientation.w = ORI_W;

    msg.pose.covariance[0] = X_DEVIATION_WEIGHT; 
    msg.pose.covariance[7] = Y_DEVIATION_WEIGHT; 
    msg.pose.covariance[35] = YAW_DEVIATION_WEIGHT; 
    publisher.publish( msg );
}