costmap_2d详解3:costmap_2d_ros.cpp

本文深入解析了MoveBase中Costmap2DROS类的作用与工作流程,阐述了如何通过YAML配置文件加载不同类型的Layer plugin,以及Costmap2DROS构造函数、reconfigureCB函数和updateMap函数的具体实现细节。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

整体概述:

    Costmap2DROS 类,它是move_base 调用costmap_2d 生成代价地图的入口,整体的逻辑都在这里面。

Costmap2DROS构造函数,实例化LayeredCostmap的指针,根据yaml配置文件中plugins参数生成相应类型的各个层Layer plugin,并添加到LayeredCostmap 的plugins_指针列表中后面使用,调用到各层的初始化函数,调用重配置函数reconfigureCB,创建多线程,同时更新地图

reconfigureCB 创建多线程,并调用mapUpdateLoop,在它中调用updateMap() 函数更新各层costmap 边界和cost值,之后在发布出去

在updateMap()中,得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息,并调用LayeredCostmap 的updateMap(x, y, yaw) 函数,更新各层costmap 边界和cost 值

 

关键变量:

LayeredCostmap* layered_costmap_; //LayeredCostmap 类指针,管理各层costmap
global_frame_;
robot_base_frame_;
tf_;
plugin_loader_; //加载各层costmap 到 LayeredCostmap 中

 

关键函数

/*作用: 构造函数,

*name : costmap 名称,如global_costmap和local_costmap

* tf :  从move_base 传过来的tf 转换

*/

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf)

{

  //在这里可以区分global_costmap 和local_costmap
  ros::NodeHandle private_nh("~/" + name);

/*如果是global_costmap,两参数分别是
*   global_frame: /map
*  robot_base_frame: /base_footprint
*****************************************************
* 如果是local_costmap 两参数分别是
*   global_frame: /odom_combined
*  robot_base_frame: /base_footprint
*/
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));


/*如果是global_costmap,rolling_window=false
*如果是local_costmap,rolling_window=true
*track_unknown_space 都默认是false
*/
private_nh.param("rolling_window", rolling_window, false);private_nh.param("track_unknown_space", track_unknown_space, false);

//实例化LayeredCostmap的指针,也初始化来master costmap
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

/*
*判断导入的参数是否有plugins
*/
if (private_nh.hasParam("plugins"))
{
XmlRpc::XmlRpcValue my_list;

//把要加载的costmap 插件层添加进来
private_nh.getParam("plugins", my_list);
for (int32_t i = 0; i < my_list.size(); ++i)
{
//创建一个以 type为类类型的实例变量,然后让plugin这个指针指向这个实例
/* 两个值例如为:
* name: static_layer,       type: "costmap_2d::StaticLayer”
*/
std::string pname = static_cast<std::string>(my_list[i]["name"]);
std::string type = static_cast<std::string>(my_list[i]["type"]);

/* 创建一个以 type为类类型的实例变量,然后让plugin这个指针指向这个实例
并把这层地图加入到layered_costmap_ 中统一管理
*/

//定义Layer 的智能指针,并实例化
boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);

//添加到 LayeredCostmap 中的plugins_ 列表末尾
layered_costmap_->addPlugin(plugin);

/*这里就会调用到Layer的initialize 函数,initialize 在父类layer.cpp 中,它会调用虚函数onInitialize,
* 最后各个plugin 层会实现该方法
*/
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);


/*还没详细看*/    
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));


//把地图更新过的部分发布出去

  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",always_send_full_costmap);

  // create a thread to handle updating the map
  stop_updates_ = false;
  initialized_ = true;
  stopped_ = false;

  // Create a time r to check if the robot is moving
  robot_stopped_ = false;
  /*回调函数movementCB 实现,是通过比较前后两个pose的差,判断机器人是否在移动*/
  timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));

  /*除了对一些类成员的配置值做赋值以外,还会开启一个更新map的线程*/

  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType \

cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

}
}
}

 

/*根据参数对地图进行调整,另外还会重新启动地图更新线程mapUpdateLoop(因此,地图更新线程在这里启动)。*/

