基于ROS平台的移动机器人-目录

说明

本系列博文将介绍基于ROS平台的移动机器人的搭建，包括小车的底盘的制作，控制和与上位机的通信，建图和导航。

目录

1. 基于ROS平台的移动机器人-1-小车底盘的搭建
2. 基于ROS平台的移动机器人-2-小车底盘控制
3. 基于ROS平台的移动机器人-3-小车底盘与ROS的通信
4. 基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动
5. 基于ROS平台的移动机器人-5-Kinect2驱动的安装和ROS下的测试
6. 基于ROS平台的移动机器人-6-使用Kinect2获取激光数据
7. 基于ROS平台的移动机器人-7-使用Kinect2建图
8. 基于ROS平台的移动机器人-8-使用Kinect2导航

注意

本人Ubuntu版本为14.04，ROS版本为indigo

基于ROS平台的移动机器人-1-小车底盘的搭建

说明

本博文将介绍小车底盘的搭建需要的硬件和搭建过程

物品清单

1. 亚克力板或者铝合金小车底板一套
2. 小车轮子一对
3. 带编码器减速电机一对
4. 万向轮一个
5. 12V充电电池一个
6. 降压模块一个
7. 电机驱动模块一个
8. STM32核心板一块
9. Kinect2一个
10. 工控机或者笔记本一个

搭建步骤

1. 分别将电池，降压模块，电机，万向轮，电机驱动板，STM32核心板，Kinect2和工控机装好在底盘板上
2. 将轮子分别装上电机
3. 将电池电源引出两路，一路接在降压模块，一路接在电机驱动模块
4. 将电机驱动线接到驱动模块
5. 降压模块得到的5V给STM32核心板供电
6. 将STM32核心板的串口接到工控机
7. 将Kinect2的usb口接到工控机的USB3.0口
8. 小车搭建完成
9. 基于ROS平台的移动机器人-2制-小车底盘控

10. 说明

    本博文将介绍小车底盘控制的原理，如PID控制，控制程序的编写等。

    小车控制思想

11. 控制电机转动。电机的控制我们分为两部分，一部分为电机转动方向的控制，另一个为电机转速的控制。电机转动的方向我们用两个MCU引脚来控制，假如PIN_A=1，PIN_B=0 时，电机正转；PIN_A=0，PIN_B=1 时，电机反转；PIN_A=0，PIN_B=0 时，电机停止。电机速度的控制则需要一个PWM输出引脚，我们通过控制输出不同的PWM值来控制电机转动的速度。
12. PID控制。如果我们想控制小车以一米每秒的速度做直线，但由于地面的摩擦阻力的影响，会造成左右轮速度与我们想控制的速度不同，所以会走不直，这时我们就需要加入PID控制，PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比，差则补，多则减。这样基本就可以实现理想控制。详细PID介绍查看。
13. 小车转弯控制。一般我们要是想控制小车以多少的速度前进或者后退，我们只需要PID控制两个轮子的速度一致就可以基本做到。但如果要想控制小车以多少的角速度转弯，我们需要做一定的计算，如图所示：

14. 

    推算过程这里就不算了，我们可以得到左右轮速度和线速度角速度之间的关系如下：

    

    通过以上的公式我们就可以控制小车的任意行走了。

    程序结构

    以下为我的STM32工程主要文件

    1.main.c 接收和发送串口数据，控制电机

    /***********************************************  说明  *****************************************************************
    *
    *   1.串口接收
    *    （1）内容：小车左右轮速度,单位:mm/s（所有数据都为   float型，float型占4字节）
    *    （2）格式：10字节 [右轮速度4字节][左轮速度4字节][结束符"\r\n"2字节]
    *
    *   2.串口发送
    *    （1）内容：里程计（x,y坐标、线速度、角速度和方向角，单位依次为：mm,mm,mm/s,rad/s,rad，所有数据都为float型，float型占4字节）
    *    （2）格式：21字节 [x坐标4字节][y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符"\n"1字节]
    *
    ************************************************************************************************************************/
    #include "stm32f10x.h"
    #include "stm32f10x_it.h"#include "delay.h"
    #include "nvic_conf.h"
    #include "rcc_conf.h"#include "usart.h"
    #include "encoder.h"
    #include "contact.h"
    #include "gpio_conf.h"
    #include "tim2_5_6.h"
    #include "odometry.h"
    #include "tim2_5_6.h"#include "stdbool.h"
    #include <stdio.h>
    #include "string.h"
    #include "math.h"
    /***********************************************  输出  *****************************************************************/char odometry_data[21]={0};   //发送给串口的里程计数据数组float odometry_right=0,odometry_left=0;//串口得到的左右轮速度/***********************************************  输入  *****************************************************************/extern float position_x,position_y,oriention,velocity_linear,velocity_angular;         //计算得到的里程计数值extern u8 USART_RX_BUF[USART_REC_LEN];     //串口接收缓冲,最大USART_REC_LEN个字节.
    extern u16 USART_RX_STA;                   //串口接收状态标记   extern float Milemeter_L_Motor,Milemeter_R_Motor;     //dt时间内的左右轮速度,用于里程计计算/***********************************************  变量  *****************************************************************/u8 main_sta=0; //用作处理主函数各种if，去掉多余的flag（1打印里程计）（2调用计算里程计数据函数）（3串口接收成功）（4串口接收失败）union recieveData  //接收到的数据
    {float d;    //左右轮速度unsigned char data[4];
    }leftdata,rightdata;       //接收的左右轮数据union odometry  //里程计数据共用体
    {float odoemtry_float;unsigned char odometry_char[4];
    }x_data,y_data,theta_data,vel_linear,vel_angular;     //要发布的里程计数据，分别为：X，Y方向移动的距离，当前角度，线速度，角速度/****************************************************************************************************************/  
    int main(void)
    {       
    u8 t=0;
    u8 i=0,j=0,m=0;RCC_Configuration();      //系统时钟配置      
    NVIC_Configuration();     //中断优先级配置
    GPIO_Configuration();     //电机方向控制引脚配置
    USART1_Config();          //串口1配置TIM2_PWM_Init();          //PWM输出初始化
    ENC_Init();               //电机处理初始化
    TIM5_Configuration();     //速度计算定时器初始化
    TIM1_Configuration();     //里程计发布定时器初始化while (1)
    {       if(main_sta&0x01)//执行发送里程计数据步骤{//里程计数据获取x_data.odoemtry_float=position_x;//单位mmy_data.odoemtry_float=position_y;//单位mmtheta_data.odoemtry_float=oriention;//单位radvel_linear.odoemtry_float=velocity_linear;//单位mm/svel_angular.odoemtry_float=velocity_angular;//单位rad/s//将所有里程计数据存到要发送的数组for(j=0;j<4;j++){odometry_data[j]=x_data.odometry_char[j];odometry_data[j+4]=y_data.odometry_char[j];odometry_data[j+8]=theta_data.odometry_char[j];odometry_data[j+12]=vel_linear.odometry_char[j];odometry_data[j+16]=vel_angular.odometry_char[j];}odometry_data[20]='\n';//添加结束符//发送数据要串口for(i=0;i<21;i++){USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题             USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口 while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束            }main_sta&=0xFE;//执行计算里程计数据步骤}if(main_sta&0x02)//执行计算里程计数据步骤{odometry(Milemeter_R_Motor,Milemeter_L_Motor);//计算里程计main_sta&=0xFD;//执行发送里程计数据步骤} if(main_sta&0x08)        //当发送指令没有正确接收时{USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题for(m=0;m<3;m++){USART_SendData(USART1,0x00);    while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);}       USART_SendData(USART1,'\n');    while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); main_sta&=0xF7;}if(USART_RX_STA&0x8000)  // 串口1接收函数{           //接收左右轮速度for(t=0;t<4;t++){rightdata.data[t]=USART_RX_BUF[t];leftdata.data[t]=USART_RX_BUF[t+4];}//储存左右轮速度odometry_right=rightdata.d;//单位mm/sodometry_left=leftdata.d;//单位mm/sUSART_RX_STA=0;//清楚接收标志位}car_control(rightdata.d,leftdata.d);     //将接收到的左右轮速度赋给小车   
    }//end_while
    }//end main
    /*********************************************END OF FILE**************************************************/
    

