ROS_Robot/mapviz
1
0
Fork 0
mirror of https://github.com/swri-robotics/mapviz.git synced 2025-09-15 17:48:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

don't draw arrow for invalid orientation (#673)

Browse Source 
Co-authored-by: Kevin Nickels <knickels@swri.org>
This commit is contained in:
knickels 2020-04-03 15:14:10 -05:00 committed by GitHub
parent a9fae88b2a
commit f4d09d8dc3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
mapviz_plugins/src/point_drawing_plugin.cpp
View File

@ -427,6 +427,16 @@ namespace mapviz_plugins
double arrow_width = size / 5.0;
double head_length = size * 0.75;
// If quaternion malformed, just draw point instead
const tf::Quaternion q(point.orientation);
if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
{
orientation = tf::Transform(tf::Transform(transform.GetOrientation()));
arrow_width = 0.0;
head_length = 0.0;
size = 0;
}
point.transformed_arrow_point =
point.transformed_point + orientation * tf::Point(size, 0.0, 0.0);
point.transformed_arrow_left =