提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
基于ROS的核辐射传感器驱动包
前言
- 记录一下成都西核仪器有限公司的WNB-8301G探测器的使用,其测量范围在0.1uSv/h~1Sv/h,由于需要集成在ROS框架上,基于C++,自己编写了一个ROS驱动包。对于该核辐射传感器,供电12v,基于RS485通信,使用RS485转USB转接器将其与工控机建立数据通信。
一、通信协议
- 波特率9600,校验位N,数据位8,停止位1。
- 读取实时剂量率:(根据探头地址有所变化)
例如:控制器发送数据包
03 31 00 30 04 7B 3F
解析:
03:仪表地址;
31:命令;
00,30:剂量率存储起始地址;
04:数据字节数4;
7B 3F:CRC16校验码;
传感器返回数据包
03 31 00 30 04 00 00 00 1E A8 E0
解析:
03:仪表地址;
31:命令;
00,30:剂量率存储起始地址;
04:数据字节数4;
00 00 00 14:返回数据(4byte 16进值数据0x00000014);
A8 E0:CRC16校验码;
二、驱动包框架
1.radiation.cpp
类函数代码如下:
#include "radiation.hpp"
using namespace std;
using namespace boost::asio;
//串口相关对象
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");
boost::system::error_code err;
/********************************************************
串口发送控制命令
********************************************************/
const unsigned char header[7] = {0x03, 0x31, 0x00, 0x30, 0x04, 0x7B, 0x3F};
bool radiation::radiation_init()//初始化函数
{
//串口参数初始化
sp.set_option(serial_port::baud_rate(9600));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));
pub_ = nh.advertise<std_msgs::Float64>("radiation/data", 50);
return true;
}
/********************************************************
函数功能:获得8位循环冗余校验值
入口参数:数组地址、长度
出口参数:校验值
********************************************************/
uint16_t radiation::getCrc16(unsigned char *ptr)
{
uint16_t CRCV = 0xFFFF;
unsigned char iB, i, j;
for(i = 0;i < 9; i++)
{
CRCV ^= *ptr++;
for(j = 0; j < 8 ;j++)
{
iB = (unsigned char)(CRCV & 0X0001);
CRCV = (uint16_t)(CRCV >> 1);
if(iB == 1)
{
CRCV = (uint16_t)(CRCV ^ 0XA001);
}
}
}
return CRCV;
}
/********************************************************
函数功能:下发控制命令
入口参数:
出口参数:
********************************************************/
void radiation::writeContral()
{
static unsigned char buf[7] = {0}; //
// 控制消息
for (int i = 0; i < 7; i++)
{
buf[i] = header[i];
//ROS_INFO("%02x",buf[i]);
}
// 通过串口下发数据
boost::asio::write(sp, boost::asio::buffer(buf));
}
/********************************************************
函数功能:读取数据,解析出辐射值
入口参数:
出口参数:bool
********************************************************/
bool radiation::readradiation()
{
int receive_data;//接受数据
uint16_t check;//CRC检验值
unsigned char buf[150] = {0};//接受的缓存区
unsigned char low_byte;
unsigned char high_byte;
//=========================================================
//此段代码可以读数据的结尾,进而来进行读取数据的头部
try
{
boost::asio::streambuf response;
//ROS_INFO("ready to read");
// boost::asio::read_until(sp, response, "\r\n", err);//\r\n
boost::asio::read( sp, boost::asio::buffer( buf , 11 ) );
ROS_INFO("read successful");
// copy(istream_iterator<unsigned char>(istream(&response) >> noskipws),
// istream_iterator<unsigned char>(),
// buf);
// for(int i = 0; i < 11 ; i++)
// ROS_INFO("%02x",buf[i]);
}
catch (boost::system::system_error &err)
{
ROS_INFO("read_until error");
}
//=========================================================
ROS_INFO("check!");
// 检查信息头
if (buf[0] != header[0] || buf[1] != header[1]) //buf[0] buf[1]
{
ROS_ERROR("Received message header error!");
return false;
}
//ROS_INFO("CRC校验");
// 检查信息校验值
check = getCrc16(buf); //getCrc16计算得出
//ROS_INFO("%02X",check);
low_byte = (check >> 0)& 0x00ff ;
//ROS_INFO("%02X",low_byte);
high_byte = (check >> 8)& 0x00ff ;
//ROS_INFO("%02X",high_byte);
if (low_byte != buf[9] || high_byte != buf[10]) //接受的低字节放在高位,高字节放在底位
{
ROS_ERROR("Received data check sum error!");
return false;
}
ROS_INFO("CRC校验成功!");
//ROS_INFO("开始计算辐射值!");
// 读取辐射值 -----------
receive_data = (buf[5] << 24) + (buf[6] << 16) + (buf[7] << 8) + (buf[8]);
//ROS_INFO("%d",receive_data);
final_data.data = double(receive_data / 100.0);
ROS_INFO("辐射值%f",final_data.data);
// 辐射数据发布
pub_.publish(final_data);
return true;
}
2.radiation_node.cpp
主函数代码如下:
/*
*file radiation.cpp
*@author yangkang
*@brief radiation driver
*@version 0.1
*@date 2022.11.17
*@email yangkang123@mails.swust.edu.cn
*/
#include "radiation.hpp"
int main(int argc, char **argv)
{
//初始化ROS节点
ros::init(argc, argv, "radiation_driver");
setlocale(LC_ALL,"");
radiation my_driver;//对象
if(!my_driver.radiation_init())
ROS_ERROR("radiation_driver initialized failed.");
ROS_INFO("radiation_driver initialized successful.");
//循环运行
ros::Rate loop_rate(200);
while (ros::ok())
{
my_driver.writeContral();
my_driver.readradiation();
loop_rate.sleep();
ros::spinOnce();
}
}
3.radiation.h
头文件代码如下:
#ifndef RADIATION_DRIVER_H
#define RADIATION_DRIVER_H
#include <ros/ros.h>
#include <ros/time.h>
#include <boost/asio.hpp>
#include <std_msgs/Float64.h>
class radiation
{
private:
ros::NodeHandle nh;
ros::Publisher pub_;
std_msgs::Float64 final_data;
public:
radiation(){}//构造函数
virtual ~radiation(){}//虚析构函数
bool radiation_init();//初始化函数
void writeContral();//发送控制命令
bool readradiation();//读取辐射数据
uint16_t getCrc16(unsigned char *ptr);//CRC校验
};
#endif
4.CMakeLists.txt
CMAKE工程:
cmake_minimum_required(VERSION 3.0.2)
project(radiation_driver)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
sensor_msgs
tf
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(radaation_driver
src/radiation_node.cpp
src/radiation.cpp)
target_link_libraries(radaation_driver ${catkin_LIBRARIES})
三、使用步骤
1.克隆代码
在工作空间下的src目录下:
git clone https://github.com/yangkang-1/radiation_driver.git
2.给串口权限
代码如下:
sudo chmod 777 /dev/ttyUSB0
3.编译并执行
在工作空间下:
catkin_make
在此之前开启roscore:
rosrun radiation_driver radaation_driver
4.截图
—
总结
以上就是今天要讲的内容,本文仅仅简单介绍了核辐射驱动包的组成及使用,大家可以根据自己的需要在此基础上进行更改。