15. 2.odometry.c 里程计计算

    #include "odometry.h"/***********************************************  输出  *****************************************************************/float position_x=0,position_y=0,oriention=0,velocity_linear=0,velocity_angular=0;/***********************************************  输入  *****************************************************************/extern float odometry_right,odometry_left;//串口得到的左右轮速度/***********************************************  变量  *****************************************************************/float wheel_interval= 268.0859f;//    272.0f;        //  1.0146
    //float wheel_interval=276.089f;    //轴距校正值=原轴距/0.987float multiplier=4.0f;           //倍频数
    float deceleration_ratio=90.0f;  //减速比
    float wheel_diameter=100.0f;     //轮子直径，单位mm
    float pi_1_2=1.570796f;          //π/2
    float pi=3.141593f;              //π
    float pi_3_2=4.712389f;          //π*3/2
    float pi_2_1=6.283186f;          //π*2
    float dt=0.005f;                 //采样时间间隔5ms
    float line_number=4096.0f;       //码盘线数
    float oriention_interval=0;  //dt时间内方向变化值float sin_=0;        //角度计算值
    float cos_=0;float delta_distance=0,delta_oriention=0;   //采样时间间隔内运动的距离float const_frame=0,const_angle=0,distance_sum=0,distance_diff=0;float oriention_1=0;u8 once=1;/****************************************************************************************************************///里程计计算函数
    void odometry(float right,float left)
    {   if(once)  //常数仅计算一次{const_frame=wheel_diameter*pi/(line_number*multiplier*deceleration_ratio);const_angle=const_frame/wheel_interval;once=0;}distance_sum = 0.5f*(right+left);//在很短的时间内，小车行驶的路程为两轮速度和distance_diff = right-left;//在很短的时间内，小车行驶的角度为两轮速度差//根据左右轮的方向，纠正短时间内，小车行驶的路程和角度量的正负if((odometry_right>0)&&(odometry_left>0))            //左右均正{delta_distance = distance_sum;delta_oriention = distance_diff;}else if((odometry_right<0)&&(odometry_left<0))       //左右均负{delta_distance = -distance_sum;delta_oriention = -distance_diff;}else if((odometry_right<0)&&(odometry_left>0))       //左正右负{delta_distance = -distance_diff;delta_oriention = -2.0f*distance_sum;       }else if((odometry_right>0)&&(odometry_left<0))       //左负右正{delta_distance = distance_diff;delta_oriention = 2.0f*distance_sum;}else{delta_distance=0;delta_oriention=0;}oriention_interval = delta_oriention * const_angle;//采样时间内走的角度  oriention = oriention + oriention_interval;//计算出里程计方向角oriention_1 = oriention + 0.5f * oriention_interval;//里程计方向角数据位数变化，用于三角函数计算sin_ = sin(oriention_1);//计算出采样时间内y坐标cos_ = cos(oriention_1);//计算出采样时间内x坐标position_x = position_x + delta_distance * cos_ * const_frame;//计算出里程计x坐标position_y = position_y + delta_distance * sin_ * const_frame;//计算出里程计y坐标velocity_linear = delta_distance*const_frame / dt;//计算出里程计线速度velocity_angular = oriention_interval / dt;//计算出里程计角速度//方向角角度纠正if(oriention > pi){oriention -= pi_2_1;}else{if(oriention < -pi){oriention += pi_2_1;}}
    }
    

16. 3.PID.c PID处理函数

    #include "PID.h"extern int span;float MaxValue=9;//输出最大限幅
    float MinValue=-9;//输出最小限幅float OutputValue;//PID输出暂存变量,用于积分饱和抑制float PID_calculate(struct PID *Control,float CurrentValue_left )//位置PID计算B
    {float Value_Kp;//比例分量float Value_Ki;//积分分量float Value_Kd;//微分分量Control->error_0 = Control->OwenValue - CurrentValue_left + 0*span;//基波分量，Control->OwenValue为想要的速度，CurrentValue_left为电机真实速度Value_Kp = Control->Kp * Control->error_0 ;Control->Sum_error += Control->error_0;     /***********************积分饱和抑制********************************************/OutputValue = Control->OutputValue;if(OutputValue>5 || OutputValue<-5) {Control->Ki = 0; }/*******************************************************************/Value_Ki = Control->Ki * Control->Sum_error;Value_Kd = Control->Kd * ( Control->error_0 - Control->error_1);Control->error_1 = Control->error_0;//保存一次谐波Control->OutputValue = Value_Kp  + Value_Ki + Value_Kd;//输出值计算，注意加减//限幅if( Control->OutputValue > MaxValue)Control->OutputValue = MaxValue;if (Control->OutputValue < MinValue)Control->OutputValue = MinValue;return (Control->OutputValue) ;
    }
    

17. 4.encoder.c 电机编码处理

    #include "encoder.h"/****************************************************************************************************************/s32 hSpeed_Buffer2[SPEED_BUFFER_SIZE]={0}, hSpeed_Buffer1[SPEED_BUFFER_SIZE]={0};//左右轮速度缓存数组
    static unsigned int hRot_Speed2;//电机A平均转速缓存
    static unsigned int hRot_Speed1;//电机B平均转速缓存
    unsigned int Speed2=0; //电机A平均转速 r/min，PID调节
    unsigned int Speed1=0; //电机B平均转速 r/min，PID调节static volatile u16 hEncoder_Timer_Overflow1;//电机B编码数采集 
    static volatile u16 hEncoder_Timer_Overflow2;//电机A编码数采集//float A_REMP_PLUS;//电机APID调节后的PWM值缓存
    float pulse = 0;//电机A PID调节后的PWM值缓存
    float pulse1 = 0;//电机B PID调节后的PWM值缓存int span;//采集回来的左右轮速度差值static bool bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位
    static bool bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位struct PID Control_left  ={0.01,0.1,0.75,0,0,0,0,0,0};//左轮PID参数，适于新电机4096
    struct PID Control_right ={0.01,0.1,0.75,0,0,0,0,0,0};//右轮PID参数，适于新电机4096/****************************************************************************************************************/s32 hPrevious_angle2, hPrevious_angle1;/****************************************************************************************************************/void ENC_Init2(void)//电机A码盘采集定时器，TIM4编码器模式
    {TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;TIM_ICInitTypeDef TIM_ICInitStructure;    GPIO_InitTypeDef GPIO_InitStructure;RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);GPIO_StructInit(&GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOB, &GPIO_InitStructure);TIM_DeInit(ENCODER2_TIMER);TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);TIM_TimeBaseStructure.TIM_Prescaler = 0;  TIM_TimeBaseStructure.TIM_Period = (4*ENCODER2_PPR)-1;TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;   TIM_TimeBaseInit(ENCODER2_TIMER, &TIM_TimeBaseStructure);TIM_EncoderInterfaceConfig(ENCODER2_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);TIM_ICStructInit(&TIM_ICInitStructure);TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;TIM_ICInit(ENCODER2_TIMER, &TIM_ICInitStructure);TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);TIM_ITConfig(ENCODER2_TIMER, TIM_IT_Update, ENABLE);TIM_Cmd(ENCODER2_TIMER, ENABLE); 
    }void ENC_Init1(void)//电机B码盘采集定时器，TIM3编码器模式
    {TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;TIM_ICInitTypeDef TIM_ICInitStructure;GPIO_InitTypeDef GPIO_InitStructure;RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);GPIO_StructInit(&GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOA, &GPIO_InitStructure);TIM_DeInit(ENCODER1_TIMER);TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);TIM_TimeBaseStructure.TIM_Prescaler = 0;TIM_TimeBaseStructure.TIM_Period = (4*ENCODER1_PPR)-1;  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;   TIM_TimeBaseInit(ENCODER1_TIMER, &TIM_TimeBaseStructure);TIM_EncoderInterfaceConfig(ENCODER1_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);TIM_ICStructInit(&TIM_ICInitStructure);TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;TIM_ICInit(ENCODER1_TIMER, &TIM_ICInitStructure);TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);TIM_ITConfig(ENCODER1_TIMER, TIM_IT_Update, ENABLE);TIM_Cmd(ENCODER1_TIMER, ENABLE); 
    }/****************************************************************************************************************/s16 ENC_Calc_Rot_Speed2(void)//计算电机A的编码数
    {   s32 wDelta_angle;u16 hEnc_Timer_Overflow_sample_one;u16 hCurrent_angle_sample_one;s32 temp;s16 haux;if (!bIs_First_Measurement2)//电机A以清除速度缓存数组{  hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow2;  hCurrent_angle_sample_one = ENCODER2_TIMER->CNT;hEncoder_Timer_Overflow2 = 0;haux = ENCODER2_TIMER->CNT;   if ( (ENCODER2_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)  {// encoder timer down-counting 反转的速度计算     wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR) -(hCurrent_angle_sample_one - hPrevious_angle2));}else  {//encoder timer up-counting 正转的速度计算wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle2 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR));}       temp=wDelta_angle;} else{bIs_First_Measurement2 = false;//电机A以清除速度缓存数组标志位temp = 0;hEncoder_Timer_Overflow2 = 0;haux = ENCODER2_TIMER->CNT;       }hPrevious_angle2 = haux;  return((s16) temp);
    }s16 ENC_Calc_Rot_Speed1(void)//计算电机B的编码数
    {   s32 wDelta_angle;u16 hEnc_Timer_Overflow_sample_one;u16 hCurrent_angle_sample_one;s32 temp;s16 haux;if (!bIs_First_Measurement1)//电机B以清除速度缓存数组{   hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow1;  //得到采样时间内的编码数   hCurrent_angle_sample_one = ENCODER1_TIMER->CNT;hEncoder_Timer_Overflow1 = 0;//清除脉冲数累加haux = ENCODER1_TIMER->CNT;   if ( (ENCODER1_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)  {// encoder timer down-counting 反转的速度计算wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR) -(hCurrent_angle_sample_one - hPrevious_angle1));  }else  {//encoder timer up-counting 正转的速度计算wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle1 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR));}temp=wDelta_angle;} else{bIs_First_Measurement1 = false;//电机B以清除速度缓存数组标志位temp = 0;hEncoder_Timer_Overflow1 = 0;haux = ENCODER1_TIMER->CNT;       }hPrevious_angle1 = haux;  return((s16) temp);
    }/****************************************************************************************************************/void ENC_Clear_Speed_Buffer(void)//速度存储器清零
    {   u32 i;//清除左右轮速度缓存数组for (i=0;i<SPEED_BUFFER_SIZE;i++){hSpeed_Buffer2[i] = 0;hSpeed_Buffer1[i] = 0;}bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位
    }void ENC_Calc_Average_Speed(void)//计算三次电机的平均编码数
    {   u32 i;signed long long wtemp3=0;signed long long wtemp4=0;//累加缓存次数内的速度值for (i=0;i<SPEED_BUFFER_SIZE;i++){wtemp4 += hSpeed_Buffer2[i];wtemp3 += hSpeed_Buffer1[i];}//取平均，平均脉冲数单位为 个/s  wtemp3 /= (SPEED_BUFFER_SIZE);wtemp4 /= (SPEED_BUFFER_SIZE); //平均脉冲数 个/s  //将平均脉冲数单位转为 r/minwtemp3 = (wtemp3 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER1_PPR);wtemp4 = (wtemp4 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER2_PPR); hRot_Speed2= ((s16)(wtemp4));//平均转速 r/minhRot_Speed1= ((s16)(wtemp3));//平均转速 r/minSpeed2=hRot_Speed2;//平均转速 r/minSpeed1=hRot_Speed1;//平均转速 r/min
    }/****************************************************************************************************************/void Gain2(void)//设置电机A PID调节 PA2
    {//static float pulse = 0;span=1*(Speed1-Speed2);//采集回来的左右轮速度差值pulse= pulse + PID_calculate(&Control_right,hRot_Speed2);//PID调节//pwm幅度抑制if(pulse > 3600) pulse = 3600;if(pulse < 0) pulse = 0;//A_REMP_PLUS=pulse;//电机APID调节后的PWM值缓存
    }void Gain1(void)//设置电机B PID调节 PA1
    {//static float pulse1 = 0;span=1*(Speed2-Speed1);//采集回来的左右轮速度差值pulse1= pulse1 + PID_calculate(&Control_left,hRot_Speed1);//PID调节pwm 幅度抑制if(pulse1 > 3600) pulse1 = 3600;if(pulse1 < 0) pulse1 = 0;TIM2->CCR2 = pulse1;//电机B赋值PWM//TIM2->CCR3 = A_REMP_PLUS;//电机A赋值PWMTIM2->CCR3 = pulse;//电机A赋值PWM
    }/****************************************************************************************************************/void ENC_Init(void)//电机处理初始化
    {ENC_Init2();              //设置电机A TIM4编码器模式PB6 PB7 右电机ENC_Init1();              //设置电机B TIM3编码器模式PA6 PA7 左电机ENC_Clear_Speed_Buffer();//速度存储器清零
    }/****************************************************************************************************************/void TIM4_IRQHandler (void)//执行TIM4(电机A编码器采集)计数中断
    {   TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);if (hEncoder_Timer_Overflow2 != U16_MAX)//不超范围  {hEncoder_Timer_Overflow2++; //脉冲数累加}
    }void TIM3_IRQHandler (void)//执行TIM3(电机B编码器采集)计数中断
    {  TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);if (hEncoder_Timer_Overflow1 != U16_MAX)//不超范围    {hEncoder_Timer_Overflow1++;  //脉冲数累加}
    }
    

