设为首页 加入收藏

TOP

move_base代码学习一(二)
2017-10-13 10:23:26 】 浏览:10298
Tags:move_base 代码 学习
mped temp_goal = planner_goal_; //路径规划global bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); if(gotPlan){ //成功规划路径 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner"); } //if we didn't get a plan and we are in the planning state (the robot isn't moving) else if(state_==PLANNING){ //check if we've tried to make a plan for over our time limit if(ros::Time::now() > attempt_end && runPlanner_){ //we'll move into our obstacle clearing mode state_ = CLEARING; publishZeroVelocity(); recovery_trigger_ = PLANNING_R; } } //定时器,多久没有规划路径,就通知一次规划路径 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this); } } //这个是在global costmap下做的global planner bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ // global costmap boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex())); //得到机器人现在的位置信息 if(!planner_costmap_ros_->getRobotPose(global_pose)) { ROS_WARN("Unable to get starting pose of robot, unable to create global plan"); return false; } //global planner if(!planner_->makePlan(start, goal, plan) || plan.empty()){ ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); return false; } } bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){ //发布速度topic geometry_msgs::Twist cmd_vel; //push the feedback out //发布一些反馈信息 move_base_msgs::MoveBaseFeedback feedback; feedback.base_position = current_position; as_->publishFeedback(feedback); //check to see if we've moved far enough to reset our oscillation timeout if(distance(current_position, oscillation_pose_) >= oscillation_distance_) { last_oscillation_reset_ = ros::Time::now(); oscillation_pose_ = current_position; //if our last recovery was caused by oscillation, we want to reset the recovery index if(recovery_trigger_ == OSCILLATION_R) recovery_index_ = 0; } //if we have a new plan then grab it and give it to the controller if(new_global_plan_){ std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_; latest_plan_ = temp_plan; //这是local planner的规划 if(!tc_->setPlan(*controller_plan_)){ //ABORT and SHUTDOWN COSTMAPS ROS_ERROR("Failed to pass global plan to the controller, aborting."); resetState(); //disable the planner thread lock.lock(); runPlanner_ = false; lock.unlock(); as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller."); return true; } //make sure to reset recovery_index_ since we were able to find a valid plan if(recovery_trigger_ == PLANNING_R) recovery_index_ = 0; } //the move_base state machine, handles the control logic for navigation switch(state_){ //if we are in a planning state, then we'll attempt to make a plan case PLANNING: runPlanner_ = true; planner_cond_.notify_one(); ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state."); break; //if we're controlling, we'll attempt to find valid velocity c
首页 上一页 1 2 3 4 5 6 7 下一页 尾页 2/8/8
】【打印繁体】【投稿】【收藏】 【推荐】【举报】【评论】 【关闭】 【返回顶部
上一篇洛谷P1177快速排序 下一篇GCC与G++的区别

最新文章

热门文章

Hot 文章

Python

C 语言

C++基础

大数据基础

linux编程基础

C/C++面试题目