Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DDS: ensure zero rotation quaternions are normalised 

- ROS expects quaternions to be normalised and the default message constructor does not enforce this.
- Fix normalisation for pose stamped.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
Rhys Mainwaring 2024-05-23 13:02:08 +01:00 committed by Randy Mackay
parent 63a6004db9
commit cd7f325565
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -232,6 +232,12 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
msg.transforms[i].transform.translation.y = -1 * offset[1];
msg.transforms[i].transform.translation.z = -1 * offset[2];
// Ensure rotation is normalised
msg.transforms[i].transform.rotation.x = 0.0;
msg.transforms[i].transform.rotation.y = 0.0;
msg.transforms[i].transform.rotation.z = 0.0;
msg.transforms[i].transform.rotation.w = 1.0;
msg.transforms_size++;
}
@ -343,6 +349,11 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
msg.pose.orientation.x = orientation[1];
msg.pose.orientation.y = orientation[2];
msg.pose.orientation.z = orientation[3];
} else {
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 1.0;
}
}
@ -422,6 +433,11 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
msg.pose.orientation.x = orientation[1];
msg.pose.orientation.y = orientation[2];
msg.pose.orientation.z = orientation[3];
} else {
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 1.0;
}
}