ros DWA局部规划模块

ROS-DWA模块

    • 主要流程
    • DWAPlannerROS::computeVelocityCommands
      • DWAPlannerROS::dwaComputeVelocityCommands
        • DWAPlanner::findBestPath
          • SimpleScoredSamplingPlanner::findBestTrajectory
    • 调参技巧
      • DWA被目标点过度吸引，且不听全局规划器指挥
    • 消融实验
      • goal_front_costs_
      • alignment_costs_
      • path_costs_
      • goal_costs_
      • 评价

在之前的学习中我们了解到，设定机器人的导航目标位置后，会在 MoveBase::executeCb函数中执行全局路径规划，并通过while循环反复执行 executeCycle函数控制机器人跟踪全局路径，这里控制机器人跟踪就是我们常说的局部规划，常用的局部规划方法有dwa、teb、pid、mpc、pure pursuit等，这一节对dwa算法进行研究。

主要流程

  bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
    if (! isInitialized()) {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    //when we get a new plan, we also want to clear any latch we may have on goal tolerances
    latchedStopRotateController_.resetLatching();

    ROS_INFO("Got new plan");
    return dp_->setPlan(orig_global_plan);
  }

这里调用的是dp_的setPlan()，dp_是指向DWAPlanner的指针。

  bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
    oscillation_costs_.resetOscillationFlags();
    return planner_util_->setPlan(orig_global_plan);    // 将orig_global_plan设置给LocalPlannerUtil的global_plan_
  }

接着就是调用tc_->computeVelocityCommands(cmd_vel)进行局部规划。

DWAPlannerROS::computeVelocityCommands

bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
    // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
    // 获取当前位姿
    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }
    // 调用planner_util_的getLocalPlan方法，以当前位姿为起点，
    // 获取局部路径，并将其存储在transformed_plan中。如果失败，则返回false。
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
      ROS_ERROR("Could not get local plan");
      return false;
    }

    //if the global plan passed in is empty... we won't do anything
    if(transformed_plan.empty()) {
      ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
      return false;
    }
    ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());

    // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
    dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
    // 使用latchedStopRotateController_来检查是否到达了目标位置。如果到达了，则执行停止和旋转的逻辑。
    if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
      //publish an empty plan because we've reached our goal position
      std::vector<geometry_msgs::PoseStamped> local_plan;
      std::vector<geometry_msgs::PoseStamped> transformed_plan;
      publishGlobalPlan(transformed_plan);
      publishLocalPlan(local_plan);
      base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
      return latchedStopRotateController_.computeVelocityCommandsStopRotate(
          cmd_vel,
          limits.getAccLimits(),
          dp_->getSimPeriod(),
          &planner_util_,
          odom_helper_,
          current_pose_,
          boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
    } else {
      // 计算DWA规划器的速度命令
      bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
      if (isOk) {
        publishGlobalPlan(transformed_plan);
      } else {
        ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
        std::vector<geometry_msgs::PoseStamped> empty_plan;
        publishGlobalPlan(empty_plan);
      }
      return isOk;
    }
  }

主要的几个函数是：
1、
通过DWAPlannerROS::dwaComputeVelocityCommands计算施加在机器人上的控制速度，在该函数内部调用了dp_->findBestPath, 机器人控制命令通过drive_cmds拿到。

