ROS melodic 串口通信

topic_serial_node.cpp

// topic_serial_node.cpp
// serial API: http://wjwwood.io/serial/doc/1.1.0/index.html
// serial install: sudo apt install ros-melodic-serial

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

serial::Serial ser; //声明串口对象

//回调函数
void topic_read_callback(const std_msgs::String::ConstPtr &msg)
{
    ROS_INFO_STREAM("topic_serial_read:" << msg->data);
    //ser.write(msg->data); //发送串口数据,有了控制台,所以把串口打印关了
}

int main(int argc, char **argv)
{
    //初始化节点
    ros::init(argc, argv, "topic_serial_node");
    //声明节点句柄
    ros::NodeHandle nh;

    //订阅主题,并配置回调函数
    ros::Subscriber read_sub = nh.subscribe("topic_serial_read", 1000, topic_read_callback);
    //发布主题
    ros::Publisher write_pub = nh.advertise<std_msgs::String>("topic_serial_write", 1000);

    try
    {
        //设置串口属性,并打开串口
        ser.setPort("/dev/ttyUSB0");    // 此处要注串口设备权限,见下文
        ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException &e)
    {
        std::string err = e.what();
        ROS_ERROR_STREAM(err);
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }

    //检测串口是否已经打开,并给出提示信息
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    //指定循环的频率
    ros::Rate loop_rate(50);
    while (ros::ok())
    {
        if (ser.available())
        {
            //ROS_INFO_STREAM("Reading from serial port\n");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data);
            write_pub.publish(result);
        }

        //处理ROS的信息,比如订阅消息,并调用回调函数
        ros::spinOnce();
        loop_rate.sleep();
    }
}

launch:

<launch>
<node pkg="topic_serial" type="topic_serial_node" name="topic_serial_node"  output="screen"/>
</launch>

CMakefile.txt

cmake_minimum_required(VERSION 2.8.3)
project(topic_serial)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## 这是进行catkin构建时所需的组件包。
## 依赖包是message_generation、 std_msgs和roscpp。如果这些包不存在,在构建过程中会发生错误。
find_package(catkin REQUIRED 
        COMPONENTS 
        message_generation 
        std_msgs 
        roscpp
        serial
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)

# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################


###################################
## catkin specific configuration ##
###################################

catkin_package(
  #INCLUDE_DIRS include
  LIBRARIES   
  CATKIN_DEPENDS 
  std_msgs
  roscpp     
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include 
  ${catkin_INCLUDE_DIRS}
)

add_executable(topic_serial_node
        src/topic_serial_node.cpp
)
add_dependencies(topic_serial_node  
        ${${PROJECT_NAME}_EXPORTED_TARGETS} 
        ${catkin_EXPORTED_TARGETS}
        serial
)
target_link_libraries(topic_serial_node
        ${catkin_LIBRARIES}
)


Linux/ROS——获取串口权限

此处参考:https://blog.csdn.net/weixin_30258027/article/details/97362051

创建ttyUSB权限规则文件etc/udev/rules.d/70-ttyUSB.rules

$ sudo nano -p /etc/udev/rules.d/70-ttyUSB.rules

70-ttyUSB.rules文件中写入下列代码:

KERNEL=="ttyUSB*", OWNER="root", GROUP="root", MODE="0666" 

保存即可,重启或注销用户在登录后生效

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章