From 913ee271e9dec08e93a3892b52f2a500d3589617 Mon Sep 17 00:00:00 2001 From: charon-cheung Date: Tue, 21 May 2024 20:36:51 +0800 Subject: [PATCH] hexo blog --- .../index.html" | 2 +- .../index.html" | 2 +- .../index.html" | 2 +- .../index.html" | 2 +- .../index.html" | 26 ++++++++++++++++--- archives/2021/09/index.html | 2 +- archives/2021/page/4/index.html | 2 +- archives/page/26/index.html | 2 +- .../DWA\347\256\227\346\263\225/index.html" | 2 +- .../page/7/index.html" | 2 +- index.html | 24 +++++++++++++++-- page/26/index.html | 18 ++++++------- 12 files changed, 63 insertions(+), 23 deletions(-) diff --git "a/2021/09/15/SLAM\345\267\245\345\205\267/Eigen(\344\272\214) \346\227\213\350\275\254\357\274\214\347\211\271\345\276\201\345\200\274\357\274\214\345\271\263\347\247\273/index.html" "b/2021/09/15/SLAM\345\267\245\345\205\267/Eigen(\344\272\214) \346\227\213\350\275\254\357\274\214\347\211\271\345\276\201\345\200\274\357\274\214\345\271\263\347\247\273/index.html" index 066e733d7a..a5258ae55f 100644 --- "a/2021/09/15/SLAM\345\267\245\345\205\267/Eigen(\344\272\214) \346\227\213\350\275\254\357\274\214\347\211\271\345\276\201\345\200\274\357\274\214\345\271\263\347\247\273/index.html" +++ "b/2021/09/15/SLAM\345\267\245\345\205\267/Eigen(\344\272\214) \346\227\213\350\275\254\357\274\214\347\211\271\345\276\201\345\200\274\357\274\214\345\271\263\347\247\273/index.html" @@ -49,7 +49,7 @@ 

1
2
3
4
5
6
7
8
9
#include <Eigen/Eigenvalues>

A << 2,-2,0, -2,1,-2, 0,-2,0;
EigenSolver<Matrix3d> eigensolver(A);
if (eigensolver.info() == Success)
{
    cout << eigensolver.eigenvalues() << endl<<endl;
    cout << eigensolver.eigenvectors() << endl;
}

输出结果并不是我们平时熟悉的形式，而是

1
2
3
4
5
6
7
(4,0)
(1,0)
(-2,0)

(-0.666667,0) (-0.666667,0) (-0.333333,0)
(0.666667,0) (-0.333333,0) (-0.666667,0)
(-0.333333,0)  (0.666667,0) (-0.666667,0)

实际特征值是4,1,-2. 4对应的特征向量分别为(-0.666667, -0.666667,-0.333333)

参考：
C++ Eigen库计算矩阵特征值及特征向量
Eigen函数与Matlab对应关系

-
沉默杀手HomeArchivesTagsCategories
DWA算法(四) dwaComputeVelocityCommands
|路径规划DWA算法|
Word count: 412|Reading time: 2 min

接上一篇，分析如果还没到x y误差范围内，进行DWA规划的情况