DWAPlannerROS::dwaComputeVelocityCommands

  bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel) {
    // dynamic window sampling approach to get useful velocity commands
    if(! isInitialized()){
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    // 获取odom速度 
    geometry_msgs::PoseStamped robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    /* For timing uncomment
    struct timeval start, end;
    double start_t, end_t, t_diff;
    gettimeofday(&start, NULL);
    */

    //compute what trajectory to drive along
    geometry_msgs::PoseStamped drive_cmds;
    drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
    
    // call with updated footprint
    base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
    //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.pose.position.x;
    cmd_vel.linear.y = drive_cmds.pose.position.y;
    cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);

    //if we cannot move... tell someone
    std::vector<geometry_msgs::PoseStamped> local_plan;
    if(path.cost_ < 0) {
      ROS_DEBUG_NAMED("dwa_local_planner",
          "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
      local_plan.clear();
      publishLocalPlan(local_plan);
      return false;
    }

    ROS_DEBUG_NAMED("dwa_local_planner", "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);

    return true;
  }
DWAPlanner::findBestPath
  base_local_planner::Trajectory DWAPlanner::findBestPath(
      const geometry_msgs::PoseStamped& global_pose,
      const geometry_msgs::PoseStamped& global_vel,
      geometry_msgs::PoseStamped& drive_velocities) {

    //make sure that our configuration doesn't change mid-run
    boost::mutex::scoped_lock l(configuration_mutex_);

    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
    Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation));
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();
    Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
    base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();

    // prepare cost functions and generators for this run
    generator_.initialise(pos,
        vel,
        goal,
        &limits,
        vsamples_);

    result_traj_.cost_ = -7;
    // find best trajectory by sampling and scoring the samples
    std::vector<base_local_planner::Trajectory> all_explored;
    scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

    // debrief stateful scoring functions
    oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);

    //if we don't have a legal trajectory, we'll just command zero
    if (result_traj_.cost_ < 0) {
      drive_velocities.pose.position.x = 0;
      drive_velocities.pose.position.y = 0;
      drive_velocities.pose.position.z = 0;
      drive_velocities.pose.orientation.w = 1;
      drive_velocities.pose.orientation.x = 0;
      drive_velocities.pose.orientation.y = 0;
      drive_velocities.pose.orientation.z = 0;
    } else {
      drive_velocities.pose.position.x = result_traj_.xv_;
      drive_velocities.pose.position.y = result_traj_.yv_;
      drive_velocities.pose.position.z = 0;
      tf2::Quaternion q;
      q.setRPY(0, 0, result_traj_.thetav_);
      tf2::convert(q, drive_velocities.pose.orientation);
    }

    return result_traj_;
  }

generator_是轨迹生成器base_local_planner::SimpleTrajectoryGenerator，在DWAPlanner的构造函数中，可以看到，generator_被放置到了generator_list，然后传入了base_local_planner::SimpleScoredSamplingPlanner的构造函数构造出了scored_sampling_planner_。

std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
critics.push_back(&path_costs_); // prefers trajectories on global path
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin

// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);

scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);

因此，DWAPlanner::findBestPath中下面这部分代码，就是对scored_sampling_planner_中所使用的轨迹生成器进行初始化。

generator_.initialise(pos,
    vel,
    goal,
    &limits,
    vsamples_);

vsamples_表示采样的数量。
接着调用了SimpleScoredSamplingPlanner::findBestTrajectory函数，dwa的最优轨迹通过参数traj返回，而参数all_explored则保存了DWA的所有搜索轨迹，下面来重点看这个函数。。

SimpleScoredSamplingPlanner::findBestTrajectory
  bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
    Trajectory loop_traj;
    Trajectory best_traj;
    double loop_traj_cost, best_traj_cost = -1;
    bool gen_success;
    int count, count_valid;
    // 检查所有的TrajectoryCostFunction是否准备好
    for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
      TrajectoryCostFunction* loop_critic_p = *loop_critic;
      if (loop_critic_p->prepare() == false) {
        ROS_WARN("A scoring function failed to prepare");
        return false;
      }
    }

    for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
      count = 0;
      count_valid = 0;
      TrajectorySampleGenerator* gen_ = *loop_gen;
      // 遍历轨迹生成器生成的全部轨迹  
      while (gen_->hasMoreTrajectories()) {
        gen_success = gen_->nextTrajectory(loop_traj);
        if (gen_success == false) {
          // TODO use this for debugging
          continue;
        }
        // 对该轨迹进行打分  
        loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
        if (all_explored != NULL) {
          loop_traj.cost_ = loop_traj_cost;
          all_explored->push_back(loop_traj);
        }

        if (loop_traj_cost >= 0) {
          count_valid++;
          if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
            best_traj_cost = loop_traj_cost;   // 更新最佳得分
            best_traj = loop_traj;    // 更新最佳轨迹  
          }
        }
        count++;
        if (max_samples_ > 0 && count >= max_samples_) {
          break;
        }        
      }
      // 最佳轨迹的得分是合法的，说明找到了最佳轨迹 ，将最佳轨迹信息交给traj
      if (best_traj_cost >= 0) {
        traj.xv_ = best_traj.xv_;
        traj.yv_ = best_traj.yv_;
        traj.thetav_ = best_traj.thetav_;
        traj.cost_ = best_traj_cost;
        traj.resetPoints();
        double px, py, pth;
        for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
          best_traj.getPoint(i, px, py, pth);
          traj.addPoint(px, py, pth);
        }
      }
      ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
      if (best_traj_cost >= 0) {
        // do not try fallback generators
        break;
      }
    }
    return best_traj_cost >= 0;
  }

