【ROS2】学习笔记（1）

Xiaolinbulin

已于 2022-09-01 17:27:19 修改

阅读量610

点赞数 1

分类专栏： ROS2 文章标签：学习

于 2022-09-01 17:24:30 首次发布

本文链接：https://blog.csdn.net/weixin_46302951/article/details/126646168

版权

ROS2 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

ROS2dashing学习笔记（1）

ROS2增加serial头文件

介绍如何增加头文件来声明类或函数
安装ros-dashing-serial-driver无果后，只好通过添加第三方写好的文件，将话题订阅发布的数据通过串口发送出去
参照https://www.ncnynl.com/archives/202205/5197.html对文件进行修改增加串口通信功能

新建功能包

// An highlighted block
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_header
cd ~/dev_ws/src/cpp_header/include/cpp_header
touch minimal_publisher.hpp

在minimal_publisher.hpp添加以下代码

#ifndef PUB_SERIALPORT_HPP
#define PUB_SERIALPORT_HPP

#include  <stdio.h>
#include  <stdlib.h>
#include  <unistd.h>
#include  <sys/types.h>
#include  <sys/signal.h>
#include  <sys/stat.h>
#include  <fcntl.h>
#include  <errno.h>
#include  <limits.h>
#include  <string.h>


enum sp_dev_e
{
    COM0 = 0,
    COM1,
    COM2,
    COM3,
    ttyUSB0,
    ttyUSB1,
    ttyUSB2
};
class Serial
{
    public:
    Serial();
    ~Serial();

    int OpenPort(int index);
    int SetPara(int serialfd,int speed=2,int databits=8,int stopbits=1,int parity=0);
    int WriteData(int fd,const char *data,int datalength);
    int ReadData(int fd,unsigned char *data,int datalength);
    void ClosePort(int fd);
    int BaudRate( int baudrate);
};


#endif // PUB_SERIALPORT_HPP

进入src目录，新建文件minimal_publisher.cpp

cd ~/dev_ws/src/cpp_header/src
touch minimal_publisher.cpp

在minimal_publisher.cpp添加

#include "cpp_header/minimal_publisher.hpp"

#include  <termios.h>

Serial::Serial()
{

}
Serial::~Serial()
{

}

int Serial::BaudRate( int baudrate)
{
    if(7 == baudrate)
        return B460800;
    else if(6 == baudrate)
        return B115200;
    else if(5 == baudrate)
        return B57600;
    else if(4 == baudrate)
        return B38400;
    else if(3 == baudrate)
        return B19200;
    else if(2 == baudrate)
        return B9600;
    else if(1 == baudrate)
        return B4800;
    else if(0 == baudrate)
        return B2400;
    else
        return B9600;

}

