效果：通过can总线直接向机械手控制器发送控制指令，实现机械手闭合控制，同时实现六维力传感器数据实时监测。
环境：ubuntu14.04+ROS indigo+pcan驱动（插入pcan后，能够使用ls /dev/pcan*显示出端口名称）。
[正文]
通过libpcan库操作can总线实现数据发送、读取，按照机械手控制器的数据格式要求，即可实现机械手和六维力传感器的驱动。可以通过两种方法获得机械手和六维力传感器个数据格式：其一是通过在控制机械手同时，外接一根pcan usb到windows下PcanView软件观察总结机械手控制时的数据格式，然后进行模仿，该方法较为直接，但是需要将数据和机械手控制所用的角速度、角度等信息进行对应，因此需要一定的经验。其二是从barrett的节点文件barrett_hand-indigo-devel/bhand_controller/src/bhand_controller/下的bhand_node.py，pyHand_api.py入手，找到其与机械手操作相关的函数或类的数据格式定义。而六维力传感器的数据格式及初始化操作要从MonitorForceTorque.cpp中分析得到。
3.2.1 显式调用libpcan.so 动态链接库实现can总线操作
下面给出一个libcan库的操作例子，本例博文地址：http://blog.csdn.net/ hookie1990/article/details/52629043。
1，添加载入需要的头文件及文件中自己使用的头文件。载入相关的头文件为dlfcn.h，fcntl.h，由于目标使用libpcan函数，所以libpcan.h也要加入。
l dlfcn.h : Linux动态库的显式调用库，包括dlopen()，dlclose()等。
l fcntl.h ：fcntl.h定义了很多宏和open,fcntl函数原型，包括close open等关闭文件的系列操作。本文中用到了其打开文件的宏定义，即打开文件的控制模式。
int open(const char*pathname, int oflag, ... /* mode_t mode */);
返回值：成功则返回文件描述符，否则返回 -1
对于open函数来说，第三个参数（...）仅当创建新文件时（即使用了O_CREAT 时）才使用，用于指定文件的访问权限位（access permission bits）。pathname是待打开/创建文件的路径名（如 C:/cpp/a.cpp）；oflag用于指定文件的打开/创建模式，这个参数可由以下常量（定义于 fcntl.h）通过逻辑或构成。
O_RDONLY 只读模式
O_WRONLY 只写模式
O_RDWR 读写模式
打开/创建文件时，至少得使用上述三个常量中的一个。以下常量是选用的：
O_APPEND 每次写操作都写入文件的末尾
O_CREAT 如果指定文件不存在，则创建这个文件
O_EXCL 如果要创建的文件已存在，则返回 -1，并且修改errno 的值
O_TRUNC 如果文件存在，并且以只写/读写方式打开，则清空文件全部内容(即将其长度截短为0)
O_NOCTTY 如果路径名指向终端设备，不要把这个设备用作控制终端。
O_NONBLOCK 如果路径名指向FIFO/块文件/字符文件，则把文件的打开和后继 I/O设置为非阻塞模式（nonblockingmode）
以下三个常量同样是选用的，它们用于同步输入输出
O_DSYNC 等待物理 I/O 结束后再write。在不影响读取新写入的数据的前提下，不等待文件属性更新。
O_RSYNC read 等待所有写入同一区域的写操作完成后再进行
O_SYNC 等待物理 I/O 结束后再write，包括更新文件属性的 I/O open 返回的文件描述符一定是最小的未被使用的描述符。
2，根据目标使用的函数及其参数形式定义目标指针。
此处暂时使用两个函数，一个CAN_Init，一个LINUX_CAN_Open函数。
下面为libpcan.h头文件中的定义。
根据上面定义我们声明两个函数指针的类型，方便后文使用
然后用声明的类型定义映射函数名
3，定义文件访问访问的指针，实现文件加载。
其中用到的dlopen的参数解释如下：
void *dlopen(const char *filename, intflag);
其中flag有：RTLD_LAZYRTLD_NOW RTLD_GLOBAL，其含义分别为：
RTLD_LAZY:在dlopen返回前，对于动态库中存在的未定义的变量(如外部变量extern，也可以是函数)不执行解析，就是不解析这个变量的地址。
RTLD_NOW：与上面不同，他需要在dlopen返回前，解析出每个未定义变量的地址，如果解析不出来，在dlopen会返回NULL，错误为:undefined symbol: xxxx.......
RTLD_GLOBAL:它的含义是它的含义是使得库中的解析的定义变量在随后的随后其它的链接库中变得可以使用。
4，实现动态库函数到目标函数的映射。
5，定义访问硬件的HANDLE（指针），实现自定义函数对于can总线的访问，访问完成要关闭对象。
此前文中已有定义。
此处实现如下：
最终映射使用的函数如下：
funCAN_Init =(funCAN_Init_TYPE) dlsym(libm_handle,"CAN_Init");
funLINUX_CAN_Open =(funLINUX_CAN_Open_TYPE) dlsym(libm_handle,"LINUX_CAN_Open");
funCAN_Close =(funCAN_Close_TYPE) dlsym(libm_handle,"CAN_Close");
funCAN_VersionInfo =(funCAN_VersionInfo_TYPE) dlsym(libm_handle,"CAN_VersionInfo");
funLINUX_CAN_Read =(funLINUX_CAN_Read_TYPE) dlsym(libm_handle,"LINUX_CAN_Read");
funCAN_Status =(funCAN_Status_TYPE) dlsym(libm_handle,"CAN_Status");
funnGetLastError =(funnGetLastError_TYPE) dlsym(libm_handle,"nGetLastError");
funCAN_Write =(funCAN_Write_TYPE) dlsym(libm_handle,"CAN_Write");
3.2.2 机械手位置控制实现
根据我们对于机械手复位的检测，发现在复位过程中上位机软件想机械手发送了一组数据，通过对发送过程的简单模仿，可使Barrett机械手复位。
15. void barett_hand_init()
16. {
17. TPCANMsg test_data;
18. //query the bhand state.
19. for(chari=0;i<=3;i++)
20. {
21. test_data.ID=0xcb+i;
22. test_data.LEN=1;
23. test_data.MSGTYPE=MSGTYPE_STANDARD;
24. test_data.DATA[0]=0x05;
25. funCAN_Write(pcan_handle,&test_data);
26. }
27.
28. for(chari=0;i<=3;i++)
29. {
30. test_data.ID=0xcb+i;
31. test_data.LEN=6;
32. test_data.MSGTYPE=MSGTYPE_STANDARD;
33. test_data.DATA[0]=0xb2;
34. for(charj=1;j<6;j++)test_data.DATA[j]=0;
35. funCAN_Write(pcan_handle,&test_data);
36. }
37.
38. for(chari=0;i<=3;i++)
39. {
40. test_data.ID=0xcb+i;
41. test_data.LEN=6;
42. test_data.MSGTYPE=MSGTYPE_STANDARD;
43. for(charj=0;j<6;j++)test_data.DATA[j]=0;
44. test_data.DATA[0]=0xce;
45. test_data.DATA[2]=0xfa;
46. funCAN_Write(pcan_handle,&test_data);
47. }
48.
49. //query the bhand state.
50. for(chari=0;i<=3;i++)
51. {
52. test_data.ID=0xcb+i;
53. test_data.LEN=1;
54. test_data.MSGTYPE=MSGTYPE_STANDARD;
55. test_data.DATA[0]=0x00;
56. funCAN_Write(pcan_handle,&test_data);
57. }
58. //query the bhand state.
59. for(chari=0;i<=3;i++)
60. {
61. test_data.ID=0xcb+i;
62. test_data.LEN=1;
63. test_data.MSGTYPE=MSGTYPE_STANDARD;
64. test_data.DATA[0]=0x5a;
65. funCAN_Write(pcan_handle,&test_data);
66. }
67. //query the bhand state.
68. for(chari=0;i<=3;i++)
69. {
70. test_data.ID=0xcb+i;
71. test_data.LEN=1;
72. test_data.MSGTYPE=MSGTYPE_STANDARD;
73. test_data.DATA[0]=0x08;
74. funCAN_Write(pcan_handle,&test_data);
75. }
76. //init the bhand.
77. for(chari=0;i<=3;i++)
78. {
79. test_data.ID=0xcb+i;
80. test_data.LEN=4;
81. test_data.MSGTYPE=MSGTYPE_STANDARD;
82. test_data.DATA[0]=0x9d;
83. test_data.DATA[1]=0x00;
84. test_data.DATA[2]=0x0d;
85. test_data.DATA[3]=0x00;
86. funCAN_Write(pcan_handle,&test_data);
87. }
88. sleep(5);
89. ROS_INFO("BHandInit successfully!");
90.
91. }
解析出来对于机械手位置控制的实现，其中参数范围如下式所示，此处还没有转换为角度，后期在sia_bhand_controller中已转换为对应的0-2.5的弧度值。
11. void bhand_position_control(long f1_pos,long f2_pos,longf3_pos,long sp_pos)
12. {
13. TPCANMsg CANMsg;
14.
15.
16. CANMsg.LEN = 6;
17. CANMsg.MSGTYPE =MSGTYPE_STANDARD;
18.
19. CANMsg.DATA[0] = 0xba;
20. CANMsg.DATA[1] = 0;
21. CANMsg.DATA[2] = 0;
22. CANMsg.DATA[3] = 0;
23. CANMsg.DATA[4] = 0;
24. CANMsg.DATA[5] = 0;
25.
26. CANMsg.DATA[2] = f1_pos & 0xff;
27. CANMsg.DATA[3] =(f1_pos>>8) & 0xff;
28. CANMsg.DATA[4] =(f1_pos>>16) & 0xff;
29.
30. CANMsg.ID = 0xcb;
31. funCAN_Write(pcan_handle,&CANMsg);
32. usleep(1000);
33. CANMsg.DATA[2] = f2_pos& 0xff;
34. CANMsg.DATA[3] =(f2_pos>>8) & 0xff;
35. CANMsg.DATA[4] =(f2_pos>>16) & 0xff;
36.
37. CANMsg.ID = 0xcc;
38. funCAN_Write(pcan_handle,&CANMsg);
39. usleep(1000);
40.
41. CANMsg.DATA[2] = f3_pos& 0xff;
42. CANMsg.DATA[3] =(f3_pos>>8) & 0xff;
43. CANMsg.DATA[4] =(f3_pos>>16) & 0xff;
44.
45. CANMsg.ID = 0xcd;
46. funCAN_Write(pcan_handle,&CANMsg);
47. usleep(1000);
48.
49. CANMsg.DATA[2] = sp_pos& 0xff;
50. CANMsg.DATA[3] =(sp_pos>>8) & 0xff;
51. CANMsg.DATA[4] =(sp_pos>>16) & 0xff;
52.
53. CANMsg.ID = 0xce;
54. funCAN_Write(pcan_handle,&CANMsg);
55. usleep(1000);
56. }
由于本例中我们选择对于pcan的处理是非阻塞的模式，因此必须有一个函数处理接收到的无意义数据或者请求。其中的if(m.Msg.ID == 0x50a || m.Msg.ID == 0x50b)判断则是标示该帧头开始数据为六维力传感器的数据。
57. int read_loop(HANDLE h,ros::Publisher pub, bool display_on,boolpublish_on)
58. {
59. TPCANRdMsg m;
60. __u32 status;
61.
62. if (funLINUX_CAN_Read(h,&m)) {
63. //perror("receivetest:LINUX_CAN_Read()");
64. return errno;
65. }
66. if(m.Msg.ID == 0x50a ||m.Msg.ID == 0x50b)
67. {
68. if (display_on)
69. print_message_ex(&m);
70. if (publish_on)
71. publish_forcedata(&m,pub,force_display_on);
72.
73. // check if a CANstatus is pending
74. if (m.Msg.MSGTYPE& MSGTYPE_STATUS) {
75. status =funCAN_Status(h);
76. if ((int)status< 0) {
77. errno =funnGetLastError();
78. perror("receivetest:CAN_Status()");
79. returnerrno;
80. }
81.
82. printf("receivetest: pendingCAN status 0x%04x read.\n",
83. (__u16)status);
84. }
85. }
86. return 0;
87. }
3.2.3 Barrett Hand腕部六维力传感器操作
从MonitorForceTorque.cpp中我们可以查到六维力传感器的初始化步骤：由两个函数完成，wavePuck()和setPropertySlow().完成设备唤醒和启动校准（非初始受力置零）。
88. //wake puck
89. wavePuck(0,8);
90. //tare
91. setPropertySlow(0,FTSENSORID,FTDATAPROPERTY,0,0);
函数的原型如下：
11.int wavePuck(int bus,int id)
12.{
13.
14. TPCANMsg msgOut;
15. DWORD err;
16.
17. //Generate the outbound message
18. msgOut.ID = id;
19. msgOut.MSGTYPE = MSGTYPE_STANDARD;
20. msgOut.LEN = 4;
21. msgOut.DATA[0] = 0x85; // Set Status
22. msgOut.DATA[1] = 0x00;
23. msgOut.DATA[2] = 0x02; // Status = Ready
24. msgOut.DATA[3] = 0x00;
25.
26. //Send the message
27. //err = CAN_Write( &msgOut );
28. err=funCAN_Write(pcan_handle,&msgOut);
29. sleep(1);
30.
31. return(err);
32.}
33.
34.int compile(
35. intproperty /** The property beingcompiled (use the enumerations in btcan.h) */,
36. longlongVal /** The value to set theproperty to */,
37. unsigned char *data /** A pointer to acharacter buffer in which to build the data payload */,
38. int*dataLen /** A pointer to thetotal length of the data payload for this packet */,
39. intisForSafety /** A flag indicatingwhether this packet is destined for the safety circuit or not */)
40.{
41. inti;
42.
43. //Check the property
44. //if(property > PROP_END)
45. //{
46. // syslog(LOG_ERR,"compile(): Invalid property = %d", property);
47. // return(1);
48. //}
49.
50. /*Insert the property */
51. data[0]= property;
52. data[1] = 0; /* To align the values for thetater's DSP */
53.
54. /*Append the value */
55. for(i = 2; i < 6; i++)
56. {
57. data[i] = (char)(longVal &0x000000FF);
58. longVal >>= 8;
59. }
60.
61. /*Record the proper data length */
62. *dataLen = 6; //(dataType[property] &0x0007) + 2;
63.
64. return (0);
65.}
66.
67.int setPropertySlow(int bus, int id, intproperty, int verify, long value)
68.{
69. TPCANMsg msgOut, msgIn;
70. DWORD err;
71. intdataHeader, i;
72. longresponse;
73. //unsigned char data[8];
74. int len;
75.
76. //syslog(LOG_ERR, "About to compilesetProperty, property = %d", property);
77. //Compile 'set' packet
78. err= compile(property, value, msgOut.DATA, &len, 0);
79.
80. //Generate the outbound message
81. msgOut.ID = id;
82. msgOut.MSGTYPE = MSGTYPE_STANDARD;
83. msgOut.LEN = len;
84.
85.
86.
87. //syslog(LOG_ERR, "After compilationdata[0] = %d", data[0]);
88. msgOut.DATA[0] |= 0x80; // Set the 'Set'bit
89.
90. //Send the message
91. //CAN_Write( &msgOut );
92. funCAN_Write(pcan_handle,&msgOut);
93.
94. //Sleep(250);
95.
96. //BUG: This will not verify properties from groups of pucks
97. return(0);
98.}
99.
100.int getPropertyFT(int bus,HANDLE h)
101.
102.{
103.
104.
105. TPCANMsg msgOut, msgInOne, msgInTwo;
106. DWORD err;
107.
108.
109. //Generate the outbound message
110. msgOut.ID = FTSENSORID;
111. msgOut.MSGTYPE = MSGTYPE_STANDARD;
112. msgOut.LEN = 1;
113. msgOut.DATA[0] = FTDATAPROPERTY; // Get theft data
114.
115.}
线程循环检测有效数据即read_loop()所执行的功能，检测到帧头后要进行如下的数据解析，规则是每两个字节表示一个浮点数，以x轴力为例表示一个完整的处理如下：
x_force=mr->Msg.DATA[1];
x_force<<=8;
x_force|=mr->Msg.DATA[0];
force_x_float=x_force/256.0;
得到受力或力矩的数值后便需要根据力和力矩的数值通知机械臂或主控制系统进行进一步的处理，该部分工作由bhand_axis_force_warning()函数实现。完整代码参看src下对应cpp文件或者个人博客。
116.void publish_forcedata(TPCANRdMsg*mr,ros::Publisher axis_force_pub,bool display_on)
117.{
118. short int x_force,y_force,z_force;
119. short int torque_x,torque_y,torque_z;
120.
121.
122. if(0x50a==mr->Msg.ID )
123. {
124. x_force=mr->Msg.DATA[1];
125. x_force<<=8;
126. x_force|=mr->Msg.DATA[0];
127.
128. y_force=mr->Msg.DATA[3];
129. y_force<<=8;
130. y_force|=mr->Msg.DATA[2];
131.
132. z_force=mr->Msg.DATA[5];
133. z_force<<=8;
134. z_force|=mr->Msg.DATA[4];
135. force_x_float=x_force/256.0;
136. force_y_float=y_force/256.0;
137. force_z_float=z_force/256.0;
138. if(display_on) ROS_INFO(" Forcex:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
139. }
140.
141. if(0x50b==mr->Msg.ID )
142. {
143. torque_x=mr->Msg.DATA[1];
144. torque_x<<=8;
145. torque_x|=mr->Msg.DATA[0];
146.
147. torque_y=mr->Msg.DATA[3];
148. torque_y<<=8;
149. torque_y|=mr->Msg.DATA[2];
150.
151. torque_z=mr->Msg.DATA[5];
152. torque_z<<=8;
153. torque_z|=mr->Msg.DATA[4];
154. torque_x_float=torque_x/256.0;
155. torque_y_float=torque_y/256.0;
156. torque_z_float=torque_z/256.0;
157. if(display_on) ROS_INFO("Torquex:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
158. }
159.
160.}
3.3 launch文件及节点数配置
实际参数、节点的配置均由launch文件实现，launch文件可以接收int，string，double类型的数据，如下为一个示例。
161.<launch>
162. <nodepkg="beginner_tutorials" type="bhand_axis_force_limit"name="bhand_axis_force" output="screen">
163. <paramname="dev_name" type="string"value="/dev/pcanusb0" />
164. <paramname="bhand_open_pos" type="int" value="6" />
165. <paramname="bhand_close_pos" type="int" value="14"/>
166. <paramname="force_display_on" type="bool" value="false"/>
167. <paramname="force_threshold" type="double" value="18.0"/>
168. <paramname="torque_threshold" type="double"value="20.0" />
169. <paramname="force_danger" type="double" value="25.0"/>
170. <paramname="torque_danger" type="double" value="40.0"/>
171. <paramname="left_torque_x_threshold" type="double" value="-2.0" />
172. <paramname="right_torque_x_threshold" type="double"value="2.0" />
173. </node>
174.</launch>
节点代码中的接受处理流程与launch文件一一对应，如下：
175. string dev_name;
176. ros::param::get("~dev_name",dev_name);
177. ROS_INFO("PCANDevice:%s",dev_name.c_str());
178.
179. intbhand_open_pos=5;
180. ros::param::get("~bhand_open_pos",bhand_open_pos);
181. ROS_INFO("Bhand open position:%d",bhand_open_pos);
182.
183. intbhand_close_pos=13;
184. ros::param::get("~bhand_close_pos",bhand_close_pos);
185. ROS_INFO("Bhand close position:%d",bhand_close_pos);
186.
187. ros::param::get("~force_display_on",force_display_on);
188. ROS_INFO("ForceDatadisplay:%s",force_display_on==false?"false":"true");
189.
190. ros::param::get("~force_threshold",force_threshold);
191. ROS_INFO("Force threshold :%f",force_threshold);
192. ros::param::get("~torque_threshold",torque_threshold);
193. ROS_INFO("Torque threshold :%f",torque_threshold);
194.
195. ros::param::get("~force_danger",force_danger);
196. ROS_WARN(" Force dangerous threshold:%f",force_danger);
197. ros::param::get("~torque_danger",torque_danger);
198. ROS_WARN("Torque dangerous threshold:%f",torque_danger);
199.
200. ros::param::get("~left_torque_x_threshold",left_torque_x_threshold);
201. ROS_WARN(" left torque x threshold:%f",left_torque_x_threshold);
202. ros::param::get("~right_torque_x_threshold",right_torque_x_threshold);
203. ROS_WARN("right torque x threshold:%f",right_torque_x_threshold);
204.
下附本节完整代码bhand_axis_force_limit.cpp
#include <ros/ros.h>
#include <stdio.h>
#include <dlfcn.h>
#include <libpcan.h>
#include <fcntl.h>
#include <errno.h>
#include <signal.h>
#include "id_data_msgs/ID_Data.h"//using for notie event
#include "ros/callback_queue.h"
#include "string"
using namespace std;
//define mapping function according to target function in libpcan.h
typedef DWORD (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
typedef HANDLE (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
typedef DWORD (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
typedef DWORD (*funCAN_Close_TYPE)(HANDLE hHandle);
typedef DWORD (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
typedef DWORD (*funCAN_Status_TYPE)(HANDLE hHandle);
typedef int (*funnGetLastError_TYPE)(void);
typedef DWORD (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
//the target device name
#define DEFAULT_NODE "/dev/pcan0"
#define FTSENSORID (8)
#define FTDATAPROPERTY (54)
//GLOBALS
void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
HANDLE pcan_handle =NULL;//void *pcan_handle
id_data_msgs::ID_Data axis_force_msg;
bool force_display_on=true;
float force_threshold=4.0;
float torque_threshold=10.0;
double force_danger=15.0;
double torque_danger=30.0;
double left_torque_x_threshold=-1.0;
double right_torque_x_threshold=1.0;
//globals
bool close_hand_flag=false;
bool open_hand_flag=false;
float torque_x_float,torque_y_float,torque_z_float;
float force_x_float,force_y_float,force_z_float;
void notice_data_clear(id_data_msgs::ID_Data *test);
int wavePuck(int bus,int id);
int compile(
int property /** The property being compiled (use the enumerations in btcan.h) */,
long longVal /** The value to set the property to */,
unsigned char *data /** A pointer to a character buffer in which to build the data payload */,
int *dataLen /** A pointer to the total length of the data payload for this packet */,
int isForSafety /** A flag indicating whether this packet is destined for the safety circuit or not */);
int setPropertySlow(int bus, int id, int property, int verify, long value);
//define function pointer,there is a one-to-one mapping between target function and your defined function
funCAN_Init_TYPE funCAN_Init;
funLINUX_CAN_Open_TYPE funLINUX_CAN_Open;
funCAN_VersionInfo_TYPE funCAN_VersionInfo;
funCAN_Close_TYPE funCAN_Close;
funLINUX_CAN_Read_TYPE funLINUX_CAN_Read;
funCAN_Status_TYPE funCAN_Status;
funnGetLastError_TYPE funnGetLastError;
funCAN_Write_TYPE funCAN_Write;
class notice_pub_sub
{
public:
boost::function<void (const id_data_msgs::ID_Data::ConstPtr&)> notice_pub_sub_msgCallbackFun;
notice_pub_sub();
void notice_pub_sub_listener();
void notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data);
void notice_display(id_data_msgs::ID_Data notice_msg,bool set);
void notice_sub_spinner(char set);
private:
ros::NodeHandle notice_handle;
ros::Subscriber notice_subscriber;
ros::Publisher notice_publisher;
ros::SubscribeOptions notice_ops;
ros::AsyncSpinner *notice_spinner;
ros::CallbackQueue notice_callbackqueue;
void notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg);
};
notice_pub_sub::notice_pub_sub()
{
notice_pub_sub_msgCallbackFun=boost::bind(¬ice_pub_sub::notice_msgCallback,this,_1);
notice_ops=ros::SubscribeOptions::create<id_data_msgs::ID_Data>(
"/notice",
1,
notice_pub_sub_msgCallbackFun,
ros::VoidPtr(),
¬ice_callbackqueue
);
notice_subscriber=notice_handle.subscribe(notice_ops);
notice_spinner=new ros::AsyncSpinner(1,¬ice_callbackqueue);
notice_publisher=notice_handle.advertise<id_data_msgs::ID_Data>("/notice",1);
}
void notice_pub_sub::notice_pub_sub_listener()
{
}
void notice_pub_sub::notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data)
{
notice_publisher.publish(id_data);
}
void notice_pub_sub::notice_display(id_data_msgs::ID_Data notice_msg,bool set)
{
if(set)
{
printf("REC Notice message,ID: %d,Data: ",notice_msg.id);
for(char i=0;i<8;i++)
{
printf("%d ",notice_msg.data[i]);
if(i==7) printf("\n");
}
}
}
void notice_pub_sub::notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg)
{
id_data_msgs::ID_Data notice_message;
notice_message.id=0;
for(char i=0;i<8;i++)notice_message.data[i]=0;
notice_message.id=notice_msg->id;
for(char i=0;i<8;i++)notice_message.data[i]=notice_msg->data[i];
notice_pub_sub::notice_display(notice_message,true);
if(notice_message.id==1 && notice_message.data[0]==1)//close flag
{
close_hand_flag=true;
notice_data_clear(¬ice_message);
notice_message.id=1;
notice_message.data[0]=14;
notice_publisher.publish(notice_message);
}
if(notice_message.id==1 && notice_message.data[0]==0)//open flag
{
open_hand_flag=true;
notice_data_clear(¬ice_message);
notice_message.id=1;
notice_message.data[0]=14;
notice_publisher.publish(notice_message);
}
}
void notice_pub_sub::notice_sub_spinner(char set)
{
if(set==1)
notice_spinner->start();
if(set==0)
notice_spinner->stop();
}
//function declaration
int read_loop(HANDLE h,ros::Publisher pub,bool display_on,bool publish_on);
void print_message(TPCANMsg *m);
void print_message_ex(TPCANRdMsg *mr);
void do_exit(void *file,HANDLE h,int error);
void signal_handler(int signal);
void init();
void publish_forcedata(TPCANRdMsg *mr, ros::Publisher axis_force_pub, bool display_on);
void barett_hand_init();
void bhand_position_control(long f1_pos,long f2_pos,long f3_pos,long sp_pos);
void bhand_axis_force_warning(notice_pub_sub *notice_test,float force_x,float force_y,float force_z,float torque_x,float torque_y,float torque_z);
//
int sys_count=0;
int main(int argc,char **argv)
{
ros::init(argc,argv,"bhand_axis_force");
ros::NodeHandle handle_test;
init();
string dev_name;
ros::param::get("~dev_name",dev_name);
ROS_INFO("PCAN Device:%s",dev_name.c_str());
int bhand_open_pos=5;
ros::param::get("~bhand_open_pos",bhand_open_pos);
ROS_INFO("Bhand open position :%d",bhand_open_pos);
int bhand_close_pos=13;
ros::param::get("~bhand_close_pos",bhand_close_pos);
ROS_INFO("Bhand close position :%d",bhand_close_pos);
ros::param::get("~force_display_on",force_display_on);
ROS_INFO("ForceData display:%s",force_display_on==false?"false":"true");
ros::param::get("~force_threshold",force_threshold);
ROS_INFO("Force threshold :%f",force_threshold);
ros::param::get("~torque_threshold",torque_threshold);
ROS_INFO("Torque threshold :%f",torque_threshold);
ros::param::get("~force_danger",force_danger);
ROS_WARN(" Force dangerous threshold :%f",force_danger);
ros::param::get("~torque_danger",torque_danger);
ROS_WARN("Torque dangerous threshold :%f",torque_danger);
ros::param::get("~left_torque_x_threshold",left_torque_x_threshold);
ROS_WARN(" left torque x threshold :%f",left_torque_x_threshold);
ros::param::get("~right_torque_x_threshold",right_torque_x_threshold);
ROS_WARN("right torque x threshold :%f",right_torque_x_threshold);
//load libpcan.so using dlopen function,return handle for further use
libm_handle = dlopen("libpcan.so", RTLD_LAZY );
if (!libm_handle){
printf("Open Error:%s.\n",dlerror());//if file can't be loaded,return null,get reason using dlerror function
return 0;
}
char *errorInfo;//error information pointer
//one-to-one mapping using dlsym function,if return null,mapping would be failed
funCAN_Init =(funCAN_Init_TYPE) dlsym(libm_handle,"CAN_Init");
funLINUX_CAN_Open =(funLINUX_CAN_Open_TYPE) dlsym(libm_handle,"LINUX_CAN_Open");
funCAN_Close =(funCAN_Close_TYPE) dlsym(libm_handle,"CAN_Close");
funCAN_VersionInfo =(funCAN_VersionInfo_TYPE) dlsym(libm_handle,"CAN_VersionInfo");
funLINUX_CAN_Read =(funLINUX_CAN_Read_TYPE) dlsym(libm_handle,"LINUX_CAN_Read");
funCAN_Status =(funCAN_Status_TYPE) dlsym(libm_handle,"CAN_Status");
funnGetLastError =(funnGetLastError_TYPE) dlsym(libm_handle,"nGetLastError");
funCAN_Write =(funCAN_Write_TYPE) dlsym(libm_handle,"CAN_Write");
errorInfo = dlerror();//get error using dlerror function,and clear the error list in memory
if (errorInfo != NULL){
printf("Dlsym Error:%s.\n",errorInfo);
return 0;
}
char txt[VERSIONSTRING_LEN]; //store information of can version
unsigned short wBTR0BTR1 = CAN_BAUD_1M; //set the communicate baud rate of can bus
int nExtended = CAN_INIT_TYPE_ST; //set can message int standard model
const char *szDevNode = DEFAULT_NODE; //define const pointer point to device name
if(dev_name.c_str() !="")
pcan_handle = funLINUX_CAN_Open(dev_name.c_str(), O_RDWR | O_NONBLOCK);
else
{
pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR | O_NONBLOCK);//use mapping function
dev_name=DEFAULT_NODE;
}
//pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR | O_NONBLOCK );//use mapping function
//judge whether the call is success.if pcan_handle=null,the call would be failed
if(pcan_handle){
printf("CAN Bus test: %s have been opened\n", szDevNode);
errno = funCAN_VersionInfo(pcan_handle, txt);
if (!errno)
printf("CAN Bus test: driver version = %s\n", txt);
else {
perror("CAN Bus test: CAN_VersionInfo()");
}
if (wBTR0BTR1) {
errno = funCAN_Init(pcan_handle, wBTR0BTR1, nExtended);
if (errno) {
perror("CAN Bus test: CAN_Init()");
}
else
printf("Device Info: %s; CAN_BAUD_1M; CAN_INIT_TYPE_ST\n", szDevNode);
}
}
else
printf("CAN Bus test: can't open %s\n", szDevNode);
//test of can send data
barett_hand_init();
bhand_position_control(bhand_open_pos*10000,bhand_open_pos*10000,bhand_open_pos*10000,0);//position: (0--2.5)*10 0000
sleep(5);//waiting for bhand ready...
ros::NodeHandle axis_force_handle;
ros::Publisher axis_force_pub=axis_force_handle.advertise<id_data_msgs::ID_Data>("/notice",1);//advertise axis force to the topic named "/notice"
ros::Rate loop_rate(100);
//data receive test
//wake puck
wavePuck(0,8);
//tare
setPropertySlow(0,FTSENSORID,FTDATAPROPERTY,0,0);
TPCANMsg test_data;
//query the axis sensor.
test_data.ID=0x008;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x36;
notice_pub_sub notice_test;
id_data_msgs::ID_Data notice_data_pub;
while(ros::ok())
{
sys_count++;
if(sys_count%20==0)funCAN_Write(pcan_handle,&test_data);//query the axis sensor.
read_loop(pcan_handle,axis_force_pub,false,true);
bhand_axis_force_warning(¬ice_test,force_x_float,force_y_float,force_z_float,torque_x_float,torque_y_float,torque_z_float);
//HANDLE h,ros::Publisher pub, bool display_on,bool publish_on
if(close_hand_flag)
{
open_hand_flag=false;
bhand_position_control(bhand_close_pos*10000,bhand_close_pos*10000,bhand_close_pos*10000,0);//position: (0-2.5)*10 0000
sleep(2);
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=1;
notice_data_pub.data[0]=2;
notice_test.notice_pub_sub_pulisher(notice_data_pub);
close_hand_flag=false;
}
if(open_hand_flag)
{
close_hand_flag=false;
bhand_position_control((bhand_open_pos-2)*10000,(bhand_open_pos-2)*10000,(bhand_open_pos-2)*10000,0);//position: (0-2.5)*10 0000
sleep(2);
bhand_position_control(bhand_open_pos*10000,bhand_open_pos*10000,bhand_open_pos*10000,0);//position: (0-2.5)*10 0000
sleep(1);
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=1;
notice_data_pub.data[0]=2;
notice_test.notice_pub_sub_pulisher(notice_data_pub);
open_hand_flag=false;
ROS_INFO("Open hand successfully!\n");
}
notice_test.notice_sub_spinner(1);
loop_rate.sleep();
}
return 0;
}
int wavePuck(int bus,int id)
{
TPCANMsg msgOut;
DWORD err;
// Generate the outbound message
msgOut.ID = id;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = 4;
msgOut.DATA[0] = 0x85; // Set Status
msgOut.DATA[1] = 0x00;
msgOut.DATA[2] = 0x02; // Status = Ready
msgOut.DATA[3] = 0x00;
// Send the message
//err = CAN_Write( &msgOut );
err=funCAN_Write(pcan_handle,&msgOut);
sleep(1);
return(err);
}
int compile(
int property /** The property being compiled (use the enumerations in btcan.h) */,
long longVal /** The value to set the property to */,
unsigned char *data /** A pointer to a character buffer in which to build the data payload */,
int *dataLen /** A pointer to the total length of the data payload for this packet */,
int isForSafety /** A flag indicating whether this packet is destined for the safety circuit or not */)
{
int i;
// Check the property
//if(property > PROP_END)
//{
// syslog(LOG_ERR,"compile(): Invalid property = %d", property);
// return(1);
//}
/* Insert the property */
data[0] = property;
data[1] = 0; /* To align the values for the tater's DSP */
/* Append the value */
for (i = 2; i < 6; i++)
{
data[i] = (char)(longVal & 0x000000FF);
longVal >>= 8;
}
/* Record the proper data length */
*dataLen = 6; //(dataType[property] & 0x0007) + 2;
return (0);
}
int setPropertySlow(int bus, int id, int property, int verify, long value)
{
TPCANMsg msgOut, msgIn;
DWORD err;
int dataHeader, i;
long response;
//unsigned char data[8];
int len;
//syslog(LOG_ERR, "About to compile setProperty, property = %d", property);
// Compile 'set' packet
err = compile(property, value, msgOut.DATA, &len, 0);
// Generate the outbound message
msgOut.ID = id;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = len;
//syslog(LOG_ERR, "After compilation data[0] = %d", data[0]);
msgOut.DATA[0] |= 0x80; // Set the 'Set' bit
// Send the message
//CAN_Write( &msgOut );
funCAN_Write(pcan_handle,&msgOut);
//Sleep(250);
// BUG: This will not verify properties from groups of pucks
return(0);
}
int getPropertyFT(int bus,HANDLE h)
{
TPCANMsg msgOut, msgInOne, msgInTwo;
DWORD err;
// Generate the outbound message
msgOut.ID = FTSENSORID;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = 1;
msgOut.DATA[0] = FTDATAPROPERTY; // Get the ft data
}
void barett_hand_init()
{
TPCANMsg test_data;
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x05;
funCAN_Write(pcan_handle,&test_data);
}
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=6;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0xb2;
for(char j=1;j<6;j++)test_data.DATA[j]=0;
funCAN_Write(pcan_handle,&test_data);
}
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=6;
test_data.MSGTYPE=MSGTYPE_STANDARD;
for(char j=0;j<6;j++)test_data.DATA[j]=0;
test_data.DATA[0]=0xce;
test_data.DATA[2]=0xfa;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x00;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x5a;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x08;
funCAN_Write(pcan_handle,&test_data);
}
//init the bhand.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=4;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x9d;
test_data.DATA[1]=0x00;
test_data.DATA[2]=0x0d;
test_data.DATA[3]=0x00;
funCAN_Write(pcan_handle,&test_data);
}
sleep(5);
ROS_INFO("BHand Init successfully!");
}
void bhand_position_control(long f1_pos,long f2_pos,long f3_pos,long sp_pos)
{
TPCANMsg CANMsg;
CANMsg.LEN = 6;
CANMsg.MSGTYPE =MSGTYPE_STANDARD;
CANMsg.DATA[0] = 0xba;
CANMsg.DATA[1] = 0;
CANMsg.DATA[2] = 0;
CANMsg.DATA[3] = 0;
CANMsg.DATA[4] = 0;
CANMsg.DATA[5] = 0;
CANMsg.DATA[2] = f1_pos & 0xff;
CANMsg.DATA[3] = (f1_pos>>8) & 0xff;
CANMsg.DATA[4] = (f1_pos>>16) & 0xff;
CANMsg.ID = 0xcb;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = f2_pos & 0xff;
CANMsg.DATA[3] = (f2_pos>>8) & 0xff;
CANMsg.DATA[4] = (f2_pos>>16) & 0xff;
CANMsg.ID = 0xcc;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = f3_pos & 0xff;
CANMsg.DATA[3] = (f3_pos>>8) & 0xff;
CANMsg.DATA[4] = (f3_pos>>16) & 0xff;
CANMsg.ID = 0xcd;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = sp_pos & 0xff;
CANMsg.DATA[3] = (sp_pos>>8) & 0xff;
CANMsg.DATA[4] = (sp_pos>>16) & 0xff;
CANMsg.ID = 0xce;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
}
int read_loop(HANDLE h,ros::Publisher pub, bool display_on,bool publish_on)
{
TPCANRdMsg m;
__u32 status;
if (funLINUX_CAN_Read(h, &m)) {
//perror("receivetest: LINUX_CAN_Read()");
return errno;
}
if(m.Msg.ID == 0x50a || m.Msg.ID == 0x50b)
{
if (display_on)
print_message_ex(&m);
if (publish_on)
publish_forcedata(&m,pub,force_display_on);
// check if a CAN status is pending
if (m.Msg.MSGTYPE & MSGTYPE_STATUS) {
status = funCAN_Status(h);
if ((int)status < 0) {
errno = funnGetLastError();
perror("receivetest: CAN_Status()");
return errno;
}
printf("receivetest: pending CAN status 0x%04x read.\n",
(__u16)status);
}
}
return 0;
}
void publish_forcedata(TPCANRdMsg *mr,ros::Publisher axis_force_pub,bool display_on)
{
short int x_force,y_force,z_force;
short int torque_x,torque_y,torque_z;
if(0x50a==mr->Msg.ID )
{
x_force=mr->Msg.DATA[1];
x_force<<=8;
x_force|=mr->Msg.DATA[0];
y_force=mr->Msg.DATA[3];
y_force<<=8;
y_force|=mr->Msg.DATA[2];
z_force=mr->Msg.DATA[5];
z_force<<=8;
z_force|=mr->Msg.DATA[4];
force_x_float=x_force/256.0;
force_y_float=y_force/256.0;
force_z_float=z_force/256.0;
if(display_on) ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
}
if(0x50b==mr->Msg.ID )
{
torque_x=mr->Msg.DATA[1];
torque_x<<=8;
torque_x|=mr->Msg.DATA[0];
torque_y=mr->Msg.DATA[3];
torque_y<<=8;
torque_y|=mr->Msg.DATA[2];
torque_z=mr->Msg.DATA[5];
torque_z<<=8;
torque_z|=mr->Msg.DATA[4];
torque_x_float=torque_x/256.0;
torque_y_float=torque_y/256.0;
torque_z_float=torque_z/256.0;
if(display_on) ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
}
void bhand_axis_force_warning(notice_pub_sub *notice_test,float force_x,float force_y,float force_z,float torque_x,float torque_y,float torque_z)
{
id_data_msgs::ID_Data notice_data_pub;
if(fabs(force_x)>force_threshold || fabs(force_y)>force_threshold || fabs(force_z)>force_threshold)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=13;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN(" Force threshhold:%8.4f BHAND COLLISION!",force_threshold);
//ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
if(torque_x<=left_torque_x_threshold )
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=12;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("LEFT arm Axis force sensor detect BHAND COLLISION!");
usleep(10000);
}
if(torque_x>=right_torque_x_threshold )
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=11;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("RIGHT Axis force sensor detect BHAND COLLISION!");
usleep(10000);
}
}
if(fabs(torque_x)>torque_threshold || fabs(torque_y)>torque_threshold || fabs(torque_z)>torque_threshold)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=13;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("Torque threshhold:%8.4f BHAND COLLISION!",torque_threshold);
//ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
if(fabs(torque_x)>torque_danger || fabs(torque_y)>torque_danger || fabs(torque_z)>torque_danger || fabs(force_x)>force_danger || fabs(force_y)>force_danger || fabs(force_z)>force_danger)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=14;
notice_test->notice_pub_sub_pulisher(notice_data_pub);
ROS_ERROR("Axis force sensor detect BHAND DANGER!");
ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
}
void print_message(TPCANMsg *m)
{
int i;
//print RTR, 11 or 29, CAN-Id and datalength
printf("receivetest: %c %c 0x%08x %1d ",
((m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm') -
((m->MSGTYPE ) ? 0x20 : 0),
(m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
m->ID,
m->LEN);
//don't print any telegram contents for remote frames
if (!(m->MSGTYPE & MSGTYPE_RTR))
for (i = 0; i < m->LEN; i++)
printf("%02x ", m->DATA[i]);
//printf("%3d ", m->DATA[i]);//decimal format print.
printf("\n");
}
void print_message_ex(TPCANRdMsg *mr)
{
printf("%u.%3u ", mr->dwTime, mr->wUsec);
print_message(&mr->Msg);
}
// exit handler
void do_exit(void *file,HANDLE h,int error)
{
//Must close h handle firstly,then close file using dlclose
if (h) {
funCAN_Close(h);
}
printf("\nCAN Bus test: finished (%d).\n\n", error);
//after call the target function in ELF object,close it using dlclose
dlclose(file);
exit(error);
}
// the signal handler for manual break Ctrl-C
void signal_handler(int signal)
{
do_exit(libm_handle,pcan_handle,0);
}
// what has to be done at program start
void init()
{
/* install signal handlers */
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
}
void notice_data_clear(id_data_msgs::ID_Data *test)
{
test->id=0;
for(int i=0;i<8;i++) test->data[i]=0;
}P.S. 文中或代码中有关/notice话题的部分详见博客内该博文http://blog.csdn.net/hookie1990/article/details/54138234
评论（0）
您还未登录，请登录后发表或查看评论