机器人自转与eband_local_planner源码

eband_local_planner源码解读：

https://blog.csdn.net/WangQiang319670/article/details/78910828

获取机器人当前角度，参考rotate_recovery.cpp

ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);

  geometry_msgs::PoseStamped global_pose;
  local_costmap_->getRobotPose(global_pose);

  double current_angle = tf2::getYaw(global_pose.pose.orientation);
  // step 1 : 恢复前的位姿朝向
  double start_angle = current_angle;

angles.h 的用法

https://blog.csdn.net/u011608180/article/details/108962700

怎么控制自转rotate_recovery

    while (sim_angle < dist_left)
    {
      double theta = current_angle + sim_angle;
    
      // 确保这个位姿没有碰撞风险，不然会停止
      double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
      if (footprint_cost < 0.0)
      {
        ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f",
                  footprint_cost);
        return;
      }
    
      sim_angle += sim_granularity_;
    }
    
    // step 5: 计算下发速度，让机器人可以在到达终点时停止
    double vel = sqrt(2 * acc_lim_th_ * dist_left);
    
    // step 6: 确保下发旋转速度满足速度限制要求
    vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
    
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = vel;
    
    vel_pub.publish(cmd_vel);
    
    r.sleep();

  }
}

   #记录当前时间
        current_t = time.time()
        #记录开始时间
        start_t = current_t
        #执行时间结束前，循环执行
        while current_t-start_t<need_time:
            #在前置时间内，需要直行走过0.07米
            if current_t-start_t<prev_time:
                self.action = np.array([real_speed,0.0])
            else: #剩余的时间内，按计算出的线速度和角速度行驶
                self.action = np.array([real_speed, omega])
            #计算剩余时间占总执行时间的百分比
            feedback_msg.countdown = (1-(current_t-start_t)/need_time)*100
            #通过反馈主题发送给客户端
            goal_handle.publish_feedback(feedback_msg)
            #线程休眠0.1秒，继续循环
            time.sleep(0.1)
            #更新当前时间
            current_t = time.time()