基于ROS平台的移动机器人-目录
说明
本系列博文将介绍基于ROS平台的移动机器人的搭建，包括小车的底盘的制作，控制和与上位机的通信，建图和导航。
目录
- 基于ROS平台的移动机器人-1-小车底盘的搭建
- 基于ROS平台的移动机器人-2-小车底盘控制
- 基于ROS平台的移动机器人-3-小车底盘与ROS的通信
- 基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动
- 基于ROS平台的移动机器人-5-Kinect2驱动的安装和ROS下的测试
- 基于ROS平台的移动机器人-6-使用Kinect2获取激光数据
- 基于ROS平台的移动机器人-7-使用Kinect2建图
- 基于ROS平台的移动机器人-8-使用Kinect2导航
注意
本人Ubuntu版本为14.04，ROS版本为indigo
基于ROS平台的移动机器人-1-小车底盘的搭建
说明
本博文将介绍小车底盘的搭建需要的硬件和搭建过程
物品清单
- 亚克力板或者铝合金小车底板一套
- 小车轮子一对
- 带编码器减速电机一对
- 万向轮一个
- 12V充电电池一个
- 降压模块一个
- 电机驱动模块一个
- STM32核心板一块
- Kinect2一个
- 工控机或者笔记本一个
搭建步骤
- 分别将电池，降压模块，电机，万向轮，电机驱动板，STM32核心板，Kinect2和工控机装好在底盘板上
- 将轮子分别装上电机
- 将电池电源引出两路，一路接在降压模块，一路接在电机驱动模块
- 将电机驱动线接到驱动模块
- 降压模块得到的5V给STM32核心板供电
- 将STM32核心板的串口接到工控机
- 将Kinect2的usb口接到工控机的USB3.0口
- 小车搭建完成
- 基于ROS平台的移动机器人-2制-小车底盘控
-
说明
本博文将介绍小车底盘控制的原理，如PID控制，控制程序的编写等。
小车控制思想
- 控制电机转动。电机的控制我们分为两部分，一部分为电机转动方向的控制，另一个为电机转速的控制。电机转动的方向我们用两个MCU引脚来控制，假如PIN_A=1，PIN_B=0 时，电机正转；PIN_A=0，PIN_B=1 时，电机反转；PIN_A=0，PIN_B=0 时，电机停止。电机速度的控制则需要一个PWM输出引脚，我们通过控制输出不同的PWM值来控制电机转动的速度。
- PID控制。如果我们想控制小车以一米每秒的速度做直线，但由于地面的摩擦阻力的影响，会造成左右轮速度与我们想控制的速度不同，所以会走不直，这时我们就需要加入PID控制，PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比，差则补，多则减。这样基本就可以实现理想控制。详细PID介绍查看。
- 小车转弯控制。一般我们要是想控制小车以多少的速度前进或者后退，我们只需要PID控制两个轮子的速度一致就可以基本做到。但如果要想控制小车以多少的角速度转弯，我们需要做一定的计算，如图所示：
-
推算过程这里就不算了，我们可以得到左右轮速度和线速度角速度之间的关系如下：
通过以上的公式我们就可以控制小车的任意行走了。
程序结构
以下为我的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;//单位mm y_data.odoemtry_float=position_y;//单位mm theta_data.odoemtry_float=oriention;//单位rad vel_linear.odoemtry_float=velocity_linear;//单位mm/s vel_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/s odometry_left=leftdata.d;//单位mm/s USART_RX_STA=0;//清楚接收标志位 } car_control(rightdata.d,leftdata.d); //将接收到的左右轮速度赋给小车 }//end_while }//end main /*********************************************END OF FILE**************************************************/ -
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.987 float 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; } } } -
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) ; } -
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/min wtemp3 = (wtemp3 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER1_PPR); wtemp4 = (wtemp4 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER2_PPR); hRot_Speed2= ((s16)(wtemp4));//平均转速 r/min hRot_Speed1= ((s16)(wtemp3));//平均转速 r/min Speed2=hRot_Speed2;//平均转速 r/min Speed1=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赋值PWM TIM2->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++; //脉冲数累加 } } -
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 //} -
版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/55001231
-
基于ROS平台的移动机器人-3-小车底盘与ROS的通信
1.ROS平台与底盘通信协议
ROS平台与小车底盘的通信一般是通过串口或者CAN总线。我这里采用的是串口，以下为我自定义的通信数据格式：
（1）底盘串口部分
-
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字节] -
（2）ROS平台串口节点部分
1.写入串口 （1）内容：左右轮速度，单位为mm/s （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节] 2.读取串口 （1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节] -
（1）小车底盘串口处理程序。
底盘串口处理的程序主要写在了 main.c 文件中。
底盘向串口发送里程计代码：
if(main_sta&0x01)//执行发送里程计数据步骤 { //里程计数据获取 x_data.odoemtry_float=position_x;//单位mm y_data.odoemtry_float=position_y;//单位mm theta_data.odoemtry_float=oriention;//单位rad vel_linear.odoemtry_float=velocity_linear;//单位mm/s vel_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(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/s odometry_left=leftdata.d;//单位mm/s USART_RX_STA=0;//清楚接收标志位 } -
（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/s linear_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/s right_speed_data.d*=ratio;//放大１０００倍，mm/s for(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); } -
ROS平台接收串口发来的里程计代码：
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据 const char *receive_data=rec_buffer.data(); //保存串口发送来的数据2.串口通信处理程序
-
基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动
准备工作
1.下载串口通信的ROS包
(1)cd ~/catkin_ws/src (2)git clone https://github.com/Forrest-Z/serial.git -
2.下载键盘控制的ROS包
(1)cd ~/catkin_ws/src (2)git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git -
进入下载好的ROS包的文件夹，选中 keyboard_teleop_zbot.py ，右键>属性>权限>勾选 允许作为程序执行文件。
最后一步：（1）cd ~/catkin_ws (2)catkin_make -
这样子我们的键盘控制包就能使用了。
3.新建 base_controller ROS 包
（1）cd ~/catkin_ws/src （2）catkin_create_pkg base_controller roscpp -
在新建好的ROS包文件夹下新建一个“src”的文件夹，然后进入该文件夹，新建一个base_controller.cpp文件，将本博文最后提供的代码粘贴进去，然后修改一下 CMakeLists.txt ：
第一处修改
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation serial tf nav_msgs ) -
第二处修改
catkin_package( # INCLUDE_DIRS include # LIBRARIES base_controller CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) -
第三处修改
include_directories( ${catkin_INCLUDE_DIRS} ${serial_INCLUDE_DIRS} ) -
第四处修改
add_executable(base_controller src/base_controller.cpp) target_link_libraries(base_controller ${catkin_LIBRARIES}) -
然后修改一下 package.xml ：
<?xml version="1.0"?> <package> <name>base_controller</name> <version>0.0.0</version> <description>The base_controller package</description> email="[email protected]">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> -
控制原理
1.当我们按下键盘时，teleop_twist_keyboard 包会发布 /cmd_vel 主题发布速度
2.我们在 base_controller 节点订阅这个话题，接收速度数据，在转换成与底盘通信的格式，然后写入串口
3.我们在 base_controller 节点读取底盘向串口发来的里程计数据，然后进行处理再将里程计发布出去，同时更新tf
4.当小车底盘接收到串口发来的速度后，就控制电机运转，从而实现键盘控制小车的移动
运行
1.启动键盘节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py -
2.启动小车底盘控制节点
rosrun base_controller base_controller -
注意事项
1.我们在启动小车底盘控制节点时，有可以启动不了，大多数是因为串口的端口号不对，在 base_controller.cpp 文件里，我用的是”/dev/ttyUSB0”串口端口号
2.我们在启动启动小车底盘控制节点前，应该查看一下我们底盘的串口号是否正确，查看指令如下：
ls -l /dev |grep ttyUSB -
如果运行后显示的端口号和我们程序中的一样，那就没问题，如果不一样，我们将程序的代码改动一下便可。
-
base_controller.cpp 完整代码：
-
/****************************************************************** 基于串口通信的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/s linear_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/s right_speed_data.d*=ratio;//放大１０００倍，mm/s for(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_x 0, 0.01, 0, 0, 0, 0, // covariance on gps_y 0, 0, 99999, 0, 0, 0, // covariance on gps_z 0, 0, 0, 99999, 0, 0, // large covariance on rot x 0, 0, 0, 0, 99999, 0, // large covariance on rot y 0, 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; //m position_y.d/=1000; //m vel_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; } -
基于ROS平台的移动机器人-5-Kinect2驱动的安装和ROS下的测试
说明
我们这里要测试的对象是Kinect2！！！
实物如图：
驱动安装
1.首先git下载代码，很快下载好，放到~下面
git clone https://github.com/OpenKinect/libfreenect2.git -
2.然后安装依赖项如下,最好事先编译安装好OpenCV
sudo apt-get install build-essential cmake pkg-config libturbojpeg libjpeg-turbo8-dev mesa-common-dev freeglut3-dev libxrandr-dev libxi-dev -
3.然后安装libusb。此处需要添加一个PPA
sudo apt-add-repository ppa:floe/libusb sudo apt-get update sudo apt-get install libusb-1.0-0-dev -
4.接着运行下面的命令，安装GLFW3
sudo apt-get install libglfw3-dev -
如果没有成功的话，使用下面的命令，来代替上面的
cd libfreenect2/depends sh install_ubuntu.sh sudo dpkg -i libglfw3*_3.0.4-1_*.deb -
5.接着编译库
cd .. mkdir build && cd build cmake .. make sudo make install -
6.测试
（1）需要把libfreenect2文件夹下面的rules里面的一个90开头的文件复制到/etc/udev/rules.d/下面
sudo cp ../platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/ -
（2）插上kinect2，此时黄灯变成白色的，表示有驱动。注意：只能用于USB3的接口
（3）在中断进入刚才上面新建的build文件夹里的bin文件夹，运行：
./bin/Protonect -
不出意外的话就可以看到显示的图像了。
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 -
2.测试
（1）启动kinect2驱动
roslaunch kinect2_bridge kinect2_bridge.launch -
（2）启动显示界面
rosrun kinect2_viewer kinect2_viewer -
如果能显示出画面的话就没问题了
注意事项
Kinect2只能用于USB3的接口！！！
-
基于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 -
2.我这里新建了一个 bringup 的包来专门存放 launch 文件
（1）cd ~/catkin_ws/src （2）catkin_create_pkg bringup roscpp -
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> -
4.在 bringup 包里新建 rviz 文件夹，然后在 rviz 文件夹里添加
kinect2_depthimage_to_laserscan_view.rviz文件Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /LaserScan1 Splitter Ratio: 0.5 Tree Height: 566 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: <Fixed Frame> Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 0 Min Value: 0 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.01 Style: Points Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true kinect2_depth_frame: Value: true kinect2_ir_optical_frame: Value: true kinect2_link: Value: true kinect2_rgb_optical_frame: Value: true laser: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: base_footprint: kinect2_depth_frame: {} kinect2_link: kinect2_rgb_optical_frame: kinect2_ir_optical_frame: {} laser: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: laser Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 Z: 0 Name: Current View Near Clip Distance: 0.01 Pitch: 0.810398 Target Frame: <Fixed Frame> Value: Orbit (rviz) Yaw: 3.2504 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1200 X: 50 Y: 45 -
5.基本工作我们都做完了，现在我们需要编译一下
（1）cd ~/catkin_ws （2）catkin_make （3）rospack profile -
6.我们现在可以接上KinectV2（注意！！！接USB3.0口），执行
roslaunch bringup kinect2_depthimage_to_laserscan_rviz_view.launch -
没有问题的话我们可以看到一下界面：
查看TF树：文件生成在主文件夹
rosrun tf view_frames -
这是我的TF树
-
基于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 -
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> -
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> -
4.还有在新建bringup/rviz/下新建
kinect2_gmapping.rviz文件Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Map1 Splitter Ratio: 0.5 Tree Height: 566 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: <Fixed Frame> Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true base_link: Value: true kinect2_depth_frame: Value: true kinect2_ir_optical_frame: Value: true kinect2_link: Value: true kinect2_rgb_optical_frame: Value: true laser: Value: true map: Value: true odom: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: map: odom: base_footprint: base_link: kinect2_depth_frame: {} kinect2_link: kinect2_rgb_optical_frame: kinect2_ir_optical_frame: {} laser: {} Update Interval: 0 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 1 Min Value: 1 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: odom Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 70.1093 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 3.9777 Y: -3.7323 Z: -7.60875 Name: Current View Near Clip Distance: 0.01 Pitch: 0.355398 Target Frame: <Fixed Frame> Value: Orbit (rviz) Yaw: 0.0404091 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1200 X: 50 Y: 45 -
5.最后我们依次启动我们的launch文件便可
(1)roslaunch kinect2_gmapping.launch (2)roslaunch kinect2_gmapping_rviz_view.launch (3)rosrun teleop_twist_keyboard teleop_twist_keyboard.py -
这样子我们就可以愉快的控制小车移动来建图了，当你建好图后运行一下指令就可以保存地图：
rosrun map_server map_saver -f mymap -
地图保存在主文件夹，两个文件，mymap.pgm 和 mymap.yaml
这是我建好的图
6.最后看一波TF树
版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/56666922
-
基于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 -
2.在我们之前的建的bringup/下，新建nav_config文件夹,然后新建四个文件
base_local_planner_params.yaml costmap_common_params.yaml global_costmap_params.yaml local_costmap_params.yaml -
这四个文件主要用于导航配置
（1）
base_local_planner_params.yaml文件controller_frequency: 2.0 recovery_behavior_enabled: false clearing_rotation_allowed: false TrajectoryPlannerROS: max_vel_x: 0.3 min_vel_x: 0.05 max_vel_y: 0.0 # zero for a differential drive robot min_vel_y: 0.0 min_in_place_vel_theta: 0.5 escape_vel: -0.1 acc_lim_x: 2.5 acc_lim_y: 0.0 # zero for a differential drive robot acc_lim_theta: 3.2 holonomic_robot: false yaw_goal_tolerance: 0.1 # about 6 degrees xy_goal_tolerance: 0.15 # 10 cm latch_xy_goal_tolerance: false pdist_scale: 0.8 gdist_scale: 0.6 meter_scoring: true heading_lookahead: 0.325 heading_scoring: false heading_scoring_timestep: 0.8 occdist_scale: 0.1 oscillation_reset_dist: 0.05 publish_cost_grid_pc: false prune_plan: true sim_time: 2.5 sim_granularity: 0.025 angular_sim_granularity: 0.025 vx_samples: 8 vy_samples: 0 # zero for a differential drive robot vtheta_samples: 20 dwa: true simple_attractor: false -
（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} -
（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 -
（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 -
3.在我们之前的建的bringup/下，新建map文件夹，然后将之前建图生成的两个文件
mymap.pgm 和 mymap.yaml
移到map文件夹里，然后修改 mymap.yaml ，做如下修改：
将image这一行的改为你的mymap.pgm文件所在路径，如image: /home/forrest/catkin_ws/src/bringup/map/mymap.pgm -
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> -
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> -
6.还有在新建bringup/rviz/下新建
kinect2_nav.rviz文件Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Odometry1 - /RobotModel1/Links1/base_footprint1 - /Marker1/Namespaces1 Splitter Ratio: 0.652661 Tree Height: 457 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 Name: Tool Properties Splitter Ratio: 0.428571 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 0.5 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 80 Reference Frame: odom Value: true - Angle Tolerance: 0.1 Class: rviz/Odometry Color: 221; 200; 14 Enabled: true Keep: 100 Length: 0.6 Name: Odometry Position Tolerance: 0.1 Topic: /odom Value: true - Angle Tolerance: 0.1 Class: rviz/Odometry Color: 255; 85; 0 Enabled: false Keep: 100 Length: 0.6 Name: Odometry EKF Position Tolerance: 0.1 Topic: /odom_ekf Value: false - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order base_footprint: Alpha: 1 Show Axes: false Show Trail: false Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true camera_depth_frame: Alpha: 1 Show Axes: false Show Trail: false camera_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true camera_rgb_frame: Alpha: 1 Show Axes: false Show Trail: false camera_rgb_optical_frame: Alpha: 1 Show Axes: false Show Trail: false front_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true gyro_link: Alpha: 1 Show Axes: false Show Trail: false laser: Alpha: 1 Show Axes: false Show Trail: false Value: true left_cliff_sensor_link: Alpha: 1 Show Axes: false Show Trail: false left_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true leftfront_cliff_sensor_link: Alpha: 1 Show Axes: false Show Trail: false plate_0_link: Alpha: 1 Show Axes: false Show Trail: false Value: true plate_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true plate_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true plate_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true rear_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true right_cliff_sensor_link: Alpha: 1 Show Axes: false Show Trail: false right_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true rightfront_cliff_sensor_link: Alpha: 1 Show Axes: false Show Trail: false spacer_0_link: Alpha: 1 Show Axes: false Show Trail: false Value: true spacer_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true spacer_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true spacer_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_0_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_4_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_5_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_6_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_2in_7_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_8in_0_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_8in_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_8in_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_8in_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_kinect_0_link: Alpha: 1 Show Axes: false Show Trail: false Value: true standoff_kinect_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true wall_sensor_link: Alpha: 1 Show Axes: false Show Trail: false Name: RobotModel Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - Alpha: 0.2 Class: rviz/Map Color Scheme: map Draw Behind: true Enabled: true Name: Map Topic: /map Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Name: Local Plan Topic: /move_base/TrajectoryPlannerROS/local_plan Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 213; 0 Enabled: true Name: Global Plan Topic: /move_base/TrajectoryPlannerROS/global_plan Value: true - Class: rviz/Marker Enabled: true Marker Topic: /waypoint_markers Name: Marker Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 0.304 Min Value: 0.304 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.01 Style: Spheres Topic: /scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.1 Class: rviz/Pose Color: 0; 255; 0 Enabled: true Head Length: 0.1 Head Radius: 0.15 Name: Mouse Goal Shaft Length: 0.5 Shaft Radius: 0.03 Shape: Arrow Topic: /move_base/current_goal Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.1 Class: rviz/Pose Color: 0; 255; 0 Enabled: true Head Length: 0.1 Head Radius: 0.15 Name: Goal Pose Shaft Length: 0.5 Shaft Radius: 0.03 Shape: Arrow Topic: /move_base_simple/goal Value: true - Arrow Length: 0.3 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: false Name: Pose Array Topic: "" Value: false Enabled: true Global Options: Background Color: 0; 0; 0 Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/Select - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal Value: true Views: Current: Angle: -1.57 Class: rviz/TopDownOrtho Name: Current View Near Clip Distance: 0.01 Scale: 205.257 Target Frame: <Fixed Frame> Value: TopDownOrtho (rviz) X: 0.755064 Y: 0.634474 Saved: ~ Window Geometry: Displays: collapsed: false Height: 695 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd00000004000000000000012d00000258fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000258000000dd00ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000270000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002b80000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1003 X: 787 Y: 348 -
7.最后我们依次启动我们的launch文件便可进行导航
(1)roslaunch kinect2_nav.launch (2)roslaunch kinect2_nav_rviz_view.launch -
启动成功的话会看到一下画面：
如果rviz显示的机器人位置和实际机器人所处的位置不一样，我们可以根据机器人实际所处的位置在rviz上初始化机器人在地图上的位置
方法为点击rviz界面上方的2D Pose Estimate ，然后点击地图，确定机器人所在位置
最后一步就是给机器人定个目的地让它行驶到那里，
方法为点击rviz界面上方的2D Nav Goal ， 然后点击地图上的某个位置，这样子就可以看到机器人开始向目标移动啦！
版权声明：本文为博主原创文章，如若转载，请标明作者和原文出处！ https://blog.csdn.net/Forrest_Z/article/details/56670514
