Problem running collvoid_local_planner

edit

So I found out how to fix the warning "MSG to TF: Quaternion not properly normalized". But that does not influence the weird motion of my robot. It's still rotating in place. I guess there is somesthing wrong with the params in collvoid_params.yaml. That is what I have done to fix the warning.

In the package collvoid_local_planner in file ROSAgent.cpp there is a function setAgentParams(). In that function the orientation will is calculated by tf::getYaw() which calls the method quaternionMsgToTF where the warning will be printed. So I normalized the quaternion myself in setAgentParams():

Before:

yaw = tf::getYaw(agent->base_odom_.pose.pose.orientation);

After:

tf::Quaternion quat(agent->base_odom_.pose.pose.orientation.x, agent->base_odom_.pose.pose.orientation.y, agent->base_odom_.pose.pose.orientation.z, agent->base_odom_.pose.pose.orientation.w);

quat = quat.normalize();

yaw = tf::getYaw(quat);