我正在开发一个应用程序,一个模拟器,四旋翼飞行器从航点飞到航点.在我的代码中,我实现了一个使用atan2函数计算偏航的函数.但是当四旋翼飞行器翻转360°时,它不会以最短的速度移动,但它会在360°范围内移动以达到新的方向.
我在这里发布了一个视频.看看它在360°时的行为.
好的家伙现在完成这个功能:
geometry_msgs::Pose getYaw( double x1, double x2, double y1, double y2 ) {
geometry_msgs::Pose output_trajectory;
/* Extrapolate the yaw information between two contigous points */
double yaw = atan2( ( y2 - y1 ), ( x2 - x1 ) );
if( yaw < 0.0f ) // * read later on
yaw += 2.0f * M_PI;
output_trajectory.orientation = tf::createQuaternionMsgFromYaw( yaw );
return output_trajectory;
}
其中tf :: createQuaternionMsgFromYaw是来自ROS框架的库.这里的定义:链接.geometry_msgs :: Pose只是一个容器:链接.
*:这里我已经在stackoverflow中阅读了相关的主题和问题,这个函数将atan2的返回输出映射到0°-360°
更新:这里是偏航值的摘录: …