ROS Navigation的base_local_planner类继承关系与实现方法
来源:互联网 发布:网络水军兼职违法吗 编辑:程序博客网 时间:2024/05/20 05:57
base_local_planner
局部规划根据传感器的数据为机器人选择适当的线速度、角速度,来完成全局路径当前局部片段的执行。局部规划从全局和局部costmap中选择一条路径执行,因此局部回话可以重新计算机器人的路径,以防止撞击物体,但仍然需要它到达目的地。为了有效地评分轨迹,使用了栅格地图。对于每个控制周期,在机器人周围创建一个局部网格(局部地图的大小),并将全局路径映射到该区域。这意味着某些栅格单元(全局路径经过的栅格)将被标记为距离0。然后有效地将所有其他单元的距离标记为到最近的被标记为0的栅格的曼哈顿距离。
目标函数与类继承关系
关于局部规划路径的选择,首先是代价函数的计算与相关的算法类:
cost = pdist_scale * (distance to path from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) + gdist_scale * (distance to local goal from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))
在TrajectoryCostFunction定义了接口scoreTrajectory()
用来评估每一条轨迹的代价。TrajectorySampleGenerator产生一系列轨迹,然后TrajectoryCostFunction遍历轨迹打分,TrajectorySearch找到最好的轨迹拿来给小车导航;由于小车不是一个质点,worldModel会检查小车有没有碰到障碍物。
同前面所讲,在机器人周围创建一个局部网格(局部地图的大小),并将全局路径映射到该区域。这意味着某些栅格单元(全局路径经过的栅格)将被标记为距离0。然后有效地将所有其他单元的距离标记为到最近的被标记为0的栅格的曼哈顿距离。MapGridCostFunction就是局部规划的路径离全局规划的路径的距离,也能够来评估到目标的距离,它维护了一个base_local_planner::MapGrid map_,MapGrid是一系列MapCell,而MapCell包含了一个target_dist,也就是说,MapGridCostFunction建立后随时知道地图上一个点到全局规划轨迹的距离,或者是到目标的距离。具体是在computeTargetDistance中实现的。
ObstacleCostFunction就是计算机器人在局部的costmap上路径的代价,检测是否撞到障碍。其中ObstacleCostFunction用到了worldModel(如下图)接口来检测底座是不是撞到障碍物,从初始化函数可以看出来,ObstacleCostFunction选用了CostmapModel作为具体实现,想改为其他具体实现随时可以改这一行。
OscillationCostFunction:Oscillation occur when in either of the x, y, or theta dimensions, positive and negative values are chosen consecutively. To prevent oscillations, when the robot moves in any direction, for the next cycles the opposite direction is marked invalid, until the robot has moved beyond a certain distance from the position where the flag was set.
PreferForwardCostFunction:This cost function class was designed with robots in mind having good sensor coverage only in front of the robot (tilting laser). The costs function prefers motions in the front direction, penalizing backwards and strafing motions. On other robots or in other domains this may be very undesirable behavior.
类的参数与动态调整
在这些类中出现的参数可以被分类为:
-robot configuration-goal tolerance-forward simulation-trajectory scoring-oscillation prevention-global plan
……
在参数调整中会很麻烦,使用rqt_reconfigure toll
来动态的调整参数:
rosrun rqt_reconfigure rqt_reconfigure
主体计算
主体的思想在TrajectoryPlannerROS::computerVelocityCommands()
:
bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){ if (! isInitialized()) { ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); return false; } std::vector<geometry_msgs::PoseStamped> local_plan; tf::Stamped<tf::Pose> global_pose; if (!costmap_ros_->getRobotPose(global_pose)) { return false; } std::vector<geometry_msgs::PoseStamped> transformed_plan; //get the global plan in our frame if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) { ROS_WARN("Could not transform the global plan to the frame of the controller"); return false; } //now we'll prune the plan based on the position of the robot if(prune_plan_) prunePlan(global_pose, transformed_plan, global_plan_); tf::Stamped<tf::Pose> drive_cmds; drive_cmds.frame_id_ = robot_base_frame_; tf::Stamped<tf::Pose> robot_vel; odom_helper_.getRobotVel(robot_vel); //if the global plan passed in is empty... we won't do anything if(transformed_plan.empty()) return false; tf::Stamped<tf::Pose> goal_point; tf::poseStampedMsgToTF(transformed_plan.back(), goal_point); //we assume the global goal is the last point in the global plan double goal_x = goal_point.getOrigin().getX(); double goal_y = goal_point.getOrigin().getY(); double yaw = tf::getYaw(goal_point.getRotation()); double goal_th = yaw; //check to see if we've reached the goal position if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) { //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll //just rotate in place if (latch_xy_goal_tolerance_) { xy_tolerance_latch_ = true; } double angle = getGoalOrientationAngleDifference(global_pose, goal_th); //check to see if the goal orientation has been reached if (fabs(angle) <= yaw_goal_tolerance_) { //set the velocity command to zero cmd_vel.linear.x = 0.0; cmd_vel.linear.y = 0.0; cmd_vel.angular.z = 0.0; rotating_to_goal_ = false; xy_tolerance_latch_ = false; reached_goal_ = true; } else { //we need to call the next two lines to make sure that the trajectory //planner updates its path distance and goal distance grids tc_->updatePlan(transformed_plan); Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); map_viz_.publishCostCloud(costmap_); //copy over the odometry information nav_msgs::Odometry base_odom; odom_helper_.getOdom(base_odom); //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, rot_stopped_velocity_, trans_stopped_velocity_)) { if ( ! stopWithAccLimits(global_pose, robot_vel, cmd_vel)) { return false; } } //if we're stopped... then we want to rotate to goal else{ //set this so that we know its OK to be moving rotating_to_goal_ = true; if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) { return false; } } } //publish an empty plan because we've reached our goal position publishPlan(transformed_plan, g_plan_pub_); publishPlan(local_plan, l_plan_pub_); //we don't actually want to run the controller when we're just rotating to goal return true; } tc_->updatePlan(transformed_plan); //compute what trajectory to drive along Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); map_viz_.publishCostCloud(costmap_); //pass along drive commands cmd_vel.linear.x = drive_cmds.getOrigin().getX(); cmd_vel.linear.y = drive_cmds.getOrigin().getY(); cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation()); //if we cannot move... tell someone if (path.cost_ < 0) { ROS_DEBUG_NAMED("trajectory_planner_ros", "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories."); local_plan.clear(); publishPlan(transformed_plan, g_plan_pub_); publishPlan(local_plan, l_plan_pub_); return false; } ROS_DEBUG_NAMED("trajectory_planner_ros", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z); // Fill out the local plan for (unsigned int i = 0; i < path.getPointsSize(); ++i) { double p_x, p_y, p_th; path.getPoint(i, p_x, p_y, p_th); tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose( tf::createQuaternionFromYaw(p_th), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_); geometry_msgs::PoseStamped pose; tf::poseStampedTFToMsg(p, pose); local_plan.push_back(pose); } //publish information to the visualizer publishPlan(transformed_plan, g_plan_pub_); publishPlan(local_plan, l_plan_pub_); return true; }
- ROS Navigation的base_local_planner类继承关系与实现方法
- ROS Navigation的costmap_2d类继承关系与实现方法
- ROS Navigation-----base_local_planner类
- ROS Navigation的global_planner类继承关系与实现算法
- ROS Navigation-----base_local_planner简介
- ROS Navigation的move_base类实现方法
- ROS导航之base_local_planner
- ROS Navigation-----dwa_planner_ros类
- Java 继承 父类变量, 方法 与子类的关系:
- ROS 学习系列 -- 执行turtlebot navigation的方法
- 抽象类与接口之间的继承和实现关系
- java实现接口与继承的关系
- 抽象类的继承与方法实现
- 通过TurtleBot学navigation与ROS的笔记
- 抽象类,接口继承与实现关系
- base_local_planner
- ROS navigation
- Using ROS Navigation Package 实现导航
- ArcGIS Server路径分析结果汉化
- pycharm远程调试配置
- Perl
- weblogic启动项目在 Server started in RUNNING mode时,卡住不动
- jqPaginator分页样式不对的问题
- ROS Navigation的base_local_planner类继承关系与实现方法
- Python字典初识—Python取经之路
- jackson annotations注解详解
- 操作系统08-内存管理
- 《Java编程技巧1001条》 第344条:使用floor方法
- LeetCode 111. Minimum Depth of Binary Tree
- 9. base_link、odom、map关系
- java.sql.SQLException: The user specified as a definer ('root'@'%') does not exist 解决方法
- SSH Client连接Linux出现~server responded “Algorithm negotiation failes”