Go to the documentation of this file.
27 #define RET_ERROR(msg) \
29 cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl; \
41 if (n >=
'0' && n <=
'9')
45 if (n >=
'A' && n <=
'F')
46 return (n -
'A' + 10);
54 CCANBusReader::CCANBusReader()
89 bool thereIsObservation;
93 if (thereIsObservation)
96 cout <<
"No frame received" << endl;
103 bool& outThereIsObservation,
106 outThereIsObservation =
false;
107 hardwareError =
false;
111 hardwareError =
true;
118 uint8_t out_prio, out_pdu_format, out_pdu_spec, out_src_address,
121 vector<uint8_t> out_data;
122 vector<char> out_raw_frame;
124 out_prio, out_pdu_format, out_pdu_spec, out_src_address,
125 out_data_length, out_pgn, out_data, out_raw_frame))
141 outObservation.
m_pgn = out_pgn;
143 outObservation.
m_data.resize(out_data.size());
144 for (
size_t k = 0; k < out_data.size(); ++k)
145 outObservation.
m_data[k] = out_data[k];
146 outObservation.
m_raw_frame.resize(out_raw_frame.size());
147 for (
size_t k = 0; k < out_raw_frame.size(); ++k)
151 outThereIsObservation =
true;
159 const std::string& iniSection)
186 if (err_msg) *err_msg =
"";
199 throw std::logic_error(
200 "ERROR: No serial port attached with bindIO, neither it "
201 "set with 'setSerialPort'");
206 bool just_open =
false;
224 if (!just_open)
return true;
231 cout <<
"Setting up serial comms in port " <<
m_com_port;
233 cout <<
" ... done" << endl;
239 cout <<
"Setting up CAN BUS Speed at: " <<
m_canbus_speed << endl;
240 for (
int nTry = 0; nTry < 250000 ; nTry++)
242 if (!res)
return false;
243 cout <<
" ... done" << endl;
247 cout <<
"Opening CAN BUS and starting to receive." << endl;
248 for (
int nTry = 0; nTry < 250000 ; nTry++)
250 if (!res)
return false;
251 cout <<
" ... done" << endl;
263 catch (
const std::exception& e)
266 "[CCANBusReader] Error trying to open CANBusReader at port ";
268 if (err_msg) *err_msg = s;
281 unsigned char cmd[2];
323 unsigned char cmd[1];
332 unsigned char cmd[1];
343 unsigned char cmd[1];
351 unsigned char cmd[2];
360 unsigned char cmd[1];
370 uint8_t& out_prio, uint8_t& out_pdu_format, uint8_t& out_pdu_spec,
371 uint8_t& out_src_address, uint8_t& out_data_length, uint16_t& out_pgn,
372 vector<uint8_t>& out_data, vector<char>& out_raw_frame)
374 size_t nRead, nBytesToRead;
375 size_t nFrameBytes = 0;
377 unsigned char buf[40];
380 for (
unsigned char& k : buf) k = 0;
383 while (nFrameBytes < (lengthField = (10U + dlc + 1U)))
386 if (lengthField > 30)
388 cout <<
"#" << int(dlc) <<
" ";
390 for (
unsigned char& k : buf) k = 0;
394 if (nFrameBytes < 10)
400 nBytesToRead = (lengthField)-nFrameBytes;
412 catch (
const std::exception& e)
416 "[waitContinuousSampleFrame] Disconnecting due to comms error: "
421 if (!nRead)
return false;
423 if (nRead < nBytesToRead) std::this_thread::sleep_for(30ms);
427 if (nFrameBytes > 0 || (nFrameBytes == 0 && buf[0] == 0x54 ))
428 nFrameBytes += nRead;
432 for (
unsigned char& k : buf) k = 0;
439 out_raw_frame.resize(nFrameBytes);
440 for (uint8_t k = 0; k < nFrameBytes; ++k)
443 out_raw_frame[k] = buf[k];
446 out_prio = (aux[1] << 2) | (aux[2] >> 2);
447 out_pdu_format = (aux[3] << 4) | aux[4];
448 out_pdu_spec = (aux[5] << 4) | aux[6];
449 out_src_address = (aux[7] << 4) | aux[8];
450 out_data_length = aux[9];
451 out_pgn = uint16_t(out_pdu_format) << 8 | uint16_t(out_pdu_spec);
452 out_data.resize(out_data_length);
453 for (uint8_t k = 0, k2 = 0; k < 2 * out_data_length; k += 2, k2++)
454 out_data[k2] = (aux[10 + k] << 4) | aux[11 + k];
456 if (buf[nFrameBytes - 1] != 0x0D)
459 "[CCANBusReader::waitContinuousSampleFrame] expected 0x0D "
460 "ending flag, 0x%X found instead",
479 cerr << err_str << endl;
480 throw std::logic_error(err_str);
496 int detected_rate = 0;
501 int rates[] = {0, 9600, 38400, 57600, 500000};
510 !detected_rate && i <
sizeof(rates) /
sizeof(rates[0]); i++)
515 std::this_thread::sleep_for(100ms);
520 cout << endl <<
"Closing CAN Channel " << endl;
521 for (
int nTry = 0; nTry < 250000 ; nTry++)
523 cout <<
" ... done" << endl;
526 std::this_thread::sleep_for(100ms);
529 for (
int nTry = 0; nTry < 250000 && !detected_rate; nTry++)
536 detected_rate = rates[i];
539 std::this_thread::sleep_for(20ms);
546 std::this_thread::sleep_for(5000ms);
556 std::this_thread::sleep_for(500ms);
572 uint16_t cmd_len = 1;
592 cout << int(b) << endl;
596 }
while (tictac.
Tac() < timeout_ms * 1e-3);
609 unsigned int nBytes = 0;
613 const double maxTime = timeout * 1e-3;
623 if (nBytes > 0 || (nBytes == 0 && b ==
'V'))
630 if (tictac.
Tac() >= maxTime)
632 cout <<
"Version timeout" << endl;
641 "[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, "
650 for (uint8_t k = 0; k < nBytes; ++k)
660 unsigned int nBytes = 0;
664 const double maxTime = timeout * 1e-3;
666 while (nBytes < 10 || (nBytes < (10U + dlc + 1U )))
671 if (nBytes > 1 || (!nBytes && b == 0x54 ))
686 if (tictac.
Tac() >= maxTime)
return false;
692 "[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final "
693 "flag, received %x\n",
700 for (
unsigned int i=0;i<nBytes;i++)
710 const uint8_t*
cmd,
const uint16_t cmd_len, [[maybe_unused]]
bool wait)
712 uint8_t cmd_full[1024];
713 ASSERT_(
sizeof(cmd_full) > cmd_len);
717 cmd_full[cmd_len] = 0x0D;
719 const size_t toWrite = cmd_len + 1;
723 for (
unsigned int i = 0; i < toWrite; i++) printf(
"%02X ", cmd_full[i]);
733 cout <<
"[CCANBusReader::SendCommandToCANReader] Error writing data to "
char hexCharToInt(char n)
unsigned int m_nTries_current
bool isOpen() const
Returns if port has been correctly open.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
std::vector< uint8_t > m_data
The data within this frame (0-8 bytes)
bool tryToOpenComms(std::string *err_msg=nullptr)
Tries to open the com port and setup all the LMS protocol.
mrpt::comms::CSerialPort * m_mySerialPort
Will be !=nullptr only if I created it, so I must destroy it at the end.
A high-performance stopwatch, with typical resolution of nanoseconds.
void initialize() override
Set-up communication with the laser.
uint8_t m_received_frame_buffer[2000]
uint8_t m_priority
The priority.
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
static Ptr Create(Args &&... args)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
A communications serial port built as an implementation of a utils::CStream.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
~CCANBusReader() override
Destructor
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool sendCommandToCANReader(const uint8_t *cmd, const uint16_t cmd_len, bool wait=true)
void purgeBuffers()
Purge tx and rx buffers.
#define ASSERT_(f)
Defines an assertion mechanism.
bool m_CANBusChannel_isOpen
unsigned int m_nTries_connect
Default = 1.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
This namespace contains representation of robot actions and observations.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
int m_com_baudRate
Baudrate: 9600, 38400, 500000.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
bool CANBusOpenChannel()
Opens the CAN Channel.
double Tac() noexcept
Stops the stopwatch.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
bool m_canreader_timestamp
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
bool waitACK(uint16_t timeout_ms)
std::shared_ptr< mrpt::obs ::CObservationCANBusJ1939 > Ptr
This class allows loading and storing values and vectors of different types from a configuration text...
bool waitContinuousSampleFrame(uint8_t &out_prio, uint8_t &out_pdu_format, uint8_t &out_pdu_spec, uint8_t &out_src_address, uint8_t &out_data_length, uint16_t &out_pgn, std::vector< uint8_t > &out_data, std::vector< char > &out_raw_frame)
bool queryVersion(bool printOutVersion=false)
uint8_t m_data_length
Data length
TCLAP::CmdLine cmd("system_control_rate_timer_example")
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
This class stores a message from a CAN BUS with the protocol J1939.
void Tic() noexcept
Starts the stopwatch.
std::string m_sensorLabel
See CGenericSensor.
bool sendCANBusReaderSpeed()
Sends the specified speed to the CAN Converter.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Versatile class for consistent logging and management of output messages.
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser sca...
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservationCANBusJ1939 &outObservation, bool &hardwareError)
Specific laser scanner "software drivers" must process here new data from the I/O stream,...
void open()
Open the port.
uint8_t m_src_address
The address of the source node within this frame.
std::vector< char > m_raw_frame
The ASCII frame.
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
bool waitIncomingFrame(uint16_t timeout)
bool CANBusCloseChannel()
Closes the CAN Channel.
void setBaudRate(int baud)
Changes the serial port baud rate (call prior to 'doProcess'); valid values are 9600,...
bool waitForVersion(uint16_t timeout, bool printOutVersion=false)
void setSerialPortName(const std::string &COM_name)
Sets the serial port to open (it is an error to try to change this while open yet).
uint16_t m_pgn
The Parameter Group Number within this frame.
uint8_t m_pdu_spec
PDU Specific.
std::string std::string format(std::string_view fmt, ARGS &&... args)
uint8_t m_pdu_format
PDU Format.
size_t Read(void *Buffer, size_t Count) override
Implements the virtual method responsible for reading from the stream - Unlike CStream::ReadBuffer,...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020 | |