00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 00002 00003 // -- BEGIN LICENSE BLOCK ---------------------------------------------- 00004 00024 // -- END LICENSE BLOCK ------------------------------------------------ 00025 00026 //---------------------------------------------------------------------- 00033 //---------------------------------------------------------------------- 00034 00035 #include <sick_safetyscanners/cola2/CreateSession.h> 00036 00037 #include <sick_safetyscanners/cola2/Cola2Session.h> 00038 #include <sick_safetyscanners/cola2/Command.h> 00039 00040 namespace sick { 00041 namespace cola2 { 00042 00043 CreateSession::CreateSession(Cola2Session& session) 00044 : Command(session, 0x4F, 0x58) // see cola2 manual 0x4F = O, 0x58 = X 00045 { 00046 m_writer_ptr = std::make_shared<sick::data_processing::ReadWriteHelper>(); 00047 } 00048 00049 void CreateSession::addTelegramData(sick::datastructure::PacketBuffer::VectorBuffer& telegram) const 00050 { 00051 uint8_t* data_ptr = prepareTelegramAndGetDataPtr(telegram); 00052 writeHeartbeatTimeoutToDataPtr(data_ptr); 00053 writeClientIdToDataPtr(data_ptr); 00054 } 00055 00056 uint8_t* CreateSession::prepareTelegramAndGetDataPtr( 00057 sick::datastructure::PacketBuffer::VectorBuffer& telegram) const 00058 { 00059 uint16_t prevSize = telegram.size(); 00060 telegram.resize(prevSize + 5); 00061 return telegram.data() + prevSize; 00062 } 00063 00064 bool CreateSession::canBeExecutedWithoutSessionID() const 00065 { 00066 return true; 00067 } 00068 00069 bool CreateSession::processReply() 00070 { 00071 if ((getCommandType() == 'O' && getCommandMode() == 'A') || 00072 (getCommandType() == 0x4F && getCommandMode() == 0x41)) 00073 { 00074 m_session.setSessionID(getSessionID()); 00075 ROS_INFO("Successfully opened Cola2 session with sessionID: %u", m_session.getSessionID()); 00076 return true; 00077 } 00078 else 00079 { 00080 ROS_WARN("Could not open Cola2 session"); 00081 return false; 00082 } 00083 } 00084 00085 void CreateSession::writeHeartbeatTimeoutToDataPtr(uint8_t*& data_ptr) const 00086 { 00087 uint8_t heartBeatTimeoutSeconds = 60; 00088 m_writer_ptr->writeuint8_tBigEndian(data_ptr, heartBeatTimeoutSeconds, 0); 00089 } 00090 00091 void CreateSession::writeClientIdToDataPtr(uint8_t*& data_ptr) const 00092 { 00093 uint32_t clientID = 1; // can be any random number 00094 m_writer_ptr->writeuint32_tBigEndian(data_ptr, clientID, 1); 00095 } 00096 00097 } // namespace cola2 00098 } // namespace sick