From 3b8e1e5c73ae67ca1ead3d6529bd9cd29dfaeb76 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Mon, 15 Jul 2024 16:32:05 +0800 Subject: [PATCH] doc:add comment --- .../src/mpc_local_planner_ros.cpp | 370 +++++++++++++----- 1 file changed, 278 insertions(+), 92 deletions(-) diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index ca0ba3a..a258bb5 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -58,6 +58,14 @@ MpcLocalPlannerROS::MpcLocalPlannerROS() MpcLocalPlannerROS::~MpcLocalPlannerROS() {} +/** + * @brief 控制器重配置回调函数 + * + * 该函数用于处理控制器参数的动态重配置。 + * + * @param config 动态重配置参数。 + * @param level 重配置级别。 + */ void MpcLocalPlannerROS::reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level) { boost::mutex::scoped_lock l(config_mutex_); @@ -70,6 +78,14 @@ void MpcLocalPlannerROS::reconfigureControllerCB(ControllerReconfigureConfig& co _params.global_plan_viapoint_sep = config.global_plan_viapoint_sep; } +/** + * @brief 足迹重配置回调函数 + * + * 该函数用于处理足迹参数的动态重配置。 + * + * @param config 动态重配置参数。 + * @param level 重配置级别。 + */ void MpcLocalPlannerROS::reconfigureFootprintCB(FootprintReconfigureConfig& config, uint32_t level) { boost::mutex::scoped_lock l(config_mutex_); @@ -77,6 +93,14 @@ void MpcLocalPlannerROS::reconfigureFootprintCB(FootprintReconfigureConfig& conf _params.is_footprint_dynamic = config.is_footprint_dynamic; } +/** + * @brief 碰撞检测重配置回调函数 + * + * 该函数用于处理碰撞检测参数的动态重配置。 + * + * @param config 动态重配置参数。 + * @param level 重配置级别。 + */ void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& config, uint32_t level) { boost::mutex::scoped_lock l(config_mutex_); @@ -87,6 +111,15 @@ void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& conf _params.collision_check_no_poses = config.collision_check_no_poses; } +/** + * @brief 初始化局部规划器 + * + * 该函数初始化MpcLocalPlannerROS，包括参数加载和组件配置。 + * + * @param name 规划器的名称。 + * @param tf TF2缓冲区指针，用于变换计算。 + * @param costmap_ros 指向costmap_2d::Costmap2DROS对象的指针，用于获取代价地图信息。 + */ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) { // 检查插件是否已经初始化 @@ -259,6 +292,14 @@ bool MpcLocalPlannerROS::setPlan(const std::vector& return true; } + /** + * @brief 计算速度命令 + * + * 该函数用于计算机器人在当前时刻应执行的速度命令。 + * + * @param cmd_vel 输出参数，存储计算出的速度命令。 + * @return bool 如果成功计算出有效的速度命令，则返回true；否则返回false。 + */ bool MpcLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { std::string dummy_message; @@ -268,11 +309,21 @@ bool MpcLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel = cmd_vel_stamped.twist; return outcome == mbf_msgs::ExePathResult::SUCCESS; } - +/** + * @brief 计算速度命令（内部函数） + * + * 该函数用于计算机器人在当前时刻应执行的速度命令，并提供详细的错误信息。 + * + * @param pose 输入参数，当前机器人的位姿。 + * @param velocity 输入参数，当前机器人的速度。 + * @param cmd_vel 输出参数，存储计算出的速度命令。 + * @param message 输出参数，存储错误信息。 + * @return uint32_t 返回状态码，指示计算是否成功。 + */ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, geometry_msgs::TwistStamped& cmd_vel, std::string& message) { - // check if plugin initialized + // 检查插件是否已初始化 if (!_initialized) { ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); @@ -287,22 +338,22 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; _goal_reached = false; - // Get robot pose + // 获取机器人位姿 geometry_msgs::PoseStamped robot_pose; _costmap_ros->getRobotPose(robot_pose); _robot_pose = PoseSE2(robot_pose.pose); - // Get robot velocity + // 获取机器人速度 geometry_msgs::PoseStamped robot_vel_tf; _odom_helper.getRobotVel(robot_vel_tf); _robot_vel.linear.x = robot_vel_tf.pose.position.x; _robot_vel.linear.y = robot_vel_tf.pose.position.y; _robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); - // prune global plan to cut off parts of the past (spatially before the robot) + // 修剪全局计划，剪掉机器人之前的部分 pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance); - // Transform global plan to the frame of interest (w.r.t. the local costmap) + // 修剪全局计划，剪掉机器人之前的部分 std::vector transformed_plan; int goal_idx; geometry_msgs::TransformStamped tf_plan_to_global; @@ -314,10 +365,10 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::INTERNAL_ERROR; } - // update via-points container + // 更新路径点容器 if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep); - // check if global goal is reached + // 检查是否到达全局目标 geometry_msgs::PoseStamped global_goal; tf2::doTransform(_global_plan.back(), global_goal, tf_plan_to_global); double dx = global_goal.pose.position.x - _robot_pose.x(); @@ -329,7 +380,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::SUCCESS; } - // Return false if the transformed global plan is empty + // 如果转换后的全局计划为空，则返回false if (transformed_plan.empty()) { ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); @@ -337,14 +388,14 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::INVALID_PATH; } - // Get current goal point (last point of the transformed plan) + // 获取当前目标点（转换计划的最后一点） _robot_goal.x() = transformed_plan.back().pose.position.x; _robot_goal.y() = transformed_plan.back().pose.position.y; - // Overwrite goal orientation if needed + // 如果需要，覆盖目标方向 if (_params.global_plan_overwrite_orientation) { _robot_goal.theta() = estimateLocalGoalOrientation(_global_plan, transformed_plan.back(), goal_idx, tf_plan_to_global); - // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) + // 使用实际目标更新转换计划的目标方向（允许使用计划作为初始化） tf2::Quaternion q; q.setRPY(0, 0, _robot_goal.theta()); tf2::convert(q, transformed_plan.back().pose.orientation); @@ -354,41 +405,41 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt _robot_goal.theta() = tf2::getYaw(transformed_plan.back().pose.orientation); } - // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) + // 使用实际的机器人位置覆盖转换计划的起点（允许使用计划作为初始轨迹） if (transformed_plan.size() == 1) // plan only contains the goal { transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) } transformed_plan.front() = robot_pose; // update start - // clear currently existing obstacles + // 清除当前存在的障碍物 _obstacles.clear(); - // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin + // 使用代价地图信息或由代价地图转换器插件提供的多边形更新障碍物容器 if (_costmap_converter) updateObstacleContainerWithCostmapConverter(); else updateObstacleContainerWithCostmap(); - // also consider custom obstacles (must be called after other updates, since the container is not cleared) + // 还需考虑自定义障碍物（必须在其他更新之后调用，因为容器未被清除） updateObstacleContainerWithCustomObstacles(); - // estimate current state vector and previous control + // 估计当前状态向量和之前的控制 // updateControlFromTwist() - // Do not allow config changes during the following optimization step + // 在以下优化步骤期间不允许配置更改 boost::mutex::scoped_lock cfg_lock(configMutex()); - // Now perform the actual planning + // 现在执行实际的规划 // bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); ros::Time t = ros::Time::now(); - // controller_frequency might not be established in case planning takes longer: - // value dt affects e.g. control deviation bounds (acceleration limits) and we want a goot start value - // let's test how it works with the expected frequency instead of using the actual one + // 如果规划耗时较长，则可能未建立控制频率： + // 值dt影响例如控制偏差界限（加速度限制），我们希望有一个好的起始值 + // 让我们测试一下使用预期的频率而不是实际的频率效果如何 double dt = 1.0 / _params.controller_frequency; // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec(); - // set previous control value for control deviation bounds + // 设置前一个控制值以用于控制偏差界限 if (_u_seq && !_u_seq->isEmpty()) _controller.getOptimalControlProblem()->setPreviousControlInput(_u_seq->getValuesMap(0), dt); bool success = false; @@ -401,20 +452,20 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt if (!success) { - _controller.reset(); // force reinitialization for next time + _controller.reset(); // 强制下次重新初始化 ROS_WARN("mpc_local_planner was not able to obtain a local plan for the current setting."); - ++_no_infeasible_plans; // increase number of infeasible solutions in a row + ++_no_infeasible_plans; // 增加连续不可行方案的数量 _time_last_infeasible_plan = ros::Time::now(); _last_cmd = cmd_vel.twist; message = "mpc_local_planner was not able to obtain a local plan"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } - // Check feasibility (but within the first few states only) + // 检查可行性（但仅在前几个状态内） if (_params.is_footprint_dynamic) { - // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + // 更新机器人的足迹，以及从机器人中心到其足迹顶点的最小和最大距离。 _footprint_spec = _costmap_ros->getRobotFootprint(); costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); } @@ -425,41 +476,41 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt { cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - // now we reset everything to start again with the initialization of new trajectories. - _controller.reset(); // force reinitialization for next time + // 现在我们重置所有内容，以便下次重新初始化新轨迹。 + _controller.reset(); // 强制下次重新初始化 ROS_WARN("MpcLocalPlannerROS: trajectory is not feasible. Resetting planner..."); - ++_no_infeasible_plans; // increase number of infeasible solutions in a row + ++_no_infeasible_plans; // 增加连续不可行方案的数量 _time_last_infeasible_plan = ros::Time::now(); _last_cmd = cmd_vel.twist; message = "mpc_local_planner trajectory is not feasible"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } - // Get the velocity command for this sampling interval - // TODO(roesmann): we might also command more than just the imminent action, e.g. in a separate thread, until a new command is available + // 获取此采样间隔的速度命令 + // TODO(roesmann)：我们可能还会命令除即将采取的行动外的更多行动，例如在单独的线程中，直到有新命令可用为止 if (!_u_seq || !_controller.getRobotDynamics()->getTwistFromControl(_u_seq->getValuesMap(0), cmd_vel.twist)) { _controller.reset(); ROS_WARN("MpcLocalPlannerROS: velocity command invalid. Resetting controller..."); - ++_no_infeasible_plans; // increase number of infeasible solutions in a row + ++_no_infeasible_plans; // 增加连续不可行方案的数量 _time_last_infeasible_plan = ros::Time::now(); _last_cmd = cmd_vel.twist; message = "mpc_local_planner velocity command invalid"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } - // Saturate velocity, if the optimization results violates the constraints (could be possible due to early termination or soft cosntraints). + // 如果优化结果违反约束（可能由于提前终止或软约束），则限制速度。 // saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, // cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); - // a feasible solution should be found, reset counter + // 应找到可行的解决方案，重置计数器 _no_infeasible_plans = 0; - // store last command (for recovery analysis etc.) + // 存储最后一个命令（用于恢复分析等） _last_cmd = cmd_vel.twist; _time_last_cmd = ros::Time::now(); - // Now visualize everything + // 现在可视化所有内容 _publisher.publishLocalPlan(*_x_seq); _publisher.publishObstacles(_obstacles); _publisher.publishGlobalPlan(_global_plan); @@ -468,6 +519,13 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::SUCCESS; } +/** + * @brief 检查目标是否已到达 + * + * 该函数用于检查机器人是否已经到达目标位置。 + * + * @return bool 如果目标已到达，返回true；否则返回false。 + */ bool MpcLocalPlannerROS::isGoalReached() { if (_goal_reached) @@ -479,6 +537,11 @@ bool MpcLocalPlannerROS::isGoalReached() return false; } +/** + * @brief 使用代价地图更新障碍物容器 + * + * 该函数从代价地图中提取障碍物信息，并更新内部的障碍物容器。 + */ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() { // Add costmap obstacles if desired @@ -495,7 +558,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() Eigen::Vector2d obs; _costmap->mapToWorld(i, j, obs.coeffRef(0), obs.coeffRef(1)); - // check if obstacle is interesting (e.g. not far behind the robot) + // 检查障碍物是否在机器人前方（例如，不远离机器人） Eigen::Vector2d obs_dir = obs - _robot_pose.position(); if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue; @@ -506,11 +569,16 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() } } +/** + * @brief 使用代价地图转换器更新障碍物容器 + * + * 该函数从代价地图转换器中提取障碍物信息，并更新内部的障碍物容器。 + */ void MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter() { if (!_costmap_converter) return; - // Get obstacles from costmap converter + // 从代价地图转换器获取障碍物 costmap_converter::ObstacleArrayConstPtr obstacles = _costmap_converter->getObstacles(); if (!obstacles) return; @@ -519,20 +587,20 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter() const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i); const geometry_msgs::Polygon* polygon = &obstacle->polygon; - if (polygon->points.size() == 1 && obstacle->radius > 0) // Circle + if (polygon->points.size() == 1 && obstacle->radius > 0) // 圆形 { _obstacles.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius))); } - else if (polygon->points.size() == 1) // Point + else if (polygon->points.size() == 1) // 点 { _obstacles.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); } - else if (polygon->points.size() == 2) // Line + else if (polygon->points.size() == 2) // 线 { _obstacles.push_back( ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, polygon->points[1].x, polygon->points[1].y))); } - else if (polygon->points.size() > 2) // Real polygon + else if (polygon->points.size() > 2) // 多边形 { PolygonObstacle* polyobst = new PolygonObstacle; for (std::size_t j = 0; j < polygon->points.size(); ++j) @@ -543,19 +611,24 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter() _obstacles.push_back(ObstaclePtr(polyobst)); } - // Set velocity, if obstacle is moving + // 如果障碍物在移动，设置速度 if (!_obstacles.empty()) _obstacles.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation); } } +/** + * @brief 使用自定义障碍物更新障碍物容器 + * + * 该函数从自定义障碍物消息中提取障碍物信息，并更新内部的障碍物容器。 + */ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles() { - // Add custom obstacles obtained via message + // 添加通过消息获得的自定义障碍物 std::lock_guard l(_custom_obst_mutex); if (!_custom_obstacle_msg.obstacles.empty()) { - // We only use the global header to specify the obstacle coordinate system instead of individual ones + // 我们只使用全局header来指定障碍物坐标系，而不是单独的header Eigen::Affine3d obstacle_to_map_eig; try { @@ -617,13 +690,21 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles() _obstacles.push_back(ObstaclePtr(polyobst)); } - // Set velocity, if obstacle is moving + // 如果障碍物在移动，设置速度 if (!_obstacles.empty()) _obstacles.back()->setCentroidVelocity(_custom_obstacle_msg.obstacles[i].velocities, _custom_obstacle_msg.obstacles[i].orientation); } } } +/** + * @brief 更新路径点容器 + * + * 该函数根据转换后的计划和最小间隔距离更新路径点容器。 + * + * @param transformed_plan 转换后的全局计划。 + * @param min_separation 路径点之间的最小间隔距离。 + */ void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) { _via_points.clear(); @@ -631,17 +712,25 @@ void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector& global_plan, double dist_behind_robot) { @@ -657,7 +757,7 @@ bool MpcLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geomet try { - // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) + // 将机器人位姿转换到计划的参考框架（这里不等待，因为即使错过几次修剪也不会有大问题） geometry_msgs::TransformStamped global_to_plan_transform = tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0)); geometry_msgs::PoseStamped robot; @@ -665,7 +765,7 @@ bool MpcLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geomet double dist_thresh_sq = dist_behind_robot * dist_behind_robot; - // iterate plan until a pose close the robot is found + // 迭代计划，直到找到一个接近机器人的路径点 std::vector::iterator it = global_plan.begin(); std::vector::iterator erase_end = it; while (it != global_plan.end()) @@ -692,13 +792,29 @@ bool MpcLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geomet return true; } +/** + * @brief 转换全局计划 + * + * 该函数将全局计划转换为局部代价地图的参考框架，并修剪计划以适应局部规划器的需求。 + * + * @param tf TF缓冲区，用于变换计算。 + * @param global_plan 输入的全局计划，包含路径点。 + * @param global_pose 当前机器人的全局位姿。 + * @param costmap 局部代价地图。 + * @param global_frame 全局参考框架。 + * @param max_plan_length 计划的最大长度。 + * @param transformed_plan 输出参数，转换后的全局计划。 + * @param current_goal_idx 输出参数，当前目标点的索引。 + * @param tf_plan_to_global 输出参数，从计划框架到全局框架的变换。 + * @return bool 如果转换成功，返回true；否则返回false。 + */ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, std::vector& transformed_plan, int* current_goal_idx, geometry_msgs::TransformStamped* tf_plan_to_global) const { - // this method is a slightly modified version of base_local_planner/goal_functions.h + // 该方法是 base_local_planner/goal_functions.h 的稍作修改版本 const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; @@ -713,33 +829,32 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st return false; } - // get plan_to_global_transform from plan frame to global_frame + // 从计划框架到全局框架的变换 geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform( global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5)); - // let's get the pose of the robot in the frame of the plan + // 获取机器人在计划框架中的位姿 geometry_msgs::PoseStamped robot_pose; tf.transform(global_pose, robot_pose, plan_pose.header.frame_id); - // we'll discard points on the plan that are outside the local costmap + // 丢弃计划中超出局部代价地图的点 double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); - dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are - // located on the border of the local costmap + dist_threshold *= 0.85; // 仅考虑代价地图大小的85%，以更好地纳入位于局部代价地图边界上的点障碍物 int i = 0; double sq_dist_threshold = dist_threshold * dist_threshold; double sq_dist = 1e10; - // we need to loop to a point on the plan that is within a certain distance of the robot + // 我们需要循环直到找到一个距离机器人一定距离内的计划点 for (int j = 0; j < (int)global_plan.size(); ++j) { double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y; double new_sq_dist = x_diff * x_diff + y_diff * y_diff; - if (new_sq_dist > sq_dist_threshold) break; // force stop if we have reached the costmap border + if (new_sq_dist > sq_dist_threshold) break; // 如果已到达代价地图边界，则强制停止 - if (new_sq_dist < sq_dist) // find closest distance + if (new_sq_dist < sq_dist) // 找到最近距离 { sq_dist = new_sq_dist; i = j; @@ -748,9 +863,9 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st geometry_msgs::PoseStamped newer_pose; - double plan_length = 0; // check cumulative Euclidean distance along the plan + double plan_length = 0; // 检查沿计划的累计欧几里得距离 - // now we'll transform until points are outside of our distance threshold + // 现在我们将转换，直到点超出我们的距离阈值 while (i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length)) { const geometry_msgs::PoseStamped& pose = global_plan[i]; @@ -762,31 +877,30 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y; sq_dist = x_diff * x_diff + y_diff * y_diff; - // caclulate distance to previous pose + // 计算与前一个位姿的距离 if (i > 0 && max_plan_length > 0) plan_length += teb_local_planner::distance_points2d(global_plan[i - 1].pose.position, global_plan[i].pose.position); ++i; } - // if we are really close to the goal (>0) - // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. + // 如果我们非常接近目标(>0），则结果转换计划可能为空。在这种情况下，我们显式注入全局目标。 if (transformed_plan.empty()) { tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform); transformed_plan.push_back(newer_pose); - // Return the index of the current goal point (inside the distance threshold) + // 返回当前目标点的索引（在距离阈值内） if (current_goal_idx) *current_goal_idx = int(global_plan.size()) - 1; } else { - // Return the index of the current goal point (inside the distance threshold) - if (current_goal_idx) *current_goal_idx = i - 1; // subtract 1, since i was increased once before leaving the loop + // 返回当前目标点的索引（在距离阈值内） + if (current_goal_idx) *current_goal_idx = i - 1; // 减去1，因为在离开循环前i增加了一次 } - // Return the transformation from the global plan to the global planning frame if desired + // 如果需要，返回从全局计划到全局规划框架的变换 if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; } catch (tf::LookupException& ex) @@ -812,16 +926,28 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st return true; } +/** + * @brief 估计局部目标方向 + * + * 该函数根据全局计划和当前局部目标估计局部目标的方向。 + * + * @param global_plan 全局计划，包含路径点。 + * @param local_goal 当前局部目标。 + * @param current_goal_idx 当前目标点的索引。 + * @param tf_plan_to_global 从计划框架到全局框架的变换。 + * @param moving_average_length 移动平均长度。 + * @return double 局部目标的方向（弧度）。 + */ double MpcLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::PoseStamped& local_goal, int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, int moving_average_length) const { int n = (int)global_plan.size(); - // check if we are near the global goal already + // 检查是否已接近全局目标 if (current_goal_idx > n - moving_average_length - 2) { - if (current_goal_idx >= n - 1) // we've exactly reached the goal + if (current_goal_idx >= n - 1) // 刚好到达目标 { return tf2::getYaw(local_goal.pose.orientation); } @@ -831,14 +957,14 @@ double MpcLocalPlannerROS::estimateLocalGoalOrientation(const std::vector candidates; geometry_msgs::PoseStamped tf_pose_k = local_goal; @@ -847,10 +973,10 @@ double MpcLocalPlannerROS::estimateLocalGoalOrientation(const std::vector l(_custom_obst_mutex); _custom_obstacle_msg = *obst_msg; } +/** + * @brief 自定义路径点回调函数 + * + * 该函数处理自定义路径点的消息，并更新路径点容器。 + * + * @param via_points_msg 自定义路径点消息。 + */ void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg) { ROS_INFO_ONCE("Via-points received. This message is printed once."); @@ -895,6 +1045,15 @@ void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_p _custom_via_points_active = !_via_points.empty(); } +/** + * @brief 从参数服务器获取机器人足迹模型 + * + * 该函数从参数服务器获取机器人足迹模型，用于轨迹优化。 + * + * @param nh ROS节点句柄。 + * @param costmap_ros 指向costmap_2d::Costmap2DROS的指针。 + * @return teb_local_planner::RobotFootprintModelPtr 返回指向机器人足迹模型的智能指针。 + */ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros) { @@ -905,7 +1064,7 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } - // from costmap_2d + // 从costmap_2d获取 if (model_name.compare("costmap_2d") == 0) { if (!costmap_ros) @@ -917,17 +1076,17 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return getRobotFootprintFromCostmap2d(*costmap_ros); } - // point + // 点模型 if (model_name.compare("point") == 0) { ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); return boost::make_shared(); } - // circular + // 圆形模型 if (model_name.compare("circular") == 0) { - // get radius + // 获取半径 double radius; if (!nh.getParam("footprint_model/radius", radius)) { @@ -939,17 +1098,17 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(radius); } - // line + // 线模型 if (model_name.compare("line") == 0) { - // check parameters + // 检查参数 if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) { ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); return boost::make_shared(); } - // get line coordinates + // 获取线坐标 std::vector line_start, line_end; nh.getParam("footprint_model/line_start", line_start); nh.getParam("footprint_model/line_end", line_end); @@ -968,10 +1127,10 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF Eigen::Map(line_end.data())); } - // two circles + // 两个圆形 if (model_name.compare("two_circles") == 0) { - // check parameters + // 检查参数 if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") || !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) { @@ -992,11 +1151,11 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); } - // polygon + // 多边形 if (model_name.compare("polygon") == 0) { - // check parameters + // 检查参数 XmlRpc::XmlRpcValue footprint_xmlrpc; if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc)) { @@ -1004,7 +1163,7 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF << nh.getNamespace() << "/footprint_model/vertices' does not exist. Using point-model instead."); return boost::make_shared(); } - // get vertices + // 获取顶点 if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) { try @@ -1029,12 +1188,20 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF } } - // otherwise + // 其他情况 ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() << "/footprint_model/type'. Using point model instead."); return boost::make_shared(); } +/** + * @brief 从costmap_2d获取机器人足迹 + * + * 该函数从costmap_2d::Costmap2DROS对象中获取机器人足迹，并转换为teb_local_planner的足迹模型。 + * + * @param costmap_ros costmap_2d::Costmap2DROS对象的引用。 + * @return teb_local_planner::RobotFootprintModelPtr 返回指向机器人足迹模型的智能指针。 + */ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros) { Point2dContainer footprint; @@ -1051,10 +1218,20 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(footprint); } + +/** + * @brief 从XMLRPC值创建足迹 + * + * 该函数从XMLRPC值中提取足迹顶点，并转换为Point2dContainer。 + * + * @param footprint_xmlrpc XMLRPC值，包含足迹顶点信息。 + * @param full_param_name 参数的完整名称。 + * @return teb_local_planner::Point2dContainer 返回包含足迹顶点的Point2dContainer对象。 + */ teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) { - // Make sure we have an array of at least 3 elements. + // 确保我们有至少包含3个元素的数组 if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || footprint_xmlrpc.size() < 3) { ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", full_param_name.c_str(), @@ -1069,7 +1246,7 @@ teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC( for (int i = 0; i < footprint_xmlrpc.size(); ++i) { - // Make sure each element of the list is an array of size 2. (x and y coordinates) + // 确保列表中的每个元素都是包含2个元素的数组（x和y坐标） XmlRpc::XmlRpcValue point = footprint_xmlrpc[i]; if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2) { @@ -1090,6 +1267,15 @@ teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC( return footprint; } +/** + * @brief 从XMLRPC值中提取数值 + * + * 该函数从XMLRPC值中提取数值，并确保其类型为int或double。 + * + * @param value XMLRPC值。 + * @param full_param_name 参数的完整名称。 + * @return double 返回提取的数值。 + */ double MpcLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) { // Make sure that the value we're looking at is either a double or an int.