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