int Serial::SetPara(int serialfd,int speed,int databits , int stopbits ,int parity )
{
    struct termios termios_new;
    bzero( &termios_new, sizeof(termios_new));//等价于memset(&termios_new,sizeof(termios_new));
    cfmakeraw(&termios_new);//就是将终端设置为原始模式
    termios_new.c_cflag=BaudRate(speed);
    termios_new.c_cflag |= CLOCAL | CREAD;
  //  termios_new.c_iflag = IGNPAR | IGNBRK;

    termios_new.c_cflag &= ~CSIZE;
    switch (databits)
    {
    case 0:
        termios_new.c_cflag |= CS5;
        break;
    case 1:
        termios_new.c_cflag |= CS6;
        break;
    case 2:
        termios_new.c_cflag |= CS7;
        break;
    case 3:
        termios_new.c_cflag |= CS8;
        break;
    default:
        termios_new.c_cflag |= CS8;
        break;
    }

    switch (parity)
    {
    case 0:  				//as no parity
        termios_new.c_cflag &= ~PARENB;    //Clear parity enable
      //  termios_new.c_iflag &= ~INPCK; /* Enable parity checking */  //add by fu
        break;
    case 1:
        termios_new.c_cflag |= PARENB;     // Enable parity
        termios_new.c_cflag &= ~PARODD;
        break;
    case 2:
        termios_new.c_cflag |= PARENB;
        termios_new.c_cflag |= ~PARODD;
        break;
    default:
        termios_new.c_cflag &= ~PARENB;   // Clear parity enable
        break;
    }
    switch (stopbits)// set Stop Bit
    {
    case 1:
        termios_new.c_cflag &= ~CSTOPB;
        break;
    case 2:
        termios_new.c_cflag |= CSTOPB;
        break;
    default:
        termios_new.c_cflag &= ~CSTOPB;
        break;
    }
    tcflush(serialfd,TCIFLUSH); // 清除输入缓存
   tcflush(serialfd,TCOFLUSH); // 清除输出缓存
    termios_new.c_cc[VTIME] = 1;   // MIN与 TIME组合有以下四种：1.MIN = 0 , TIME =0  有READ立即回传 否则传回 0 ,不读取任何字元
    termios_new.c_cc[VMIN] = 1;  //    2、 MIN = 0 , TIME >0  READ 传回读到的字元,或在十分之一秒后传回TIME 若来不及读到任何字元,则传回0
    tcflush (serialfd, TCIFLUSH);  //    3、 MIN > 0 , TIME =0  READ 会等待,直到MIN字元可读
    return tcsetattr(serialfd,TCSANOW,&termios_new);  //    4、 MIN > 0 , TIME > 0 每一格字元之间计时器即会被启动 READ 会在读到MIN字元,传回值或
}

int Serial::WriteData(int fd,const char *data, int datalength )//index 代表串口号 0 串口/dev/ttyAMA1 ......
{
    if(fd <0){ return -1;}
    int len = 0, total_len = 0;//modify8.
    for (total_len = 0 ; total_len < datalength;)
    {
        len = 0;
        len = write(fd, &data[total_len], datalength - total_len);
        printf("WriteData fd = %d ,len =%d,data = %s\n",fd,len,data);
        if (len > 0)
        {
            total_len += len;
        }
        else if(len <= 0)
        {
            len = -1;
            break;
        }
     }
       return len;
}

int Serial::ReadData(int fd,unsigned char *data, int datalength)
{
       if(fd <0){ return -1;}
       int len = 0;
       memset(data,0,datalength);

       int max_fd = 0;
       fd_set readset ={0};
       struct timeval tv ={0};

       FD_ZERO(&readset);
       FD_SET((unsigned int)fd, &readset);
       max_fd = fd +1;
       tv.tv_sec=0;
       tv.tv_usec=1000;
       if (select(max_fd, &readset, NULL, NULL,&tv ) < 0)
       {
               printf("ReadData: select error\n");
       }
       int nRet = FD_ISSET(fd, &readset);
      if (nRet)
       {
              len = read(fd, data, datalength);
       }
       return len;
}

void Serial::ClosePort(int fd)
{
    struct termios termios_old;
    if(fd > 0)
    {
        tcsetattr (fd, TCSADRAIN, &termios_old);
        ::close (fd);
    }
}

int  Serial::OpenPort(int index)
{
    char *device;
    struct termios termios_old;
    int fd;
    switch(index)
    {
    case COM0:  device="/dev/ttyS0";  break;
    case COM1:  device="/dev/ttyS1";  break;
    case COM2:  device="/dev/ttyS2";  break;
    case COM3:  device="/dev/pts/2";  break;
    case ttyUSB0:  device="/dev/ttyUSB0";  break;
    case ttyUSB1:  device="/dev/ttyUSB1";  break;
    case ttyUSB2:  device="/dev/ttyUSB2";  break;
    default: device="/dev/ttyAMA2"; break;
    }

    fd = open( device, O_RDWR | O_NOCTTY |O_NONBLOCK);//O_RDWR | O_NOCTTY | O_NDELAY   //O_NONBLOCK
    if (fd < 0)
    { return -1;}
    tcgetattr(fd , &termios_old);
    return fd;
}

进入src目录，新建文件minimal_publisher_node.cpp

cd ~/dev_ws/src/cpp_header/src
touch minimal_publisher_node.cpp

