is-a
基类与派生类的关系
实际应用---ROS当中的数据接口
sdk.h
RosNodeControlInterface是个抽象类(因为有纯虚函数)
cpp
// 控制接口
class RosNodeControlInterface {
public:
virtual ~RosNodeControlInterface() = default;
virtual void setDtofSubframeODR(int odr) = 0;
virtual int getDtofSubframeODR() const = 0;
virtual void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) = 0;
virtual bool sendOdomBaseLinkTF() const = 0;
virtual void setCloudRawConfidenceThreshold(int threshold) = 0;
virtual int cloudRawConfidenceThreshold() const = 0;
};
// 声明函数return回控制接口
RosNodeControlInterface* getRosNodeControl();
sdk.cpp
cpp
// 继承控制接口进行实现-Implementation
class RosNodeControlImpl : public RosNodeControlInterface {
public:
void setDtofSubframeODR(int interval) override {
dtof_subframe_interval_time = interval;
}
int getDtofSubframeODR() const override {
return dtof_subframe_interval_time;
}
void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) override {
pub_odom_baselink_tf = send_odom_baselink_tf;
}
bool sendOdomBaseLinkTF() const override {
return pub_odom_baselink_tf;
}
void setCloudRawConfidenceThreshold(int threshold) {
cloud_raw_confidence_threshold = threshold;
}
int cloudRawConfidenceThreshold() const {
return cloud_raw_confidence_threshold;
}
private:
int dtof_subframe_interval_time = 0;
bool pub_use_host_ros_time = false;
bool pub_odom_baselink_tf = false;
int cloud_raw_confidence_threshold = 35;
};
// 静态限制可见范围 实例化
static RosNodeControlImpl g_rosNodeControlImpl;
// 返回接口
RosNodeControlInterface* getRosNodeControl() {
return &g_rosNodeControlImpl;
}
has-a
类与成员对象的关系
实际应用---ROS当中的串口数据收发
serial.h
cpp
// 串口类
class Serial
{
public:
//打开串口返回打开结果
bool open(const char *device, int rate, int flow_ctrl, int databits,int stopbits, int parity, serial_recv_cb cb);
//发送数据
int send(uint8_t *data, int len);
//关闭串口
void close(void);
private:
//私有-成员函数---设置
bool config(int speed, int flow_ctrl, int databits, int stopbits,int parity);
//接收线程的句柄
pthread_t recv_thread_;
//私有-静态-成员函数---
//pthread_create 要求入口函数签名是:void *(*)(void *)非 static 成员函数隐含 this 参数,签名不匹配 pthread_create。static 成员函数没有隐含 this,就能作为 C 风格函数指针传给 pthread_create。
static void *serial_recv(void *p);
// 运行标志位:1 运行,0 停止。
int running_;
// 上层注册的接收回调。
serial_recv_cb recv_cb_;
// 文件描述符
int fd_;
};
serial.cpp
cpp
/***private****************************************************************/
//私有-静态-成员函数
/*
在线程创建时由 pthread 库自动调用。
pthread_create 成功后,系统会在新线程中进入这个入口函数
新线程启动时,进入serial_recv 函数。
*/
void *Serial::serial_recv(void *p)
{
uint8_t recvbuff[1024];
int recvlen = 0;
fd_set rfds;
Serial *pThis = (Serial *)p;
usleep(100000);
FD_ZERO(&rfds);
FD_SET(pThis->fd_, &rfds);
//这里的while会使得这个线程一直循环执行(running运行标志位为1时)
while (pThis->running_)
{
// 清空接收缓冲区。
memset(recvbuff, 0, sizeof(recvbuff));
// 阻塞等待串口可读。
select(pThis->fd_ + 1, &rfds, NULL, NULL, NULL);
// 读取串口数据。
recvlen = read(pThis->fd_, recvbuff, sizeof(recvbuff));
if (recvlen <= 0)
{
continue;
}
//接收回调
pThis->recv_cb_(recvbuff, recvlen);
}
return NULL;
}
bool Serial::config(int speed, int flow_ctrl, int databits, int stopbits, int parity)
{
int i;
int status;
int speed_arr[] = {B460800, B230400, B115200, B57600, B38400, B19200, B9600};
int name_arr[] = {460800, 230400, 115200, 57600, 38400, 19200, 9600};
struct termios options;
//获取设备属性信息
if (tcgetattr(fd_, &options) != 0)
{
perror("SetupSerial");
return false;
}
//设置串口输入波特率和输出波特率 /i o 入和出
for (i = 0; i < sizeof(speed_arr) / sizeof(int); i++)
{
if (speed == name_arr[i])
{
cfsetispeed(&options, speed_arr[i]);
cfsetospeed(&options, speed_arr[i]);
}
}
//修改控制模式,保证程序不会占用串口
options.c_cflag |= CLOCAL;
//修改控制模式,使得能够从串口中读取输入数据
options.c_cflag |= CREAD;
//设置数据流控制
switch (flow_ctrl)
{
case 0: //不使用流控制
options.c_cflag &= ~CRTSCTS;
break;
case 1: //使用硬件流控制
options.c_cflag |= CRTSCTS;
break;
case 2: //使用软件流控制
options.c_cflag |= IXON | IXOFF | IXANY;
break;
default:
fprintf(stderr, "Unsupported flow ctrl\n");
return false;
}
//设置数据位
options.c_cflag &= ~CSIZE; //屏蔽其他标志位
switch (databits)
{
case 5:
options.c_cflag |= CS5;
break;
case 6:
options.c_cflag |= CS6;
break;
case 7:
options.c_cflag |= CS7;
break;
case 8:
options.c_cflag |= CS8;
break;
default:
fprintf(stderr, "Unsupported data size\n");
return false;
}
//设置校验位
switch (parity)
{
case 'n':
case 'N': //无奇偶校验位。
options.c_cflag &= ~PARENB;
options.c_iflag &= ~INPCK;
break;
case 'o':
case 'O': //设置为奇校验
options.c_cflag |= (PARODD | PARENB);
options.c_iflag |= INPCK;
break;
case 'e':
case 'E': //设置为偶校验
options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD;
options.c_iflag |= INPCK;
break;
case 's':
case 'S': //设置为空格
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
break;
default:
fprintf(stderr, "Unsupported parity\n");
return false;
}
// 设置停止位
switch (stopbits)
{
case 1:
options.c_cflag &= ~CSTOPB;
break;
case 2:
options.c_cflag |= CSTOPB;
break;
default:
fprintf(stderr, "Unsupported stop bits\n");
return false;
}
//修改输出模式,原始数据输出
options.c_oflag &= ~OPOST;
options.c_lflag |= ~(ICANON | ECHOE | ECHO | ISIG);
//(ICANON | ECHO | ECHOE);
//设置等待时间和最小接收字符
options.c_cc[VTIME] = 1; /* 读取一个字符等待1*(1/10)s */
options.c_cc[VMIN] = 1; /* 读取字符的最少个数为1 */
//如果发生数据溢出,接收数据,但是不再读取
tcflush(fd_, TCIFLUSH);
//激活配置 (将修改后的termios数据设置到串口中)
if (tcsetattr(fd_, TCSANOW, &options) != 0)
{
perror("com set error!/n");
return false;
}
return true;
}
/***public****************************************************************/
bool Serial::open(const char *device, int rate, int flow_ctrl, int databits, int stopbits, int parity,serial_recv_cb cb)
{
int ret;
pthread_attr_t attr;
recv_thread_ = 0;
//句柄检查
if (NULL == device || NULL == cb)
{
DBG_LOG_ERROR("NULL == device || NULL == recv_callback");
return false;
}
recv_cb_ = cb;
//打开设备
fd_ = ::open(device, O_RDWR | O_NOCTTY);
if (fd_ < 0)
{
return false;
}
if (flock(fd_, LOCK_EX | LOCK_NB) != 0)
{
DBG_LOG_ERROR("serial %s already opened", device);
::close(fd_);
goto error;
}
//设定属性
if (config(rate, flow_ctrl, databits, stopbits, parity) == false)
{
goto error;
}
running_ = 1;
//创建线程,接收数据
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); //分离属性
ret = pthread_create(&recv_thread_, &attr, serial_recv, this);
if (0 != ret)
{
DBG_LOG_ERROR("serial receive thread create failed!");
goto error;
}
return true;
error:
running_ = 0;
if (fd_ > 0)
{
::close(fd_);
}
if (0 != recv_thread_)
{
pthread_join(recv_thread_, NULL);
}
return false;
}
int Serial::send(uint8_t *data, int len)
{
int sended_len = 0;
if (fd_ <= 0)
{
return -1;
}
//反复发送,直到全部发完
while (sended_len != len)
{
int retlen = 0;
retlen = write(fd_, data + sended_len, len - sended_len);
if (retlen < 0)
{
close();
return -1;
}
sended_len += retlen;
}
return 0;
}
void Serial::close(void)
{
//将标志位置0---结束线程
running_ = 0;
if (fd_ > 0)
{
::close(fd_);
}
if (0 != recv_thread_)
{
pthread_join(recv_thread_, NULL);
}
}
core.h
cpp
class TianbotCore
{
public:
// 串口对象:负责与底盘控制板进行数据收发。
Serial serial_;
// 调试结果发布器:将底盘返回的调试字符串发布到 ROS 话题。
ros::Publisher debug_result_pub_;
// 调试命令订阅器:接收上位机发来的调试命令。
ros::Subscriber debug_cmd_sub_;
ros::NodeHandle nh_;
// 心跳定时器:周期发送心跳并检测发送异常。
ros::Timer heartbeat_timer_;
// 调试命令执行完成标志及结果缓存。
bool debugResultFlag_;
string debugResultStr_;
// 初始化完成标志:避免初始化前处理业务数据。
bool initDone_;
TianbotCore(ros::NodeHandle *nh);
// 校验参数中的设备类型与实际底盘类型是否一致。
void checkDevType(void);
// 纯虚接口:由具体机型实现业务数据解析与处理。
virtual void tianbotDataProc(unsigned char *buf, int len) = 0;
private:
// 通信超时定时器:超时则提示与底盘通信异常。
ros::Timer communication_timer_;
// 串口接收回调:按协议组帧并做 BCC 校验。
void serialDataProc(uint8_t *data, unsigned int data_len);
// 心跳回调:发送心跳,失败时执行串口重连。
void heartCallback(const ros::TimerEvent &);
// 通信超时回调:输出错误日志。
void communicationErrorCallback(const ros::TimerEvent &);
// 话题调试命令回调:透传字符串到串口协议。
void debugCmdCallback(const std_msgs::String::ConstPtr &msg);
// 服务调试命令回调:同步等待底盘返回。
bool debugCmdSrv(tianbot_core::DebugCmd::Request &req, tianbot_core::DebugCmd::Response &res);
// 调试服务句柄。
ros::ServiceServer param_set_;
core.cpp
cpp
void TianbotCore::serialDataProc(uint8_t *data, unsigned int data_len)
{
static uint8_t state = 0;
uint8_t *p = data;
static vector<uint8_t> recv_msg;
static uint32_t len;
uint32_t j;
while (data_len != 0)
{
switch (state)
{
case 0:
if (*p == (PROTOCOL_HEAD & 0xFF))
{
recv_msg.clear();
recv_msg.push_back(PROTOCOL_HEAD & 0xFF);
state = 1;
}
p++;
data_len--;
break;
case 1:
if (*p == ((PROTOCOL_HEAD >> 8) & 0xFF))
{
recv_msg.push_back(((PROTOCOL_HEAD >> 8) & 0xFF));
p++;
data_len--;
state = 2;
}
else
{
state = 0;
}
break;
case 2: // len
recv_msg.push_back(*p);
len = *p;
p++;
data_len--;
state = 3;
break;
case 3: // len
recv_msg.push_back(*p);
len += (*p) * 256;
if (len > 1024 * 10)
{
state = 0;
break;
}
p++;
data_len--;
state = 4;
break;
case 4: // pack_type
recv_msg.push_back(*p);
p++;
data_len--;
len--;
state = 5;Serial
case 5: // pack_type
recv_msg.push_back(*p);
p++;
data_len--;
len--;
state = 6;
break;
case 6: //
if (len--)
{
recv_msg.push_back(*p);
p++;
data_len--;
}
else
{
state = 7;
}
break;
case 7: {
int i;
uint8_t bcc = 0;
recv_msg.push_back(*p);
p++;
data_len--;
state = 0;
for (i = 4; i < recv_msg.size(); i++)
{
bcc ^= recv_msg[i];
}
if (bcc == 0)
{
if (initDone_)
{
tianbotDataProc(&recv_msg[0], recv_msg.size()); // process recv msg
}
communication_timer_.stop(); // restart timer for communication timeout
communication_timer_.start();
}
else
{
ROS_INFO("BCC error");
}
state = 0;
}
break;
default:
state = 0;
break;
}
}
}
void TianbotCore::communicationErrorCallback(const ros::TimerEvent &)
{
ROS_ERROR_THROTTLE(5, "Communication with base error");
}
void TianbotCore::heartCallback(const ros::TimerEvent &)
{
vector<uint8_t> buf;
uint16_t dummy = 0;
buildCmd(buf, PACK_TYPE_HEART_BEAT, (uint8_t *)&dummy, sizeof(dummy));
if (serial_.send(&buf[0], buf.size()) != 0)
{
std::string param_serial_port;
int32_t param_serial_baudrate;
nh_.param<std::string>("serial_port", param_serial_port, DEFAULT_SERIAL_DEVICE);
nh_.param<int>("serial_baudrate", param_serial_baudrate, DEFAULT_SERIAL_BAUDRATE);
heartbeat_timer_.stop();
communication_timer_.stop();
while (serial_.open(param_serial_port.c_str(), param_serial_baudrate, 0, 8, 1, 'N',
boost::bind(&TianbotCore::serialDataProc, this, _1, _2)) != true)
{
ROS_ERROR_THROTTLE(5.0, "Device %s disconnected", param_serial_port.c_str());
ros::Duration(0.5).sleep();
}
ROS_INFO("Device %s connected", param_serial_port.c_str());
heartbeat_timer_.start();
communication_timer_.start();
}
}
void TianbotCore::debugCmdCallback(const std_msgs::String::ConstPtr &msg)
{
vector<uint8_t> buf;
buildCmd(buf, PACK_TYPE_DEBUG, (uint8_t *)msg->data.c_str(), msg->data.length());
serial_.send(&buf[0], buf.size());
}
bool TianbotCore::debugCmdSrv(tianbot_core::DebugCmd::Request &req, tianbot_core::DebugCmd::Response &res)
{
vector<uint8_t> buf;
debugResultFlag_ = false;
uint32_t count = 200;
buildCmd(buf, PACK_TYPE_DEBUG, (uint8_t *)req.cmd.c_str(), req.cmd.length());
serial_.send(&buf[0], buf.size());
if (req.cmd == "reset")
{
res.result = "reset";
return true;
}
else if (req.cmd == "param save" || req.cmd == "param reset")
{
count = 2000;
}
else if (req.cmd.find("set_") != req.cmd.npos) // adaptation for old racecar
{
count = 3000;
}
while (count-- && !debugResultFlag_)
{
ros::Duration(0.001).sleep();
}
if (debugResultFlag_)
{
res.result = debugResultStr_;
return true;
}
else
{
return false;
}
}
void TianbotCore::checkDevType(void)
{
string type_keyword_list[] = {"base_type: ", "end"};
string type;
string dev_param;
string dev_type;
string::size_type start;
string::size_type end;
string cmd = "param get";
vector<uint8_t> buf;
uint32_t count;
uint32_t retry;
for (retry = 0; retry < 5; retry++)
{
debugResultFlag_ = false;
count = 300;
buf.clear();
buildCmd(buf, PACK_TYPE_DEBUG, (uint8_t *)cmd.c_str(), cmd.length());
serial_.send(&buf[0], buf.size());
while (count-- && !debugResultFlag_)
{
ros::Duration(0.001).sleep();
}
if (debugResultFlag_)
{
dev_param = debugResultStr_;
break;
}
else
{
ROS_INFO("Get Device type failed, retry after 1s ...");
ros::Duration(1).sleep();
}
}
if (retry == 5)
{
ROS_ERROR("No valid device type found");
return;
}
for (int i = 0; type_keyword_list[i] != "end"; i++)
{
start = dev_param.find(type_keyword_list[i]);
if (start != dev_param.npos)
{
start += type_keyword_list[i].length();
end = dev_param.find("\r\n", start);
if (end == dev_param.npos)
{
end = dev_param.length();
}
dev_type = dev_param.substr(start, end - start);
ROS_INFO("Get device type [%s]", dev_type.c_str());
nh_.param<std::string>("type", type, DEFAULT_TYPE);
if (dev_type == "omni" || dev_type == "mecanum")
{
dev_type = "omni";
}
if (type == dev_type)
{
ROS_INFO("Device type match");
}
else
{
ROS_ERROR("Device type mismatch, set [%s] get [%s]", type.c_str(), dev_type.c_str());
}
return;
}
}
ROS_ERROR("No valid device type found");
}
TianbotCore::TianbotCore(ros::NodeHandle *nh) : nh_(*nh), initDone_(false)
{
std::string param_serial_port;
int32_t param_serial_baudrate;
nh_.param<std::string>("serial_port", param_serial_port, DEFAULT_SERIAL_DEVICE);
nh_.param<int>("serial_baudrate", param_serial_baudrate, DEFAULT_SERIAL_BAUDRATE);
debug_result_pub_ = nh_.advertise<std_msgs::String>("debug_result", 1);
debug_cmd_sub_ = nh_.subscribe("debug_cmd", 1, &TianbotCore::debugCmdCallback, this);
param_set_ = nh_.advertiseService<tianbot_core::DebugCmd::Request, tianbot_core::DebugCmd::Response>("debug_cmd_srv", boost::bind(&TianbotCore::debugCmdSrv, this, _1, _2));
heartbeat_timer_ = nh_.createTimer(ros::Duration(0.2), &TianbotCore::heartCallback, this);
communication_timer_ = nh_.createTimer(ros::Duration(0.2), &TianbotCore::communicationErrorCallback, this);
heartbeat_timer_.stop();
communication_timer_.stop();
while (serial_.open(param_serial_port.c_str(), param_serial_baudrate, 0, 8, 1, 'N',
boost::bind(&TianbotCore::serialDataProc, this, _1, _2)) != true)
{
ROS_ERROR_THROTTLE(5.0, "Device %s connect failed", param_serial_port.c_str());
ros::Duration(0.5).sleep();
}
ROS_INFO("Device %s connect successfully", param_serial_port.c_str());
heartbeat_timer_.start();
communication_timer_.start();
}