| 85 | } |
| 86 | |
| 87 | void ImuAxesVisual::setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) |
| 88 | { |
| 89 | if (checkQuaternionValidity(msg)) |
| 90 | { |
| 91 | if (!quat_valid_) |
| 92 | { |
| 93 | RVIZ_COMMON_LOG_INFO_STREAM( |
| 94 | "rviz_imu_plugin got valid quaternion, " |
| 95 | "displaying true orientation"); |
| 96 | quat_valid_ = true; |
| 97 | } |
| 98 | orientation_ = Ogre::Quaternion(msg->orientation.w, msg->orientation.x, |
| 99 | msg->orientation.y, msg->orientation.z); |
| 100 | } else |
| 101 | { |
| 102 | if (quat_valid_) |
| 103 | { |
| 104 | RVIZ_COMMON_LOG_WARNING_STREAM( |
| 105 | "rviz_imu_plugin got invalid quaternion (" |
| 106 | << msg->orientation.w << "," << msg->orientation.x << "," |
| 107 | << msg->orientation.y << "," << msg->orientation.z |
| 108 | << "will display neutral orientation instead"); |
| 109 | quat_valid_ = false; |
| 110 | } |
| 111 | // if quaternion is invalid, give a unit quat to Ogre |
| 112 | orientation_ = Ogre::Quaternion(); |
| 113 | } |
| 114 | |
| 115 | if (orientation_axes_) orientation_axes_->setOrientation(orientation_); |
| 116 | } |
| 117 | |
| 118 | void ImuAxesVisual::setScale(float scale) |
| 119 | { |
nothing calls this directly
no test coverage detected