1
2
3
4
5
6
7
8
9
10
11
12
13
14
else
{
  // 是否规划成功
  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;

关键就是dwaComputeVelocityCommands

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
bool DWAPlannerROS::dwaComputeVelocityCommands(
	tf::Stamped<tf::Pose> &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;
    }

    tf::Stamped<tf::Pose> 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
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    
    // call with updated footprint
    base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
    //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

    /* For timing uncomment
    gettimeofday(&end, NULL);
    start_t = start.tv_sec + double(start.tv_usec) / 1e6;
    end_t = end.tv_sec + double(end.tv_usec) / 1e6;
    t_diff = end_t - start_t;
    ROS_INFO("Cycle time: %.9f", t_diff);
    */

    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    //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);

-
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
    // Fill out the local plan
    for(unsigned int i = 0; i < path.getPointsSize(); ++i)
    {
      double p_x, p_y, p_th;
      path.getPoint(i, p_x, p_y, p_th);

      tf::Stamped<tf::Pose> p =
              tf::Stamped<tf::Pose>(tf::Pose(
                      tf::createQuaternionFromYaw(p_th),
                      tf::Point(p_x, p_y, 0.0)),
                      ros::Time::now(),
                      costmap_ros_->getGlobalFrameID());
      geometry_msgs::PoseStamped pose;
      tf::poseStampedTFToMsg(p, pose);
      local_plan.push_back(pose);
    }

    //publish information to the visualizer
    publishLocalPlan(local_plan);
    return true;
}
沉默杀手HomeArchivesTagsCategories
日志的格式输出
|C++C++ 基础|
Word count: 10|Reading time: 1 min
+}
沉默杀手HomeArchivesTagsCategories
日志的格式输出
|C++C++ 基础|
Word count: 20|Reading time: 1 min
@@ -17,16 +17,36 @@ + + + + + + + + + + + + - - + + + + + + + + + +
unit16_t%hu
unit32_t%u
unit64_t %llu
unit32_t%zu
unit32_t %u
unit16_t%huunit32_t%u
unit32_t%u
unit32_t%u
diff --git a/archives/2021/09/index.html b/archives/2021/09/index.html index 386acacae3..cb5edbf54a 100644 --- a/archives/2021/09/index.html +++ b/archives/2021/09/index.html @@ -7,7 +7,7 @@ error: 'Copy error', noSupport: 'The browser does not support' } -}
\ No newline at end of file diff --git a/archives/2021/page/4/index.html b/archives/2021/page/4/index.html index c274ce735a..3b5b18ceaf 100644 --- a/archives/2021/page/4/index.html +++ b/archives/2021/page/4/index.html @@ -7,7 +7,7 @@ error: 'Copy error', noSupport: 'The browser does not support' } -}
\ No newline at end of file diff --git a/archives/page/26/index.html b/archives/page/26/index.html index b172ba7a11..9cdadc2a24 100644 --- a/archives/page/26/index.html +++ b/archives/page/26/index.html @@ -7,7 +7,7 @@ error: 'Copy error', noSupport: 'The browser does not support' } -}
\ No newline at end of file diff --git "a/categories/\350\267\257\345\276\204\350\247\204\345\210\222/DWA\347\256\227\346\263\225/index.html" "b/categories/\350\267\257\345\276\204\350\247\204\345\210\222/DWA\347\256\227\346\263\225/index.html" index faadaca77b..21d19a8b4a 100644 --- "a/categories/\350\267\257\345\276\204\350\247\204\345\210\222/DWA\347\256\227\346\263\225/index.html" +++ "b/categories/\350\267\257\345\276\204\350\247\204\345\210\222/DWA\347\256\227\346\263\225/index.html" @@ -7,7 +7,7 @@ error: 'Copy error', noSupport: 'The browser does not support' } -}
\ No newline at end of file diff --git "a/categories/\350\267\257\345\276\204\350\247\204\345\210\222/page/7/index.html" "b/categories/\350\267\257\345\276\204\350\247\204\345\210\222/page/7/index.html" index 338de2f21d..881efb5fe2 100644 --- "a/categories/\350\267\257\345\276\204\350\247\204\345\210\222/page/7/index.html" +++ "b/categories/\350\267\257\345\276\204\350\247\204\345\210\222/page/7/index.html" @@ -7,7 +7,7 @@ error: 'Copy error', noSupport: 'The browser does not support' } -}
\ No newline at end of file diff --git a/index.html b/index.html index 562305a476..9f4967da66 100644 --- a/index.html +++ b/index.html @@ -25,16 +25,36 @@

unit16_t +%hu + + +unit32_t +%u + + unit64_t %llu unit32_t +%zu + + +unit32_t %u -unit16_t -%hu +unit32_t +%u + + +unit32_t +%u + + +unit32_t +%u diff --git a/page/26/index.html b/page/26/index.html index f8af88b97c..bcc31a5388 100644 --- a/page/26/index.html +++ b/page/26/index.html @@ -9,7 +9,15 @@ } }
DWA算法(四) dwaComputeVelocityCommands|路径规划DWA算法

接上一篇，分析如果还没到x y误差范围内，进行DWA规划的情况

1
2
3
4
5
6
7
8
9
10
11
12
13
14
else
{
  // 是否规划成功
  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;

关键就是dwaComputeVelocityCommands

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
bool DWAPlannerROS::dwaComputeVelocityCommands(
	tf::Stamped<tf::Pose> &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;
    }

    tf::Stamped<tf::Pose> 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
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    
    // call with updated footprint
    base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
    //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

    /* For timing uncomment
    gettimeofday(&end, NULL);
    start_t = start.tv_sec + double(start.tv_usec) / 1e6;
    end_t = end.tv_sec + double(end.tv_usec) / 1e6;
    t_diff = end_t - start_t;
    ROS_INFO("Cycle time: %.9f", t_diff);
    */

    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    //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);

-
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
    // Fill out the local plan
    for(unsigned int i = 0; i < path.getPointsSize(); ++i)
    {
      double p_x, p_y, p_th;
      path.getPoint(i, p_x, p_y, p_th);

      tf::Stamped<tf::Pose> p =
              tf::Stamped<tf::Pose>(tf::Pose(
                      tf::createQuaternionFromYaw(p_th),
                      tf::Point(p_x, p_y, 0.0)),
                      ros::Time::now(),
                      costmap_ros_->getGlobalFrameID());
      geometry_msgs::PoseStamped pose;
      tf::poseStampedTFToMsg(p, pose);
      local_plan.push_back(pose);
    }

    //publish information to the visualizer
    publishLocalPlan(local_plan);
    return true;
}
DWA算法(二) dwa_ros 源码|路径规划DWA算法

