ros串口通信代码段，控制下位机真实机器人(键盘遥控）

#include <ros/ros.h>
#include <serial/serial.h>  //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include"geometry_msgs/Twist.h"

using namespace std;
 
serial::Serial sp; //声明串口对象
unsigned char buf[6] = {0xff,0x00,0x00,0x00,0x00,0xfe};
//回调函数
void chatterCallback(const geometry_msgs::Twist& msg)
{
    ROS_INFO_STREAM(std::setprecision(2) << std::fixed << "linear.x=("<< msg.linear.x<<" msg.angular.z="<<msg.angular.z);
    cout<<"线速度："<<msg.linear.x<<"           "<<"角速度："<<msg.angular.z<<endl;


    sp.write(buf,6);   //发送串口数据
    cout<<"发送数据成功"<<endl;
}
 
int main (int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "sp_car");
    //声明节点句柄
    ros::NodeHandle nh;
 
    //订阅主题，并配置回调函数
    ros::Subscriber write_sub = nh.subscribe("/teleop/cmd_vel", 1000, chatterCallback);
    //发布主题
   
 
    try
    {
    //设置串口属性，并打开串口
        sp.setPort("/dev/ttyUSB0");
        sp.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        sp.setTimeout(to);
        sp.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("打开串口失败");
        return -1;
    }
 
    //检测串口是否已经打开，并给出提示信息
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("打开串口成功");
    }
    else
    {
        return -1;
    }
 
    //指定循环的频率
    ros::Rate loop_rate(5000);
    ros::spin();
    return 0;

} 




/*



ros::Rate loop_rate( ); 写在循环外部 //ros::Rate rate();
loop_rate.sleep();写在循环内部 //rate.sleep();
    while(ros::ok())
    {
 
        //处理ROS的信息，比如订阅消息,并调用回调函数
        ros::spinOnce();
         ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
 
        if(ser.available()){
            ROS_INFO_STREAM("Reading from serial port");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data);
            read_pub.publish(result);
        }
        loop_rate.sleep();
 
    }
*/

#!/usr/bin/env python
import rospy

from geometry_msgs.msg import Twist

import sys, select, termios, tty

msg = """
Control The Robot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = .2
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('robot_teleop')
    pub = rospy.Publisher('/teleop/cmd_vel', Twist, queue_size=5)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0
    try:
        print msg
        print vels(speed,turn)
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                count = 0

                print vels(speed,turn)
                if (status == 14):
                    print msg
                status = (status + 1) % 15
            elif key == ' ' or key == 'k' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    th = 0
                if (key == '\x03'):
                    break

            target_speed = speed * x
            target_turn = turn * th

            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            twist = Twist()
            twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
            pub.publish(twist)

            #print("loop: {0}".format(count))
            #print("target: vx: {0}, wz: {1}".format(target_speed, target_turn))
            #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)