在一些基于ros的机器人项目中,上位机需要向下位机传送速度信息。
#上位机发布信息
上位机可以利用Ros官方的Serial包或者Python的Serial包发送字符串信息
这里给出基于Python Serial包的程序
import serial
import rospy
from geometry_msgs.msg import Twist
ser = serial.Serial('/dev/ttyACM2', 9600, timeout=0.1)
def poseCallback(cmd_vel):
rospy.loginfo("Publsh linear x = %.4f, angular.z = %.4f", cmd_vel.linear.x, cmd_vel.angular.z)
try:
# Send speed to Arduino
data = "v: " + str(round(cmd_vel.linear.x, 4)) + ", w: " + str(round(cmd_vel.angular.z, 4)) + ";"
ser.write(data)
# print the data of serial-monitor
#response = ser.readall()
#print(response)
except:
print("Error!!!")
if __name__ == '__main__':
rospy.init_node('vel_subscriber', anonymous=True)
rospy.Subscriber("/cmd_vel", Twist, poseCallback)
rospy.spin()
#下位机接收并处理信息
问题在于下位机如何处理上位机传送来的信息
arduino的库中实现了c标准的
int sscanf(const char *str, const char *format, ...)
但是在arduino中,sscanf默认不支持double类型,如果要支持就需要修改解释器文件
所以我们可以利用 double atof(const char *str)
把参数 str 所指向的字符串转换为一个浮点数(类型为 double 型)
读取范围从指针开始一直读到不是数字为止,并且可以忽略空格
利用这一特性,我们就可以利用定义好的协议,从上位机发送的字符串中提取所需浮点信息
代码如下
void setup(){
Serial.begin(9600);
}
void loop(){
int num = 0;
char str[20];
while (Serial.available()){
str[num] = Serial.read();
num++;
delay(2);
}
str[num] = '\0';
double v_value = 0;
double w_value = 0;
int colon_flag = 1;
//定义的协议为str="v:v_value,w:_value;"
if (strlen(str) != 0){
for (int i = 0; i < strlen(str); i++){
if (str[i] == ':'){
if (colon_flag ){
v_value = atof(&str[i + 1]);
colon_flag++;
}
else{
w_value = atof(&str[i + 1]);
break;
}
}
}
}
}