这里是未到达导航点的情况,如果未到达会反复运行这里的代码
if (!goal_reached)
{
for (int i = 0; i < map_path.poses.size(); i++)
{
geometry_msgs::PoseStamped map_path_pose = map_path.poses[i];
geometry_msgs::PoseStamped odom_path_pose;
// geometry_msgs::PoseStamped odom_path_pose = map_path.poses[i];
try
{
tf_listener.transformPose("odom", ros::Time(0), map_path_pose, "map", odom_path_pose);
geometry_msgs::Point odom_path_wayPt = odom_path_pose.pose.position;
bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt, carPose);
if (_isForwardWayPt && start_flag == false)
{
start_flag = true;
}
if (start_flag == false)
{
continue;
}
if (_isForwardWayPt || 1)
{
bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt, carPose_pos);
if (_isWayPtAwayFromLfwDist)
{
forwardPt = odom_path_wayPt;
foundForwardPt = true;
break;
}
}
if (car2goal_dist < Lfw)
{
forwardPt = odom_goal_pos;
foundForwardPt = true;
}
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
}
}
}
else if (goal_reached)
{
forwardPt = odom_goal_pos;
foundForwardPt = false;
// ROS_INFO("goal REACHED!");
}
如果 未达到目标: 遍历 地图路径 中的所有姿势: 获取 当前地图路径姿势 尝试: 将 当前地图路径姿势 从 'map' 坐标系 转换到 'odom' 坐标系,结果存储在 里程计路径姿势 获取 转换后的里程计路径点 判断 该点是否在前向方向上(相对于车辆当前位置): 如果是,并且 起始标志 为假: 设置 起始标志 为真 如果 起始标志 为假: 跳过当前循环迭代 如果 该点在前向方向上(或总是为真,因为这里有个硬编码的1): 判断 该点是否远离左侧前方距离(基于车辆位置和点位置): 如果是: 将该点作为 前向点 设置 找到前向点 标志 为真 跳出循环 判断 车辆到目标的距离 是否小于 左侧前方距离: 如果是: 将 目标位置 作为 前向点 设置 找到前向点 标志 为真 捕获 坐标转换异常: 打印 错误信息 等待 1 秒 否则 如果 已达到目标: 将 目标位置 作为 前向点 设置 找到前向点 标志 为假 // (可选)打印 "目标已到达!"