How to write my covariance matrix to PoseWithCovarianceStamped [closed]
Hello! I want to write the covariance I have calculated into the covariance field of a PoseWIthCovarianceStamped message.
First, I need to call a function(void Dynamic_covariance(..., ..., type? Cov_matrix);
) that receives the vector(Cov_matrix) and two more variables. This function adds the elements to Cov_matrix so that, when it finishes, the main function could load those values saved in Cov_matrix into the message (pub_msg.pose.covariance=Cov_matrix;
).
The problem is that the type does not match the ones specified by the message (boost::array< double, 36ul > (?)). If I try to use that type, I am not allowed to modify the vector because the compiler says that it is a read only one. What kind of type should I use for Cov_matrix or how can I fill it without doing any brute force code?
Thanks.