Hi,
I want to visualize the pose of my imu with a robot model. It all works fine for a couple of hundred transforms and then crashes with the message:
Assertion failed: (!pos.isNaN() && "Invalid vector supplied as parameter"), function setPosition, file /tmp/ogre-dmWe/ogre_src_v1-7-4/OgreMain/src/OgreNode.cpp, line 413.
I tried to catch the NaN before it is visualized but its not directly in my code. I really don't get where the NaN could still come from in here:
geometry_msgs::Transform transform;
if (!(x != x || y != y || z != z)) {
transform.translation.x=x;
transform.translation.y=y;
transform.translation.z=z;
} else {
ROS_WARN("Translation NaN");
ROS_WARN("%f, %f, %f", x, y, z);
}
if (!(isnan(imu_msg->orientation.x) || isnan(imu_msg->orientation.y) || isnan(imu_msg->orientation.x)|| isnan(imu_msg->orientation.w))){
transform.rotation.x=imu_msg->orientation.x;
transform.rotation.y=imu_msg->orientation.y;
transform.rotation.z=imu_msg->orientation.z;
transform.rotation.w=imu_msg->orientation.w;
} else {
ROS_WARN("Orientation NaN");
ROS_WARN("%f, %f, %f, %f", imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z, imu_msg->orientation.w);
}
transformStamped.header.stamp=current_time;
transformStamped.header.frame_id="world";
transformStamped.child_frame_id="drone";
transformStamped.transform=transform;
transformBroadcaster->sendTransform(transformStamped);
=====
Also the last transforms before the crash look ok:
transforms:
-
header:
seq: 0
stamp:
secs: 1402081686
nsecs: 585921000
frame_id: world
child_frame_id: drone
transform:
translation:
x: 0.0
y: 0.0
z: 0.2
rotation:
x: -0.0690463855863
y: 0.000421883538365
z: 0.451825141907
w: 0.889430403709
---
transforms:
-
header:
seq: 0
stamp:
secs: 1402081686
nsecs: 598353000
frame_id: world
child_frame_id: drone
transform:
translation:
x: 0.0
y: 0.0
z: 0.2
rotation:
x: -0.0704767182469
y: 0.000757452158723
z: 0.446980148554
w: 0.891762971878
---
transforms: []
---
transforms: []
---
transforms:
-
header:
seq: 0
stamp:
secs: 1402081686
nsecs: 619488000
frame_id: world
child_frame_id: drone
transform:
translation:
x: 0.0
y: 0.0
z: 0.2
rotation:
x: -0.0718987062573
y: 0.000403782352805
z: 0.450937718153
w: 0.889654755592
---
Any suggestions what i could do?