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 (
uint8_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 (
uint8_t k = 0; k < out_raw_frame.size(); ++k)
151 outThereIsObservation =
true;
195 if (err_msg) *err_msg =
"";
208 throw std::logic_error(
209 "ERROR: No serial port attached with bindIO, neither it " 210 "set with 'setSerialPort'");
215 bool just_open =
false;
233 if (!just_open)
return true;
240 cout <<
"Setting up serial comms in port " <<
m_com_port;
242 cout <<
" ... done" << endl;
248 cout <<
"Setting up CAN BUS Speed at: " <<
m_canbus_speed << endl;
249 for (
int nTry = 0; nTry < 250000 ; nTry++)
251 if (!
res)
return false;
252 cout <<
" ... done" << endl;
256 cout <<
"Opening CAN BUS and starting to receive." << endl;
257 for (
int nTry = 0; nTry < 250000 ; nTry++)
259 if (!
res)
return false;
260 cout <<
" ... done" << endl;
272 catch (
const std::exception& e)
275 "[CCANBusReader] Error trying to open CANBusReader at port ";
277 if (err_msg) *err_msg =
s;
290 unsigned char cmd[2];
332 unsigned char cmd[1];
341 unsigned char cmd[1];
352 unsigned char cmd[1];
360 unsigned char cmd[2];
369 unsigned char cmd[1];
381 vector<uint8_t>& out_data, vector<char>& out_raw_frame)
383 size_t nRead, nBytesToRead;
384 size_t nFrameBytes = 0;
386 unsigned char buf[40];
389 for (
unsigned char& k : buf) k = 0;
392 while (nFrameBytes < (lengthField = (10U + dlc + 1U)))
395 if (lengthField > 30)
397 cout <<
"#" << int(dlc) <<
" ";
399 for (
unsigned char& k : buf) k = 0;
403 if (nFrameBytes < 10)
409 nBytesToRead = (lengthField)-nFrameBytes;
421 catch (
const std::exception& e)
425 "[waitContinuousSampleFrame] Disconnecting due to comms error: " 430 if (!nRead)
return false;
432 if (nRead < nBytesToRead) std::this_thread::sleep_for(30ms);
436 if (nFrameBytes > 0 || (nFrameBytes == 0 && buf[0] == 0x54 ))
437 nFrameBytes += nRead;
441 for (
unsigned char& k : buf) k = 0;
448 out_raw_frame.resize(nFrameBytes);
449 for (
uint8_t k = 0; k < nFrameBytes; ++k)
452 out_raw_frame[k] = buf[k];
455 out_prio = (aux[1] << 2) | (aux[2] >> 2);
456 out_pdu_format = (aux[3] << 4) | aux[4];
457 out_pdu_spec = (aux[5] << 4) | aux[6];
458 out_src_address = (aux[7] << 4) | aux[8];
459 out_data_length = aux[9];
461 out_data.resize(out_data_length);
462 for (
uint8_t k = 0, k2 = 0; k < 2 * out_data_length; k += 2, k2++)
463 out_data[k2] = (aux[10 + k] << 4) | aux[11 + k];
465 if (buf[nFrameBytes - 1] != 0x0D)
468 "[CCANBusReader::waitContinuousSampleFrame] expected 0x0D " 469 "ending flag, 0x%X found instead",
488 cerr << err_str << endl;
489 throw std::logic_error(err_str);
505 int detected_rate = 0;
510 int rates[] = {0, 9600, 38400, 57600, 500000};
519 !detected_rate && i <
sizeof(rates) /
sizeof(rates[0]); i++)
524 std::this_thread::sleep_for(100ms);
529 cout << endl <<
"Closing CAN Channel " << endl;
530 for (
int nTry = 0; nTry < 250000 ; nTry++)
532 cout <<
" ... done" << endl;
535 std::this_thread::sleep_for(100ms);
538 for (
int nTry = 0; nTry < 250000 && !detected_rate; nTry++)
545 detected_rate = rates[i];
548 std::this_thread::sleep_for(20ms);
555 std::this_thread::sleep_for(5000ms);
565 std::this_thread::sleep_for(500ms);
601 cout << int(
b) << endl;
605 }
while (tictac.
Tac() < timeout_ms * 1e-3);
618 unsigned int nBytes = 0;
622 const double maxTime = timeout * 1e-3;
632 if (nBytes > 0 || (nBytes == 0 &&
b ==
'V'))
639 if (tictac.
Tac() >= maxTime)
641 cout <<
"Version timeout" << endl;
650 "[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, " 659 for (
uint8_t k = 0; k < nBytes; ++k)
669 unsigned int nBytes = 0;
673 const double maxTime = timeout * 1e-3;
675 while (nBytes < 10 || (nBytes < (10U + dlc + 1U )))
680 if (nBytes > 1 || (!nBytes &&
b == 0x54 ))
695 if (tictac.
Tac() >= maxTime)
return false;
701 "[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final " 702 "flag, received %x\n",
709 for (
unsigned int i=0;i<nBytes;i++)
723 ASSERT_(
sizeof(cmd_full) > cmd_len);
726 memcpy(cmd_full, cmd, cmd_len);
727 cmd_full[cmd_len] = 0x0D;
729 const size_t toWrite = cmd_len + 1;
733 for (
unsigned int i = 0; i < toWrite; i++) printf(
"%02X ", cmd_full[i]);
743 cout <<
"[CCANBusReader::SendCommandToCANReader] Error writing data to " double Tac() noexcept
Stops the stopwatch.
size_t Read(void *Buffer, size_t Count) override
Implements the virtual method responsible for reading from the stream - Unlike CStream::ReadBuffer, this method will not raise an exception on zero bytes read, as long as there is not any fatal error in the communications.
bool sendCANBusReaderSpeed()
Sends the specified speed to the CAN Converter.
bool m_canreader_timestamp
unsigned __int16 uint16_t
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
A communications serial port built as an implementation of a utils::CStream.
bool waitACK(uint16_t timeout_ms)
char hexCharToInt(char n)
std::string m_sensorLabel
See CGenericSensor.
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)
This class stores a message from a CAN BUS with the protocol J1939.
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...
A high-performance stopwatch, with typical resolution of nanoseconds.
void open()
Open the port.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
bool waitIncomingFrame(uint16_t timeout)
void setBaudRate(int baud)
Changes the serial port baud rate (call prior to 'doProcess'); valid values are 9600,38400 and 500000.
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
uint8_t m_priority
The priority.
#define ASSERT_(f)
Defines an assertion mechanism.
bool CANBusCloseChannel()
Closes the CAN Channel.
This class allows loading and storing values and vectors of different types from a configuration text...
bool waitForVersion(uint16_t timeout, bool printOutVersion=false)
uint8_t m_pdu_spec
PDU Specific.
Versatile class for consistent logging and management of output messages.
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).
This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser sca...
bool isOpen() const
Returns if port has been correctly open.
This namespace contains representation of robot actions and observations.
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
void initialize() override
Set-up communication with the laser.
void purgeBuffers()
Purge tx and rx buffers.
uint8_t m_received_frame_buffer[2000]
bool tryToOpenComms(std::string *err_msg=nullptr)
Tries to open the com port and setup all the LMS protocol.
GLsizei const GLchar ** string
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
~CCANBusReader() override
Destructor.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
bool sendCommandToCANReader(const uint8_t *cmd, const uint16_t cmd_len, bool wait=true)
unsigned int m_nTries_current
bool m_CANBusChannel_isOpen
uint8_t m_pdu_format
PDU Format.
static Ptr Create(Args &&... args)
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
std::vector< char > m_raw_frame
The ASCII frame.
mrpt::comms::CSerialPort * m_mySerialPort
Will be !=nullptr only if I created it, so I must destroy it at the end.
uint16_t m_pgn
The Parameter Group Number within this frame.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
int m_com_baudRate
Baudrate: 9600, 38400, 500000.
uint8_t m_src_address
The address of the source node within this frame.
void Tic() noexcept
Starts the stopwatch.
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
std::vector< uint8_t > m_data
The data within this frame (0-8 bytes)
uint8_t m_data_length
Data length.
bool CANBusOpenChannel()
Opens the CAN Channel.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
bool queryVersion(bool printOutVersion=false)
unsigned int m_nTries_connect
Default = 1.