ROS开发（二）——添加传感器话题(压力传感器)

发表于

这是一个将压力传感器的电阻信息值，经电阻转电压模块，在经Nano模拟量采样，作为i2c从单元读取

足底压力传感器

1.关于传感器

1.1 传感器参数

初始电阻值 迟滞性 漂移 工作温度 反应时间 工作电压
20M <10% <10% -60~60 <5msec 3.5~5V

pic

1.2 压力转电压传感器(RFP-ZH)参数

工作电压 模拟口输出电压 元器件精度 测量范围
3.3~5V 0~5V 1% 20kg

ｐｉｃ

1.3 arduino Nano 模拟采集转i2c通信

因为树莓派ARM版上没有足够模拟采集口，因此通过Arduino Nano 开发板的６个模拟采集口读取单个所需薄膜压力传感器转为压力后的数据，在将Arduino Nano作为i2c从设备地址分别设为0x07,0x08，将数据分为高位和低位共１２个数据单元。
Arduino Nano 程序

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
// SCL A5
// SDA A4
// A0~3 read analog input
// 共12个uint_8数据，高低位为一组数据，共有6组 20181022
#include <Wire.h>
uint8_t i2cData[13];//高低位一组
const int analogInPin0=A0;
const int analogInPin1=A1;
const int analogInPin2=A2;
const int analogInPin3=A3;
const int analogInPin4=A6;
const int analogInPin5=A7;
static int i=0;
void setup() {
  Wire.begin(0x08);
  pinMode(LED_BUILTIN, OUTPUT);
  Wire.onReceive(receiveEvent);
  Wire.onRequest(requestEvent);
  Serial.begin(115200);
}
void receiveEvent(int howMany){
  while (1 < Wire.available()) { // loop through all but the last
    char c = Wire.read(); // receive byte as a character
    Serial.print(c);         // print the character
  }
  Serial.println("i am 0x08");
}
void requestEvent(){
  digitalWrite(LED_BUILTIN, HIGH);
  Wire.write(i2cData[i]);
  Serial.println(i2cData[i]);
  i++;
  if(i==12)i=0;
  digitalWrite(LED_BUILTIN, LOW);
}
void loop() {
  // put your main code here, to run repeatedly:
  if(i==0){
    uint16_t sensor0=analogRead(analogInPin0);
    uint16_t sensor1=analogRead(analogInPin1);
    uint16_t sensor2=analogRead(analogInPin2);
    uint16_t sensor3=analogRead(analogInPin3);
    uint16_t sensor4=analogRead(analogInPin4);
    uint16_t sensor5=analogRead(analogInPin5);
    i2cData[0]=(sensor0>>8)&0xff;
    i2cData[1]=(sensor0)&0xff;
    i2cData[2]=(sensor1>>8)&0xff;
    i2cData[3]=(sensor1)&0xff;
    i2cData[4]=(sensor2>>8)&0xff;
    i2cData[5]=(sensor2)&0xff;
    i2cData[6]=(sensor3>>8)&0xff;
    i2cData[7]=(sensor3)&0xff;
    i2cData[8]=(sensor4>>8)&0xff;
    i2cData[9]=(sensor4)&0xff;
    i2cData[10]=(sensor5>>8)&0xff;
    i2cData[11]=(sensor5)&0xff;
    i2cData[12]='\0';
  }
  delay(5);
}

2.编写发布话题数据包

2.1 安装WiringPi

WiringPi是在ARM平台上GPIO操作库，能调用i2c,spi等资源,在树莓派上调用

- sudo apt-get install git-core
- git clone git://git.drogon.net/wiringPi
- cd wiringPi
- ./build

尝试安装是否成功

- gpio -V

2.2 编写FindWiringPi.cmake文件

该文件是编写CMake文件时用于索引WiringPi库的文件，存放在/opt/cmake-3.12.1/Modules文件夹中，新建该文件