我们需要看的是DWAPlannerROS类的源码，继承自nav_core::BaseLocalPlanner。 但是它还用到了base_local_planner的LatchedStopRotateController模块，用于latch机制和到达位置后转向。

+
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
    // Fill out the local plan
    for(unsigned int i = 0; i < path.getPointsSize(); ++i)
    {
      double p_x, p_y, p_th;
      path.getPoint(i, p_x, p_y, p_th);

      tf::Stamped<tf::Pose> p =
              tf::Stamped<tf::Pose>(tf::Pose(
                      tf::createQuaternionFromYaw(p_th),
                      tf::Point(p_x, p_y, 0.0)),
                      ros::Time::now(),
                      costmap_ros_->getGlobalFrameID());
      geometry_msgs::PoseStamped pose;
      tf::poseStampedTFToMsg(p, pose);
      local_plan.push_back(pose);
    }

    //publish information to the visualizer
    publishLocalPlan(local_plan);
    return true;
}
DWA算法(三) latched Stop Rotate|路径规划DWA算法

接着上一篇，继续DWAPlannerROS::computeVelocityCommands

+

如果x y坐标已经在目标的距离误差范围内

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
if (latchedStopRotateController_.isPositionReached(
      &planner_util_, current_pose_)  )
{
  // 发布空的路径，这段可以删除
  std::vector<geometry_msgs::PoseStamped> local_plan;
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  publishGlobalPlan(transformed_plan);
  publishLocalPlan(local_plan);
  // 这个类保存了所有 DWA参数，只有一个getAccLimits函数
  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)  );
}

latchedStopRotateController_ 是 base_local_planner::LatchedStopRotateController，它用到一个参数latch_xy_goal_tolerance

+

isPositionReached

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/**
 * returns true if we have passed the goal position.
 * Meaning we might have overshot on the position beyond tolerance, yet still return true.
 * Also goal orientation might not yet be true
 */
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
    tf::Stamped<tf::Pose> global_pose)
{
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;

  // 认为 global goal 是 global plan的最后一个点
  // 显然是把全局路径的最后一个点赋值给 goal_pose
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose))
  {
    return false;
  }
  double goal_x = goal_pose.getOrigin().getX();
  double goal_y = goal_pose.getOrigin().getY();

  // check to see if we've reached the goal position
  if (  (latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
      base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance)
  {
    xy_tolerance_latch_ = true;
    return true;
  }
  return false;
}
+

这里有两种情况可以出现 if(true)， getGoalPositionDistance不用看也知道是求 global_pose 和 goal的空间距离，再跟参数xy_goal_tolerance比较。

+

latch_xy_goal_tolerance_本意是在LatchedStopRotateController构造函数里获取设置的参数，但是源码在这里出现了bug，应当是命名空间导致的。在DWA参数里设置其为true没有效果，从参数服务器看到的确实是true，但是在源码里的latch_xy_goal_tolerance_还是默认的false. 可以直接修改构造函数的设置值。 剩下的关键就是变量xy_tolerance_latch_了，有一个关键函数void resetLatching() { xy_tolerance_latch_ = false; }。 如何实现这一情况，在下面函数。

+

注意isPositionReached只是判断，没有发布零速度，没有停止

+

computeVelocityCommandsStopRotate

机器人到达位置时，停止平移，只做旋转

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
bool LatchedStopRotateController::computeVelocityCommandsStopRotate(
	geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper_,
    tf::Stamped<tf::Pose> global_pose,
    // obstacle_check 在调用里是 DWAPlanner::checkTrajectory
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check)
{
  // global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose) )
  {
    ROS_ERROR("Could not get goal pose");
    return false;
  }
  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
  // latch goal tolerance, if we ever reach the goal location, 
  // we'll  just rotate in place
  // 这里就是latched机制了，在第5篇再详细解释。这里先不考虑latch，看下面的情况。
  if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ )
  {
    ROS_INFO("Goal position reached, stopping and turning in place");
    xy_tolerance_latch_ = true;
  }
  // 检查是否达到了 goal orientation
  double goal_th = tf::getYaw(goal_pose.getRotation());
  double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
  if (fabs(angle) <= limits.yaw_goal_tolerance)
  {
    // 到达， 停止转动
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    rotating_to_goal_ = false;
  }

