| 96 | } |
| 97 | |
| 98 | void ImuAccVisual::setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) |
| 99 | { |
| 100 | raw_acc_ = |
| 101 | Ogre::Vector3(msg->linear_acceleration.x, msg->linear_acceleration.y, |
| 102 | msg->linear_acceleration.z); |
| 103 | arrow_length_ = raw_acc_.length(); |
| 104 | |
| 105 | if (checkQuaternionValidity(msg)) |
| 106 | { |
| 107 | if (!quat_valid_) |
| 108 | { |
| 109 | RVIZ_COMMON_LOG_INFO_STREAM( |
| 110 | "rviz_imu_plugin got valid quaternion, " |
| 111 | "applying acceleration derotation"); |
| 112 | quat_valid_ = true; |
| 113 | } |
| 114 | orientation_ = Ogre::Quaternion(msg->orientation.w, msg->orientation.x, |
| 115 | msg->orientation.y, msg->orientation.z); |
| 116 | } else |
| 117 | { |
| 118 | if (quat_valid_) |
| 119 | { |
| 120 | RVIZ_COMMON_LOG_WARNING_STREAM( |
| 121 | "rviz_imu_plugin got invalid quaternion (" |
| 122 | << msg->orientation.w << "," << msg->orientation.x << "," |
| 123 | << msg->orientation.y << "," << msg->orientation.z |
| 124 | << "); skipping acceleration derotation"); |
| 125 | quat_valid_ = false; |
| 126 | } |
| 127 | orientation_ = Ogre::Quaternion(); |
| 128 | } |
| 129 | |
| 130 | updateDirection(); |
| 131 | } |
| 132 | |
| 133 | void ImuAccVisual::updateDirection() |
| 134 | { |
nothing calls this directly
no outgoing calls
no test coverage detected