void main() {
Init();
unsigned int od_data[2];
while (1) {
OdometryData(od_data);
ser_printf("od_data=%u,%u\n", od_data[0], od_data[1]);
}
}
// NOTE: Odometry data is used to set the 6 virtual degrees of freedom.
void OdometryStateReceiverSM::odometryMessageCallback(nav_msgs::Odometry & odom)
{
// CONTROLIT_INFO_RT << "Method called!";
if (!rcvdInitOdomMsg)
{
CONTROLIT_INFO_RT << "Received initial odometry message.";
rcvdInitOdomMsg = true;
}
else
{
// if (!rcvdFirstOdomMsg)
// {
// CONTROLIT_INFO_RT << "Received first odometry message.";
// rcvdFirstOdomMsg = true;
// }
// Assuming odometry information in the correct frame...
Vector x(3);
x(0) = odom.pose.pose.position.x;
x(1) = odom.pose.pose.position.y;
x(2) = odom.pose.pose.position.z;
// Get the orientation
Eigen::Quaterniond q(odom.pose.pose.orientation.w,
odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z);
// Get the velocity
Vector x_dot(6);
x_dot(0) = odom.twist.twist.linear.x;
x_dot(1) = odom.twist.twist.linear.y;
x_dot(2) = odom.twist.twist.linear.z;
x_dot(3) = odom.twist.twist.angular.x;
x_dot(4) = odom.twist.twist.angular.y;
x_dot(5) = odom.twist.twist.angular.z;
if (!controlit::addons::eigen::checkMagnitude(x) || !controlit::addons::eigen::checkMagnitude(q) || !controlit::addons::eigen::checkMagnitude(x_dot))
{
CONTROLIT_ERROR_RT << "Received invalid odometry data!\n"
<< " - x: " << x.transpose() << "\n"
<< " - q: [" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << "]\n"
<< " - x_dot: " << x_dot.transpose();
assert(false);
}
else
{
// the writeFromNonRT can be used in RT, if you have the guarantee that
// * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
// * there is only one single rt thread
odometryDataBuffer.writeFromNonRT( OdometryData(x, q, x_dot) );
receivedOdometryState = true;
}
}
}
