move_ベースソース解析
ブログを書くのは久しぶりですが、最近はmove_baseソースパッケージを研究して、ついでにブログを書く形式でまとめて、同時に中に自分の心得が混ざっていて、各道の大神は異なるプロフィールがあればプライベートで評論することができます.
1 move_base_node.cppファイル全体の開始はmove_base_node.cppが始まります.MoveBase::MoveBaseの初期化から開始します.as_メンテナンス動作サービスの変数として使用します.
ターゲットポイントのフォーマットが正しいかどうかを判断するために使用されます.1.2次に実行
ターゲットポイントのフォーマットを汎用フォーマットに変換し、次にターゲットポイントをパブリッシュし、パブリッシュ頻度を設定します.
そしてcost_を更新map、およびタイムスタンプの設定
次にwhileループ関数に入ります.if文は複雑です.
こんなにたくさん話してやっと初期化moveについて話しました.ベースの最初の文.は、次に、いくつかの基本的なパラメータ構成の問題である.たとえば、そのグローバルパスプランニング、ローカルパスプランニングを使用します.
次に3つの容器を設置しました.
次に、パス計画スレッドです.
2.1次はこのスレッドに進みます
3.経路計画スレッドを終了し、moveに戻りましょう.ベース初期化.次の作業はパブリッシャの作成です
そしてrvizに関する目標点を設定します
次にcostmapパラメータの設定で、地図膨張層のようなパラメータです.
次はグローバル・プランナ、ローカル・プランナの設定です
残りの基本はあまり違わない.
4.以上、よくわからないところもありますが、模索しているうちに、まだまだ勉強しなければならないところがたくさんあることに気づきました.リンクも提供されていて、よく書けたと思います:[https://www.cnblogs.com/JuiceCat/p/13040552.html]
注:本文はチョコレート～唯心オリジナル文章です.創作は容易ではありませんが、引用する場合は出典を明記してください!!!
1 move_base_node.cppファイル全体の開始はmove_base_node.cppが始まります.MoveBase::MoveBaseの初期化から開始します.as_メンテナンス動作サービスの変数として使用します.
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
MoveBase::executeCb 。
1. MoveBase::executeCb 。**
2. 1.1
isQuaternionValid(move_base_goal->target_pose.pose.orientation)
ターゲットポイントのフォーマットが正しいかどうかを判断するために使用されます.1.2次に実行
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose)
ターゲットポイントのフォーマットを汎用フォーマットに変換し、次にターゲットポイントをパブリッシュし、パブリッシュ頻度を設定します.
current_goal_pub_.publish(goal);
ros::Rate r(controller_frequency_);
そしてcost_を更新map、およびタイムスタンプの設定
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
次にwhileループ関数に入ります.if文は複雑です.
//action
if(as_->isPreemptRequested())
{
//
if(as_->isNewGoalAvailable())
{
// ， ，
if(!isQuaternionValid(new_goal.target_pose.pose.orientation))
{
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
goal = goalToGlobalFrame(new_goal.target_pose);
recovery_index_ = 0;
state_ = PLANNING;
//
lock.lock();
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
//
ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
current_goal_pub_.publish(goal);
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
}
else//
{
//14. ,
resetState();
ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
as_->setPreempted();
return;
}
//15. ， ，
if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
goal = goalToGlobalFrame(goal);
//we want to go back to the planning state for the next execution cycle
recovery_index_ = 0;
state_ = PLANNING;
//we have a new goal so make sure the planner is awake
lock.lock();
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
//publish the goal point to the visualizer
ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
current_goal_pub_.publish(goal);
//make sure to reset our timeouts and counters
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
}
//19. ，
bool done = executeCycle(goal);
}
executeCycle(goal) ， ， 。
1.2.1 ecuteCycle(goal)
```cpp
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
//we need to be able to publish velocity commands
geometry_msgs::Twist cmd_vel;
geometry_msgs::PoseStamped global_pose;
//
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;
//push the feedback out
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
//
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;
}
// ， ， ， false， publishZeroVelocity（） geometry_msgs::Twist cmd_vel 0 :
if(!controller_costmap_ros_->isCurrent())
{
ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
publishZeroVelocity();
return false;
}
if(new_global_plan_)
{
//make sure to set the new plan flag to false
new_global_plan_ = false;
ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
//do a pointer swap under mutex
std::vector<:posestamped>* temp_plan = controller_plan_;
boost::unique_lock<:recursive_mutex> lock(planner_mutex_);
controller_plan_ = latest_plan_;
latest_plan_ = temp_plan;
lock.unlock();
ROS_DEBUG_NAMED("move_base","pointers swapped!");
//tc
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;
if(recovery_trigger_ == PLANNING_R)
recovery_index_ = 0;
}
// switch ，
// move_base ， goal PLANNING，
// state_ PLANNING CONTROLLING， PLANNING CLEARING
switch(state_){
//if we are in a planning state, then we'll attempt to make a plan
case PLANNING://
{
boost::recursive_mutex::scoped_lock lock(planner_mutex_);
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 commands
case CONTROLLING:// ， ，
ROS_DEBUG_NAMED("move_base","In controlling state.");
//check to see if we've reached our goal
if(tc_->isGoalReached()){
ROS_DEBUG_NAMED("move_base","Goal reached!");
resetState();
//disable the planner thread
boost::unique_lock<:recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;//
lock.unlock();
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
//check for an oscillation condition
// ， ， ， ： ，
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();// 0
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
boost::unique_lock<:costmap2d::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
// , ， cmd_vel： ，
if(tc_->computeVelocityCommands(cmd_vel))
{
ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
last_valid_control_ = ros::Time::now();
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
else // ，
{
ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
//check if we've tried to find a valid control for longer than our time limit
// ， ， ， ：
if(ros::Time::now() > attempt_end){
//we'll move into our obstacle clearing mode
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
}
else // ， ：
{
//otherwise, if we can't find a valid control, we'll go back to planning
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
publishZeroVelocity();
//enable the planner thread in case it isn't running on a clock
boost::unique_lock<:recursive_mutex> lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
}
}
break;
//we'll try to clear out space with any user-provided recovery behaviors
case CLEARING://
ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
//we'll invoke whatever recovery behavior we're currently on if they're enabled
// ， ， PLANNING：
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_+1, recovery_behaviors_.size());
move_base_msgs::RecoveryStatus msg;
msg.pose_stamped = current_position;
msg.current_recovery_number = recovery_index_;
msg.total_number_of_recoveries = recovery_behaviors_.size();
msg.recovery_behavior_name = recovery_behavior_names_[recovery_index_];
recovery_status_pub_.publish(msg);
recovery_behaviors_[recovery_index_]->runBehavior();
//we at least want to give the robot some time to stop oscillating after executing the behavior
last_oscillation_reset_ = ros::Time::now();
//we'll check if the recovery behavior actually worked
ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
//update the index of the next recovery behavior that we'll try
recovery_index_++;
}
else{// ， ， true：
ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
//disable the planner thread
boost::unique_lock<:recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
if(recovery_trigger_ == CONTROLLING_R){
ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == PLANNING_R){
ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == OSCILLATION_R){
ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}
resetState();
return true;
}
break;
default:
ROS_ERROR("This case should never be reached, something is wrong, aborting");
resetState();
//disable the planner thread
boost::unique_lock<:recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
return true;
}
こんなにたくさん話してやっと初期化moveについて話しました.ベースの最初の文.
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
// （ ： 、 、 ） “ ”：
recovery_trigger_ = PLANNING_R;
//get some parameters that will be global to the move base node，
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
// parameters of make_plan service
private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);`
次に3つの容器を設置しました.
// ：（planner_plan_ ， latest_plan_，
// latest_plan_ executeCycle controller_plan_）
//
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
次に、パス計画スレッドです.
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
2.1次はこのスレッドに進みます
void MoveBase::planThread()
{
ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;
//1.
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
// ， while
while(n.ok())
{
// ，
while(wait_for_wake || !runPlanner_)
{
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
ros::Time start_time = ros::Time::now();
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
//
planner_plan_->clear();
// makeplan
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
// plan, latest_plan_
if(gotPlan){
ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
lock.lock();
planner_plan_ = latest_plan_;
latest_plan_ = temp_plan;
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
new_global_plan_ = true;
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
// ，
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
//5. ,
// ， PLANNING ，
// ， MoveBase::executeCycle
else if(state_==PLANNING){
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
lock.lock();
planning_retries_++;//
if(runPlanner_ &&
(ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)))
{
//we'll move into our obstacle clearing mode
state_ = CLEARING;
runPlanner_ = false; // proper solution for issue #523
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
lock.unlock();
}
if(planner_frequency_ > 0)
{
ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
if (sleep_time > ros::Duration(0.0)){
wait_for_wake = true;
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
}
}
3.経路計画スレッドを終了し、moveに戻りましょう.ベース初期化.次の作業はパブリッシャの作成です
// ， cmd_vel， cunrrent_goal， goal：
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
ros::NodeHandle action_nh("move_base");
ac tion_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
そしてrvizに関する目標点を設定します
// geometry_msgs::PoseStamped goals ， cb MoveBase::goalCB，
// rviz ：
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
次にcostmapパラメータの設定で、地図膨張層のようなパラメータです.
// costmap ， ：
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
次はグローバル・プランナ、ローカル・プランナの設定です
// ，planner_costmap_ros_ costmap_2d::Costmap2DROS*
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
//initialize the global planner
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
//
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
//create a local planner
try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
残りの基本はあまり違わない.
// costmap：
planner_costmap_ros_->start();
controller_costmap_ros_->start();
//advertise a service for getting a plan
//
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
//advertise a service for clearing the costmaps
// ： ， MoveBase::clearCostmapsService（） ， costmap
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
//if we shutdown our costmaps when we're deactivated... we'll do that now
// costmap， ：
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
//load any user specified recovery behaviors, and if that fails load the defaults
// ， ， 360：
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
//initially, we'll need to make a plan
// ， ：
state_ = PLANNING;
//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;
//we're all set up now so we can start the action server
as_->start();
// ：
dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
4.以上、よくわからないところもありますが、模索しているうちに、まだまだ勉強しなければならないところがたくさんあることに気づきました.リンクも提供されていて、よく書けたと思います:[https://www.cnblogs.com/JuiceCat/p/13040552.html]
注:本文はチョコレート～唯心オリジナル文章です.創作は容易ではありませんが、引用する場合は出典を明記してください!!!