void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
{
 //都是1.0
  transform_tolerance_ = config.transform_tolerance;

//如果已经更新则不用更新
  if (map_update_thread_ != NULL)
  {
    map_update_thread_shutdown_ = true;
    map_update_thread_->join();
    delete map_update_thread_;
  }
  map_update_thread_shutdown_ = false;
  //配置地图更新频率
  double map_update_frequency = config.update_frequency;

  double map_publish_frequency = config.publish_frequency;
/*
如果是global_costmap:
map_width_meters = config.width  //暂时不知
map_height_meters = config.height  //暂时不知
resolution = config.resolution //0.05
origin_x = config.origin_x  //应该是地图左下角坐标(即像素坐标系原点)
origin_y = config.origin_y

**************************************
如果是local_costmap:
map_width_meters = config.width  //3.0
map_height_meters = config.height  //3.0
resolution = config.resolution //0.05
origin_x = config.origin_x  //应该是机器人局部costmap左下角
origin_y = config.origin_y      
*/

double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =config.origin_x,origin_y = config.origin_y;

//如果已经膨胀填充,重新计算填充一次
  if (footprint_padding_ != config.footprint_padding)
  {
    footprint_padding_ = config.footprint_padding;
    setUnpaddedRobotFootprint(unpadded_footprint_);
  }

  readFootprintFromConfig(config, old_config_);
//记录现在的配置为旧的配置,可能用于恢复以前的配置
  old_config_ = config;

  //启动地图更新线程mapUpdateLoop
  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
}

 

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  while (nh.ok() && !map_update_thread_shutdown_)
  {
    //更新边界和地图
    updateMap();
  }
}

 

/*updateMap函数是地图信息更新的地方,其内部调用了LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)函数。

 * 在该函数中,先依据各层的更新情况,判断地图更新过的范围的边界。然后用初始值重置全局地图更新

 * 边界范围内的地图信息,并用各层的信息在更新边界内部更新地图信息

*/

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
tf::Stamped < tf::Pose > pose;
//得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息
    if (getRobotPose (pose))
    {
      //获取当前机器人位置,必须要弄清楚这个x,y 代表什么意思
      double x = pose.getOrigin().x(),
      y = pose.getOrigin().y(),
      yaw = tf::getYaw(pose.getRotation());

/*调用LayeredCostmap 类中的updateMap 更新边界和cost值
*先依据各层的更新情况,判断地图更新过的范围的边界。
* 然后用初始值重置全局地图更新边界范围内的地图信息,
* 并用各层的信息在更新边界内部更新地图信息
*/
      layered_costmap_->updateMap(x, y, yaw);

      geometry_msgs::PolygonStamped footprint;
      footprint.header.frame_id = global_frame_;
      footprint.header.stamp = ros::Time::now();
      transformFootprint(x, y, yaw, padded_footprint_, footprint);
      footprint_pub_.publish(footprint);
      initialized_ = true;
    }
  }
}

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  //local_costmap和global_costmap的robot_base_frame_都是是/base_footprint
  robot_pose.frame_id_ = robot_base_frame_;

  try
  {
    /*
    *如果是local_costmap,global_frame_是/map
    * 如果是global_costmap 是/odom_combined
    * 然后根据tf信息,转换的到global_pose,即机器人当前位置??
    *得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息
    */
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  return true;
}

 

/*启动地图运行,内部包括激活各层、启动地图更新循环。可以在stop或者

 * pause调用之后用于重新启动地图

*/

void Costmap2DROS::start()

 

/*停止地图运行,内部包括去激活各层、停止地图更新循环*/

void Costmap2DROS::stop()

/*暂停地图运行,内部包括停止地图更新循环*/

void Costmap2DROS::pause()

/*复位地图运行,内部包括恢复地图更新循环*/

void Costmap2DROS::resume()

/*重置地图,内部包括重置总地图、重置地图各层*/

void Costmap2DROS::resetLayers()

 