如果发布了零速度，会直接运行到最后的return true

+

最后看角度没有到导航误差范围的情况，比较复杂

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
else 
{
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);
    nav_msgs::Odometry base_odom;
    odom_helper_.getOdom(base_odom);
    // if we're not stopped yet... we want to stop... 
    // taking into account the acceleration limits of the robot

    /*  这里是机器人在继续运动的情况
     rotating_to_goal_  在构造函数里是 false
      stopped函数： 如果里程计的角速度小于 rot_stopped_vel，且线速度小于 
    trans_stopped_vel， 则返回 true，认为已经停止 */
    if ( ! rotating_to_goal_ && !base_local_planner::stopped(
    	 base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)  )
    {
      if ( ! stopWithAccLimits(
          global_pose,
          robot_vel,
          cmd_vel,
          acc_lim,
          sim_period,
          obstacle_check) )
      {
        ROS_INFO("Error when stopping.");
        return false;
      }
      ROS_DEBUG("Stopping...");
    }
    //if we're stopped... then we want to rotate to goal
    else
    {
      //set this so that we know its OK to be moving
      rotating_to_goal_ = true;
      if ( ! rotateToGoal(
          global_pose,
          robot_vel,
          goal_th,
          cmd_vel,
          acc_lim,
          sim_period,
          limits,
          obstacle_check)  )
      {
        ROS_INFO("Error when rotating.");
        return false;
      }
      ROS_DEBUG("Rotating...");
    }
  }
  return true;
}

+
DWA算法(二) dwa_ros 源码|路径规划DWA算法

我们需要看的是DWAPlannerROS类的源码，继承自nav_core::BaseLocalPlanner。 但是它还用到了base_local_planner的LatchedStopRotateController模块，用于latch机制和到达位置后转向。

initialize

原型是void DWAPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)

其实主要是if( !isInitialized())的部分，直接看里面的内容

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
// 在局部代价地图中的位姿
costmap_ros_->getRobotPose(current_pose_);

// 局部代价地图
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
// base_local_planner::LocalPlannerUtil
// 其实就是把三个参数赋值给 LocalPlannerUtil 的三个成员变量
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

//这里创建了dwa的共享指针，也就是  boost::shared_ptr<DWAPlanner> dp_;
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));

if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
  odom_helper_.setOdomTopic( odom_topic_ );
}

initialized_ = true;

// Warn about deprecated parameters -- remove this block in N-turtle
// 这些不必关注
nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");
nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");
nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");
nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");
// 动态调整
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
@@ -20,14 +28,6 @@

updatePlanAndLocalCosts

1
2
3
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_,  transformed_plan, 
     costmap_ros_->getRobotFootprint()  );
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
void DWAPlanner::updatePlanAndLocalCosts(
      const geometry_msgs::PoseStamped&  global_pose,
      const std::vector<geometry_msgs::PoseStamped>&  new_plan,
      const std::vector<geometry_msgs::Point>& footprint_spec)
{
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i)
    {
      global_plan_[i] = new_plan[i];
    }

    obstacle_costs_.setFootprint(footprint_spec);

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) 
                                             +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);

    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
   // keeping the nose on the path
  if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_)
  {
    alignment_costs_.setScale(path_distance_bias_);
    // costs for robot being aligned with path (nose on path, not ju
    alignment_costs_.setTargetPoses(global_plan_);
  }
  else
  {
    // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
    alignment_costs_.setScale(0.0);
  }
}
-
DWA算法(三) latched Stop Rotate|路径规划DWA算法

接着上一篇，继续DWAPlannerROS::computeVelocityCommands

-

如果x y坐标已经在目标的距离误差范围内

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
if (latchedStopRotateController_.isPositionReached(
      &planner_util_, current_pose_)  )
{
  // 发布空的路径，这段可以删除
  std::vector<geometry_msgs::PoseStamped> local_plan;
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  publishGlobalPlan(transformed_plan);
  publishLocalPlan(local_plan);
  // 这个类保存了所有 DWA参数，只有一个getAccLimits函数
  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)  );
}

latchedStopRotateController_ 是 base_local_planner::LatchedStopRotateController，它用到一个参数latch_xy_goal_tolerance

-

