hexo blog

2021/09/15/SLAM工具/Eigen(二) 旋转，特征值，平移/index.html

@@ -49,7 +49,7 @@ <h2 id="计算特征值与特征向量"><a href="#计算特征值与特征向量
 <figure class="highlight cpp"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line"><span class="meta">#<span class="meta-keyword">include</span> <span class="meta-string">&lt;Eigen/Eigenvalues&gt;</span></span></span><br><span class="line"></span><br><span class="line">A &lt;&lt; <span class="number">2</span>,<span class="number">-2</span>,<span class="number">0</span>, <span class="number">-2</span>,<span class="number">1</span>,<span class="number">-2</span>, <span class="number">0</span>,<span class="number">-2</span>,<span class="number">0</span>;</span><br><span class="line"><span class="function">EigenSolver&lt;Matrix3d&gt; <span class="title">eigensolver</span><span class="params">(A)</span></span>;</span><br><span class="line"><span class="keyword">if</span> (eigensolver.<span class="built_in">info</span>() == Success)</span><br><span class="line">&#123;</span><br><span class="line">    cout &lt;&lt; eigensolver.<span class="built_in">eigenvalues</span>() &lt;&lt; endl&lt;&lt;endl;</span><br><span class="line">    cout &lt;&lt; eigensolver.<span class="built_in">eigenvectors</span>() &lt;&lt; endl;</span><br><span class="line">&#125;</span><br></pre></td></tr></table></figure>
 <p>输出结果并不是我们平时熟悉的形式，而是<br><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">(4,0)</span><br><span class="line">(1,0)</span><br><span class="line">(-2,0)</span><br><span class="line"></span><br><span class="line">(-0.666667,0) (-0.666667,0) (-0.333333,0)</span><br><span class="line">(0.666667,0) (-0.333333,0) (-0.666667,0)</span><br><span class="line">(-0.333333,0)  (0.666667,0) (-0.666667,0)</span><br></pre></td></tr></table></figure><br>实际特征值是4,1,-2.   4对应的特征向量分别为<code>(-0.666667, -0.666667,-0.333333)</code></p>
 <p>参考：<br><a target="_blank" rel="noopener" href="https://blog.csdn.net/wokaowokaowokao12345/article/details/47375427">C++ Eigen库计算矩阵特征值及特征向量</a><br><a target="_blank" rel="noopener" href="http://eigen.tuxfamily.org/dox/AsciiQuickReference.txt">Eigen函数与Matlab对应关系</a></p>