#include "RelativeMove.h" namespace rei_relative_move { RelativeMove::RelativeMove() : motionModel_(0), getPoseReady_(false), moveFinished_(true), robotModel_(0) {} RelativeMove::~RelativeMove() { if (listenTfThread_.joinable()) { listenTfThread_.join(); } geometry_msgs::Twist stopCmd; velPub_.publish(stopCmd); } int8_t RelativeMove::Init(ros::NodeHandle& nh) { tfBuffer_ = std::make_unique<tf2_ros::Buffer>(); tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_); xPid_ = std::make_shared<rei_tools::ReiPID>(1.0, 0, 0.0); yPid_ = std::make_shared<rei_tools::ReiPID>(1.0, 0, 0.0); thetaPid_ = std::make_shared<rei_tools::ReiPID>(2.0, 0, 0.0); xPid_->setOutputLimit(0.5, 0.05); yPid_->setOutputLimit(0.5, 0.05); thetaPid_->setOutputLimit(1.0, 0.2); relativeMoveServer_ = nh_.advertiseService( "relative_move", &RelativeMove::RelativeMoveCallback, this); velPub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 5); xErrPub_ = nh_.advertise<std_msgs::Float64>("relative/x_err", 5); yErrPub_ = nh_.advertise<std_msgs::Float64>("relative/y_err", 5); thetaErrPub_ = nh_.advertise<std_msgs::Float64>("relative/theta_err", 5); expectXErr_ = 0.01; expectYErr_ = 0.01; expectThetaErr_ = 0.01; return 0; } void RelativeMove::SetXPid(int p, int i, int d) { xPid_->setP(p); xPid_->setI(i); xPid_->setD(d); } void RelativeMove::SetYPid(int p, int i, int d) { yPid_->setP(p); yPid_->setI(i); yPid_->setD(d); } void RelativeMove::SetThetaPid(int p, int i, int d) { thetaPid_->setP(p); thetaPid_->setI(i); thetaPid_->setD(d); } void RelativeMove::ListenTf(std::string frameId, std::string childFrameId) { geometry_msgs::TransformStamped transform; std::string errMsg; if (!tfBuffer_->canTransform(frameId, childFrameId, ros::Time(0), ros::Duration(2.0), &errMsg)) { ROS_ERROR("Unable to get pose from TF: %s", errMsg.c_str()); return; } { std::unique_lock<std::mutex> lock(getFlagMutex_); getPoseReady_ = true; } ros::Rate rate(10.0); while (ros::ok() && (!GetFinishFlag())) { try { transform = tfBuffer_->lookupTransform(frameId, childFrameId, ros::Time(0)); SetTfPose(transform.transform); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); continue; } rate.sleep(); } } void RelativeMove::SetTfPose(geometry_msgs::Transform& tfPose) { std::unique_lock<std::mutex> lock(getTfPoseMutex_); tfPose_ = tfPose; } void RelativeMove::GetTfPose(geometry_msgs::Transform& trans) { std::unique_lock<std::mutex> lock(getTfPoseMutex_); trans = tfPose_; } geometry_msgs::Pose2D RelativeMove::GetTargetGoal( const geometry_msgs::Pose2D& goal) { tf2::Quaternion goal_quat; goal_quat.setRPY(0, 0, goal.theta); geometry_msgs::Transform robotPose; GetTfPose(robotPose); tf2::Transform robotPoseTrans, goalPoseTrans, goalBaseRobotTrans; goalBaseRobotTrans.setOrigin(tf2::Vector3(goal.x, goal.y, 0)); goalBaseRobotTrans.setRotation(goal_quat); geometry_msgs::Transform goalPose; tf2::fromMsg(robotPose, robotPoseTrans); goalPoseTrans = robotPoseTrans * goalBaseRobotTrans; // ROS_INFO("robotPose: %lf, %lf, %lf, %lf", robotPose.translation.x, // robotPose.translation.y, robotPose.rotation.z, // robotPose.rotation.w); // ROS_INFO("goalBaseRobotTrans: %lf, %lf, %lf, %lf", // goalBaseRobotTrans.getOrigin().x(), // goalBaseRobotTrans.getOrigin().y(), // goalBaseRobotTrans.getRotation().z(), // goalBaseRobotTrans.getRotation().w()); // ROS_INFO("goalPoseTrans: %lf, %lf, %lf, %lf", // goalPoseTrans.getOrigin().x(), // goalPoseTrans.getOrigin().y(), // goalPoseTrans.getRotation().z(), // goalPoseTrans.getRotation().w()); geometry_msgs::Pose2D goalReuslt; goalReuslt.x = goalPoseTrans.getOrigin().getX(); goalReuslt.y = goalPoseTrans.getOrigin().getY(); tf2::Matrix3x3 mat(goalPoseTrans.getRotation()); double roll, pitch; mat.getRPY(roll, pitch, goalReuslt.theta); return goalReuslt; } int8_t RelativeMove::GetBaseToGoal(std::string frameId, const geometry_msgs::Pose2D& inGoal) { std::string errMsg; if (!tfBuffer_->canTransform(frameId, "base_link", ros::Time(0), ros::Duration(2.0), &errMsg)) { ROS_ERROR("Unable to get pose from TF: %s", errMsg.c_str()); return -1; } try { geometry_msgs::TransformStamped transform = tfBuffer_->lookupTransform(frameId, "base_link", ros::Time(0)); SetTfPose(transform.transform); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); return -1; } baseToTargetPose_ = GetTargetGoal(inGoal); return 0; } int8_t RelativeMove::MovXY(double x, double y) { xPid_->reset_integral(); yPid_->reset_integral(); geometry_msgs::Pose2D goalReal; geometry_msgs::Pose2D targetGoal; targetGoal.x = x; targetGoal.y = y; double xErr, yErr; ros::Rate loop(10); while (ros::ok()) { goalReal = GetTargetGoal(targetGoal); // ROS_WARN("goalReal: %lf, %lf", goalReal.x, goalReal.y); xErr = .0; yErr = .0; if (x != 0) xErr = goalReal.x; if (y != 0) yErr = goalReal.y; std_msgs::Float64 errMsg; errMsg.data = xErr; xErrPub_.publish(errMsg); errMsg.data = yErr; yErrPub_.publish(errMsg); // ROS_WARN("err: %lf, %lf", xErr, yErr); if ((fabs(xErr) < expectXErr_) && (fabs(yErr) < expectYErr_)) { velPub_.publish(stopCmd_); break; } else { geometry_msgs::Twist velCmd; if (fabs(xErr) > expectXErr_) velCmd.linear.x = xPid_->compute(0.0, xErr); if (fabs(yErr) > expectYErr_) velCmd.linear.y = yPid_->compute(0.0, yErr); velPub_.publish(velCmd); } loop.sleep(); } return 0; } int8_t RelativeMove::MovTheta(double theta) { thetaPid_->reset_integral(); geometry_msgs::Pose2D goalReal; geometry_msgs::Pose2D targetGoal; targetGoal.theta = theta; double thetaErr; ros::Rate loop(10); while (ros::ok()) { goalReal = GetTargetGoal(targetGoal); // ROS_WARN("goalReal: %lf, %lf", goalReal.x, goalReal.y); thetaErr = goalReal.theta; // ROS_WARN("thetaErr: %lf",thetaErr); if (fabs(thetaErr) < expectThetaErr_) { velPub_.publish(stopCmd_); break; } else { geometry_msgs::Twist velCmd; velCmd.angular.z = thetaPid_->compute(0.0, thetaErr); velPub_.publish(velCmd); } loop.sleep(); } return 0; } bool RelativeMove::RelativeMoveCallback( relative_move::SetRelativeMove::Request& req, relative_move::SetRelativeMove::Response& res) { if (listenTfThread_.joinable()) { res.message = "last move task still run"; return false; } getPoseReady_ = false; moveFinished_ = false; res.message = "Get tf trans error"; if (GetBaseToGoal(req.global_frame, req.goal) < 0) return true; listenTfThread_ = std::thread(&RelativeMove::ListenTf, this, "base_link", req.global_frame); double lastTime = ros::Time::now().toSec(); ros::Rate loop(10); while (ros::ok() && (!GetReadyFlag())) { if ((ros::Time::now().toSec() - lastTime) > 2) return true; loop.sleep(); } if (robotModel_) { MovXY(baseToTargetPose_.x, baseToTargetPose_.y); if (req.goal.theta != 0) MovTheta(baseToTargetPose_.theta); } else { geometry_msgs::Pose2D tmpPose; tmpPose = GetTargetGoal(req.goal); double tmpTheta = atan2(tmpPose.y, tmpPose.x); if(tmpTheta>3.1415927) tmpTheta = tmpTheta - M_PI; else if(tmpTheta<-3.1415927) tmpTheta = tmpTheta + M_PI; MovTheta(tmpTheta); MovXY(baseToTargetPose_.x, 0.0); MovTheta(baseToTargetPose_.theta); } SetFinishFlag(true); if (listenTfThread_.joinable()) listenTfThread_.join(); res.message = ""; res.success = true; return true; } } // namespace rei_relative_move翻译该代码
最新发布
06-29
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值