调参技巧

DWA被目标点过度吸引，且不听全局规划器指挥

如下图：
在这里插入图片描述
这是由于目标吸引的权重过大导致的，应该提高轨迹对齐的权重降低目标吸引的权重，如下，将goal_distance_bias的权重由24下降到5，机器人就能按照全局轨迹走而不是被目标吸引的卡住不动。
在这里插入图片描述

在这里插入图片描述

消融实验

goal_front_costs_

在这里插入图片描述
在这里插入图片描述
轨迹歪歪扭扭的，还喜欢拼命往墙上靠，感觉这个cost就是瞎搞的，还不如不要。。

alignment_costs_

在这里插入图片描述

在这里插入图片描述
轨迹平滑了很多，也不会往墙上靠，但是，变得很慢，而且到目的地后原地转圈。。

path_costs_

在这里插入图片描述
在这里插入图片描述

不按全局规划的路径走，原地转圈，怀疑原地转圈是属于恢复行为，但是速度为0，应该是由于该cost计算的是当前位姿和规划路径的偏移，而速度为0时偏移最小。

goal_costs_

在这里插入图片描述

在这里插入图片描述
按照对这个cost的理解，应该是始终的往离目的地最近的地方开，但是实际轨迹却歪歪扭扭，原地转圈，撞墙。

评价

ros dwa_local_planner包原生的这几个cost的实现各有各的问题，不是很好的实现，最好的解决办法就是直接抛弃重写。。。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/667164.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Java 异步编编程——Java内置线程池（Executor 线程池）

Java 异步编编程——Java内置线程池（Executor 线程池）

文章目录 知道线程池是什么以及解决什么问题Java 内置线程池Java 内置线程池设计结构及执行机制ThreadPoolExecutor 中的概念生命周期核心参数阻塞队列4 种任务拒绝策略 线程池使用场景 知道线程池是什么以及解决什么问题 线程池&#xff08;Thread Pool&#xff09;是一种基于…
阅读更多...
Kafka Java API

Kafka Java API

1、增加依赖 <dependency><groupId>org.apache.kafka</groupId><artifactId>kafka-clients</artifactId><version>1.0.0</version> </dependency>2、三个案例 案例1&#xff1a;生产数据 import org.apache.kafka.clients.p…
阅读更多...
SpringBoot HelloWorld 之 实现注册功能

SpringBoot HelloWorld 之 实现注册功能

