爱折腾-创客智造实验室 - ROS机器人Diego制作
通过ROS机器人Diego制作学习，了解轮式底盘控制，PID调速，语音控制，SLAM导航，机器臂控制等。(来自Diego)
ROS机器人Diego制作目录 说明： 本专栏为转载，在原教程基础上，内容重新整理和补充 介绍从头开始打造自组装的轮式机器人 介绍如何融合ROS和自组装机器人 介绍如何进行线速度和角速度的标定或校准 介绍如何增加语音控制应用 介绍如何实现SLAM导航应用 介绍如何制作URDF模型 介绍如何增加机械臂和利用机械臂 小车图： 作者： 昵称：Diego 邮箱：mwlwlm@163.com QQ：38182368 简介：软件开发出身，爱好嵌入式开发，关注ROS机器人、人工智能、机器学习。 博客地址：http://blog.csdn.net/mwlwlm 原文： http://blog.csdn.net/mwlwlm/article/category/6449432/1 视频： http://v.youku.com/v_show/id_XMTgxNDgyMDU4OA==.html 目录： ROS机器人Diego制作目录 ROS机器人Diego制作1-机器人硬件清单 ROS机器人Diego制作2-利用rosserial_arduino实现base controller ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller ROS机器人Diego制作4-线速度的标定 ROS机器人Diego制作5-角速度的标定 ROS机器人Diego制作6-实现两个马达独立PID调速 ROS机器人Diego制作7-ROS语音系统之sphinx ROS机器人Diego制作8-ROS语音系统之科大讯飞语音 ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion ROS机器人Diego制作10-ROS视觉系统之摄像头TF数据的发布 ROS机器人Diego制作11-ROS视觉系统之点云数据转换成激光数据 ROS机器人Diego制作12-SLAM导航的配置 ROS机器人Diego制作13-整合统一的launch启动文件 ROS机器人Diego制作14-机械臂控制之arduino驱动 ROS机器人Diego制作15-机械臂控制之键盘控制 ROS机器人Diego制作16-创建机器人的urdf模型描述文件 ROS机器人Diego制作17-配置moveit，生成配置包
ROS机器人Diego制作1-机器人硬件清单 说明： 介绍教程需要的硬件 均采用开源软硬件，可以方便在淘宝购买 硬件清单： 序号 物料名称 说明 1 树莓派3 B+ 作为上位机，安装ROS机器人操作系统 2 Arduino UNO 作为下位机，实现对底盘等硬件设备的控制 3 底盘 可以自己到淘宝上选，注意一定要带码盘测速的，因为ROS要精确控制 4 电源模块 为树莓派3提供5V2.5A的电源 5 电源 9V~12V 18650充电电池 6 Arduino 电机驱动板，Arduino电机驱动板 L298P占有IO少 7 机械臂 6自由度机械臂 8 其他 线材，六角铜柱、螺丝等 ROS控制模型： 树莓派开发板： 安装kinetic ROS版本 Arduino开发板: 官网IDE下载
ROS机器人Diego制作2-利用rosserial_arduino实现base controller
ROS机器人Diego制作2-利用rosserial_arduino实现base controller 说明： 介绍软件包rosserial_arduino使用，此软件包用于驱动底盘 ROS的控制模型中，对底盘的驱动+马达的PID调节 = base controller 参考：http://wiki.ros.org/rosserial_arduino/Tutorials rosserial_arduino的安装 详细的安装和使用参考，ROS与Arduino专栏。 下载：进入你的workspace目录下的src目录，catkin_ws是workspace cd ~/catkin_ws/src git clone https://github.com/ros-drivers/rosserial.git 编译：在workspace目录编译 cd <catkin_ws> catkin_make 在Arduino的/libraries目录下生产ros_lib包 一般情况Arduino的libraries目录是/home/username/sketchbook/libraries 其中username代表的是登录linux的用户名 cd /home/username/sketchbook/libraries rosrun rosserial_arduino make_libraries.py 如果提示找不到rosserial_arduino make_libraries.py，则需要执行如下代码： rosserial_arduino make_libraries.py 这时候libraries目录下就会有一个ros_lib的目录，重启Arduino IDE既可以看到Ros_lib包了 也可以copy出来在其他电脑上使用 Arduino上实现ROS Node，订阅Twist msg 首先需要包含ros的头文件： #include <PID_v1.h> #include <ArduinoHardware.h> #include <ros.h> #include <geometry_msgs/Twist.h> #include <ros/time.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> 声明node句柄 ros::NodeHandle nh; 定义收到Twist msg后的处理函数 void motor_cb(const geometry_msgs::Twist& vel) { linear = vel.linear.x * 100; //ROS中的单位是m/s;这里换算成cm的单位，在Diego机器人中使用CM作为单位 angular = vel.angular.z; } //这里先暂时订阅Turtle1 package的Twist消息，后面根据自己的需要可以修改 ros::Subscriber<geometry_msgs::Twist> sub("/turtle1/cmd_vel", motor_cb); 在loop中执行 nh.spinOnce(); 到这里Arduino已经可以作为一个Node的节点接收上位机的Twist msg了 底盘驱动及PID控制 引脚定义,底盘马达驱动采用了L298P模块 #define E_left 5 //L298P直流电机驱动板的左轮电机使能端口连接到数字接口5 #define M_left 4 //L298P直流电机驱动板的左轮电机转向端口连接到数字接口4 #define E_right 6 //连接小车右轮电机的使能端口到数字接口6 #define M_right 7 //连接小车右轮电机的转向端口到数字接口7 电机马达码盘中断定义 #define Pin_left 2 //外部中断0,左轮 #define Pin_right 3 //外部中断1,右轮 底盘前进-后退-停止控制 void advance()//前进 { digitalWrite(M_left, HIGH); analogWrite(E_left, val_left); digitalWrite(M_right, HIGH); analogWrite(E_right, val_right); } void back()//后退 { digitalWrite(M_left, LOW); analogWrite(E_left, val_left); digitalWrite(M_right, LOW); analogWrite(E_right, val_right); } void Stop()//停止 { digitalWrite(E_right, LOW); digitalWrite(E_left, LOW); } PID控制 采用Ardunio的PID控制包Arduino-PID-Library https://github.com/br3ttb/Arduino-PID-Library/ 由于需要分别对两个马达控制所以需要分别设定两个马达的PID控制参数 //PID double left_Setpoint, left_Input, left_Output, left_setpoint; double left_kp = 1, left_ki = 0.005, left_kd = 0.0001; PID left_PID(&left_Input, &left_Output, &left_Setpoint, left_kp, left_ki, left_kd, DIRECT); double right_Setpoint, right_Input, right_Output, right_setpoint; double right_kp = 0.8, right_ki = 0.005, right_kd = 0.0021; PID right_PID(&right_Input, &right_Output, &right_Setpoint, right_kp, right_ki, right_kd, DIRECT); 即使相同型号的电机，其PID的调节参数都可能不一样，需要单独调节，需要反复测试调节，相关调节方法可以到百度 PID处理函数 void PID_left() { Serial.println("********************************begin PID left"); left_Input = count_left * 10; left_PID.Compute(); val_left = val_left + left_Output; if (val_left > 255) val_left = 255; if (val_left < 0) val_left = 0; if (run_direction == 'f') //根据刚刚调节后的小车电机PWM功率值，及时修正小车前进或者后退状态 advance(); if (run_direction == 'b') back(); Serial.println("********************************end PID Left"); } void PID_right() { Serial.println("********************************begin PID Right"); right_Input = count_right * 10; right_PID.Compute(); val_right = val_right + right_Output; if (val_right > 255) val_right = 255; if (val_right < 0) val_right = 0; if (run_direction == 'f') //根据刚刚调节后的小车电机PWM功率值，及时修正小车前进或者后退状态 advance(); if (run_direction == 'b') back(); Serial.println("********************************end PID Right"); } 在本中PID调节在两个中断处理函数中调用 // 左侧车轮电机的编码器码盘计数中断子程序 void Code1() { //为了不计入噪音干扰脉冲， //当2次中断之间的时间大于2ms时，计一次有效计数 //Serial.println("Code1"); if ((millis() - time1) > 2) { //当编码器码盘的OUT脉冲信号下跳沿每中断一次， count_left++; // 编码器码盘计数加一 if ((millis() - left_old_time) > 100) { ////////100ms进行一次PID调节 PID_left(); left_old_time = millis(); count_left = 0;//把脉冲计数值清零，以便计算下一秒的脉冲计数 } time1 == millis(); } } // 右侧车轮电机的编码器码盘计数中断子程序 void Code2() { //Serial.println("Code2"); if ((millis() - time2) > 2) { //当编码器码盘的OUT脉冲信号下跳沿每中断一次， count_right++; // 编码器码盘计数加一 if ((millis() - right_old_time) > 100) { ////////100ms进行一次PID调节 PID_right(); right_old_time = millis(); count_right = 0; //把脉冲计数值清零，以便计算下一秒的脉冲计数 } time2 == millis(); } } 之所以在中断处理函数中调用，ROS Twist的发布时按照一定的频率不断的发送给下位机的 在主loop中不断的接收Twist消息并设定行驶的线速度，和角速度 而PID调节必须比ROS发布短的周期里面去调节速度，如果不能这样PID调节其实就没什么意义了。 完整代码： #include <PID_v1.h> //#include <ArduinoHardware.h> #include <ros.h> #include <geometry_msgs/Twist.h> #include <ros/time.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> #define Pin_left 2 //外部中断0,左轮 #define Pin_right 3 //外部中断1,右轮 #define max_linear 20 //最大线速度cm/秒 #define max_turn_line 18 //最大转弯线速度 //#define max_angular 1.45 #define max_linear_pwd 255 #define hole_number 2 //码盘孔数 #define diameter 18.535 //轮cm直径 #define diamete_ratio 1.12167 //左轮相对于右轮轮径比系数,往左偏,调小,往右偏调大 #define center_speed 220 //小车电机的PWM功率初始值 #define gear_ratio 75 //转速比 #define car_width 27 //小车宽度 #define car_length 27 //小车长度 #define E_left 5 //L298P直流电机驱动板的左轮电机使能端口连接到数字接口5 #define M_left 4 //L298P直流电机驱动板的左轮电机转向端口连接到数字接口4 #define E_right 6 //连接小车右轮电机的使能端口到数字接口6 #define M_right 7 //连接小车右轮电机的转向端口到数字接口7 int val_right_count_target = 0; //小车右轮码盘每秒计数PID调节目标值,根据这个值PID val_rigth; int val_right = 0; //小车右轮电机的PWM功率值 int val_left_count_target = 0; //小车左轮码盘每秒计数PID调节目标值,根据这个值PID val_left; int val_left = 0; //左轮电机PWM功率值。以左轮为基速度,PID调节右轮的速度 int count_left = 0; //左轮编码器码盘脉冲计数值;用于PID调整 int count_right = 0; //右轮编码器码盘脉冲计数值;用于PID调整 ///////// char run_direction = 'f'; //f:前进;b:后退;s:stop int linear = 0;//15; //cm/second线速度 int angular = 0;//1; //角速度,ros的angular.z ///转弯半径一定要大于小车宽度的一半,也就是linear / angular一定是大于13.5,也就是最小转弯半径是13.5 ///////// unsigned long left_old_time = 0, right_old_time = 0; // 时间标记 unsigned long time1 = 0, time2 = 0; // 时间标记 ////ros ros::NodeHandle nh; //geometry_msgs::TransformStamped t; //tf::TransformBroadcaster broadcaster; //char base_link[] = "/base_link"; //char odom[] = "/odom"; //nav_msgs::Odometry odom1; void motor_cb(const geometry_msgs::Twist& vel) { linear = vel.linear.x * 100; //ROS中的单位是m/s;这里换算成cm的单位 angular = vel.angular.z; } ros::Subscriber<geometry_msgs::Twist> sub("/turtle1/cmd_vel", motor_cb); //////PID double left_Setpoint, left_Input, left_Output, left_setpoint; double left_kp = 1, left_ki = 0.005, left_kd = 0.0001; //kp = 0.040,ki = 0.0005,kd =0.0011; PID left_PID(&left_Input, &left_Output, &left_Setpoint, left_kp, left_ki, left_kd, DIRECT); double right_Setpoint, right_Input, right_Output, right_setpoint; double right_kp = 0.8, right_ki = 0.005, right_kd = 0.0021; //kp = 0.040,ki = 0.0005,kd =0.0011; PID right_PID(&right_Input, &right_Output, &right_Setpoint, right_kp, right_ki, right_kd, DIRECT); void setup() { // put your setup code here, to run once: Serial.begin(9600); // 启动串口通信，波特率为9600b/s // reserve 200 bytes for the inputString pinMode(M_left, OUTPUT); //L298P直流电机驱动板的控制端口设置为输出模式 pinMode(E_left, OUTPUT); pinMode(M_right, OUTPUT); pinMode(E_right, OUTPUT); //定义外部中断0和1的中断子程序Code(),中断触发为下跳沿触发 //当编码器码盘的OUT脉冲信号发生下跳沿中断时， //将自动调用执行中断子程序Code()。 left_old_time = millis(); right_old_time = millis(); attachInterrupt(0, Code1, FALLING);//小车左车轮电机的编码器脉冲中断函数 attachInterrupt(1, Code2, FALLING);//小车右车轮电机的编码器脉冲中断函数 nh.initNode(); nh.subscribe(sub); //broadcaster.init(nh); left_PID.SetOutputLimits(-254, 254); left_PID.SetSampleTime(500); left_PID.SetMode(AUTOMATIC); left_PID.SetTunings(left_kp, left_ki, left_kd); right_PID.SetOutputLimits(-254, 254); right_PID.SetSampleTime(500); right_PID.SetMode(AUTOMATIC); right_PID.SetTunings(right_kp, right_ki, right_kd); } //子程序程序段 void advance()//前进 { digitalWrite(M_left, HIGH); analogWrite(E_left, val_left); digitalWrite(M_right, HIGH); analogWrite(E_right, val_right); } void back()//后退 { digitalWrite(M_left, LOW); analogWrite(E_left, val_left); digitalWrite(M_right, LOW); analogWrite(E_right, val_right); } void Stop()//停止 { digitalWrite(E_right, LOW); digitalWrite(E_left, LOW); } void loop() { // Serial.println("*************************************loop"); // t.header.frame_id = odom; // t.child_frame_id = base_link; // t.transform.translation.x = 1.0; // t.transform.rotation.x = 0.0; // t.transform.rotation.y = 0.0; // t.transform.rotation.z = 0.0; // t.transform.rotation.w = 1.0; // t.header.stamp = nh.now(); // broadcaster.sendTransform(t); nh.spinOnce(); // put your main code here, to run repeatedly: if (angular == 0) { //直行 if (linear > 0) { //前进 Serial.println("Go Forward!\n"); if (linear > max_linear) linear = max_linear; float linear_left = linear; //左内圈线速度 float linear_right = linear; //右外圈线速度 val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左内圈线速度对应的孔数 val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈线速度对应的孔数 val_right = linear_right * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,左轮 val_left = linear_left * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,右 left_Setpoint = val_left_count_target; right_Setpoint = val_right_count_target; advance(); run_direction = 'f'; } else if (linear < 0) { //后退 Serial.println("Go Backward!\n"); linear = abs(linear); if (linear > max_linear) linear = max_linear; float linear_left = linear; //左内圈线速度 float linear_right = linear; //右外圈线速度 val_right_count_target = linear_right * gear_ratio / (diameter * diamete_ratio / hole_number); //左内圈线速度对应的孔数 val_left_count_target = linear_left * gear_ratio / (diameter / hole_number); //右外圈线速度对应的孔数 val_right = linear_right * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,左轮 val_left = linear_left * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,右轮 left_Setpoint = val_left_count_target; right_Setpoint = val_right_count_target; back(); run_direction = 'b'; } } else if (angular > 0) { //左转 Serial.println("Turn Left!\n"); if (linear > max_turn_line) //////限制最大转弯线速度 { angular = angular * max_turn_line / linear; linear = max_turn_line; } else if (linear == 0) { linear = max_turn_line; } float radius = linear / angular; //计算半径 if (radius < car_width / 2) ///////如果计算的转弯半径小于最小半径,则设置为最小转弯半径 radius = car_width / 2; float radius_left = radius - car_width / 2; //左内圈半径 float radius_right = radius + car_width / 2; //右外圈半径 float linear_left = radius_left * angular; //左内圈线速度 float linear_right = radius_right * angular; //右外圈线速度 if (linear == max_turn_line) { linear_left = 255 * (linear_left / linear_right); linear_right = 255; } val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左内圈线速度对应的孔数 val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈线速度对应的孔数 val_right = linear_right * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,左轮 val_left = linear_left * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,右轮 left_Setpoint = val_left_count_target; right_Setpoint = val_right_count_target; run_direction = 'f'; advance(); } else if (angular < 0) { //右转 Serial.println("Turn Right!"); if (linear > max_turn_line) //////限制最大转弯线速度 { angular = angular * max_turn_line / linear; linear = max_turn_line; } else if (linear == 0) { linear = max_turn_line; } float radius = linear / angular; if (radius < car_width / 2) ///////如果计算的转弯半径小于最小半径,则设置为最小转弯半径 radius = car_width / 2; float radius_left = radius + car_width / 2; float radius_right = radius - car_width / 2; float linear_left = radius_left * angular; float linear_right = radius_right * angular; if (linear == max_turn_line) { linear_right = 255 * (linear_right / linear_left); linear_left = 255; } val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左内圈线速度对应的孔数 val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈线速度对应的孔数 val_right = linear_right * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,左轮 val_left = linear_left * (max_linear_pwd / max_linear); //根据轮径参数计算出来的线速度对应的PWD值,右轮 left_Setpoint = val_left_count_target; right_Setpoint = val_right_count_target; advance(); run_direction = 'f'; } delay(1000); val_left_count_target = 0; left_Setpoint = 0; val_right_count_target = 0; right_Setpoint = 0; linear = 0; angular = 0; Stop(); run_direction = 's'; } // 左侧车轮电机的编码器码盘计数中断子程序 void Code1() { //为了不计入噪音干扰脉冲， //当2次中断之间的时间大于2ms时，计一次有效计数 //Serial.println("Code1"); if ((millis() - time1) > 2) { //当编码器码盘的OUT脉冲信号下跳沿每中断一次， count_left++; // 编码器码盘计数加一 if ((millis() - left_old_time) > 100) { ////////100ms进行一次PID调节 if (run_direction != 's') PID_left(); left_old_time = millis(); count_left = 0;//把脉冲计数值清零，以便计算下一秒的脉冲计数 } time1 == millis(); } } // 右侧车轮电机的编码器码盘计数中断子程序 void Code2() { //Serial.println("Code2"); if ((millis() - time2) > 2) { //当编码器码盘的OUT脉冲信号下跳沿每中断一次， count_right++; // 编码器码盘计数加一 if ((millis() - right_old_time) > 100) { ////////100ms进行一次PID调节 if (run_direction != 's') PID_right(); right_old_time = millis(); count_right = 0; //把脉冲计数值清零，以便计算下一秒的脉冲计数 } time2 == millis(); } } void PID_left() { Serial.println("********************************begin PID left"); left_Input = count_left * 10; left_PID.Compute(); val_left = val_left + left_Output; if (val_left > 255) val_left = 255; if (val_left < 0) val_left = 0; if (run_direction == 'f') //根据刚刚调节后的小车电机PWM功率值，及时修正小车前进或者后退状态 advance(); if (run_direction == 'b') back(); Serial.println("********************************end PID Left"); } void PID_right() { Serial.println("********************************begin PID Right"); right_Input = count_right * 10; right_PID.Compute(); val_right = val_right + right_Output; if (val_right > 255) val_right = 255; if (val_right < 0) val_right = 0; if (run_direction == 'f') //根据刚刚调节后的小车电机PWM功率值，及时修正小车前进或者后退状态 advance(); if (run_direction == 'b') back(); Serial.println("********************************end PID Right"); } 实践证明ROS_lib是非常占用arduino资源的，如果要订阅Twist，同时发布TF，Odometry消息则至少需要3k的SRAM, Arduino UNO只能作为接收Twist消息，来控制底盘，如果用rosserial_arduino做到完整的Base Controller就只能上Arduino Mega2560了，这无疑会增加不少成本 所以笔者认为又更好的选择，那就是使用ros_arduino_bridge作为Base Controller，把逻辑的运算放在上位机上运行，Arduino单纯的作为硬件的控制器。
ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller
ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller 说明： 介绍软件包ros_arduino_bridge使用，此软件包用于驱动底盘 ROS的控制模型中，对底盘的驱动+马达的PID调节 = base controller 参考：http://wiki.ros.org/ros_arduino_bridge ros_arduino_bridge封装了通过串口于底盘控制器Arduino的通信，并提供了一个标准的base controller，所以对arduino资源的占用非常小 arduino只是作为一个单纯的硬件控制器来使用，而所有的运算逻辑都放在上位机进行，通过串口指令的方式控制Arduino程序的运行。 在Arduino UNO编译一个完整的ros_arduino_bridge后的资源占用情况如下图： 从上图可以看出占用资源非常上，用户还可以在Arduino UNO开发更多的应用，接更多的传感器 ros_arduino_bridge的安装 详细的安装和使用说明，参考ROS与Arduino-ros_arduino_bridge-功能包的使用 下载：进入你的workspace目录下的src目录，catkin_ws是workspace cd ~/catkin_ws/src git clone https://github.com/hbrobotics/ros_arduino_bridge.git 编译：在workspace目录编译 cd <catkin_ws> catkin_make 使用：拷贝Arduino库文件到相应的Arduino IDE的libraries目录 $ cd SKETCHBOOK_PATH//Arduino IDE的库文件目录 $ \cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge -T ROSArduinoBridge 这时候可以把ROSArduinoBridge拷贝到其他windows， Mac电脑的Arduino IDE环境下使用，重启后既可以用 开发Arduino代码 从Example中打开ROSArduinoBridge的示例代码，另存为自己喜欢的项目名称，我们只需要根据自己的需求修改示例代码即可 示例代码文件介绍 ROSArduinoBridge.ino 主程序 commands.h 串口命令的预定义 diff_controller.h PID控制代码 encoder_driver.h 编码器，这里只是针对了Arduino UNO，使用了中断接口D2,D3,和模拟接口A4，A5；所以电机编码器的输出接线需要按照此规则接线，另外要注意编码器要有两路输出 左侧电机的编码输出接D2,D3；右侧电机的编码输出接A4,A5 encoder_driver.ino 编码器的实现代码 motor_driver.h 马达驱动的接口定义，用不动的马达驱动板都要实现此文件定义的三个函数 motor_driver.ino马达驱动实现代码，根据预定义选择不同的驱动板库，在这里我使用了L298P，所以需要自己实现一个新的驱动库，后面会介绍 sensors.h传感器的实现文件 servos.h舵机的实现文件 修改ROSArduinoBridge.ino 为了满足我们控制的需要需要修改的是ROSArduinoBridge.ino文件，主要修改的点如下： 启用Base Controller #define USE_BASE // Enable the base controller code启用base controller //#undef USE_BASE // Disable the base controller code 马达控制板定义 /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE /* The Pololu VNH5019 dual motor driver shield */ //#define POLOLU_VNH5019 /* The L298P dual motor driver shield，这里我们使用我们自己写的L298P库 */ #define L298P /* The Pololu MC33926 dual motor driver shield */ //#define POLOLU_MC33926 /* The RoboGaia encoder shield */ //#define ROBOGAIA /* Encoders directly attached to Arduino board，启用Arduino UNO的板载Encoder功能 */ #define ARDUINO_ENC_COUNTER #endif 定义电机PWM控制范围 /* Maximum PWM signal */ #define MAX_PWM 255//最大的PWM为255 修改motor_driver.ino 主要是增加对L298P马达驱动板的支持 /*************************************************************** Motor driver definitions Add a "#elif defined" block to this file to include support for a particular motor driver. Then add the appropriate #define near the top of the main ROSArduinoBridge.ino file. *************************************************************/ #ifdef USE_BASE #if defined POLOLU_VNH5019 /* Include the Pololu library */ #include "DualVNH5019MotorShield.h" /* Create the motor driver object */ DualVNH5019MotorShield drive; /* Wrap the motor driver initialization */ void initMotorController() { drive.init(); } /* Wrap the drive motor set speed function */ void setMotorSpeed(int i, int spd) { if (i == LEFT) drive.setM1Speed(spd); else drive.setM2Speed(spd); } // A convenience function for setting both motor speeds void setMotorSpeeds(int leftSpeed, int rightSpeed) { setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } #elif defined POLOLU_MC33926 /* Include the Pololu library */ #include "DualMC33926MotorShield.h" /* Create the motor driver object */ DualMC33926MotorShield drive; /* Wrap the motor driver initialization */ void initMotorController() { drive.init(); } /* Wrap the drive motor set speed function */ void setMotorSpeed(int i, int spd) { if (i == LEFT) drive.setM1Speed(spd); else drive.setM2Speed(spd); } // A convenience function for setting both motor speeds void setMotorSpeeds(int leftSpeed, int rightSpeed) { setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } #elif defined L298P////////增加对L298P的支持 #include "DualL298PMotorShield.h" /* Create the motor driver object */ DualL298PMotorShield drive; /* Wrap the motor driver initialization */ void initMotorController() { drive.init(); } /* Wrap the drive motor set speed function */ void setMotorSpeed(int i, int spd) { if (i == LEFT) drive.setM1Speed(spd); else drive.setM2Speed(spd); } // A convenience function for setting both motor speeds void setMotorSpeeds(int leftSpeed, int rightSpeed) { setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } #else #error A motor driver must be selected! #endif #endif 增加L298P的驱动库 L298P的驱动库，把.h和.cpp文件放在同一个目录下，拷贝到Arduino IDE的库文件目录下就可以 DualL298PMotorShield.h代码 #ifndef DualL298PMotorShield_h #define DualL298PMotorShield_h #include <Arduino.h> class DualL298PMotorShield { public: // CONSTRUCTORS DualL298PMotorShield(); // Default pin selection. DualL298PMotorShield(unsigned char M1DIR, unsigned char M1PWM, unsigned char M2DIR, unsigned char M2PWM); // User-defined pin selection. // PUBLIC METHODS void init(); // Initialize TIMER 1, set the PWM to 20kHZ. void setM1Speed(int speed); // Set speed for M1. void setM2Speed(int speed); // Set speed for M2. void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2. private: static const unsigned char _M1DIR = 4; static const unsigned char _M2DIR = 7; static const unsigned char _M1PWM = 5; static const unsigned char _M2PWM = 6; }; #endif DualL298PMotorShield.cpp代码 #include "DualL298PMotorShield.h" // Constructors //////////////////////////////////////////////////////////////// DualL298PMotorShield::DualL298PMotorShield() { //Pin map } // Public Methods ////////////////////////////////////////////////////////////// void DualL298PMotorShield::init() { // Define pinMode for the pins and set the frequency for timer1. pinMode(_M1DIR,OUTPUT); pinMode(_M1PWM,OUTPUT); pinMode(_M2DIR,OUTPUT); pinMode(_M2PWM,OUTPUT); } // Set speed for motor 1, speed is a number betwenn -400 and 400 void DualL298PMotorShield::setM1Speed(int speed) { unsigned char reverse = 0; if (speed < 0) { speed = -speed; // Make speed a positive quantity reverse = 1; // Preserve the direction } if (speed > 255) // Max PWM dutycycle speed = 255; if (reverse) { digitalWrite(_M1DIR,LOW); analogWrite(_M1PWM, speed); } else { digitalWrite(_M1DIR,HIGH); analogWrite(_M1PWM, speed); } } // Set speed for motor 2, speed is a number betwenn -400 and 400 void DualL298PMotorShield::setM2Speed(int speed) { unsigned char reverse = 0; if (speed < 0) { speed = -speed; // Make speed a positive quantity reverse = 1; // Preserve the direction } if (speed > 255) // Max PWM dutycycle speed = 255; if (reverse) { digitalWrite(_M2DIR,LOW); analogWrite(_M2PWM, speed); } else { digitalWrite(_M2DIR,HIGH); analogWrite(_M2PWM, speed); } } // Set speed for motor 1 and 2 void DualL298PMotorShield::setSpeeds(int m1Speed, int m2Speed) { setM1Speed(m1Speed); setM2Speed(m2Speed); } 修改完成后变可以编译upload到Arduino UNO上了。 ROS上位机开发 配置你的机器人参数，进入配置文件目录 $ roscd ros_arduino_python/config 拷贝一份新的配置文件 $ cp arduino_params.yaml my_arduino_params.yaml 用nano打开编辑 sudo nano my_arduino_params.yaml 修改后的my_arduino_params.yaml如下图 主要修改就是启用base Controller，修改PID参数，修改机器人的参数： # For a direct USB cable connection, the port name is typically # /dev/ttyACM# where is # is a number such as 0, 1, 2, etc # For a wireless connection like XBee, the port is typically # /dev/ttyUSB# where # is a number such as 0, 1, 2, etc. port: /dev/ttyACM0 baud: 57600 timeout: 0.1 rate: 50 sensorstate_rate: 10 use_base_controller: True base_controller_rate: 10 # For a robot that uses base_footprint, change base_frame to base_footprint base_frame: base_link # === Robot drivetrain parameters wheel_diameter: 0.02900 #轮胎直径 wheel_track: 0.18 #两个轮胎间距 encoder_resolution: 2 # 码盘孔数 gear_reduction: 75.0 #转速比 motors_reversed: True # === PID parameters Kp: 10 Kd: 12 Ki: 0 Ko: 50 accel_limit: 1.0 # === Sensor definitions. Examples only - edit for your robot. # Sensor type can be one of the follow (case sensitive!): # * Ping # * GP2D12 # * Analog # * Digital # * PololuMotorCurrent # * PhidgetsVoltage # * PhidgetsCurrent (20 Amp, DC) sensors: { #motor_current_left: {pin: 4, type: PololuMotorCurrent, rate: 5}, #motor_current_right: {pin: 7, type: PololuMotorCurrent, rate: 5}, #ir_front_center: {pin: 2, type: GP2D12, rate: 10}, #sonar_front_center: {pin: 5, type: Ping, rate: 10}, arduino_led: {pin: 13, type: Digital, rate: 5, direction: output} } 修改完成后，既可以运行了 运行测试 新终端，启动roscore $ roscore 新终端，增加路径到bash $ source ~/catkin_ws/devel/setup.bash 新终端，启动节点 $ roslaunch ros_arduino_python arduino.launch 启动效果图： 新终端，发布Twist消息来控制机器人的运行，如： $ rostopic pub /cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]' 运行此命令，机器人会原地打转，用如下命令查看odom的信息，此信息会不断的变化 新终端，查看里程 $ rostopic echo /odom 效果图： 至此机器人已经可以按照Twist消息进行控制，发布功move base使用的odom信息 不过做到这里，为了控制精度，我们还需要标定机器人，即标定机器人按照机器人给定的线速度，和角速度行进
ROS机器人Diego制作4-线速度的标定 说明： 介绍线速度的标定方法，校准直线行走的偏差. 让机器人动起来容易，但要精确控制是比较难的，这与控制算法、硬件都有比较大的相关性。 对于轮式机器人，base controller代码写好，机器人动起来后。 首先做的就是需要对线速度，和角速度进行标定，以保证机器人可以按照指令精确的的形式 线速度的标定方法： 线速度的标定，就是设置一个1m的距离，代码控制让机器人行驶1m，看是否刚好是1m，误差在可接受的范围内即可 如下是进行标定的python代码： #!/usr/bin/env python import rospy from geometry_msgs.msg import Twist, Point from math import copysign, sqrt, pow import tf class CalibrateLinear(): def __init__(self): # Give the node a name rospy.init_node('calibrate_linear', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) # Set the distance to travel self.test_distance = 1.0 # meters self.speed = 1.0 # meters per second self.tolerance = 0.01 # meters self.odom_linear_scale_correction = 1.0 self.start_test = True # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") self.position = Point() # Get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): # Stop the robot by default move_cmd = Twist() if self.start_test: # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() # Compute the Euclidean distance from the target point distance = sqrt(pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) # Correct the estimated distance by the correction factor distance *= self.odom_linear_scale_correction # How close are we? error = distance - self.test_distance # Are we close enough? if not self.start_test or abs(error) < self.tolerance: self.start_test = False params = False rospy.loginfo(params) else: # If not, move in the appropriate direction move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd) r.sleep() # Stop the robot self.cmd_vel.publish(Twist()) def get_position(self): # Get the current transform between the odom and base frames try: (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") return return Point(*trans) def shutdown(self): # Always stop the robot when shutting down the node rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: CalibrateLinear() rospy.spin() except: rospy.loginfo("Calibration terminated.") Python的ROS语法，参考：ROS与Python入门教程 Pyhon基础教程，参考：python入门教程 代码片段分析： x_start = self.position.x #设定起始位置的x坐标 y_start = self.position.y #设定起始位置的y坐标 move_cmd = Twist() while not rospy.is_shutdown(): # Stop the robot by default move_cmd = Twist() if self.start_test: # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() #获取当前的位置信息 # 计算当前位置与起始位置的距离 distance = sqrt(pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) # Correct the estimated distance by the correction factor distance *= self.odom_linear_scale_correction # 计算与目标位置的距离 error = distance - self.test_distance # Are we close enough? if not self.start_test or abs(error) < self.tolerance: #如果已经到达目标位置，则停止 self.start_test = False params = False rospy.loginfo(params) else: # 如果还没有到达，则继续前进，如果已经超出了目标位置，这控制电机反转，退回 move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd)#发布控制Twist消息 r.sleep() 脚本使用 接下来运行如下命令，控制小车前进 rosrun diego_nav calibrate_linear.py 如果能一次运行刚好是1m那当然是理想的效果，如不理想，可能要调整my_arduino_params.yaml文件中有关机器人的参数 首先需检查机器人的参数是否与实际的相符，如不相符,修改为与实际想否的数据 另外要注意的是ROS里面使用的单位是 米，一定要注意单位的换算 # === Robot drivetrain parameters wheel_diameter: 0.02900 #轮直径 wheel_track: 0.18 #两轮的间距 encoder_resolution: 2 #编码器一圈的脉冲计数 gear_reduction: 75.0 #减速比 motors_reversed: True 参数设置可以参考：搭建ROS小车底盘-第四篇上位机程序 其中说明一些测试例子，可以根据自己实际情况调整。 即使是数据与实际的测量数据符合，但也可能达不到你的要求，这可能与电机的性能有关系 本人的经验适当的调整wheel_diameter参数，即可以达到满意的效果 当然要达到高精度的控制效果，硬件的精度也要非常高。
ROS机器人Diego制作5-角速度的标定 说明： 介绍角速度标定，校准旋转角度偏差 角速度标定方法： 角速度标定，即控制机器人人转动固定的角度，看机器人是否按照控制指令完成，在这里我们让机器人转360度 python代码如下： #!/usr/bin/env python import rospy from geometry_msgs.msg import Twist, Quaternion from nav_msgs.msg import Odometry import tf from math import radians, copysign from transform_utils import quat_to_angle, normalize_angle import PyKDL from math import pi class CalibrateAngular(): def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = 2*pi #这里注意，在ROS中使用的弧度，不能直接写360 self.speed = 0.1 # radians per second self.tolerance = 1 # degrees converted to radians self.odom_angular_scale_correction = 1 self.start_test = True # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is usually base_link or base_footprint self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() rospy.loginfo("self.odom_angle: "+str(self.odom_angle)) last_angle = self.odom_angle turn_angle = 0 self.test_angle *= reverse error = self.test_angle - turn_angle rospy.loginfo("errir: "+str(error)) # Alternate directions between tests reverse = -reverse while abs(error) > self.tolerance and self.start_test: if rospy.is_shutdown(): return rospy.loginfo("*************************** ") # Rotate the robot to reduce the error move_cmd = Twist() move_cmd.angular.z = copysign(self.speed, error) rospy.loginfo("move_cmd.angular.z: "+str(move_cmd.angular.z)) self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() rospy.loginfo("current rotation angle: "+str(self.odom_angle)) # Compute how far we have gone since the last measurement delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) rospy.loginfo("delta_angle: "+str(delta_angle)) # Add to our total angle so far turn_angle += abs(delta_angle) rospy.loginfo("turn_angle: "+str(turn_angle)) # Compute the new error error = self.test_angle - turn_angle rospy.loginfo("error: "+str(error)) # Store the current angle for the next comparison last_angle = self.odom_angle # Stop the robot self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist()) def get_odom_angle(self): # Get the current transform between the odom and base frames try: (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") return # Convert the rotation from a quaternion to an Euler angle return quat_to_angle(Quaternion(*rot)) def shutdown(self): # Always stop the robot when shutting down the node rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: CalibrateAngular() except: rospy.loginfo("Calibration terminated.") 脚本使用： 接下来运行如下命令，控制小车旋转： rosrun diego_nav calibrate_angular.py 影响角速度的主要参数是wheel_track，所以如果发现机器人不能按照要求旋转固定角度，可以调整此参数。
ROS机器人Diego制作6-实现两个马达独立PID调速
ROS机器人Diego制作6-实现两个马达独立PID调速 说明： ros_arduino_bridge的base controller中对两个电机的PID控制是用的一套PID参数 但实际的使用中，由于马达的特性，地形，或者机器人的载重的平衡性的诸多问题使机器人不能按照预定轨迹行驶 最简单的就是机器人是否能够走直线，这就需要对两个电机分别进行PID调速 本文中将介绍如何对ros_arduino_bridge进行修改，以使支持对两路电机用不同的PID参数调速 修改diff_controller.h文件 增加左右两个马达的PID控制变量： /* PID Parameters */ int Kp = 20; int Kd = 12; int Ki = 0; int Ko = 50; int left_Kp=Kp; int left_Kd=Kd; int left_Ki=Ki; int left_Ko=Ko; int right_Kp=Kp; int right_Kd=Kd; int right_Ki=Ki; int right_Ko=Ko; 分别定义左右两个马达的dorightPID()和doleftPID()函数 /* PID routine to compute the next motor commands */ void dorightID(SetPointInfo * p) { long Perror; long output; int input; //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); input = p->Encoder - p->PrevEnc; Perror = p->TargetTicksPerFrame - input; /* * Avoid derivative kick and allow tuning changes, * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; // p->PrevErr = Perror; output = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko; p->PrevEnc = p->Encoder; output += p->output; // Accumulate Integral error *or* Limit output. // Stop accumulating when output saturates if (output >= MAX_PWM) output = MAX_PWM; else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += Ki * Perror; p->output = output; p->PrevInput = input; } /* PID routine to compute the next motor commands */ void doleftPID(SetPointInfo * p) { long Perror; long output; int input; //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); input = p->Encoder - p->PrevEnc; Perror =p->TargetTicksPerFrame + input; /* * Avoid derivative kick and allow tuning changes, * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; // p->PrevErr = Perror; output = (left_Kp * Perror - left_Kd * (input - p->PrevInput) + p->ITerm) / left_Ko; p->PrevEnc = p->Encoder; output += p->output; // Accumulate Integral error *or* Limit output. // Stop accumulating when output saturates if (output >= MAX_PWM) output = MAX_PWM; else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += Ki * Perror; p->output = output; p->PrevInput = input; } 修改updatePID()函数 void updatePID() { /* Read the encoders */ leftPID.Encoder =readEncoder(LEFT); rightPID.Encoder = readEncoder(RIGHT); /* If we're not moving there is nothing more to do */ if (!moving){ /* * Reset PIDs once, to prevent startup spikes, * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ * PrevInput is considered a good proxy to detect * whether reset has already happened */ if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID(); return; } /* Compute PID update for each motor */ dorightID(&rightPID);//执行右马达PID doleftPID(&leftPID);//执行左马达PID /* Set the motor speeds accordingly */ setMotorSpeeds(leftPID.output, rightPID.output); } 修改ROSArduinoBridge.ino文件 修改pid_args的声明为如下代码 int pid_args[8]; 修改runCommand()函数，修改case UPDATE_PID,把原来的Kp,Kd,Ki,Ko赋值的代码注释掉，修改为如下的代码： case UPDATE_PID: while ((str = strtok_r(p, ":", &p)) != '\0') { pid_args[i] = atoi(str); i++; } // Kp = pid_args[0]; // Kd = pid_args[1]; // Ki = pid_args[2]; // Ko = pid_args[3]; left_Kp = pid_args[0]; left_Kd = pid_args[1]; left_Ki = pid_args[2]; left_Ko = pid_args[3]; right_Kp = pid_args[0]; right_Kd = pid_args[1]; right_Ki = pid_args[2]; right_Ko = pid_args[3]; Serial.println("OK"); break; 现在arduino端已经支持对两个电机使用不同的PID参数调速了 修改上位机ROS包的代码 修改my_arduino_params.yaml，在PID参数部分增加左右两个马达的PID参数 # === PID parameters Kp: 10 Kd: 12 Ki: 0 Ko: 50 accel_limit: 1.0 left_Kp: 10 left_Kd: 12 left_Ki: 0 left_Ko: 50 right_Kp: 8 right_Kd: 12 right_Ki: 0 修改arduino_driver.py文件，修改update_pid()函数 def update_pid(self, left_Kp, left_Kd, left_Ki, left_Ko, right_Kp, right_Kd, right_Ki, right_Ko): ''' Set the PID parameters on the Arduino ''' print "Updating PID parameters" cmd = 'u ' + str(left_Kp) + ':' + str(left_Kd) + ':' + str(left_Ki) + ':' + str(left_Ko) + ':' + str(right_Kp) + ':' + str(right_Kd) + ':' + str(right_Ki) + ':' + str(right_Ko) self.execute_ack(cmd) 修改base_controller.py文件 修改def init(self, arduino, base_frame):函数，增加左右两个马达PID参数的初始化代码： def __init__(self, arduino, base_frame): self.arduino = arduino self.base_frame = base_frame self.rate = float(rospy.get_param("~base_controller_rate", 10)) self.timeout = rospy.get_param("~base_controller_timeout", 1.0) self.stopped = False pid_params = dict() pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) #modify by william 增加左右两个马达的PID参数的初始化代码 pid_params['left_Kp'] = rospy.get_param("~left_Kp", 20) pid_params['left_Kd'] = rospy.get_param("~left_Kd", 12) pid_params['left_Ki'] = rospy.get_param("~left_Ki", 0) pid_params['left_Ko'] = rospy.get_param("~left_Ko", 50) pid_params['right_Kp'] = rospy.get_param("~right_Kp", 20) pid_params['right_Kd'] = rospy.get_param("~right_Kd", 12) pid_params['right_Ki'] = rospy.get_param("~right_Ki", 0) pid_params['right_Ko'] = rospy.get_param("~right_Ko", 50) self.accel_limit = rospy.get_param('~accel_limit', 0.1) self.motors_reversed = rospy.get_param("~motors_reversed", False) # Set up PID parameters and check for missing values self.setup_pid(pid_params) # How many encoder ticks are there per meter? self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi) # What is the maximum acceleration we will tolerate when changing wheel speeds? self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate # Track how often we get a bad encoder count (if any) self.bad_encoder_count = 0 now = rospy.Time.now() self.then = now # time for determining dx/dy self.t_delta = rospy.Duration(1.0 / self.rate) self.t_next = now + self.t_delta # internal data self.enc_left = None # encoder readings self.enc_right = None self.x = 0 # position in xy plane self.y = 0 self.th = 0 # rotation in radians self.v_left = 0 self.v_right = 0 self.v_des_left = 0 # cmd_vel setpoint self.v_des_right = 0 self.last_cmd_vel = now # subscriptions rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) # Clear any old odometry info self.arduino.reset_encoders() # Set up the odometry broadcaster self.odomPub = rospy.Publisher('odom', Odometry) self.odomBroadcaster = TransformBroadcaster() rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") 修改def setup_pid(self, pid_params): def setup_pid(self, pid_params): # Check to see if any PID parameters are missing missing_params = False for param in pid_params: if pid_params[param] == "": print("*** PID Parameter " + param + " is missing. ***") missing_params = True if missing_params: os._exit(1) self.wheel_diameter = pid_params['wheel_diameter'] self.wheel_track = pid_params['wheel_track'] self.encoder_resolution = pid_params['encoder_resolution'] self.gear_reduction = pid_params['gear_reduction'] #self.Kp = pid_params['Kp'] #self.Kd = pid_params['Kd'] #self.Ki = pid_params['Ki'] #self.Ko = pid_params['Ko'] #self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) #modify by william self.left_Kp = pid_params['left_Kp'] self.left_Kd = pid_params['left_Kd'] self.left_Ki = pid_params['left_Ki'] self.left_Ko = pid_params['left_Ko'] self.right_Kp = pid_params['right_Kp'] self.right_Kd = pid_params['right_Kd'] self.right_Ki = pid_params['right_Ki'] self.right_Ko = pid_params['right_Ko'] #传递8个参数给update_pid函数 self.arduino.update_pid(self.left_Kp, self.left_Kd, self.left_Ki, self.left_Ko, self.right_Kp, self.right_Kd, self.right_Ki, self.right_Ko) 修改完上述代码后ros_arduino_bridge可以分别对两个马达设置不同的PID参数进行调速。
ROS机器人Diego制作7-ROS语音系统之sphinx 说明： 介绍如何接入语音控制系统sphinx 参考其他sphinx接入教程 turtlebot入门-语音控制 http://blog.csdn.net/mwlwlm/article/details/52750364 sphinx接入方法 在ROS kinetic这个版本中是没有安装sphinx的，需要手动安装 安装如下依赖包: sudo apt-get install ros-kinetic-audio-common sudo apt-get install libasound2 sudo apt-get install gstreamer0.10-* sudo apt-get install python-gst0.10 安装libsphinxbase1 下载的网站：https://packages.debian.org/jessie/libsphinxbase1 由于Diego使用的是树莓派平台，所以请下载armhf版本的 下载完后执行 sudo dpkg -i libsphinxbase1_0.8-6_amdhf.deb 安装libpocketsphinx1 下载的网站：https://packages.debian.org/jessie/libpocketsphinx1 也下载armhf版本，下载完成后后执行 sudo dpkg -i libpocketsphinx1_0.8-5_amdhf.deb 安装gstreamer0.10-pocketsphinx 下载的网站：https://packages.debian.org/jessie/gstreamer0.10-pocketsphinx 同样下载armhf版本，下载完后执行 sudo dpkg -i gstreamer0.10-pocketsphinx_0.8-5_amdhf.deb 安装pocketsphinx 进入工作目录,克隆git目录 cd ~/catkin_ws/src Git clone https://github.com/mikeferguson/pocketsphinx 下载英文语音包pocketsphinx-hmm-en-tidigits (0.8-5) 下载的网站：https://packages.debian.org/jessie/pocketsphinx-hmm-en-tidigits 在包pocketsphinx下面建一个model目录，存放语音模型文件 cd ~/catkin_ws/src/pocketsphinx mkdir model 将下载好的语音文件，解压后，将其中的model文件下的所有文件拷贝到~/catkin_ws/src/pocketsphinx/model下 在~/catkin_ws/src/pocketsphinx目录下新建launch文件夹，创建diego_voice_test.launch文件 cd ~/catkin_ws/src/pocketsphinx mkdir launch vi diego_voice_test.launch diego_voice_test.launch文件内容如下 <launch> <node name="recognizer" pkg="pocketsphinx" type="recognizer.py" output="screen"> </node> </launch> 修改recognizer.py文件,在def init(self):函数中增加hmm参数的读取 def __init__(self): # Start node rospy.init_node("recognizer") self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal self._lm_param = "~lm" self._dic_param = "~dict" self._hmm_param = "~hmm" #增加hmm参数 # Configure mics with gstreamer launch config if rospy.has_param(self._device_name_param): self.device_name = rospy.get_param(self._device_name_param) self.device_index = self.pulse_index_from_name(self.device_name) self.launch_config = "pulsesrc device=" + str(self.device_index) rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name) elif rospy.has_param('~source'): # common sources: 'alsasrc' self.launch_config = rospy.get_param('~source') else: self.launch_config = 'gconfaudiosrc' rospy.loginfo("Launch config: %s", self.launch_config) self.launch_config += " ! audioconvert ! audioresample " \ + '! vader name=vad auto-threshold=true ' \ + '! pocketsphinx name=asr ! fakesink' # Configure ROS settings self.started = False rospy.on_shutdown(self.shutdown) self.pub = rospy.Publisher('~output', String) rospy.Service("~start", Empty, self.start) rospy.Service("~stop", Empty, self.stop) if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param): self.start_recognizer() else: rospy.logwarn("lm and dic parameters need to be set to start recognizer.") 在def start_recognizer(self):函数hmm参数的代码，如下: def start_recognizer(self): rospy.loginfo("Starting recognizer... ") self.pipeline = gst.parse_launch(self.launch_config) self.asr = self.pipeline.get_by_name('asr') self.asr.connect('partial_result', self.asr_partial_result) self.asr.connect('result', self.asr_result) self.asr.set_property('configured', True) self.asr.set_property('dsratio', 1) # Configure language model if rospy.has_param(self._lm_param): lm = rospy.get_param(self._lm_param) else: rospy.logerr('Recognizer not started. Please specify a language model file.') return if rospy.has_param(self._dic_param): dic = rospy.get_param(self._dic_param) else: rospy.logerr('Recognizer not started. Please specify a dictionary.') return #读取hmm属性，从配置文件中 if rospy.has_param(self._hmm_param): hmm = rospy.get_param(self._hmm_param) else: rospy.logerr('Recognizer not started. Please specify a hmm.') return self.asr.set_property('lm', lm) self.asr.set_property('dict', dic) self.asr.set_property('hmm', hmm) #设置hmm属性 self.bus = self.pipeline.get_bus() self.bus.add_signal_watch() self.bus_id = self.bus.connect('message::application', self.application_message) self.pipeline.set_state(gst.STATE_PLAYING) self.started = True 启动shpinx 新终端，运行 roscore 新终端，运行 roslaunch pocketsphinx diego_voicd_test.launch 测试语音效果： 现在可以对着你的机器人说话了，注意要说语音模型字典中的单词 适合的语音命令： sphinx对于特定的语音环境识别还是不错的，但是一旦环境发生变化，有了不同的噪音，识别率会显著降低 这也是现在语音识别技术所面临的共同难题 语音合成 在ROS中已经集成了完整的语音合成包source_play，只支持英文的语音合成，执行如下命令，即可测试 rosrun sound_play soundplay_node.py rosrun sound_play say.py "hi, i am diego."
ROS机器人Diego制作8-ROS语音系统之科大讯飞语音
ROS机器人Diego制作8-ROS语音系统之科大讯飞语音 说明： 介绍如何接入科大讯飞语音 非常感谢ljthink提供的讯飞语音的树莓派SDK下载地址 也提供本站的下载地址：点击树莓派SDK下载地址 整合讯飞语音非常简单，网友已开发了相应的ROS讯飞包(原教程此为无效链接待修正)，按照文中的步骤执行即可。 也提供本站集成方法：ROS科大讯飞语音包介绍和下载 注意的事项及使用体验： 讯飞在为每个开发者的每一个应用提供一个appid，而且这个id是固化在libmsc.so文件中 所以在程序中在给appid赋值时要与申请的libmsc.so中的id一致，如果不一致的话编译不会出错 但是在语音识别的时候报错，但不影响语音合成的功能。 也许是树莓派本身性能的问题，在使用语音合成的时候合成出来的声音会有断断续续的现象。 语音识别有时候会出现乱码的现象，不知道是什么原因。 使用语音合成，需要安装mplayer包。 通过控制台发送xfwords主题的节点 python代码: #!/usr/bin/env python import rospy from std_msgs.msg import String if __name__=="__main__": pub = rospy.Publisher('xfwords', String, queue_size = 0) rospy.init_node('xfei_xfwords_publish', anonymous=True) rate = rospy.Rate(1) # 10hz try: while not rospy.is_shutdown(): input_str = raw_input("Please input your words:\n") pub.publish(input_str) rate.sleep() except rospy.ROSInterruptException: pass 将此节点放在xfer_asr/scripts/目录下 执行: rosrun xfei_asr xfei_xfwords_publish.py 既可以启动该节点，会把控制台输入的汉字以xfwords主题的形式发布出去，配合tts_subscribe_speak使用
ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion 说明： 介绍接入LeTV Xtion深度相机 机器人的SLAM自适应导航，最基本的要有激光雷达数据或者点云数据 但激光雷达目前价格太高，另外可替代的方法是用具有深度摄像机作为传感器发布点云数据 一般用的比较多的是微软的Kinect，或者华硕的Xtion。 目前Kinect已经有2.0版本，但Kinect2.0支持的USB3.0接口，树莓派USB接口都是2.0的 基于价格和体积的考虑，选择LeTV Xtion ，效果还不错。 效果图： 安装方法 安装OpenNI包 sudo apt-get install ros-kinetic-openni-camera sudo apt-get install ros-kinetic-openni-launch 安装Xtion的新版驱动（现在买到的都是新版本的） sudo apt-get install libopenni-sensor-primesense0 启动openni节点（先要在其他终端中启动roscore） roslaunch openni_launch openni.launch 启动成功后终端应该显示如下信息 这里的警告信息可以忽略，不影响使用 ,主要是标定没做 查看摄像头的所生成的影像 rosrun image_view disparity_view image:=/camera/depth/disparity 也可以通过rviz来查看生成的影像，执行如下命令 rosrun rviz rviz 生成点云数据 参考了两篇文档 OpenNI本身就已经有点云数据了，这篇文章完全是看了前辈的文章，就想把这些优秀的代码整合到ROS中来 官方文档http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors 古月居的http://blog.csdn.net/hcx25909/article/details/8654684 源代码： #include <ros/ros.h> #include <sensor_msgs/PointCloud.h> #include <XnCppWrapper.h> #include <iostream> #include <iomanip> #include <vector> using namespace xn; using namespace std; struct SColorPoint3D { float X; float Y; float Z; float R; float G; float B; SColorPoint3D( XnPoint3D pos, XnRGB24Pixel color ) { X = pos.X; Y = pos.Y; Z = pos.Z; R = (float)color.nRed / 255; G = (float)color.nGreen / 255; B = (float)color.nBlue / 255; } }; void GeneratePointCloud( DepthGenerator& rDepthGen, const XnDepthPixel* pDepth, const XnRGB24Pixel* pImage, vector<SColorPoint3D>& vPointCloud ) { // number of point is the number of 2D image pixel DepthMetaData mDepthMD; rDepthGen.GetMetaData( mDepthMD ); unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes(); // build the data structure for convert XnPoint3D* pDepthPointSet = new XnPoint3D[ uPointNum ]; unsigned int i, j, idxShift, idx; for( j = 0; j < mDepthMD.FullYRes(); ++j ) { idxShift = j * mDepthMD.FullXRes(); for( i = 0; i < mDepthMD.FullXRes(); ++i ) { idx = idxShift + i; pDepthPointSet[idx].X = i; pDepthPointSet[idx].Y = j; pDepthPointSet[idx].Z = pDepth[idx]; } } // un-project points to real world XnPoint3D* p3DPointSet = new XnPoint3D[ uPointNum ]; rDepthGen.ConvertProjectiveToRealWorld( uPointNum, pDepthPointSet, p3DPointSet ); delete[] pDepthPointSet; // build point cloud for( i = 0; i < uPointNum; ++ i ) { // skip the depth 0 points if( p3DPointSet[i].Z == 0 ) continue; vPointCloud.push_back( SColorPoint3D( p3DPointSet[i], pImage[i] ) ); } delete[] p3DPointSet; } int main(int argc, char** argv){ ros::init(argc, argv, "point_cloud_publisher"); ros::NodeHandle n; ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50); unsigned int num_points = 100; int count = 0; ros::Rate r(1.0); ///////////////// XnStatus eResult = XN_STATUS_OK; int i = 0; // init Context mContext; eResult = mContext.Init(); DepthGenerator mDepthGenerator; eResult = mDepthGenerator.Create(mContext); ImageGenerator mImageGenerator; eResult = mImageGenerator.Create(mContext); // set output mode XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 30; eResult = mDepthGenerator.SetMapOutputMode(mapMode); eResult = mImageGenerator.SetMapOutputMode(mapMode); // start generating eResult = mContext.StartGeneratingAll(); // read data vector<SColorPoint3D> vPointCloud; ///////////////// while(n.ok()){ eResult = mContext.WaitNoneUpdateAll(); // get the depth map const XnDepthPixel* pDepthMap = mDepthGenerator.GetDepthMap(); // get the image map const XnRGB24Pixel* pImageMap = mImageGenerator.GetRGB24ImageMap(); // generate point cloud vPointCloud.clear(); GeneratePointCloud(mDepthGenerator, pDepthMap, pImageMap, vPointCloud ); // print point cloud cout.flags(ios::left); //Left-aligned cout << "Point number: " << vPointCloud.size() << endl; num_points=vPointCloud.size(); sensor_msgs::PointCloud cloud; cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = "sensor_frame"; cloud.points.resize(num_points); //we'll also add an intensity channel to the cloud cloud.channels.resize(3); cloud.channels[0].name = "R"; cloud.channels[0].values.resize(num_points); cloud.channels[1].name = "G"; cloud.channels[1].values.resize(num_points); cloud.channels[2].name = "G"; cloud.channels[2].values.resize(num_points); //generate some fake data for our point cloud for(unsigned int i = 0; i < num_points; ++i){ cloud.points[i].x = vPointCloud[i].X; cloud.points[i].y = vPointCloud[i].Y; cloud.points[i].z = vPointCloud[i].Z; cloud.channels[0].values[i] = vPointCloud[i].R; cloud.channels[1].values[i] = vPointCloud[i].G; cloud.channels[2].values[i] = vPointCloud[i].B; } cloud_pub.publish(cloud); ++count; r.sleep(); } return 0; } 另外在包目录下的CMakeLists.txt文件中有两处修改，否则编译会出错 增加openni的引用路径 include_directories ("/usr/include/ni/") 增加新的可执行文件说明 add_executable(XtionPointCloud src/XtionPointCloud.cpp) target_link_libraries(XtionPointCloud ${catkin_LIBRARIES}) target_link_libraries(XtionPointCloud OpenNI) 修改保存后在~/catkin_ws下执行编译命令 catkin_make 测试 新终端，启动roscore roscore 新终端，启动XtionPointCloud节点 rosrun diego_nav XtionPointCloud 新终端，查看发布的点云数据 rostopic echo /cloud 效果图： 树莓派处理其点云数据还是很吃力的，这个时候树莓派的系统资源使用情况：