18. 5.contact.c 电机控制函数

    #include "contact.h"/***********************************************  输出  *****************************************************************//***********************************************  输入  *****************************************************************/extern struct PID Control_left;//左轮PID参数，适于新电机4096
    extern struct PID Control_right;//右轮PID参数，适于新电机4096/***********************************************  变量  *****************************************************************//*******************************************************************************************************************/void LeftMovingSpeedW(unsigned int val)//左轮方向和速度控制函数
    {     if(val>10000){  GPIO_SetBits(GPIOC, GPIO_Pin_6);    GPIO_ResetBits(GPIOC, GPIO_Pin_7);  Control_left.OwenValue=(val-10000);//PID调节的目标编码数            }else if(val<10000){  GPIO_SetBits(GPIOC, GPIO_Pin_7);    GPIO_ResetBits(GPIOC, GPIO_Pin_6);  Control_left.OwenValue=(10000-val);//PID调节的目标编码数     }   else{GPIO_SetBits(GPIOC, GPIO_Pin_6);   GPIO_SetBits(GPIOC, GPIO_Pin_7);Control_left.OwenValue=0;//PID调节的目标编码数}                   
    }void RightMovingSpeedW(unsigned int val2)//右轮方向和速度控制函数
    {    if(val2>10000){  /* motor A 正转*/GPIO_SetBits(GPIOC, GPIO_Pin_10);   GPIO_ResetBits(GPIOC, GPIO_Pin_11); Control_right.OwenValue=(val2-10000);//PID调节的目标编码数}else if(val2<10000){  /* motor A 反转*/GPIO_SetBits(GPIOC, GPIO_Pin_11);   GPIO_ResetBits(GPIOC, GPIO_Pin_10); Control_right.OwenValue=(10000-val2);//PID调节的目标编码数   }   else{GPIO_SetBits(GPIOC, GPIO_Pin_10);   GPIO_SetBits(GPIOC, GPIO_Pin_11);Control_right.OwenValue=0;//PID调节的目标编码数}                                               
    }void car_control(float rightspeed,float leftspeed)//小车速度转化和控制函数
    {float k2=17.179;         //速度转换比例,转/分钟  //将从串口接收到的速度转换成实际控制小车的速度？还是PWM？int right_speed=(int)k2*rightspeed;int left_speed=(int)k2*leftspeed;RightMovingSpeedW(right_speed+10000);LeftMovingSpeedW(left_speed+10000);
    }//void Contact_Init(void)//左右轮方向和速度初始化
    //{
    //  LeftMovingSpeedW(12000); //电机B
    //  RightMovingSpeedW(12000);//电机A  
    //}
    

19. 完整工程代码下载

    版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/55001231

20. 基于ROS平台的移动机器人-3-小车底盘与ROS的通信

    1.ROS平台与底盘通信协议

    ROS平台与小车底盘的通信一般是通过串口或者CAN总线。我这里采用的是串口，以下为我自定义的通信数据格式：

    （1）底盘串口部分

21. 1.串口接收（1）内容：小车左右轮速度,单位:mm/s（所有数据都为   float型，float型占4字节）（2）格式：10字节 
    [右轮速度4字节][左轮速度4字节][结束符"\r\n"2字节]2.串口发送（1）内容：里程计（x,y坐标、线速度、角速度和方向角，单位依次为：mm,mm,mm/s,rad/s,rad，所有数据都为float型，float型占4字节）（2）格式：21字节 
    [x坐标4字节][y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符"\n"1字节]

22. （2）ROS平台串口节点部分

    1.写入串口
    （1）内容：左右轮速度，单位为mm/s
    （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]
    2.读取串口
    （1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s
    （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
    

23. （1）小车底盘串口处理程序。

    底盘串口处理的程序主要写在了 main.c 文件中。

    底盘向串口发送里程计代码：

    if(main_sta&0x01)//执行发送里程计数据步骤
    {//里程计数据获取x_data.odoemtry_float=position_x;//单位mmy_data.odoemtry_float=position_y;//单位mmtheta_data.odoemtry_float=oriention;//单位radvel_linear.odoemtry_float=velocity_linear;//单位mm/svel_angular.odoemtry_float=velocity_angular;//单位rad/s//将所有里程计数据存到要发送的数组for(j=0;j<4;j++){odometry_data[j]=x_data.odometry_char[j];odometry_data[j+4]=y_data.odometry_char[j];odometry_data[j+8]=theta_data.odometry_char[j];odometry_data[j+12]=vel_linear.odometry_char[j];odometry_data[j+16]=vel_angular.odometry_char[j];}odometry_data[20]='\n';//添加结束符//发送数据要串口for(i=0;i<21;i++){USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题             USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口 while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束            }main_sta&=0xFE;//执行计算里程计数据步骤
    }
    

24. 底盘接收串口发来的速度代码：

    if(USART_RX_STA&0x8000)  // 串口1接收函数
    {           //接收左右轮速度for(t=0;t<4;t++){rightdata.data[t]=USART_RX_BUF[t];leftdata.data[t]=USART_RX_BUF[t+4];}//储存左右轮速度odometry_right=rightdata.d;//单位mm/sodometry_left=leftdata.d;//单位mm/sUSART_RX_STA=0;//清楚接收标志位
    }
    

25. （2）ROS平台串口处理程序

    ROS平台串口处理程序主要写在 base_controller.cpp 文件中。

    ROS平台向串口发送速度代码：

    void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
    {string port("/dev/ttyUSB0");    //小车串口号unsigned long baud = 115200;    //小车串口波特率serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/slinear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s//将转换好的小车速度分量为左右轮速度left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;//存入数据到要发布的左右轮速度消息left_speed_data.d*=ratio;   //放大１０００倍，mm/sright_speed_data.d*=ratio;//放大１０００倍，mm/sfor(int i=0;i<4;i++)    //将左右轮速度存入数组中发送给串口{speed_data[i]=right_speed_data.data[i];speed_data[i+4]=left_speed_data.data[i];}//在写入串口的左右轮速度数据后加入”/r/n“speed_data[8]=data_terminal0;speed_data[9]=data_terminal1;//写入数据到串口my_serial.write(speed_data,10);
    }
    

26. ROS平台接收串口发来的里程计代码：

    rec_buffer =my_serial.readline(25,"\n");    //获取串口发送来的数据
    const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
    

    2.串口通信处理程序

27. 基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动

    准备工作

    1.下载串口通信的ROS包

    (1)cd ~/catkin_ws/src
    (2)git clone https://github.com/Forrest-Z/serial.git
    

28. 2.下载键盘控制的ROS包

    (1)cd ~/catkin_ws/src
    (2)git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git
    

29. 进入下载好的ROS包的文件夹，选中 keyboard_teleop_zbot.py ，右键>属性>权限>勾选 允许作为程序执行文件。
    最后一步：

    （1）cd ~/catkin_ws(2)catkin_make
    

30. 这样子我们的键盘控制包就能使用了。

    3.新建 base_controller ROS 包

    （1）cd ~/catkin_ws/src
    （2）catkin_create_pkg base_controller roscpp
    

31. 在新建好的ROS包文件夹下新建一个“src”的文件夹，然后进入该文件夹，新建一个base_controller.cpp文件，将本博文最后提供的代码粘贴进去，然后修改一下 CMakeLists.txt ：

    第一处修改

    find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_generationserialtfnav_msgs
    )
    

