首先打开CMakeLists.txt，查看所需要链接的库文件与要编译成可执行文件的源文件：
add_library(move_base
src/move_base.cpp
)
target_link_libraries(move_base
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
add_executable(move_base_node
src/move_base_node.cpp
)库文件为move_base.cpp，源文件为move_base_node.cpp。
进入源文件内，找到main函数：
int main(int argc, char** argv){
ros::init(argc, argv, "move_base_node");
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
move_base::MoveBase move_base( buffer );
//ros::MultiThreadedSpinner s;
ros::spin();
return(0);
}先初始化了ROS的move_base_node节点，并创立了tf2_ros::Buffer类的一个对象，通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息，后续操作都是用的tf2_ros::Buffer类的成员函数完成。最后建立了一个MoveBase类的对象。接下来进入库文件中找他的构造函数：
MoveBase::MoveBase(tf2_ros::Buffer& tf)
{
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
recovery_trigger_ = PLANNING_R;
std::string global_planner, local_planner;
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));
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");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
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);
}
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
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);
}
planner_costmap_ros_->start();
controller_costmap_ros_->start();
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
state_ = PLANNING;
recovery_index_ = 0;
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);
}传入的参数为Buffer的对象的引用。
as_维护MoveBase的MoveBaseActionServer状态机，并且新建了一个executeCb回调线程。这里由action的客户端触发回调线程。
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);跳转到Move_executeCb函数：
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{
if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
publishZeroVelocity();
//we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
current_goal_pub_.publish(goal);
std::vector<geometry_msgs::PoseStamped> global_plan;
ros::Rate r(controller_frequency_);
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
//we want to make sure that we reset the last time we had a valid plan and control
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
ros::NodeHandle n;
while(n.ok())
{
if(c_freq_change_)
{
ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
r = ros::Rate(controller_frequency_);
c_freq_change_ = false;
}
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
//if we're active and a new goal is available, we'll accept it, but we won't shut anything down
move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
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);
//we'll make sure that we reset our 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","move_base has received a goal of x: %.2f, y: %.2f", 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;
}
else {
//if we've been preempted explicitly we need to shut things down
resetState();
//notify the ActionServer that we've successfully preempted
ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
as_->setPreempted();
//we'll actually return from execute after preempting
return;
}
}
//we also want to check if we've changed global frames because we need to transform our goal pose
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;
}
//for timing that gives real time even in simulation
ros::WallTime start = ros::WallTime::now();
//the real work on pursuing a goal is done here
bool done = executeCycle(goal, global_plan);
//if we're done, then we'll return from execute
if(done)
return;
//check if execution of the goal has completed in some way
ros::WallDuration t_diff = ros::WallTime::now() - start;
ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
r.sleep();
//make sure to sleep for the remainder of our cycle time
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
}
//wake up the planner thread so that it can exit cleanly
lock.lock();
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
//if the node is killed then we'll abort and return
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
return;
}这个线程主要用于接收目标点，并进行坐标变换，并转化为成员变量，将其用话题的方式再发布出去。首先用isQuaternionValid判断发出的目标点的有效性，然后向goalToGlobalFrame函数传入目标点的位姿，返回goal相对于Global坐标系的坐标。publishZeroVelocity()函数用于初始化发送都为0的cmd信号。之后用
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);给该线程上锁。并进行赋值操作，赋值完毕后，用lock.unlock()打开互斥锁。并往”current_goal“这个话题发送消息。
让我们回到MoveBase的构造函数，后三句分别创建了指向geometry_msgs::PoseStamped数据格式的三个容器,用于路径规划的三重缓冲区：
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();之后又创建了一个新的线程，用于路径规划，线程的函数为MoveBase::planThread:()：
第一步:日常操作开启互斥锁。并检查是否锁住。
第二步将成员变量planner_goal_的值传入到此线程中，赋值给temp_goal，并解开线程锁。
第三步对容器进行清空，并进入makePlan检测代价地图是否传入、获取机器人的位姿。值得注意的是这里的两个makePlan是不同的：
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
planner_->makePlan(start, goal, plan)通过智能指针，定义了一个指向nav_core::BaseGlobalPlanner类对象的一个指针，并调用那个类中的成员函数makePlan:
bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){
boost::mutex::scoped_lock lock(mutex_);
if(!initialized_){
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if(goal.header.frame_id != global_frame_){
ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
global_frame_.c_str(), goal.header.frame_id.c_str());
return false;
}
if(start.header.frame_id != global_frame_){
ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
global_frame_.c_str(), start.header.frame_id.c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int mx, my;
if(!costmap_->worldToMap(wx, wy, mx, my)){
ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, mx, my);
//make sure to resize the underlying array that Navfn uses
planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if(!costmap_->worldToMap(wx, wy, mx, my)){
if(tolerance <= 0.0){
ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
mx = 0;
my = 0;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
planner_->setStart(map_goal);
planner_->setGoal(map_start);
//bool success = planner_->calcNavFnAstar();
planner_->calcNavFnDijkstra(true);
double resolution = costmap_->getResolution();
geometry_msgs::PoseStamped p, best_pose;
p = goal;
bool found_legal = false;
double best_sdist = DBL_MAX;
p.pose.position.y = goal.pose.position.y - tolerance;
while(p.pose.position.y <= goal.pose.position.y + tolerance){
p.pose.position.x = goal.pose.position.x - tolerance;
while(p.pose.position.x <= goal.pose.position.x + tolerance){
double potential = getPointPotential(p.pose.position);
double sdist = sq_distance(p, goal);
if(potential < POT_HIGH && sdist < best_sdist){
best_sdist = sdist;
best_pose = p;
found_legal = true;
}
p.pose.position.x += resolution;
}
p.pose.position.y += resolution;
}
if(found_legal){
//extract the plan
if(getPlanFromPotential(best_pose, plan)){
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = best_pose;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
}
else{
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}
if (visualize_potential_)
{
// Publish the potentials as a PointCloud2
sensor_msgs::PointCloud2 cloud;
cloud.width = 0;
cloud.height = 0;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = global_frame_;
sensor_msgs::PointCloud2Modifier cloud_mod(cloud);
cloud_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"pot", 1, sensor_msgs::PointField::FLOAT32);
cloud_mod.resize(planner_->ny * planner_->nx);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
PotarrPoint pt;
float *pp = planner_->potarr;
double pot_x, pot_y;
for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
{
if (pp[i] < 10e7)
{
mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
iter_x[0] = pot_x;
iter_x[1] = pot_y;
iter_x[2] = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
iter_x[3] = pp[i];
++iter_x;
}
}
potarr_pub_.publish(cloud);
}
//publish the plan for visualization purposes
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}第一步传入机器人的起始坐标，并利用worldToMap把基于world的起始坐标进行坐标转换，转换到基于Map坐标系上。同理把目标点也做同样的转换。最终起始点存在map_start数组中，目标点存在map_goal数组中。并通过两句把数组传入到成员变量中。
planner_->setStart(map_goal);
planner_->setGoal(map_start);第二步清空代价地图中的像素点。
第三步计算DJIkstra
bool
NavFn::calcNavFnDijkstra(bool atStart)
{
setupNavFn(true);
// calculate the nav fn and path
propNavFnDijkstra(std::max(nx*ny/20,nx+ny),atStart);
// path
int len = calcPath(nx*ny/2);
if (len > 0) // found plan
{
ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
return true;
}
else
{
ROS_DEBUG("[NavFn] No path found\n");
return false;
}
}进入calcPath(nx*ny/2)函数中，迪杰斯特拉的主体:nx为map的x的size，stc为起始点的数组下标。
int stc = st[1]*nx + st[0];
int nearest_point=std::max(0,std::min(nx*ny-1,stc+(int)round(dx)+(int)(nx*round(dy))));
扫一扫
654
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