minimal_publisher_node.cpp中添加以下代码

#include "cpp_header/minimal_publisher.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "stdio.h"
#include "arraylist.h"
using namespace std;

int fp;

const char* constc = nullptr;
// char Msg[] = { '20' ,'22', '23' ,'4', '2', '12', '6', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '11', '4', '13', '10' };
int test1[] = {'0','3','-2'};
string tempStr1;

Serial sp;
class TopicSuscribe01 : public rclcpp::Node
{
public:
    TopicSuscribe01(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(),"我是%s，订阅话题为：/command.",name.c_str());
        command_subscribe_ = this->create_subscription<std_msgs::msg::String>("command",10,std::bind(&TopicSuscribe01::command_callback,this,std::placeholders::_1));
    }
private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscribe_;
    void command_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        double speed = 0.0f;
        if(msg->data == "9999")
        {
            speed = 0.2f;
            RCLCPP_INFO(this->get_logger(),"收到的[%s]指令，发送速度%f",msg->data.c_str(),speed);
        
            constc = msg->data.c_str();
        }
        // constc = Msg;
        // cout<<constc<<endl;
       // char buf[10];
        //for(int i = 0;i<6;i++)
        //{
            
            //sprintf(buf,"%s",test1[i]);//linux没有itoa用sprintf代替
            //tempStr1+=buf;
        //}
        // tempStr1=test1;
        // cout << tempStr1;
        //constc = tempStr1.c_str();
        //cout << constc;
        sp.WriteData(fp,constc,10);
    }

};


int main(int argc, char **argv)
{
    /* code */
    rclcpp::init(argc, argv);
    /*产生一个Wang2的节点*/
    // auto node = std::make_shared<rclcpp::Node>("pub_serial");
    auto node = std::make_shared<TopicSuscribe01>("topic_subscribe_01");
    // 打印一句自我介绍
    // int fp;
    // Serial sp;
    sp.BaudRate(115200);
    fp = sp.OpenPort(COM3);
    if(fp>0){
      RCLCPP_INFO(node->get_logger(), "serial success!.");
    }
    
    // sp.WriteData(fp,"1024",10); RCLCPP_INFO(node->get_logger(), "serial success!.");
    }
    
    // cout<<fp<<endl;

    RCLCPP_INFO(node->get_logger(), "大家好，我是topic_subscribe_01.");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    sp.ClosePort(fp);
    return 0;
    
    return 0;
}

编辑package.xml
在<buildtool_depend>ament_cmake</buildtool_depend>后增加

<depend>rclcpp</depend>
<depend>std_msgs</depend>

编辑 CMakelist.txt
在find_package(ament_cmake REQUIRED)后增加

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

再增加可执行文件，ros2 run能够调用的名称

add_executable(minimal_publisher_node src/minimal_publisher_node.cpp src/minimal_publisher.cpp) 
ament_target_dependencies(minimal_publisher_node rclcpp std_msgs)

增加可执行文件位置，ros2 run可以找到这个可执行文件

install(TARGETS
  minimal_publisher_node
  DESTINATION lib/${PROJECT_NAME})
ament_package()

编译运行

cd ~/dev_ws/
rosdep install -i --from-path src --rosdistro dashing -y
colcon build --symlink-install --packages-select cpp_header
source install/setup.bash
ros2 run cpp_header minimal_publisher_node

话题名为/command,新建个话题发布数据，minimal_publisher_node节点就能订阅到数据并且通过串口发送出去了。

运行结果

请添加图片描述使用了虚拟串口pts/3和pts/6，由于每次新建虚拟虚拟串口对都不一样，因此根据实际情况修改minimal_publisher.cpp中最底部的COM口

参考：

ROS2与C++入门教程-增加头文件：
https://www.ncnynl.com/archives/202205/5197.html
虚拟串口：
https://blog.csdn.net/qq_41742043/article/details/124931406?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166184976616781647594090%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166184976616781647594090&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allfirst_rank_ecpm_v1~rank_v33_ecpm-15-124931406-null-null.142