32. 第二处修改

    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES base_controllerCATKIN_DEPENDS roscpp rospy std_msgs
    #  DEPENDS system_lib
    )
    

33. 第三处修改

    include_directories(${catkin_INCLUDE_DIRS}${serial_INCLUDE_DIRS}
    )
    

34. 第四处修改

    add_executable(base_controller src/base_controller.cpp)
    target_link_libraries(base_controller ${catkin_LIBRARIES})
    

35. 然后修改一下 package.xml ：

    <?xml version="1.0"?>
    <package><name>base_controller</name><version>0.0.0</version><description>The base_controller package</description>
    email="siat@todo.todo">siat</maintainer><license>TODO</license><!--   <test_depend>gtest</test_depend> --><buildtool_depend>catkin</buildtool_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>message_generation</build_depend> <build_depend>tf</build_depend><build_depend>nav_msgs</build_depend> <run_depend>roscpp</run_depend><run_depend>rospy</run_depend><run_depend>std_msgs</run_depend><run_depend>message_runtime</run_depend> <run_depend>tf</run_depend><run_depend>nav_msgs</run_depend><!-- The export tag contains other, unspecified, tags --><export><!-- Other tools can request additional information be placed here --></export>
    </package>

36. 控制原理

    1.当我们按下键盘时，teleop_twist_keyboard 包会发布 /cmd_vel 主题发布速度

    2.我们在 base_controller 节点订阅这个话题，接收速度数据，在转换成与底盘通信的格式，然后写入串口

    3.我们在 base_controller 节点读取底盘向串口发来的里程计数据，然后进行处理再将里程计发布出去，同时更新tf

    4.当小车底盘接收到串口发来的速度后，就控制电机运转，从而实现键盘控制小车的移动

    运行

    1.启动键盘节点

    rosrun teleop_twist_keyboard teleop_twist_keyboard.py
    

37.  

38. 2.启动小车底盘控制节点

    rosrun base_controller base_controller
    

39. 注意事项

    1.我们在启动小车底盘控制节点时，有可以启动不了，大多数是因为串口的端口号不对，在 base_controller.cpp 文件里，我用的是”/dev/ttyUSB0”串口端口号

    2.我们在启动启动小车底盘控制节点前，应该查看一下我们底盘的串口号是否正确，查看指令如下：

    ls -l /dev |grep ttyUSB
    

40. 如果运行后显示的端口号和我们程序中的一样，那就没问题，如果不一样，我们将程序的代码改动一下便可。

41. base_controller.cpp 完整代码：

42. /******************************************************************
    基于串口通信的ROS小车基础控制器，功能如下：
    1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动
    2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动
    3.发布里程计主题/odm串口通信说明：
    1.写入串口
    （1）内容：左右轮速度，单位为mm/s
    （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]
    2.读取串口
    （1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s
    （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
    *******************************************************************/
    #include "ros/ros.h"  //ros需要的头文件
    #include <geometry_msgs/Twist.h>
    #include <tf/transform_broadcaster.h>
    #include <nav_msgs/Odometry.h>
    //以下为串口通讯需要的头文件
    #include <string>        
    #include <iostream>
    #include <cstdio>
    #include <unistd.h>
    #include <math.h>
    #include "serial/serial.h"
    /****************************************************************************/
    using std::string;
    using std::exception;
    using std::cout;
    using std::cerr;
    using std::endl;
    using std::vector;
    /*****************************************************************************/
    float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
    float D = 0.2680859f ;    //两轮间距，单位是m
    float linear_temp=0,angular_temp=0;//暂存的线速度和角速度
    /****************************************************/
    unsigned char data_terminal0=0x0d;  //“/r"字符
    unsigned char data_terminal1=0x0a;  //“/n"字符
    unsigned char speed_data[10]={0};   //要发给串口的数据
    string rec_buffer;  //串口数据接收变量//发送给下位机的左右轮速度，里程计的坐标和方向
    union floatData //union的作用为实现char数组和float之间的转换
    {float d;unsigned char data[4];
    }right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
    /************************************************************/
    void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
    {string port("/dev/ttyUSB0");    //小车串口号unsigned long baud = 115200;    //小车串口波特率serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/slinear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s//将转换好的小车速度分量为左右轮速度left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;//存入数据到要发布的左右轮速度消息left_speed_data.d*=ratio;   //放大１０００倍，mm/sright_speed_data.d*=ratio;//放大１０００倍，mm/sfor(int i=0;i<4;i++)    //将左右轮速度存入数组中发送给串口{speed_data[i]=right_speed_data.data[i];speed_data[i+4]=left_speed_data.data[i];}//在写入串口的左右轮速度数据后加入”/r/n“speed_data[8]=data_terminal0;speed_data[9]=data_terminal1;//写入数据到串口my_serial.write(speed_data,10);
    }int main(int argc, char **argv)
    {string port("/dev/ttyUSB0");//小车串口号unsigned long baud = 115200;//小车串口波特率serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口ros::init(argc, argv, "base_controller");//初始化串口节点ros::NodeHandle n;  //定义节点进程句柄ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定义要发布/odom主题static tf::TransformBroadcaster odom_broadcaster;//定义tf对象geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息nav_msgs::Odometry odom;//定义里程计对象geometry_msgs::Quaternion odom_quat; //四元数变量//定义covariance矩阵，作用为解决文职和速度的不同测量的不确定性float covariance[36] = {0.01,   0,    0,     0,     0,     0,  // covariance on gps_x0,  0.01, 0,     0,     0,     0,  // covariance on gps_y0,  0,    99999, 0,     0,     0,  // covariance on gps_z0,  0,    0,     99999, 0,     0,  // large covariance on rot x0,  0,    0,     0,     99999, 0,  // large covariance on rot y0,  0,    0,     0,     0,     0.01};  // large covariance on rot z //载入covariance矩阵for(int i = 0; i < 36; i++){odom.pose.covariance[i] = covariance[i];;}       ros::Rate loop_rate(10);//设置周期休眠时间while(ros::ok()){rec_buffer =my_serial.readline(25,"\n");    //获取串口发送来的数据const char *receive_data=rec_buffer.data(); //保存串口发送来的数据if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息{for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度{position_x.data[i]=receive_data[i];position_y.data[i]=receive_data[i+4];oriention.data[i]=receive_data[i+8];vel_linear.data[i]=receive_data[i+12];vel_angular.data[i]=receive_data[i+16];}//将X，Y坐标，线速度缩小1000倍position_x.d/=1000; //mposition_y.d/=1000; //mvel_linear.d/=1000; //m/s//里程计的偏航角需要转换成四元数才能发布odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数//载入坐标（tf）变换时间戳odom_trans.header.stamp = ros::Time::now();//发布坐标变换的父子坐标系odom_trans.header.frame_id = "odom";     odom_trans.child_frame_id = "base_footprint";       //tf位置数据：x,y,z,方向odom_trans.transform.translation.x = position_x.d;odom_trans.transform.translation.y = position_y.d;odom_trans.transform.translation.z = 0.0;odom_trans.transform.rotation = odom_quat;        //发布tf坐标变化odom_broadcaster.sendTransform(odom_trans);//载入里程计时间戳odom.header.stamp = ros::Time::now(); //里程计的父子坐标系odom.header.frame_id = "odom";odom.child_frame_id = "base_footprint";       //里程计位置数据：x,y,z,方向odom.pose.pose.position.x = position_x.d;     odom.pose.pose.position.y = position_y.d;odom.pose.pose.position.z = 0.0;odom.pose.pose.orientation = odom_quat;       //载入线速度和角速度odom.twist.twist.linear.x = vel_linear.d;//odom.twist.twist.linear.y = odom_vy;odom.twist.twist.angular.z = vel_angular.d;    //发布里程计odom_pub.publish(odom);ros::spinOnce();//周期执行loop_rate.sleep();//周期休眠}//程序周期性调用//ros::spinOnce();  //callback函数必须处理所有问题时，才可以用到}return 0;
    }
    

     

