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

AP_DDS: Use common quaternion initialization function 

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-05-25 18:55:15 -06:00 committed by Peter Barker
parent e27dea7003
commit 513938b0ff
1 changed files with 13 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -77,6 +77,14 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
AP_GROUPEND
};
static void initialize(geometry_msgs_msg_Quaternion& q)
{
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
q.w = 1.0;
}
AP_DDS_Client::~AP_DDS_Client()
{
// close transport
@ -226,11 +234,8 @@ 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;
// Ensure rotation is initialized.
initialize(msg.transforms[i].transform.rotation);
msg.transforms_size++;
}
@ -344,10 +349,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
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;
initialize(msg.pose.orientation);
}
}
@ -428,10 +430,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
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;
initialize(msg.pose.orientation);
}
}
@ -452,10 +451,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
msg.orientation.z = orientation[2];
msg.orientation.w = orientation[3];
} else {
msg.orientation.x = 0.0;
msg.orientation.y = 0.0;
msg.orientation.z = 0.0;
msg.orientation.w = 1.0;
initialize(msg.orientation);
}
msg.orientation_covariance[0] = -1;