pcan_transmit.cpp
Go to the documentation of this file.
1 // INCLUDE
2 
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <errno.h>
6 #include <unistd.h> // exit
7 #include <signal.h>
8 #include <string.h>
9 #include <stdlib.h> // strtoul
10 #include <fcntl.h> // O_RDWR
11 #include "common.h"
12 #include <ctype.h>
13 #include "ros/ros.h"
14 #include "std_msgs/String.h"
15 #include <sstream>
16 #include "pcan_transmit.h"
17 
18 
19 
21 {
28  printf("pcan_transmit - a small node, based on transmitest , which is subscribed to a ROS topic and sends CAN messages.\n");
29  printf("usage: pcan_transmit {[-f=devicenode] | {[-b=BTR0BTR1] [-?]}} \n");
30  printf("options: -f - devicenode, default=%s\n", DEFAULT_NODE);
31  printf(" -b - BTR0BTR1 code in hex, e.g.\n");
32  printf(" 0x0014 = 1 MBit/s\n");
33  printf(" 0x001C = 500 kBit/s\n");
34  printf(" 0x011C = 250 kBit/s\n");
35  printf(" 0x031C = 125 kBit/s\n");
36  printf(" 0x432F = 100 kBit/s\n");
37  printf(" 0x472F = 50 kBit/s\n");
38  printf(" 0x532F = 20 kBit/s\n");
39  printf(" 0x672F = 10 kBit/s\n");
40  printf(" 0x7F7F = 5 kBit/s\n");
41  printf(" (default: 1 Mbit).\n");
42  printf(" -? or --help - this help\n");
43  printf("\n");
44 }
45 
46 
47 
48 
50 {
57  //Initialisation
58  h = 0 ;
59  pcan_sub = n.subscribe("pcan_transmitted", 1, &pcan_transmit::transmit, this);
60 }
61 
62 void pcan_transmit::init(int argc, char **argv)
63 {
72  int nExtended = CAN_INIT_TYPE_ST;
73  int nType;
74  __u32 dwPort;
75  __u16 wIrq;
76  char *ptr;
77  __u16 wBTR0BTR1 = 0x0014;
78  // parameter wBTR0BTR1
79  // bitrate codes of BTR0/BTR1 registers
80  //#define CAN_BAUD_1M 0x0014 // 1 MBit/s
81  //#define CAN_BAUD_500K 0x001C // 500 kBit/s
82  //#define CAN_BAUD_250K 0x011C // 250 kBit/s
83  //#define CAN_BAUD_125K 0x031C // 125 kBit/s
84  //#define CAN_BAUD_100K 0x432F // 100 kBit/s
85  //#define CAN_BAUD_50K 0x472F // 50 kBit/s
86  //#define CAN_BAUD_20K 0x532F // 20 kBit/s
87  //#define CAN_BAUD_10K 0x672F // 10 kBit/s
88  //#define CAN_BAUD_5K 0x7F7F // 5 kBit/s
89  const char *szDevNode = DEFAULT_NODE;
90  bool bDevNodeGiven = false;
91  bool bTypeGiven = false;
92  char txt[VERSIONSTRING_LEN];
93 
94  // decode command line arguments
95  for (int i = 1; i < argc; i++)
96  {
97  char c;
98 
99  ptr = argv[i];
100 
101  while (*ptr == '-')
102  ptr++;
103 
104  c = *ptr;
105  ptr++;
106 
107  if (*ptr == '=')
108  ptr++;
109 
110  switch(tolower(c))
111  {
112  case '?':
113  case 'h':
114  hlpMsg();
115  do_exit(errno, h);
116  break;
117  case 'f':
118  szDevNode = ptr;
119  bDevNodeGiven = true;
120  break;
121  case 'b':
122  wBTR0BTR1 = (__u16)strtoul(ptr, NULL, 16);
123  break;
124  default:
125  errno = EINVAL;
126  do_exit(errno, h);;
127  break;
128  }
129  }
130  /* open CAN port */
131  if ((bDevNodeGiven) || (!bDevNodeGiven && !bTypeGiven))
132  {
133  h = LINUX_CAN_Open(szDevNode, O_RDWR);
134  if (!h)
135  {
136  printf("pcan_transmit: can't open %s\n", szDevNode);
137  do_exit(errno, h);;
138  }
139  }
140  else {
141  // please use what is appropriate
142  // HW_DONGLE_SJA
143  // HW_DONGLE_SJA_EPP
144  // HW_ISA_SJA
145  // HW_PCI
146  h = CAN_Open(nType, dwPort, wIrq);
147  if (!h)
148  {
149  printf("pcan_transmit: can't open %s device.\n", getNameOfInterface(nType));
150  do_exit(errno, h);;
151  }
152  }
153  /* clear status */
154  CAN_Status(h);
155  // get version info
156  errno = CAN_VersionInfo(h, txt);
157  if (!errno)
158  printf("pcan_transmit: driver version = %s\n", txt);
159  else {
160  perror("pcan_transmit: CAN_VersionInfo()");
161  do_exit(errno, h);;
162  }
163  // init to a user defined bit rate
164  if (wBTR0BTR1)
165  {
166  errno = CAN_Init(h, wBTR0BTR1, nExtended);
167  if (errno)
168  {
169  perror("pcan_transmit: CAN_Init()");
170  do_exit(errno, h);;
171  }
172  }
173 
174 }
175 TPCANMsg pcan_transmit::StringToTPCANRdMsg(std_msgs::String msg)
176 {
184  TPCANMsg m;
185  std::string can_message = msg.data;
186  std::stringstream ss;
187  std::istringstream iss(can_message);
188  std::vector<std::string> tokens;
189  std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(), std::back_inserter<std::vector<std::string> >(tokens));
190  if(tokens.size()>2)
191  {
192  m.ID = (DWORD) std::strtol(tokens.at(0).data(),NULL,0);
193  m.LEN = (BYTE) std::strtol(tokens.at(1).data(),NULL,0);
194  }
195  if(tokens.size()>3 && tokens.size()<11)
196  {
197  for(int i = 2; i < tokens.size(); i++)
198  {
199  m.DATA[i-2] = (BYTE) std::strtol(tokens.at(i).data(),NULL,0);
200  }
201 
202  }
203  return m;
204 }
205 
206 
207 void pcan_transmit::transmit(const std_msgs::String msg)
208 {
216  TPCANMsg m = StringToTPCANRdMsg(msg);
217 
218  //send the message
219  //CheckTPCANMsg(m) &&
220  if ( CAN_Write(h, &m))
221  {
222  perror("pcan_transmit: CAN_Write()");
223  }
224 }
225 
226 
std::stringstream ss
Definition: pcan_transmit.h:29
TPCANMsg StringToTPCANRdMsg(std_msgs::String msg)
ros::Subscriber pcan_sub
Definition: pcan_transmit.h:27
void do_exit(int error, HANDLE h)
Definition: common.cpp:29
#define DEFAULT_NODE
ros::NodeHandle n
Definition: pcan_transmit.h:26
char const * getNameOfInterface(int nType)
Definition: common.cpp:49
void init(int argc, char **argv)
void transmit(const std_msgs::String can_message)


pcan_topics
Author(s):
autogenerated on Sun Dec 14 2014 19:01:10