43. 基于ROS平台的移动机器人-5-Kinect2驱动的安装和ROS下的测试

    说明

    我们这里要测试的对象是Kinect2！！！

    实物如图：

    

    驱动安装

    1.首先git下载代码，很快下载好，放到~下面

    git clone https://github.com/OpenKinect/libfreenect2.git
    

44. 2.然后安装依赖项如下,最好事先编译安装好OpenCV

    sudo apt-get install build-essential cmake pkg-config libturbojpeg libjpeg-turbo8-dev mesa-common-dev freeglut3-dev libxrandr-dev libxi-dev
    

45. 3.然后安装libusb。此处需要添加一个PPA

    sudo apt-add-repository ppa:floe/libusb
    sudo apt-get update
    sudo apt-get install libusb-1.0-0-dev
    

46. 4.接着运行下面的命令，安装GLFW3

    sudo apt-get install libglfw3-dev
    

47. 如果没有成功的话，使用下面的命令，来代替上面的

    cd libfreenect2/depends
    sh install_ubuntu.sh     
    sudo dpkg -i libglfw3*_3.0.4-1_*.deb
    

48. 5.接着编译库

    cd ..
    mkdir build && cd build  
    cmake ..     
    make     
    sudo make install
    

49. 6.测试

    （1）需要把libfreenect2文件夹下面的rules里面的一个90开头的文件复制到/etc/udev/rules.d/下面

    sudo cp ../platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/
    

50. （2）插上kinect2，此时黄灯变成白色的，表示有驱动。注意：只能用于USB3的接口

    （3）在中断进入刚才上面新建的build文件夹里的bin文件夹，运行：

    ./bin/Protonect
    

51.  

52. 不出意外的话就可以看到显示的图像了。

    ROS下测试

    1.安装iai-kinect2 ROS包

    cd ~/catkin_ws/src/
    git clone https://github.com/code-iai/iai_kinect2.git
    cd iai_kinect2
    rosdep install -r --from-paths .
    cd ~/catkin_ws
    catkin_make -DCMAKE_BUILD_TYPE="Release"
    rospack profile
    

53. 2.测试

    （1）启动kinect2驱动

    roslaunch kinect2_bridge kinect2_bridge.launch
    

54. （2）启动显示界面

    rosrun kinect2_viewer kinect2_viewer
    

55. 如果能显示出画面的话就没问题了

    注意事项

    Kinect2只能用于USB3的接口！！！

56. 基于ROS平台的移动机器人-6-使用Kinect2获取激光数据

    ready

    此教程我们将利用KinectV2在ROS平台上将KinectV2获得的深度图片转化为激光数据，以便我们下面的建图和导航。

    go

    1.我们这里需要一个将深度图转为激光数据的包

    （1）cd ~/catkin_ws/src
    （2）git clone  https://github.com/ros-perception/depthimage_to_laserscan.git
    

57. 2.我这里新建了一个 bringup 的包来专门存放 launch 文件

    （1）cd ~/catkin_ws/src
    （2）catkin_create_pkg bringup  roscpp
    

58. 3.在 bringup 包内我们新建一个 launch 文件夹 ，然后 在launch文件夹里添加
    kinect2_depthimage_to_laserscan_rviz_view.launch文件

    <launch><!-- start sensor-->            <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch"><arg name="base_name"         value="kinect2"/><arg name="sensor"            value=""/><arg name="publish_tf"        value="true"/><arg name="base_name_tf"      value="kinect2"/><arg name="fps_limit"         value="-1.0"/><arg name="calib_path"        value="$(find kinect2_bridge)/data/"/><arg name="use_png"           value="false"/><arg name="jpeg_quality"      value="90"/><arg name="png_level"         value="1"/><arg name="depth_method"      value="default"/><arg name="depth_device"      value="-1"/><arg name="reg_method"        value="default"/><arg name="reg_device"        value="-1"/><arg name="max_depth"         value="12.0"/><arg name="min_depth"         value="0.1"/><arg name="queue_size"        value="5"/><arg name="bilateral_filter"  value="true"/><arg name="edge_aware_filter" value="true"/><arg name="worker_threads"    value="4"/></include>  <!-- Run the depthimage_to_laserscan node --><node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen"><!--输入图像--><remap from="image" to="/kinect2/qhd/image_depth_rect"/><!--相关图像的相机信息。通常不需要重新变形，因为camera_info将从与图像相同的命名空间订阅。--><remap from="camera_info" to="/kinect2/qhd/camera_info" /><!--输出激光数据的话题--><remap from="scan" to="/scan" /> <!--激光扫描的帧id。对于来自具有Z向前的“光学”帧的点云，该值应该被设置为具有X向前和Z向上的相应帧。--><param name="output_frame_id" value="/kinect2_depth_frame"/><!--用于生成激光扫描的像素行数。对于每一列，扫描将返回在图像中垂直居中的那些像素的最小值。--><param name="scan_height" value="30"/><!--返回的最小范围（以米为单位）。小于该范围的输出将作为-Inf输出。--><param name="range_min" value="0.45"/><!--返回的最大范围（以米为单位）。大于此范围将输出为+ Inf。--><param name="range_max" value="8.00"/></node><!-- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_footprint base_link 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0.5 0 0 0 base_link laser 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2laser" args="0 0 0.5 0 0 0 base_link kinect2_depth_frame 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2_link" args="0 0 0.5 -1.57 0 -1.57 base_link kinect2_link 50" /><!--start rviz view --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find bringup)/rviz/kinect2_depthimage_to_laserscan_view.rviz" /></launch>
    

59. 4.在 bringup 包里新建 rviz 文件夹，然后在 rviz 文件夹里添加
    kinect2_depthimage_to_laserscan_view.rviz文件

    Panels:- Class: rviz/DisplaysHelp Height: 78Name: DisplaysProperty Tree Widget:Expanded:- /Global Options1- /Status1- /LaserScan1Splitter Ratio: 0.5Tree Height: 566- Class: rviz/SelectionName: Selection- Class: rviz/Tool PropertiesExpanded:- /2D Pose Estimate1- /2D Nav Goal1- /Publish Point1Name: Tool PropertiesSplitter Ratio: 0.588679- Class: rviz/ViewsExpanded:- /Current View1Name: ViewsSplitter Ratio: 0.5- Class: rviz/TimeExperimental: falseName: TimeSyncMode: 0SyncSource: LaserScan
    Visualization Manager:Class: ""Displays:- Alpha: 0.5Cell Size: 1Class: rviz/GridColor: 160; 160; 164Enabled: trueLine Style:Line Width: 0.03Value: LinesName: GridNormal Cell Count: 0Offset:X: 0Y: 0Z: 0Plane: XYPlane Cell Count: 10Reference Frame: <Fixed Frame>Value: true- Alpha: 1Autocompute Intensity Bounds: trueAutocompute Value Bounds:Max Value: 0Min Value: 0Value: trueAxis: ZChannel Name: intensityClass: rviz/LaserScanColor: 255; 255; 255Color Transformer: AxisColorDecay Time: 0Enabled: trueInvert Rainbow: falseMax Color: 255; 255; 255Max Intensity: 4096Min Color: 0; 0; 0Min Intensity: 0Name: LaserScanPosition Transformer: XYZQueue Size: 10Selectable: trueSize (Pixels): 3Size (m): 0.01Style: PointsTopic: /scanUnreliable: falseUse Fixed Frame: trueUse rainbow: trueValue: true- Class: rviz/TFEnabled: trueFrame Timeout: 15Frames:All Enabled: truebase_footprint:Value: truekinect2_depth_frame:Value: truekinect2_ir_optical_frame:Value: truekinect2_link:Value: truekinect2_rgb_optical_frame:Value: truelaser:Value: trueMarker Scale: 1Name: TFShow Arrows: trueShow Axes: trueShow Names: trueTree:base_footprint:kinect2_depth_frame:{}kinect2_link:kinect2_rgb_optical_frame:kinect2_ir_optical_frame:{}laser:{}Update Interval: 0Value: trueEnabled: trueGlobal Options:Background Color: 48; 48; 48Fixed Frame: laserFrame Rate: 30Name: rootTools:- Class: rviz/InteractHide Inactive Objects: true- Class: rviz/MoveCamera- Class: rviz/Select- Class: rviz/FocusCamera- Class: rviz/Measure- Class: rviz/SetInitialPoseTopic: /initialpose- Class: rviz/SetGoalTopic: /move_base_simple/goal- Class: rviz/PublishPointSingle click: trueTopic: /clicked_pointValue: trueViews:Current:Class: rviz/OrbitDistance: 10Enable Stereo Rendering:Stereo Eye Separation: 0.06Stereo Focal Distance: 1Swap Stereo Eyes: falseValue: falseFocal Point:X: 0Y: 0Z: 0Name: Current ViewNear Clip Distance: 0.01Pitch: 0.810398Target Frame: <Fixed Frame>Value: Orbit (rviz)Yaw: 3.2504Saved: ~
    Window Geometry:Displays:collapsed: falseHeight: 846Hide Left Dock: falseHide Right Dock: trueQMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000Selection:collapsed: falseTime:collapsed: falseTool Properties:collapsed: falseViews:collapsed: trueWidth: 1200X: 50Y: 45
    