isPositionReached

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/**
 * returns true if we have passed the goal position.
 * Meaning we might have overshot on the position beyond tolerance, yet still return true.
 * Also goal orientation might not yet be true
 */
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
    tf::Stamped<tf::Pose> global_pose)
{
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;

  // 认为 global goal 是 global plan的最后一个点
  // 显然是把全局路径的最后一个点赋值给 goal_pose
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose))
  {
    return false;
  }
  double goal_x = goal_pose.getOrigin().getX();
  double goal_y = goal_pose.getOrigin().getY();

  // check to see if we've reached the goal position
  if (  (latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
      base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance)
  {
    xy_tolerance_latch_ = true;
    return true;
  }
  return false;
}
-

这里有两种情况可以出现 if(true)， getGoalPositionDistance不用看也知道是求 global_pose 和 goal的空间距离，再跟参数xy_goal_tolerance比较。

-

latch_xy_goal_tolerance_本意是在LatchedStopRotateController构造函数里获取设置的参数，但是源码在这里出现了bug，应当是命名空间导致的。在DWA参数里设置其为true没有效果，从参数服务器看到的确实是true，但是在源码里的latch_xy_goal_tolerance_还是默认的false. 可以直接修改构造函数的设置值。 剩下的关键就是变量xy_tolerance_latch_了，有一个关键函数void resetLatching() { xy_tolerance_latch_ = false; }。 如何实现这一情况，在下面函数。

-

注意isPositionReached只是判断，没有发布零速度，没有停止

-

computeVelocityCommandsStopRotate

机器人到达位置时，停止平移，只做旋转

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
bool LatchedStopRotateController::computeVelocityCommandsStopRotate(
	geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper_,
    tf::Stamped<tf::Pose> global_pose,
    // obstacle_check 在调用里是 DWAPlanner::checkTrajectory
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check)
{
  // global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose) )
  {
    ROS_ERROR("Could not get goal pose");
    return false;
  }
  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
  // latch goal tolerance, if we ever reach the goal location, 
  // we'll  just rotate in place
  // 这里就是latched机制了，在第5篇再详细解释。这里先不考虑latch，看下面的情况。
  if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ )
  {
    ROS_INFO("Goal position reached, stopping and turning in place");
    xy_tolerance_latch_ = true;
  }
  // 检查是否达到了 goal orientation
  double goal_th = tf::getYaw(goal_pose.getRotation());
  double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
  if (fabs(angle) <= limits.yaw_goal_tolerance)
  {
    // 到达， 停止转动
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    rotating_to_goal_ = false;
  }

如果发布了零速度，会直接运行到最后的return true

-

最后看角度没有到导航误差范围的情况，比较复杂

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
else 
{
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);
    nav_msgs::Odometry base_odom;
    odom_helper_.getOdom(base_odom);
    // if we're not stopped yet... we want to stop... 
    // taking into account the acceleration limits of the robot

    /*  这里是机器人在继续运动的情况
     rotating_to_goal_  在构造函数里是 false
      stopped函数： 如果里程计的角速度小于 rot_stopped_vel，且线速度小于 
    trans_stopped_vel， 则返回 true，认为已经停止 */
    if ( ! rotating_to_goal_ && !base_local_planner::stopped(
    	 base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)  )
    {
      if ( ! stopWithAccLimits(
          global_pose,
          robot_vel,
          cmd_vel,
          acc_lim,
          sim_period,
          obstacle_check) )
      {
        ROS_INFO("Error when stopping.");
        return false;
      }
      ROS_DEBUG("Stopping...");
    }
    //if we're stopped... then we want to rotate to goal
    else
    {
      //set this so that we know its OK to be moving
      rotating_to_goal_ = true;
      if ( ! rotateToGoal(
          global_pose,
          robot_vel,
          goal_th,
          cmd_vel,
          acc_lim,
          sim_period,
          limits,
          obstacle_check)  )
      {
        ROS_INFO("Error when rotating.");
        return false;
      }
      ROS_DEBUG("Rotating...");
    }
  }
  return true;
}

Eigen(二) 旋转，平移，特征值|SLAM工具

旋转

有旋转向量、四元数和旋转矩阵三种表示方法，在Eigen里用前两种进行定义，这样参数少，可以转化为第三种。

牢记下面这张图，根据《视觉SLAM十四讲》总结：
三种方式的转换

使用的是模板类：class Eigen::AngleAxis< Scalar >，这被称为角轴表示法，顾名思义给定旋转角度和旋转轴即可。其创建方式符合Eigen的原则，传入数据类型作为模板参数。旋转角度以弧度表示，旋转轴为Vector类型的向量，注意向量必须要被归一化(vec.normalized()即可)。Eigen中也预先定义好了AngleAxisd和AngleAxisf两种方便使用。