ROS 进阶学习笔记（12） - Communication with ROS through USART Serial Port

Communication with ROS through USART Serial Port

We always need to communicate with ROS through serial port as we have many devices like sensors, touch-screen, actuators to be controlled through USART serial protocol.

After some investigation, I found several ways that can make ROS work with the serial-port-devices:

• Use the package: rosserial. It seems like only the "connected rosserial-enabled device" can work upon that, including Aduino, Windows, XBee, etc.(ROS
  community said: rosserial is used with mcus that have rosserial code running on them.)
• Use the driver package: serial. It seems like a serial port driver for ROS programming. This should be the most free way but I didn't find out how to use it.
• Use the package: cereal_port, a serial port library for ROS, you can find it in theserial_communication stack, it's easy to use.
• The package:rosbridgemay also suit your needs (full disclosure, author of rosbridge here). Check outthis
  video of using rosbridge with an Arduino (though the principles apply to any uController).
• A simple approach is to write a ROS node that can read/write text messages exchanged with the microcontroller over a serial port and convert these serial messages to ROS style std_msgs/String messages.Here we are going to share this way first.
  
  
• Kevin: a service given by Kevin. It seems to work pretty good. This works well cuz a lot of our serial stuff is sending a message and receiving a response back, instead of separate publish/subscribes.reference
  web

A simple approach is to write a ROS node that can read/write text messages exchanged with the microcontroller over a serial port and convert these serial messages to ROS style std_msgs/String messages. Here we are going to share this way first. ref:reference
web

In that page, Bart says:

"A simple approach is to write a ROS node that can read/write text messages exchanged with the microcontroller over a serial port and convert these serial messages to ROS style std_msgs/String messages. A separate ROS node can then be written to convert
the simple text messages to specific ROS message formats (cmd_vel, odom, etc) based on whatever message protocol was defined for the exchange with the microcontroller.

Attached below is the source for code that I used for this purpose. There is nothing original in the code, but it may be helpful to someone new to ROS and Unix/Linux serial communication. Posting this type of a listing may be a bit unconventional
for this site as short answers are generally preferred. Hopefully no one is offended. I would be interested in hearing if there are other successful approaches to the problem."

Let's implement this approach:

Enter the catkin workspace, build the package: (tutorial page)

$ cd ~/catkin_ws/src
$ catkin_create_pkg r2serial_driver std_msgs rospy roscpp
$ cd ~/catkin_ws
$ catkin_make

To add the workspace to your ROS environment you need to source the generated setup file:

$ . ~/catkin_ws/devel/setup.bash
$ cd ~/catkin_ws/src/r2serial_driver/src

Add a file: "r2serial.cpp", Source code:

/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

//r2serial.cpp

// communicate via RS232 serial with a remote uController.
// communicate with ROS using String type messages.
// subscribe to command messages from ROS
// publish command responses to ROS

// program parameters - ucontroller# (0,1), serial port, baud rate

//Thread main
//  Subscribe to ROS String messages and send as commands to uController
//Thread receive
//  Wait for responses from uController and publish as a ROS messages

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <pthread.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <stdlib.h>

#define DEFAULT_BAUDRATE 19200
#define DEFAULT_SERIALPORT "/dev/ttyUSB0"

//Global data
FILE *fpSerial = NULL;   //serial port file pointer
ros::Publisher ucResponseMsg;
ros::Subscriber ucCommandMsg;
int ucIndex;          //ucontroller index number

//Initialize serial port, return file descriptor
FILE *serialInit(char * port, int baud)
{
  int BAUD = 0;
  int fd = -1;
  struct termios newtio;
  FILE *fp = NULL;

 //Open the serial port as a file descriptor for low level configuration
 // read/write, not controlling terminal for process,
  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY );
  if ( fd<0 )
  {
    ROS_ERROR("serialInit: Could not open serial device %s",port);
    return fp;
  }

  // set up new settings
  memset(&newtio, 0,sizeof(newtio));
  newtio.c_cflag =  CS8 | CLOCAL | CREAD;  //no parity, 1 stop bit

  newtio.c_iflag = IGNCR;    //ignore CR, other options off
  newtio.c_iflag |= IGNBRK;  //ignore break condition

  newtio.c_oflag = 0;        //all options off

  newtio.c_lflag = ICANON;     //process input as lines

  // activate new settings
  tcflush(fd, TCIFLUSH);
  //Look up appropriate baud rate constant
  switch (baud)
  {
     case 38400:
     default:
        BAUD = B38400;
        break;
     case 19200:
        BAUD  = B19200;
        break;
     case 9600:
        BAUD  = B9600;
        break;
     case 4800:
        BAUD  = B4800;
        break;
     case 2400:
        BAUD  = B2400;
        break;
     case 1800:
        BAUD  = B1800;
        break;
     case 1200:
        BAUD  = B1200;
        break;
  }  //end of switch baud_rate
  if (cfsetispeed(&newtio, BAUD) < 0 || cfsetospeed(&newtio, BAUD) < 0)
  {
    ROS_ERROR("serialInit: Failed to set serial baud rate: %d", baud);
    close(fd);
    return NULL;
  }
  tcsetattr(fd, TCSANOW, &newtio);
  tcflush(fd, TCIOFLUSH);

  //Open file as a standard I/O stream
  fp = fdopen(fd, "r+");
  if (!fp) {
    ROS_ERROR("serialInit: Failed to open serial stream %s", port);
    fp = NULL;
  }
  return fp;
} //serialInit