60. 5.基本工作我们都做完了，现在我们需要编译一下

    （1）cd ~/catkin_ws
    （2）catkin_make 
    （3）rospack profile
    

61. 6.我们现在可以接上KinectV2（注意！！！接USB3.0口），执行

    roslaunch bringup kinect2_depthimage_to_laserscan_rviz_view.launch
    

62. 没有问题的话我们可以看到一下界面：

    

    查看TF树：文件生成在主文件夹

    rosrun tf view_frames
    

63. 这是我的TF树

64. 基于ROS平台的移动机器人-7-使用Kinect2建图

    ready

    此教程我将利用KinectV2得到的激光数据和Gmapping来建图（PS：这里你需要一个可以通过ＲＯＳ控制的移动底盘和一个KinectV2）

    go

    1.安装Gmapping包

    （1）cd ~/catkin_ws/src
    （2）git clone  https://github.com/ros-perception/slam_gmapping.git
    （3）cd ~/catkin_ws
    （4）catkin_make 
    （5）rospack profile
    

65. 2.在我们之前的建的bringup/launch/下新建kinect2_gmapping.launch文件

    <?xml version="1.0"?><launch>    <!-- start kinect2-->            <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch"><arg name="base_name"         value="kinect2"/><arg name="sensor"            value=""/><arg name="publish_tf"        value="true"/><arg name="base_name_tf"      value="kinect2"/><arg name="fps_limit"         value="-1.0"/><arg name="calib_path"        value="$(find kinect2_bridge)/data/"/><arg name="use_png"           value="false"/><arg name="jpeg_quality"      value="90"/><arg name="png_level"         value="1"/><arg name="depth_method"      value="default"/><arg name="depth_device"      value="-1"/><arg name="reg_method"        value="default"/><arg name="reg_device"        value="-1"/><arg name="max_depth"         value="12.0"/><arg name="min_depth"         value="0.1"/><arg name="queue_size"        value="5"/><arg name="bilateral_filter"  value="true"/><arg name="edge_aware_filter" value="true"/><arg name="worker_threads"    value="4"/></include>  <!-- Run the depthimage_to_laserscan node --><node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen"><!--输入图像--><remap from="image" to="/kinect2/qhd/image_depth_rect"/><!--相关图像的相机信息。通常不需要重新变形，因为camera_info将从与图像相同的命名空间订阅。--><remap from="camera_info" to="/kinect2/qhd/camera_info" /><!--输出激光数据的话题--><remap from="scan" to="/scan" /> <!--激光扫描的帧id。对于来自具有Z向前的“光学”帧的点云，该值应该被设置为具有X向前和Z向上的相应帧。--><param name="output_frame_id" value="/kinect2_depth_frame"/><!--用于生成激光扫描的像素行数。对于每一列，扫描将返回在图像中垂直居中的那些像素的最小值。--><param name="scan_height" value="30"/><!--返回的最小范围（以米为单位）。小于该范围的输出将作为-Inf输出。--><param name="range_min" value="0.45"/><!--返回的最大范围（以米为单位）。大于此范围将输出为+ Inf。--><param name="range_max" value="8.00"/></node><!--start gmapping node --><node pkg="gmapping" type="slam_gmapping" name="simple_gmapping" output="screen"><param name="map_update_interval" value="5.0"/>  <param name="maxUrange" value="5.0"/> <param name="maxRange" value="6.0"/> <param name="sigma" value="0.05"/>  <param name="kernelSize" value="1"/>  <param name="lstep" value="0.05"/>  <param name="astep" value="0.05"/>  <param name="iterations" value="5"/>  <param name="lsigma" value="0.075"/>  <param name="ogain" value="3.0"/>  <param name="lskip" value="0"/>  <param name="minimumScore" value="50"/>  <param name="srr" value="0.1"/>  <param name="srt" value="0.2"/>  <param name="str" value="0.1"/>  <param name="stt" value="0.2"/>  <param name="linearUpdate" value="1.0"/>  <param name="angularUpdate" value="0.5"/>  <param name="temporalUpdate" value="3.0"/>  <param name="resampleThreshold" value="0.5"/>  <param name="particles" value="50"/>  <param name="xmin" value="-5.0"/>  <param name="ymin" value="-5.0"/>  <param name="xmax" value="5.0"/>  <param name="ymax" value="5.0"/>  <param name="delta" value="0.05"/>  <param name="llsamplerange" value="0.01"/>  <param name="llsamplestep" value="0.01"/>  <param name="lasamplerange" value="0.005"/>  <param name="lasamplestep" value="0.005"/>  </node><!--start serial-port and odometry node--><node name="base_controller" pkg="base_controller" type="base_controller"/><!-- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_footprint base_link 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0.5 0 0 0 base_link laser 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2laser" args="0 0 0.5 0 0 0 base_link kinect2_depth_frame 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2_link" args="0 0 0.5 -1.57 0 -1.57 base_link kinect2_link 50" /></launch>
    

66. 3.在我们之前的建的bringup/launch/下新建kinect2_gmapping_rviz_view.launch文件

    PS：我们这里不把启动RVIZ写在kinect2_gmapping.launch里是方便以后的远程启动

    <?xml version="1.0"?><launch><!--start rviz view --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find bringup)/rviz/kinect2_gmapping.rviz" /></launch>
    

67. 4.还有在新建bringup/rviz/下新建kinect2_gmapping.rviz文件

    Panels:- Class: rviz/DisplaysHelp Height: 78Name: DisplaysProperty Tree Widget:Expanded:- /Global Options1- /Status1- /Map1Splitter Ratio: 0.5Tree Height: 566- Class: rviz/SelectionName: Selection- Class: rviz/Tool PropertiesExpanded:- /2D Pose Estimate1- /2D Nav Goal1- /Publish Point1Name: Tool PropertiesSplitter Ratio: 0.588679- Class: rviz/ViewsExpanded:- /Current View1Name: ViewsSplitter Ratio: 0.5- Class: rviz/TimeExperimental: falseName: TimeSyncMode: 0SyncSource: LaserScan
    Visualization Manager:Class: ""Displays:- Alpha: 0.5Cell Size: 1Class: rviz/GridColor: 160; 160; 164Enabled: trueLine Style:Line Width: 0.03Value: LinesName: GridNormal Cell Count: 0Offset:X: 0Y: 0Z: 0Plane: XYPlane Cell Count: 10Reference Frame: <Fixed Frame>Value: true- Class: rviz/TFEnabled: trueFrame Timeout: 15Frames:All Enabled: truebase_footprint:Value: truebase_link:Value: truekinect2_depth_frame:Value: truekinect2_ir_optical_frame:Value: truekinect2_link:Value: truekinect2_rgb_optical_frame:Value: truelaser:Value: truemap:Value: trueodom:Value: trueMarker Scale: 1Name: TFShow Arrows: trueShow Axes: trueShow Names: trueTree:map:odom:base_footprint:base_link:kinect2_depth_frame:{}kinect2_link:kinect2_rgb_optical_frame:kinect2_ir_optical_frame:{}laser:{}Update Interval: 0Value: true- Alpha: 1Autocompute Intensity Bounds: trueAutocompute Value Bounds:Max Value: 1Min Value: 1Value: trueAxis: ZChannel Name: intensityClass: rviz/LaserScanColor: 255; 255; 255Color Transformer: AxisColorDecay Time: 0Enabled: trueInvert Rainbow: falseMax Color: 255; 255; 255Max Intensity: 4096Min Color: 0; 0; 0Min Intensity: 0Name: LaserScanPosition Transformer: XYZQueue Size: 10Selectable: trueSize (Pixels): 3Size (m): 0.01Style: Flat SquaresTopic: /scanUnreliable: falseUse Fixed Frame: trueUse rainbow: trueValue: true- Alpha: 0.7Class: rviz/MapColor Scheme: mapDraw Behind: falseEnabled: trueName: MapTopic: /mapUnreliable: falseValue: trueEnabled: trueGlobal Options:Background Color: 48; 48; 48Fixed Frame: odomFrame Rate: 30Name: rootTools:- Class: rviz/InteractHide Inactive Objects: true- Class: rviz/MoveCamera- Class: rviz/Select- Class: rviz/FocusCamera- Class: rviz/Measure- Class: rviz/SetInitialPoseTopic: /initialpose- Class: rviz/SetGoalTopic: /move_base_simple/goal- Class: rviz/PublishPointSingle click: trueTopic: /clicked_pointValue: trueViews:Current:Class: rviz/OrbitDistance: 70.1093Enable Stereo Rendering:Stereo Eye Separation: 0.06Stereo Focal Distance: 1Swap Stereo Eyes: falseValue: falseFocal Point:X: 3.9777Y: -3.7323Z: -7.60875Name: Current ViewNear Clip Distance: 0.01Pitch: 0.355398Target Frame: <Fixed Frame>Value: Orbit (rviz)Yaw: 0.0404091Saved: ~
    Window Geometry:Displays:collapsed: falseHeight: 846Hide Left Dock: falseHide Right Dock: trueQMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000Selection:collapsed: falseTime:collapsed: falseTool Properties:collapsed: falseViews:collapsed: trueWidth: 1200X: 50Y: 45
    