SpringBoot HelloWorld 之 实现注册功能 一.配置 创建数据库big_event CREATE TABLE user (id int(10) unsigned NOT NULL AUTO_INCREMENT COMMENT ID,username varchar(20) COLLATE utf8_unicode_ci NOT NULL COMMENT 用户名,password varchar(32) COLLATE utf8_unicode_ci …
阅读更多...
β-烟酰胺单核苷酸（NMN）功能不断得到验证 市场规模呈增长态势

β-烟酰胺单核苷酸（NMN）功能不断得到验证 市场规模呈增长态势

β-烟酰胺单核苷酸&#xff08;NMN&#xff09;功能不断得到验证 市场规模呈增长态势 β-烟酰胺单核苷酸&#xff08;β-Nicotinamide mononucleotide&#xff0c;NMN&#xff09;是一种生物活性分子&#xff0c;是一种辅酶Ⅰ&#xff08;NAD&#xff09;的前体&#xff0c;也是…
阅读更多...
WPF Binding对象

WPF Binding对象

在WinForm中&#xff0c;我们要想对控件赋值&#xff0c;需要在后台代码中拿到控件对象进行操作&#xff0c;这种赋值形式&#xff0c;从根本上是无法实现界面与逻辑分离的。 在WPF中&#xff0c;微软引入了Binding对象&#xff0c;通过Binding&#xff0c;我们可以直接将控件与…
阅读更多...
NTFS磁盘格式读写工具：Tuxera NTFS 2021 for Mac

NTFS磁盘格式读写工具：Tuxera NTFS 2021 for Mac

Tuxera NTFS 是一款用于 macOS 系统的 NTFS 文件系统驱动程序。NTFS 是 Windows 系统中常用的文件系统&#xff0c;而 macOS 默认只支持读取 NTFS 格式的磁盘&#xff0c;不能进行写入操作。因此&#xff0c;如果你需要在 macOS 上进行 NTFS 磁盘的写入操作&#xff0c;就需要安…
阅读更多...
php使用openssl返回false报错0308010C

php使用openssl返回false报错0308010C

本地php使用openssl返回false, 但是在服务器上测试正常openssl_encrypt($jsonStr, DES-ECB, $key, OPENSSL_RAW_DATA, ); 查看错误 openssl_error_string(); error:0308010C:digital envelope routines::unsupported 原因是: 服务器上的openssl是1.1版本, 本地是3.0版本 通…
阅读更多...
真北5月小结｜物事人心向上

真北5月小结｜物事人心向上

1、跑步 今年的计划是每月跑15小时。五月实际跑了13小时17分。一到五月共跑了74小时43分&#xff0c;所以按平均每月15小时&#xff0c;还欠17分&#xff0c;六月补上。 另外两个跑步的标准是&#xff1a;保持跑步三天可见&#xff0c;最近龙舟雨&#xff0c;对这一条干扰很大&…
阅读更多...
iframe内嵌网页自适应缩放 以展示源网页的比例尺寸

iframe内嵌网页自适应缩放 以展示源网页的比例尺寸

需求:这是我最近开发的低代码平台遇到的需求 ,要求将配置好的应用在弹框中预览(将预览网页内嵌入弹框中) 但是内嵌进入后 他会截取一部分(我源网站网页尺寸 是1980x1080 或者 3060X2160等等) 但是我这个dialog弹框只有我自定义的1000多px的宽高 他只会展示我iframe网页的一部分…
阅读更多...
k8s之PV、PVC

k8s之PV、PVC

文章目录 k8s之PV、PVC一、存储卷1、存储卷定义2、存储卷的作用2.1 数据持久化2.2 数据共享2.3 解耦2.4 灵活性 3、存储卷的分类3.1 emptyDir存储卷3.1.1 定义3.1.2 特点3.1.3 用途3.1.4 示例 3.2 hostPath存储卷3.2.1 定义3.2.2 特点3.2.3 用途3.2.4 示例 3.3 NFS存储卷3.3.1 …
阅读更多...
BioVendor—sHLA-G ELISA试剂盒

BioVendor—sHLA-G ELISA试剂盒

人类白细胞抗原-G (HLA-G)与其他MHC类基因的不同之处在于它的低多态性和产生七种HLA-G蛋白的选择性剪接&#xff0c;这些蛋白的组织分布局限于正常的胎儿和成人组织&#xff0c;这些组织对先天和后天免疫细胞都具有耐受性。可溶性HLA-G是一种免疫抑制分子&#xff0c;诱导活化的…
阅读更多...
PyQt5创建与MySQL数据库集成的应用程序

PyQt5创建与MySQL数据库集成的应用程序

最近&#xff0c;对之前的mysql管理系统进行了更新升级&#xff0c;制作了一版关于车牌的管理系统&#xff01; &#xff08;1&#xff09;实现了对车牌和用户基本信息的增删改查的功能 &#xff01; &#xff08;2&#xff09;加入了对数据库的刷新和状态显示功能 &#xff…
阅读更多...
Python3位运算符

Python3位运算符

前言 本文介绍的是位运算符&#xff0c;位运算可以理解成对二进制数字上的每一个位进行操作的运算&#xff0c;位运算分为 布尔位运算符 和 移位位运算符。 文章目录 前言一、位运算概览1、布尔位运算符1&#xff09;按位与运算符 ( & )2&#xff09;按位或运算符 ( | )3…
阅读更多...
一款C#开源、简单、免费的屏幕录制和GIF动画制作神器

一款C#开源、简单、免费的屏幕录制和GIF动画制作神器

前言 今天要给大家推荐一款由C#语言开发且开源的操作简单、免费的屏幕录制和GIF动画制作神器&#xff1a;ScreenToGif 。 工具介绍 ScreenToGif 是一款免费的开源屏幕录制和GIF 制作工具。它可以帮助用户捕捉计算机屏幕上的实时动画&#xff0c;并将其保存为高质量的 GIF 图像…
阅读更多...
【鸟哥】Linux笔记-硬件搭配

【鸟哥】Linux笔记-硬件搭配

在Linux这个系统当中&#xff0c;几乎所有的硬件设备文件都在/dev这个目录内。打印机与软盘呢&#xff1f;分别是/dev/lp0, /dev/fd0。 几个常见的设备与其在Linux当中的文件名&#xff1a; 如果你的机器使用的是跟网际网络供应商 &#xff08;ISP&#xff09; 申请使用的云端…
阅读更多...
Unity【入门】脚本基础

Unity【入门】脚本基础

Unity脚本基础 文章目录 1、脚本基本规则1、创建规则2、MonoBehavior基类3、不继承MonoBehavior的类4、执行的先后顺序5、默认脚本内容 2、生命周期函数1、概念2、生命周期函数有哪些3、生命周期函数支持继承多态 3、Inspector窗口可编辑的变量4、Mono中的重要内容1、重要成员2…
阅读更多...
thinkphp6 queue队列的maxTries自定义

thinkphp6 queue队列的maxTries自定义

前景需求&#xff1a;在我们用队列的时候发现maxtries的个数时255次&#xff0c;这个太影响其他队列任务 我目前使用的thinkphp版本是6.1 第一部定义一个新的类 CustomDataBase&#xff08;我用的mysql数据库存放的队列&#xff09; 重写__make 和createPlainPayload方法 …
阅读更多...
第10周 企业认证、分布式事务，分布式锁方案落地

第10周 企业认证、分布式事务，分布式锁方案落地

第10周 企业认证、分布式事务&#xff0c;分布式锁方案落地 ********************************************************************************************** 本周我们将对企业入驻认证的流程进行落地&#xff0c;并且结合分布式缓存中间件Redis与Redisson进行相关的技术方…
阅读更多...
Easy RoCE：在SONiC交换机上一键启用无损以太网

Easy RoCE：在SONiC交换机上一键启用无损以太网

RDMA&#xff08;远程直接内存访问&#xff09;技术是一种绕过 CPU 或操作系统&#xff0c;在计算机之间直接传输内存数据的技术。它释放了内存带宽和 CPU&#xff0c;使节点之间的通信具有更低的延迟和更高的吞吐量。目前&#xff0c;RDMA 技术已广泛应用于高性能计算、人工智…
阅读更多...
web项目规范配置（husky、eslint、lint-staged、commit）

web项目规范配置（husky、eslint、lint-staged、commit）

背景&#xff1a; 团队开发为了保证提交代码格式统一&#xff0c;通常在进行代码提交的时候对暂存区代码进行校验&#xff0c;如没有通过eslint(本例使用eslint)校验&#xff0c;则不能提交到远端。 安装依赖 husky 、eslint 、prettier 、lint-staged npm install husky e…
阅读更多...
最新文章

Copyright @ 2022~2023