//Process ROS command message, send to uController
void ucCommandCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_DEBUG("uc%dCommand: %s", ucIndex, msg->data.c_str());
  fprintf(fpSerial, "%s", msg->data.c_str()); //appends newline
} //ucCommandCallback

//Receive command responses from robot uController
//and publish as a ROS message
void *rcvThread(void *arg)
{
  int rcvBufSize = 200;
  char ucResponse[rcvBufSize];   //response string from uController
  char *bufPos;
  std_msgs::String msg;
  std::stringstream ss;

  ROS_INFO("rcvThread: receive thread running");

  while (ros::ok()) {
    bufPos = fgets(ucResponse, rcvBufSize, fpSerial);
    if (bufPos != NULL) {
      ROS_DEBUG("uc%dResponse: %s", ucIndex, ucResponse);
      msg.data = ucResponse;
      ucResponseMsg.publish(msg);
    }
  }
  return NULL;
} //rcvThread

int main(int argc, char **argv)
{
  char port[20];    //port name
  int baud;     //baud rate 

  char topicSubscribe[20];
  char topicPublish[20];

  pthread_t rcvThrID;   //receive thread ID
  int err;

  //Initialize ROS
  ros::init(argc, argv, "r2serial_driver");
  ros::NodeHandle rosNode;
  ROS_INFO("r2serial_driver starting");

  //Open and initialize the serial port to the uController
  if (argc > 1) {
    if(sscanf(argv[1],"%d", &ucIndex)==1) {
      sprintf(topicSubscribe, "uc%dCommand",ucIndex);
      sprintf(topicPublish, "uc%dResponse",ucIndex);
    }
    else {
      ROS_ERROR("ucontroller index parameter invalid");
      return 1;
    }
  }
  else {
    strcpy(topicSubscribe, "uc0Command");
    strcpy(topicPublish, "uc0Response");
  }

  strcpy(port, DEFAULT_SERIALPORT);
  if (argc > 2)
     strcpy(port, argv[2]);

  baud = DEFAULT_BAUDRATE;
  if (argc > 3) {
    if(sscanf(argv[3],"%d", &baud)!=1) {
      ROS_ERROR("ucontroller baud rate parameter invalid");
      return 1;
    }
  }

  ROS_INFO("connection initializing (%s) at %d baud", port, baud);
   fpSerial = serialInit(port, baud);
  if (!fpSerial )
  {
    ROS_ERROR("unable to create a new serial port");
    return 1;
  }
  ROS_INFO("serial connection successful");

  //Subscribe to ROS messages
  ucCommandMsg = rosNode.subscribe(topicSubscribe, 100, ucCommandCallback);

  //Setup to publish ROS messages
  ucResponseMsg = rosNode.advertise<std_msgs::String>(topicPublish, 100);

  //Create receive thread
  err = pthread_create(&rcvThrID, NULL, rcvThread, NULL);
  if (err != 0) {
    ROS_ERROR("unable to create receive thread");
    return 1;
  }

  //Process ROS messages and send serial commands to uController
  ros::spin();

  fclose(fpSerial);
  ROS_INFO("r2Serial stopping");
  return 0;
}

Modify the CMakeList.txt file:

$ cd ~/catkin_ws/src/r2serial_driver
$ gedit CMakeList.txt

add/modify these lines as below:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(r2serial_driver src/r2serial.cpp)

add_dependencies(r2serial_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(r2serial_driver
   ${catkin_LIBRARIES}
 )

Save and Build the package again:

$ cd ~/catkin_ws
$ catkin_make -DCMAKE_BUILD_TYPE=Release 

He also offered a simple launch file that interfaces to two microcontrollers using two serial connections, put the code into

  "~/catkin_ws/src/r2serial_driver/launch/r2serial_driver.launch" file:

<launch>
  <node pkg="r2serial_driver" type="r2serial_driver" name="r2serial0" args="0 /dev/ttyUSB0 9600" output="screen" >
  <remap from="ucCommand" to="uc0Command" />
  <remap from="ucResponse" to="uc0Response" />
  </node>

  <node pkg="r2serial_driver" type="r2serial_driver" name="r2serial1" args="1 /dev/ttyUSB1 9600" output="screen" >
  <remap from="ucCommand" to="uc1Command" />
  <remap from="ucResponse" to="uc1Response" />
  </node>

</launch>

Usage:

$ rosrun r2serial_driver r2serial_driver 0 /dev/ttyUSB0 9600

or

$ roslaunch r2serial_driver r2serial_driver.launch

try:

$ rostopic pub -r 1 /uc0Command std_msgs/String hello_my_serial
$ rostopic echo /uc0Response

You may also need to remap the /dev/ttyUSB* to some name you like cuz you may have several usb-serial devices.

If so, just Ref:重新指派usb转串口模块在linux系统中的设备调用名称(English Version: remap your usb-serial devices' names in Linux
)

But Kevin mentioned a question: "Interesting, but a lot of my serial stuff is send a message and receive a response back. Instead of separate publish/subscribes have you tried a service? That seems like it would work nicely for this."

I also noticed a problem: The Linux's System RAM and CPU will be much costed by r2serial_driver when it is running.

Next time, we'll see how to use Kevin's service to play ROS serial communication.

2. Use Service to play ROS-Serial communication

see this blog brother below...

http://blog.csdn.net/sonictl/article/details/51372534