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