- vim FindWiringPi.cmake

1
2
3
4
5
find_library(WIRINGPI_LIBRARIES NAMES wiringPi)
find_path(WIRINGPI_INCLUDE_DIRS NAMES wiringPi.h)

include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(wiringPi DEFAULT_MSG WIRINGPI_LIBRARIES WIRINGPI_INCLUDE_DIRS)

2.3 创建press_sensor包

-  roscreate-pkg press_sensor roscpp

2.4 编写CMakeLists.txt文件

编译时链接WiringPi库

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
cmake_minimum_required(VERSION 2.8.3)
project(press_sensor)

add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  sensor_msgs
)
find_package(WiringPi REQUIRED)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES press_sensor
 CATKIN_DEPENDS roscpp std_msgs  sensor_msgs
#  DEPENDS system_lib
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)
add_executable(${PROJECT_NAME}_node src/press_sensor_node.cpp src/motor.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
  ${WIRINGPI_LIBRARIES}
)

2.5 修改package.xml

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
<?xml version="1.0"?>
<package>
  <name>press_sensor</name>
  <version>0.0.0</version>
  <description>The press_sensor package</description>
  <maintainer email="shisanchuan@todo.todo">shisanchuan</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

2.6 编写press_sensor_node.cpp文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#include <std_msgs/Empty.h>
#include <sensor_msgs/JointState.h>
#include <ros/ros.h>
#include <wiringPiI2C.h>
#include <wiringPi.h>
#include <string>

int main(int argc, char  *argv[])
{
	int right_address;
	int left_address;
	std::string NodeHandle_name;
	std::string pub_name;
	ros::init(argc, argv, "press_sensor");
	ros::NodeHandle private_node_handle("~");
	private_node_handle.param<int>("righr_address",right_address,0x07);
	private_node_handle.param<int>("left_address",left_address,0x08);
	private_node_handle.param<std::string>("NodeHandle_name",NodeHandle_name,"right");
	private_node_handle.param<std::string>("pub_name",pub_name,"data");
	ros::NodeHandle n(NodeHandle_name);
	ros::Publisher pub = n.advertise<sensor_msgs::JointState>(pub_name, 1000);
	ros::Rate loop_rate(50);
	sensor_msgs::JointState press_sensor;

	int right=wiringPiI2CSetup(right_address&0xff);
	int left=wiringPiI2CSetup(left_address&0xff);
	if(-1==right||-1==left){
		ROS_ERROR_STREAM("I2C open fail!!");
		return -1;
	}
	while(ros::ok()){
		press_sensor.header.stamp=ros::Time::now();
		press_sensor.name.resize(12);
		press_sensor.position.resize(12);
		int i=0;
		for(;i<6;i++){
			int high=wiringPiI2CRead(right);
			int low=wiringPiI2CRead(right);
			ROS_INFO_STREAM("high is "<<std::hex<<high<<"\t"<<"low is"<<std::hex<<low);
			press_sensor.name[i]=std::to_string(i+1);
			press_sensor.position[i]=int(((high&0xff)<<8)|low&0xff);
		}
		for(;i<12;i++){
			int high=wiringPiI2CRead(right);
			int low=wiringPiI2CRead(right);
			ROS_INFO_STREAM("high is "<<std::hex<<high<<"\t"<<"low is"<<std::hex<<low);
			press_sensor.name[i]=std::to_string(i+1);
			press_sensor.position[i]=int(((high&0xff)<<8)|low&0xff);
		}
		pub.publish(press_sensor);
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}

2.7 编写launch文件

1
2
3
4
5
6
7
8
<launch>
  <node pkg="press_sensor" type="press_sensor_node" name="press" required="true">
    <param name="right_address" type="int"  value="7"/>
    <param name="left_address" type="int"  value="8"/>
    <param name="NodeHandle_name" value="press"/>
    <param name="pub_name" value="data"/>
  </node>
</launch>