68. 5.最后我们依次启动我们的launch文件便可

    (1)roslaunch kinect2_gmapping.launch
    (2)roslaunch kinect2_gmapping_rviz_view.launch
    (3)rosrun teleop_twist_keyboard teleop_twist_keyboard.py
    

69. 这样子我们就可以愉快的控制小车移动来建图了，当你建好图后运行一下指令就可以保存地图：

    rosrun map_server map_saver -f mymap
    

70. 地图保存在主文件夹，两个文件，mymap.pgm 和 mymap.yaml

    这是我建好的图

    

    6.最后看一波TF树

    

    版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/56666922

71. 基于ROS平台的移动机器人-8-使用Kinect2导航

    ready

    终于到写最后一篇了。。。不是经常写博文的老司机果然伤不起！
    在这一篇教程就是利用KinectV2来导航啦。

    go

    1.安装一下所需的包

    （1）cd ~/catkin_ws/src
    （2）git clone  https://github.com/ros-planning/navigation.git
    （3）cd ~/catkin_ws
    （4）catkin_make 
    （5）rospack profile
    

72. 2.在我们之前的建的bringup/下，新建nav_config文件夹,然后新建四个文件

    base_local_planner_params.yaml
    costmap_common_params.yaml
    global_costmap_params.yaml
    local_costmap_params.yaml
    

73. 这四个文件主要用于导航配置

    （1）base_local_planner_params.yaml文件

    controller_frequency: 2.0
    recovery_behavior_enabled: false
    clearing_rotation_allowed: falseTrajectoryPlannerROS:max_vel_x: 0.3min_vel_x: 0.05max_vel_y: 0.0  # zero for a differential drive robotmin_vel_y: 0.0min_in_place_vel_theta: 0.5escape_vel: -0.1acc_lim_x: 2.5acc_lim_y: 0.0 # zero for a differential drive robotacc_lim_theta: 3.2holonomic_robot: falseyaw_goal_tolerance: 0.1 # about 6 degreesxy_goal_tolerance: 0.15  # 10 cmlatch_xy_goal_tolerance: falsepdist_scale: 0.8gdist_scale: 0.6meter_scoring: trueheading_lookahead: 0.325heading_scoring: falseheading_scoring_timestep: 0.8occdist_scale: 0.1oscillation_reset_dist: 0.05publish_cost_grid_pc: falseprune_plan: truesim_time: 2.5sim_granularity: 0.025angular_sim_granularity: 0.025vx_samples: 8vy_samples: 0 # zero for a differential drive robotvtheta_samples: 20dwa: truesimple_attractor: false
    

74. （2）costmap_common_params.yaml文件

    obstacle_range: 2.5
    raytrace_range: 3.0
    robot_radius: 0.30
    inflation_radius: 0.15
    max_obstacle_height: 0.6
    min_obstacle_height: 0.0
    observation_sources: scan
    scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
    

75. （3）global_costmap_params.yaml文件

    global_costmap:
    global_frame: /map
    robot_base_frame: /base_link
    update_frequency: 1.0
    publish_frequency: 0
    static_map: true
    rolling_window: false
    resolution: 0.01
    transform_tolerance: 0.5
    map_type: costmap
    

76. （4）local_costmap_params.yaml文件

    local_costmap:
    global_frame: /odom
    robot_base_frame: /base_link
    update_frequency: 1.0
    publish_frequency: 1.0
    static_map: false
    rolling_window: true
    width: 6.0
    height: 6.0
    resolution: 0.01
    transform_tolerance: 0.5
    map_type: costmap
    

77. 3.在我们之前的建的bringup/下，新建map文件夹，然后将之前建图生成的两个文件
    mymap.pgm 和 mymap.yaml
    移到map文件夹里，然后修改 mymap.yaml ，做如下修改：
    将image这一行的改为你的mymap.pgm文件所在路径，如

    image: /home/forrest/catkin_ws/src/bringup/map/mymap.pgm
    

78. 4.在我们之前的建的bringup/launch/下新建kinect2_nav.launch文件

    <launch>
    <!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><param name="use_sim_time" value="false" /><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><!-- 启动kinect２驱动节点　-->            <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch"><arg name="base_name"         value="kinect2"/><arg name="sensor"            value=""/><arg name="publish_tf"        value="true"/><arg name="base_name_tf"      value="kinect2"/><arg name="fps_limit"         value="-1.0"/><arg name="calib_path"        value="$(find kinect2_bridge)/data/"/><arg name="use_png"           value="false"/><arg name="jpeg_quality"      value="90"/><arg name="png_level"         value="1"/><arg name="depth_method"      value="default"/><arg name="depth_device"      value="-1"/><arg name="reg_method"        value="default"/><arg name="reg_device"        value="-1"/><arg name="max_depth"         value="12.0"/><arg name="min_depth"         value="0.1"/><arg name="queue_size"        value="5"/><arg name="bilateral_filter"  value="true"/><arg name="edge_aware_filter" value="true"/><arg name="worker_threads"    value="4"/></include>  <!-- 启动深度图转换激光数据节点 --><node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen"><!--输入图像--><remap from="image" to="/kinect2/qhd/image_depth_rect"/><!--相关图像的相机信息。通常不需要重新变形，因为camera_info将从与图像相同的命名空间订阅。--><remap from="camera_info" to="/kinect2/qhd/camera_info" /><!--输出激光数据的话题--><remap from="scan" to="/scan" /> <!--激光扫描的帧id。对于来自具有Z向前的“光学”帧的点云，该值应该被设置为具有X向前和Z向上的相应帧。--><param name="output_frame_id" value="/kinect2_depth_frame"/><!--用于生成激光扫描的像素行数。对于每一列，扫描将返回在图像中垂直居中的那些像素的最小值。--><param name="scan_height" value="30"/><!--返回的最小范围（以米为单位）。小于该范围的输出将作为-Inf输出。--><param name="range_min" value="0.45"/><!--返回的最大范围（以米为单位）。大于此范围将输出为+ Inf。--><param name="range_max" value="8.00"/></node><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><!-- 启动地图服务器载入地图 --><node name="map_server" pkg="map_server" type="map_server" args="$(find bringup)/map/mymap.yaml"/><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><!-- 启动 move_base 节点 --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><rosparam file="$(find bringup)/nav_config/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find bringup)/nav_config/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find bringup)/nav_config/local_costmap_params.yaml" command="load" /><rosparam file="$(find bringup)/nav_config/global_costmap_params.yaml" command="load" /><rosparam file="$(find bringup)/nav_config/base_local_planner_params.yaml" command="load" /><!-- <rosparam file="$(find odom_tf_package)/config/nav_obstacles_params.yaml" command="load" /> --></node><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　-->  <!-- 启动 AMCL 节点 --><node pkg="amcl" type="amcl" name="amcl" clear_params="true"><param name="use_map_topic" value="false"/><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="60"/><param name="laser_max_range" value="12.0"/><param name="min_particles" value="500"/><param name="max_particles" value="2000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.2"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><!-- <param name="laser_model_type" value="beam"/> --><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.25"/><param name="update_min_a" value="0.2"/><param name="odom_frame_id" value="odom"/><param name="resample_interval" value="1"/><!-- Increase tolerance because the computer can get quite busy --><param name="transform_tolerance" value="1.0"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><!-- scan topic --><remap from="scan" to="scan"/></node><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><!-- 启动基础控制器节点：发布里程计，控制小车　--><node name="base_controller" pkg="base_controller" type="base_controller"/><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　--><!-- tf　转换 --><!-- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_footprint base_link 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0.5 0 0 0 base_link laser 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2laser" args="0 0 0.5 0 0 0 base_link kinect2_depth_frame 50" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_kinect2_link" args="0 0 0.5 -1.57 0 -1.57 base_link kinect2_link 50" /><!-- ××××××××××××××××××××××××××××××××××××××××××××　华丽分割线　××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××　-->  
    </launch>
    