-</div></article><div class="post-meta__tag-list"></div><nav id="pagination"><div class="prev-post pull-left"><a href="/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E4%B8%89)%20latched%20Stop%20Rotate/"><i class="fa fa-chevron-left">  </i><span>DWA算法(三) latched Stop Rotate</span></a></div><div class="next-post pull-right"><a href="/2021/09/14/ROS/ROS%20Kinetic%E7%9F%A5%E8%AF%86/%E5%BD%BB%E5%BA%95%E8%A7%A3%E5%86%B3%20sudo%20rosdep%20init%20%E5%92%8C%20rosdep%20update/"><span>彻底解决 sudo rosdep init 和 rosdep update失败问题</span><i class="fa fa-chevron-right"></i></a></div></nav><div id="disqus_thread"></div><script>var unused = null;
+</div></article><div class="post-meta__tag-list"></div><nav id="pagination"><div class="prev-post pull-left"><a href="/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E4%BA%8C)%20dwa_ros%20%E6%BA%90%E7%A0%81/"><i class="fa fa-chevron-left">  </i><span>DWA算法(二) dwa_ros 源码</span></a></div><div class="next-post pull-right"><a href="/2021/09/14/ROS/ROS%20Kinetic%E7%9F%A5%E8%AF%86/%E5%BD%BB%E5%BA%95%E8%A7%A3%E5%86%B3%20sudo%20rosdep%20init%20%E5%92%8C%20rosdep%20update/"><span>彻底解决 sudo rosdep init 和 rosdep update失败问题</span><i class="fa fa-chevron-right"></i></a></div></nav><div id="disqus_thread"></div><script>var unused = null;
 var disqus_config = function () {
   this.page.url = 'https://charon-cheung.github.io/2021/09/15/SLAM%E5%B7%A5%E5%85%B7/Eigen(%E4%BA%8C)%20%E6%97%8B%E8%BD%AC%EF%BC%8C%E7%89%B9%E5%BE%81%E5%80%BC%EF%BC%8C%E5%B9%B3%E7%A7%BB/';
   this.page.identifier = '2021/09/15/SLAM工具/Eigen(二) 旋转，特征值，平移/';

2021/09/20/路径规划/DWA算法/DWA算法(三) latched Stop Rotate/index.html

@@ -15,7 +15,7 @@ <h2 id="isPositionReached"><a href="#isPositionReached" class="headerlink" title
 <p>注意<code>isPositionReached</code>只是判断，没有发布零速度，没有停止</p>
 <h2 id="computeVelocityCommandsStopRotate"><a href="#computeVelocityCommandsStopRotate" class="headerlink" title="computeVelocityCommandsStopRotate"></a>computeVelocityCommandsStopRotate</h2><p>机器人到达位置时，停止平移，只做旋转<br><figure class="highlight cpp"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">bool</span> <span class="title">LatchedStopRotateController::computeVelocityCommandsStopRotate</span><span class="params">(</span></span></span><br><span class="line"><span class="params"><span class="function">	geometry_msgs::Twist&amp; cmd_vel,</span></span></span><br><span class="line"><span class="params"><span class="function">    Eigen::Vector3f acc_lim,</span></span></span><br><span class="line"><span class="params"><span class="function">    <span class="keyword">double</span> sim_period,</span></span></span><br><span class="line"><span class="params"><span class="function">    LocalPlannerUtil* planner_util,</span></span></span><br><span class="line"><span class="params"><span class="function">    OdometryHelperRos&amp; odom_helper_,</span></span></span><br><span class="line"><span class="params"><span class="function">    tf::Stamped&lt;tf::Pose&gt; global_pose,</span></span></span><br><span class="line"><span class="params"><span class="function">    <span class="comment">// obstacle_check 在调用里是 DWAPlanner::checkTrajectory</span></span></span></span><br><span class="line"><span class="params"><span class="function">    boost::function&lt;<span class="keyword">bool</span> (Eigen::Vector3f pos,</span></span></span><br><span class="line"><span class="params"><span class="function">                          Eigen::Vector3f vel,</span></span></span><br><span class="line"><span class="params"><span class="function">                          Eigen::Vector3f vel_samples)&gt; obstacle_check)</span></span></span><br><span class="line"><span class="function"></span>&#123;</span><br><span class="line">  <span class="comment">// global goal is the last point in the global plan</span></span><br><span class="line">  tf::Stamped&lt;tf::Pose&gt; goal_pose;</span><br><span class="line">  <span class="keyword">if</span> ( ! planner_util-&gt;<span class="built_in">getGoal</span>(goal_pose) )</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="built_in">ROS_ERROR</span>(<span class="string">&quot;Could not get goal pose&quot;</span>);</span><br><span class="line">    <span class="keyword">return</span> <span class="literal">false</span>;</span><br><span class="line">  &#125;</span><br><span class="line">  base_local_planner::LocalPlannerLimits limits = planner_util-&gt;<span class="built_in">getCurrentLimits</span>();</span><br><span class="line">  <span class="comment">// latch goal tolerance, if we ever reach the goal location, </span></span><br><span class="line">  <span class="comment">// we&#x27;ll  just rotate in place</span></span><br><span class="line">  <span class="comment">// 这里就是latched机制了，在第5篇再详细解释。这里先不考虑latch，看下面的情况。</span></span><br><span class="line">  <span class="keyword">if</span> (latch_xy_goal_tolerance_ &amp;&amp; ! xy_tolerance_latch_ )</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="built_in">ROS_INFO</span>(<span class="string">&quot;Goal position reached, stopping and turning in place&quot;</span>);</span><br><span class="line">    xy_tolerance_latch_ = <span class="literal">true</span>;</span><br><span class="line">  &#125;</span><br><span class="line">  <span class="comment">// 检查是否达到了 goal orientation</span></span><br><span class="line">  <span class="keyword">double</span> goal_th = tf::<span class="built_in">getYaw</span>(goal_pose.<span class="built_in">getRotation</span>());</span><br><span class="line">  <span class="keyword">double</span> angle = base_local_planner::<span class="built_in">getGoalOrientationAngleDifference</span>(global_pose, goal_th);</span><br><span class="line">  <span class="keyword">if</span> (<span class="built_in">fabs</span>(angle) &lt;= limits.yaw_goal_tolerance)</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="comment">// 到达， 停止转动</span></span><br><span class="line">    cmd_vel.linear.x = <span class="number">0.0</span>;</span><br><span class="line">    cmd_vel.linear.y = <span class="number">0.0</span>;</span><br><span class="line">    cmd_vel.angular.z = <span class="number">0.0</span>;</span><br><span class="line">    rotating_to_goal_ = <span class="literal">false</span>;</span><br><span class="line">  &#125;</span><br></pre></td></tr></table></figure><br>如果发布了零速度，会直接运行到最后的<code>return true</code></p>
 <p>最后看角度没有到导航误差范围的情况，比较复杂<br><figure class="highlight cpp"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br></pre></td><td class="code"><pre><span class="line"><span class="keyword">else</span> </span><br><span class="line">&#123;</span><br><span class="line">    tf::Stamped&lt;tf::Pose&gt; robot_vel;</span><br><span class="line">    odom_helper_.<span class="built_in">getRobotVel</span>(robot_vel);</span><br><span class="line">    nav_msgs::Odometry base_odom;</span><br><span class="line">    odom_helper_.<span class="built_in">getOdom</span>(base_odom);</span><br><span class="line">    <span class="comment">// if we&#x27;re not stopped yet... we want to stop... </span></span><br><span class="line">    <span class="comment">// taking into account the acceleration limits of the robot</span></span><br><span class="line"></span><br><span class="line">    <span class="comment">/*  这里是机器人在继续运动的情况</span></span><br><span class="line"><span class="comment">     rotating_to_goal_  在构造函数里是 false</span></span><br><span class="line"><span class="comment">      stopped函数： 如果里程计的角速度小于 rot_stopped_vel，且线速度小于 </span></span><br><span class="line"><span class="comment">    trans_stopped_vel， 则返回 true，认为已经停止 */</span></span><br><span class="line">    <span class="keyword">if</span> ( ! rotating_to_goal_ &amp;&amp; !base_local_planner::<span class="built_in">stopped</span>(</span><br><span class="line">    	 base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)  )</span><br><span class="line">    &#123;</span><br><span class="line">      <span class="keyword">if</span> ( ! <span class="built_in">stopWithAccLimits</span>(</span><br><span class="line">          global_pose,</span><br><span class="line">          robot_vel,</span><br><span class="line">          cmd_vel,</span><br><span class="line">          acc_lim,</span><br><span class="line">          sim_period,</span><br><span class="line">          obstacle_check) )</span><br><span class="line">      &#123;</span><br><span class="line">        <span class="built_in">ROS_INFO</span>(<span class="string">&quot;Error when stopping.&quot;</span>);</span><br><span class="line">        <span class="keyword">return</span> <span class="literal">false</span>;</span><br><span class="line">      &#125;</span><br><span class="line">      <span class="built_in">ROS_DEBUG</span>(<span class="string">&quot;Stopping...&quot;</span>);</span><br><span class="line">    &#125;</span><br><span class="line">    <span class="comment">//if we&#x27;re stopped... then we want to rotate to goal</span></span><br><span class="line">    <span class="keyword">else</span></span><br><span class="line">    &#123;</span><br><span class="line">      <span class="comment">//set this so that we know its OK to be moving</span></span><br><span class="line">      rotating_to_goal_ = <span class="literal">true</span>;</span><br><span class="line">      <span class="keyword">if</span> ( ! <span class="built_in">rotateToGoal</span>(</span><br><span class="line">          global_pose,</span><br><span class="line">          robot_vel,</span><br><span class="line">          goal_th,</span><br><span class="line">          cmd_vel,</span><br><span class="line">          acc_lim,</span><br><span class="line">          sim_period,</span><br><span class="line">          limits,</span><br><span class="line">          obstacle_check)  )</span><br><span class="line">      &#123;</span><br><span class="line">        <span class="built_in">ROS_INFO</span>(<span class="string">&quot;Error when rotating.&quot;</span>);</span><br><span class="line">        <span class="keyword">return</span> <span class="literal">false</span>;</span><br><span class="line">      &#125;</span><br><span class="line">      <span class="built_in">ROS_DEBUG</span>(<span class="string">&quot;Rotating...&quot;</span>);</span><br><span class="line">    &#125;</span><br><span class="line">  &#125;</span><br><span class="line">  <span class="keyword">return</span> <span class="literal">true</span>;</span><br><span class="line">&#125;</span><br></pre></td></tr></table></figure></p>
-</div></article><div class="post-meta__tag-list"></div><nav id="pagination"><div class="prev-post pull-left"><a href="/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E4%BA%8C)%20dwa_ros%20%E6%BA%90%E7%A0%81/"><i class="fa fa-chevron-left">  </i><span>DWA算法(二) dwa_ros 源码</span></a></div><div class="next-post pull-right"><a href="/2021/09/15/SLAM%E5%B7%A5%E5%85%B7/Eigen(%E4%BA%8C)%20%E6%97%8B%E8%BD%AC%EF%BC%8C%E7%89%B9%E5%BE%81%E5%80%BC%EF%BC%8C%E5%B9%B3%E7%A7%BB/"><span>Eigen(二) 旋转，平移，特征值</span><i class="fa fa-chevron-right"></i></a></div></nav><div id="disqus_thread"></div><script>var unused = null;
+</div></article><div class="post-meta__tag-list"></div><nav id="pagination"><div class="prev-post pull-left"><a href="/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E5%9B%9B)%20dwaComputeVelocityCommands/"><i class="fa fa-chevron-left">  </i><span>DWA算法(四) dwaComputeVelocityCommands</span></a></div><div class="next-post pull-right"><a href="/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E4%BA%8C)%20dwa_ros%20%E6%BA%90%E7%A0%81/"><span>DWA算法(二) dwa_ros 源码</span><i class="fa fa-chevron-right"></i></a></div></nav><div id="disqus_thread"></div><script>var unused = null;
 var disqus_config = function () {
   this.page.url = 'https://charon-cheung.github.io/2021/09/20/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/DWA%E7%AE%97%E6%B3%95/DWA%E7%AE%97%E6%B3%95(%E4%B8%89)%20latched%20Stop%20Rotate/';
   this.page.identifier = '2021/09/20/路径规划/DWA算法/DWA算法(三) latched Stop Rotate/';