ROS通过串口与单片机通信读取编码器的计数脉冲

版权声明：转载请标明出处，谢谢！ https://blog.csdn.net/SimileciWH/article/details/84975951

1，使用C++编写需要安装serial包

serial工具包，下载完成后按照提示编译安装，默认使用

make install

会将ROS编译工程需要的serial.h头文件和serialConfig.cmake等全部必要的文件移动到/tmp/usr/local…的目录下，因此在make install后，我们需要手动将这些移动到/usr/local的根目录下，否则构建节点时，会提示以下错误信息：

1. can not found serial/serial.h
2. serialConfig.cmake is not found

等其他问题，解决办法就是

sudo cp -r /tmp/usr/local/* /usr/local

然后重新加载bash的环境，

source ~/.bashrc

在编译即可。
如果还是提示缺少，那就把，/usr/local的路径添加到cmake编译的路径中去，使用export指令。

2，创建一个ros pkg

$ catkin_create_pkg getserial_data std_msgs rospy roscpp

2.1 编辑CMakeList.txt

可以参考我的CMakeList.txt文件。其中msg部分，mobileRobot_velocity.msg是与此无关的，mobileRobot_msgs.msg是存放编码起信息的数据结构，如下所示：

cmake_minimum_required(VERSION 2.8.3)
project(getodomdata_serial)

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

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  nav_msgs
  std_msgs
  geometry_msgs
  tf
  message_generation
  serial
)

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


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

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

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  mobileRobot_velocity.msg
  mobileRobot_msgs.msg
)

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs  # Or other packages containing msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
 LIBRARIES getodomdata_serial
 CATKIN_DEPENDS 
 rospy
 std_msgs
 serial
 geometry_msgs
 tf
 message_runtime
#  DEPENDS system_lib
)

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

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

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/getodomdata_serial.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/getodomdata_serial_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/getodomdata.py receiveodomdata.py
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_getodomdata_serial.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)


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

mobileRobot_msgs.msg文件内容，

string encoder_name1
int64 left_enc

string encoder_name2
int64 right_enc

这些信息的结构是依赖std_msgs。

2.1 C++源文件

我的硬件，在读取小车左右轮编码器时，需要先发送左右轮的控制指令，即代码中的lcmd_buffer && rcmd_buffer，然后读取数据20位，其中数据的5 ~ 8, 15 ~18为左右轮的编码器二进制的数据。

#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include "std_msgs/UInt8.h"
#include "std_msgs/UInt8MultiArray.h"
#include "getodomdata_serial/mobileRobot_msgs.h"
#include "string.h"

serial::Serial ros_ser;
typedef union
{
    int si;
    char sc[4];
}typechange;

void readEncoder_serial_pub(uint8_t *lcmd_buffer, uint8_t *rcmd_buffer, getodomdata_serial::mobileRobot_msgs &encoder_data, uint8_t *serial_data, ros::Publisher enc_pub)
{
    // send lcmd && rcmd
    ros_ser.write(lcmd_buffer,10);
    ros_ser.write(rcmd_buffer,10);
    ROS_INFO_STREAM("Write to serial port");
    if(ros_ser.available())
    {
        ROS_INFO_STREAM("Reading from serial port");
        //获取串口数据
        if (ros_ser.available() == 20)
        {
        ros_ser.read(serial_data, 20);
        }
        if (transferData(encoder_data, serial_data) == true)
        //将串口数据发布到主题sensor
        enc_pub.publish(encoder_data);
        else
        ROS_INFO_STREAM("transfer data failure, cannot publish topic...");
    }
}

int main (int argc, char** argv)
{
    // initial parameters
    std::string node_name;
    static uint8_t lcmd_buffer[10]={0x02, 0x40, 0x63, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb};
    static uint8_t rcmd_buffer[10]={0x01, 0x40, 0x63, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc};
    uint8_t serial_data[20];
    getodomdata_serial::mobileRobot_msgs encoder_data;
    
    // initial parameter service

     ros::init(argc, argv, "serial_node");
     node_name=ros::this_node::getName();
     ROS_INFO("%s is starting...", node_name.c_str());
     ros::NodeHandle n;

     //订阅主题command
     //ros::Subscriber vtarget_sub = n.subscribe("/wheel_vtarget", 1000, vtarget_callback);
     //发布主题sensor
     ros::Publisher enc_pub = n.advertise<getodomdata_serial::mobileRobot_msgs>("encoder", 1000);

     try 
     {
         ros_ser.setPort("/dev/ttyUSB0");
         ros_ser.setBaudrate(115200);
         serial::Timeout to = serial::Timeout::simpleTimeout(1000);
         ros_ser.setTimeout(to);
         ros_ser.open();
     }
     catch (serial::IOException& e)
     {
         ROS_ERROR_STREAM("Unable to open port ");
         return -1;
     }
     if(ros_ser.isOpen())
     {
         ROS_INFO_STREAM("Serial Port opened");
     }
     else
     {
         return -1;
     }
     ros::Rate loop_rate(10);
     while(ros::ok())
     {
        ros::spinOnce();

        // get left && right encoder and publisher /encoder topic
        readEncoder_serial_pub(lcmd_buffer, rcmd_buffer, encoder_data, serial_data, enc_pub);

        loop_rate.sleep();
     }
     return 0;
 }

3，通过共用体实现数据转化

因为共用体，公用一段存储空间，一个int类型4个字节，一个char型1个字节，因此通过4个char可以等价转化为一个int类型。而从串口读取道德数据，因为我督导的是二进制的编码格式，因此通过共用体的这个特性可以转化成int类型。

#include "string.h"
#include "iostream"
typedef union
{
    int ui;
    char uc[4];
}typechange;
int main()
{
    //char a[0]={0xb4,0xa2,0xfe,0xff };
    int b=0;
    typechange change;
    change.uc[0]=0xb4;
    change.uc[1]=0xa2;
    change.uc[2]=0xfe;
    change.uc[3]=0xff;
    b=change.ui;
    std::cout << "b= " << b << std::endl;
	return 0;
}