79. 5.在我们之前的建的bringup/launch/下新建kinect2_nav_rviz_view.launch文件

    <?xml version="1.0"?><launch><!--start rviz view --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find bringup)/rviz/kinect2_nav.rviz" /></launch>
    

80. 6.还有在新建bringup/rviz/下新建kinect2_nav.rviz文件

    Panels:- Class: rviz/DisplaysHelp Height: 78Name: DisplaysProperty Tree Widget:Expanded:- /Global Options1- /Odometry1- /RobotModel1/Links1/base_footprint1- /Marker1/Namespaces1Splitter Ratio: 0.652661Tree Height: 457- Class: rviz/SelectionName: Selection- Class: rviz/Tool PropertiesExpanded:- /2D Pose Estimate1- /2D Nav Goal1Name: Tool PropertiesSplitter Ratio: 0.428571- Class: rviz/ViewsExpanded:- /Current View1Name: ViewsSplitter Ratio: 0.5- Class: rviz/TimeExperimental: falseName: TimeSyncMode: 0SyncSource: ""
    Visualization Manager:Class: ""Displays:- Alpha: 0.5Cell Size: 0.5Class: rviz/GridColor: 160; 160; 164Enabled: trueLine Style:Line Width: 0.03Value: LinesName: GridNormal Cell Count: 0Offset:X: 0Y: 0Z: 0Plane: XYPlane Cell Count: 80Reference Frame: odomValue: true- Angle Tolerance: 0.1Class: rviz/OdometryColor: 221; 200; 14Enabled: trueKeep: 100Length: 0.6Name: OdometryPosition Tolerance: 0.1Topic: /odomValue: true- Angle Tolerance: 0.1Class: rviz/OdometryColor: 255; 85; 0Enabled: falseKeep: 100Length: 0.6Name: Odometry EKFPosition Tolerance: 0.1Topic: /odom_ekfValue: false- Alpha: 1Class: rviz/RobotModelCollision Enabled: falseEnabled: trueLinks:All Links Enabled: trueExpand Joint Details: falseExpand Link Details: falseExpand Tree: falseLink Tree Style: Links in Alphabetic Orderbase_footprint:Alpha: 1Show Axes: falseShow Trail: falseValue: truebase_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truecamera_depth_frame:Alpha: 1Show Axes: falseShow Trail: falsecamera_depth_optical_frame:Alpha: 1Show Axes: falseShow Trail: falsecamera_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truecamera_rgb_frame:Alpha: 1Show Axes: falseShow Trail: falsecamera_rgb_optical_frame:Alpha: 1Show Axes: falseShow Trail: falsefront_wheel_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truegyro_link:Alpha: 1Show Axes: falseShow Trail: falselaser:Alpha: 1Show Axes: falseShow Trail: falseValue: trueleft_cliff_sensor_link:Alpha: 1Show Axes: falseShow Trail: falseleft_wheel_link:Alpha: 1Show Axes: falseShow Trail: falseValue: trueleftfront_cliff_sensor_link:Alpha: 1Show Axes: falseShow Trail: falseplate_0_link:Alpha: 1Show Axes: falseShow Trail: falseValue: trueplate_1_link:Alpha: 1Show Axes: falseShow Trail: falseValue: trueplate_2_link:Alpha: 1Show Axes: falseShow Trail: falseValue: trueplate_3_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truerear_wheel_link:Alpha: 1Show Axes: falseShow Trail: falseValue: trueright_cliff_sensor_link:Alpha: 1Show Axes: falseShow Trail: falseright_wheel_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truerightfront_cliff_sensor_link:Alpha: 1Show Axes: falseShow Trail: falsespacer_0_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truespacer_1_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truespacer_2_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truespacer_3_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_0_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_1_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_2_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_3_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_4_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_5_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_6_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_2in_7_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_8in_0_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_8in_1_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_8in_2_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_8in_3_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_kinect_0_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truestandoff_kinect_1_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truewall_sensor_link:Alpha: 1Show Axes: falseShow Trail: falseName: RobotModelRobot Description: robot_descriptionTF Prefix: ""Update Interval: 0Value: trueVisual Enabled: true- Alpha: 0.2Class: rviz/MapColor Scheme: mapDraw Behind: trueEnabled: trueName: MapTopic: /mapValue: true- Alpha: 1Buffer Length: 1Class: rviz/PathColor: 255; 0; 0Enabled: trueName: Local PlanTopic: /move_base/TrajectoryPlannerROS/local_planValue: true- Alpha: 1Buffer Length: 1Class: rviz/PathColor: 0; 213; 0Enabled: trueName: Global PlanTopic: /move_base/TrajectoryPlannerROS/global_planValue: true- Class: rviz/MarkerEnabled: trueMarker Topic: /waypoint_markersName: MarkerNamespaces:{}Queue Size: 100Value: true- Alpha: 1Autocompute Intensity Bounds: trueAutocompute Value Bounds:Max Value: 0.304Min Value: 0.304Value: trueAxis: ZChannel Name: intensityClass: rviz/LaserScanColor: 255; 255; 255Color Transformer: FlatColorDecay Time: 0Enabled: trueInvert Rainbow: falseMax Color: 255; 255; 255Max Intensity: 4096Min Color: 0; 0; 0Min Intensity: 0Name: LaserScanPosition Transformer: XYZQueue Size: 10Selectable: trueSize (Pixels): 3Size (m): 0.01Style: SpheresTopic: /scanUse Fixed Frame: trueUse rainbow: trueValue: true- Alpha: 1Axes Length: 1Axes Radius: 0.1Class: rviz/PoseColor: 0; 255; 0Enabled: trueHead Length: 0.1Head Radius: 0.15Name: Mouse GoalShaft Length: 0.5Shaft Radius: 0.03Shape: ArrowTopic: /move_base/current_goalValue: true- Alpha: 1Axes Length: 1Axes Radius: 0.1Class: rviz/PoseColor: 0; 255; 0Enabled: trueHead Length: 0.1Head Radius: 0.15Name: Goal PoseShaft Length: 0.5Shaft Radius: 0.03Shape: ArrowTopic: /move_base_simple/goalValue: true- Arrow Length: 0.3Class: rviz/PoseArrayColor: 255; 25; 0Enabled: falseName: Pose ArrayTopic: ""Value: falseEnabled: trueGlobal Options:Background Color: 0; 0; 0Fixed Frame: mapFrame Rate: 30Name: rootTools:- Class: rviz/MoveCamera- Class: rviz/InteractHide Inactive Objects: true- Class: rviz/Select- Class: rviz/SetInitialPoseTopic: /initialpose- Class: rviz/SetGoalTopic: /move_base_simple/goalValue: trueViews:Current:Angle: -1.57Class: rviz/TopDownOrthoName: Current ViewNear Clip Distance: 0.01Scale: 205.257Target Frame: <Fixed Frame>Value: TopDownOrtho (rviz)X: 0.755064Y: 0.634474Saved: ~
    Window Geometry:Displays:collapsed: falseHeight: 695Hide Left Dock: falseHide Right Dock: falseQMainWindow State: 000000ff00000000fd00000004000000000000012d00000258fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000258000000dd00ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000270000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002b80000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000Selection:collapsed: falseTime:collapsed: falseTool Properties:collapsed: falseViews:collapsed: falseWidth: 1003X: 787Y: 348
    

81. 7.最后我们依次启动我们的launch文件便可进行导航

    (1)roslaunch kinect2_nav.launch
    (2)roslaunch kinect2_nav_rviz_view.launch
    

82. 启动成功的话会看到一下画面：

    

    如果rviz显示的机器人位置和实际机器人所处的位置不一样，我们可以根据机器人实际所处的位置在rviz上初始化机器人在地图上的位置

    方法为点击rviz界面上方的2D Pose Estimate ，然后点击地图，确定机器人所在位置

    最后一步就是给机器人定个目的地让它行驶到那里，

    方法为点击rviz界面上方的2D Nav Goal ， 然后点击地图上的某个位置，这样子就可以看到机器人开始向目标移动啦！

    版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/56670514

83.