控制kobuki底盘(二)

(一)开环控制小车的速度和角速度–使小车按简单函数轨迹移动.
坐标系的建立:差速底盘的正面为初始方向, 设初始方向与所建立坐标系的夹角θ.
θ=atan(f'(0))
(二)计算
1)匀速转向:由于发送topic需要以次数i计算.故在设置阻塞频率rate后大致可估算出w=θ/(i*T)=θ*rate/i;
w即为speed.angular.z;
2)计算轨迹对应的速度和角速度:思路是,角速度用积分;线速度用极限(保持扫过的面积相同花费同样的时间)得:
w(t)=-f”(t)/(1+[f’(t)]^2);
v(t)=sqrt(1+[f’(t)]^2);
(三)调整
补充启动转角:按上述公式计算忽略了初始方位,默认为刚好沿着曲线在t=0时的切线方向.实际上,这个初值需要额外给定.在形成路径之前应有 θ 大小的转角.
(四)实现

//此按键处理为非阻塞的
#include <ros/ros.h> //包含#include sting
#include <math.h>
#include <iostream>
#include <geometry_msgs/Twist.h>

#include "FeiDuSeExit.h"

#define PI 3.14


//延时等待 并用于 调整初始方向
void DelayForAdapt(ros::Publisher& CmdVelPub_ ,geometry_msgs::Twist& speed_, double angle_)
{
    int i(50);const static int rate_(10);
    ros::Rate loopRate(rate_);
    speed_.angular.z = angle_*rate_/i;//计算平均角速度

    while(i--)
    {
    CmdVelPub_.publish(speed_); //将速度发送给机器人
    loopRate.sleep();//按loopRate(n)设置的nHZ将程序挂起--实际上是堵塞进程
    ROS_INFO("test...%4d",i);
    }
    speed_.angular.z = 0;       //角速度清零
    CmdVelPub_.publish(speed_); //将速度发送给机器人
    ROS_INFO("Finish direction adapted");
} 


int main(int argc, char **argv)
{ 
  ROS_INFO("newkeyop cpp start...");
  ros::init(argc, argv, "newkeyop");//node name
  std::string topic = "/cmd_vel";   //topic name
  ros::NodeHandle n;
  ros::Publisher CmdVelPub=n.advertise<geometry_msgs::Twist>(topic, 10);

  geometry_msgs::Twist speed; 
  speed.linear.x =0.0;  // 初始化线速度，正为前进，负为后退
  speed.angular.z =0.0; // 初始化角速度，正为左转，负为右转

  std::string str_;
  while(str_!="adapt") std::cin>>str_;//暂停并等待用户输入adapt

  //调整初始方向
  double angle=atan(1);//atan(f'(0))
  DelayForAdapt(CmdVelPub,speed,angle);

  while(str_!="start") std::cin>>str_;//暂停并等待用户输入start 

  ros::Rate loopRate(10);
  while (feiDuSeExit())//ros::ok()监听ctrl+c
  { 
    //setbuf(stdin, NULL);
    static double T=0.0;
    if(T<PI*6)
    {
     T+=0.1;
     speed.linear.x=sqrt(1+cos(T)*cos(T)); f(t)=sin(t);
     //speed.linear.x =sqrt(1+4*T*T);       f(t)=t^2

     speed.angular.z=-sin(T)/(1+cos(T)*cos(T));f(t)=sin(t); 
     //speed.angular.z=2.0/(1+4*T*T);          f(t)=t^2
    }
    else
     {  
        speed.linear.x =0; 
        speed.angular.z =0;
     }   
    CmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人//ros::spinOnce();//用来接收消息
    loopRate.sleep();//按loopRate(rate)设置的频率将程序堵塞
 }
  speed.linear.x =0; 
  speed.angular.z =0;
  CmdVelPub.publish(speed);
  ROS_INFO("newkeyop cpp has finished...");
  return 0;
}