Read the start angle of the field data from the persistent config ins…

…tead of the current config

CMakeLists.txt

@@ -110,6 +110,7 @@ add_library(${PROJECT_NAME}_core
   src/cola2/FieldGeometryVariableCommand.cpp
   src/cola2/FieldHeaderVariableCommand.cpp
   src/cola2/MeasurementCurrentConfigVariableCommand.cpp
+  src/cola2/MeasurementPersistentConfigVariableCommand.cpp
   src/cola2/MethodCommand.cpp
   src/cola2/MonitoringCaseTableHeaderVariableCommand.cpp
   src/cola2/MonitoringCaseVariableCommand.cpp
@@ -129,6 +130,7 @@ add_library(${PROJECT_NAME}_core
   src/data_processing/ParseIntrusionData.cpp
   src/data_processing/ParseMeasurementCurrentConfigData.cpp
   src/data_processing/ParseMeasurementData.cpp
+  src/data_processing/ParseMeasurementPersistentConfigData.cpp
   src/data_processing/ParseMonitoringCaseData.cpp
   src/data_processing/ParseTCPPacket.cpp
   src/data_processing/ParseTypeCodeData.cpp

include/sick_safetyscanners/SickSafetyscanners.h

@@ -58,6 +58,7 @@
 #include <sick_safetyscanners/cola2/FieldGeometryVariableCommand.h>
 #include <sick_safetyscanners/cola2/FieldHeaderVariableCommand.h>
 #include <sick_safetyscanners/cola2/MeasurementCurrentConfigVariableCommand.h>
+#include <sick_safetyscanners/cola2/MeasurementPersistentConfigVariableCommand.h>
 #include <sick_safetyscanners/cola2/MonitoringCaseTableHeaderVariableCommand.h>
 #include <sick_safetyscanners/cola2/MonitoringCaseVariableCommand.h>
 #include <sick_safetyscanners/cola2/TypeCodeVariableCommand.h>

include/sick_safetyscanners/cola2/MeasurementCurrentConfigVariableCommand.h

@@ -44,7 +44,7 @@ namespace sick {
 namespace cola2 {
 
 /*!
- * \brief Command to read the field geometry from the sensor.
+ * \brief Command to read the current configuration from the sensor.
  */
 class MeasurementCurrentConfigVariableCommand : public VariableCommand
 {
@@ -101,4 +101,4 @@ class MeasurementCurrentConfigVariableCommand : public VariableCommand
 } // namespace cola2
 } // namespace sick
 
-#endif // SICK_SAFETYSCANNERS_COLA2_FIELDGEOMETRYVARIABLECOMMAND_H
+#endif // SICK_SAFETYSCANNERS_COLA2_MEASUREMENTCURRENTCONFIGVARIABLECOMMAND_H

include/sick_safetyscanners/cola2/MeasurementPersistentConfigVariableCommand.h

@@ -0,0 +1,104 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+
+/*!
+*  Copyright (C) 2018, SICK AG, Waldkirch
+*  Copyright (C) 2018, FZI Forschungszentrum Informatik, Karlsruhe, Germany
+*
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*    http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+
+*/
+
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!
+ * \file MeasurementPersistentConfigVariableCommand.h
+ *
+ * \author  Lennart Puck <puck@fzi.de>
+ * \date    2019-01-07
+ */
+//----------------------------------------------------------------------
+
+#ifndef SICK_SAFETYSCANNERS_COLA2_MEASUREMENTPERSISTENTCONFIGVARIABLECOMMAND_H
+#define SICK_SAFETYSCANNERS_COLA2_MEASUREMENTPERSISTENTCONFIGVARIABLECOMMAND_H
+
+
+#include <sick_safetyscanners/cola2/VariableCommand.h>
+#include <sick_safetyscanners/data_processing/ParseMeasurementPersistentConfigData.h>
+#include <sick_safetyscanners/datastructure/CommSettings.h>
+
+namespace sick {
+namespace cola2 {
+
+/*!
+ * \brief Command to read the persistent configuration from the sensor.
+ */
+class MeasurementPersistentConfigVariableCommand : public VariableCommand
+{
+public:
+  /*!
+   * \brief Typedef to reference the base class.
+   */
+  typedef sick::cola2::VariableCommand base_class;
+
+  /*!
+   * \brief Constructor of the command.
+   *
+   * Takes the current cola2 session and a reference to the field data variable which will be
+   * written on execution. The index defines which field variable will be read. Depending on the
+   * sensor up to 128 variables can be defined.
+   *
+   * \param session The current cola2 session.
+   * \param field_data The field data reference which will be modified on execution.
+   */
+  MeasurementPersistentConfigVariableCommand(Cola2Session& session,
+                                             datastructure::FieldData& field_data);
+
+  /*!
+   * \brief Adds the data to the telegram.
+   *
+   * \param telegram The telegram which will be modified by the data.
+   */
+  void addTelegramData(sick::datastructure::PacketBuffer::VectorBuffer& telegram) const;
+
+  /*!
+   * \brief Returns if the command can be executed without a session ID. Will return false for most
+   * commands except the commands to establish a connection.
+   *
+   * \returns If the command needs a session ID to be executed.
+   */
+  bool canBeExecutedWithoutSessionID() const;
+
+  /*!
+   * \brief Processes the return from the sensor.
+   *
+   * \returns If processing of the returned data was successful.
+   */
+  bool processReply();
+
+
+private:
+  std::shared_ptr<sick::data_processing::ReadWriteHelper> m_writer_ptr;
+  std::shared_ptr<sick::data_processing::ParseMeasurementPersistentConfigData>
+    m_measurement_persistent_config_parser_ptr;
+
+  sick::datastructure::FieldData& m_field_data;
+};
+
+} // namespace cola2
+} // namespace sick
+
+#endif // SICK_SAFETYSCANNERS_COLA2_MEASUREMENTPERSISTENTCONFIGVARIABLECOMMAND_H

include/sick_safetyscanners/data_processing/ParseMeasurementCurrentConfigData.h

@@ -71,11 +71,10 @@ class ParseMeasurementCurrentConfigData
 private:
   std::shared_ptr<sick::data_processing::ReadWriteHelper> m_reader_ptr;
 
-  uint32_t readStartAngle(const uint8_t* data_ptr) const;
   uint32_t readAngularBeamResolution(const uint8_t* data_ptr) const;
 };
 
 } // namespace data_processing
 } // namespace sick
 
-#endif // SICK_SAFETYSCANNERS_DATA_PROCESSING_PARSEFIELDHEADERDATA_H
+#endif // SICK_SAFETYSCANNERS_DATA_PROCESSING_PARSEMEASUREMENTCURRENTCONFIGDATA_H

include/sick_safetyscanners/data_processing/ParseMeasurementPersistentConfigData.h

@@ -0,0 +1,80 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+
+/*!
+*  Copyright (C) 2018, SICK AG, Waldkirch
+*  Copyright (C) 2018, FZI Forschungszentrum Informatik, Karlsruhe, Germany
+*
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*    http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+
+*/
+
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!
+ * \file ParseMeasurementPersistentConfigData.h
+ *
+ * \author  Lennart Puck <puck@fzi.de>
+ * \date    2018-10-16
+ */
+//----------------------------------------------------------------------
+
+#ifndef SICK_SAFETYSCANNERS_DATA_PROCESSING_PARSEMEASUREMENTPERSISTENTCONFIGDATA_H
+#define SICK_SAFETYSCANNERS_DATA_PROCESSING_PARSEMEASUREMENTPERSISTENTCONFIGDATA_H
+
+#include <sick_safetyscanners/datastructure/Data.h>
+#include <sick_safetyscanners/datastructure/FieldData.h>
+#include <sick_safetyscanners/datastructure/PacketBuffer.h>
+
+#include <sick_safetyscanners/data_processing/ReadWriteHelper.h>
+
+namespace sick {
+
+namespace data_processing {
+
+
+/*!
+ * \brief Parser to read the persistent configuration of the sensor.
+ */
+class ParseMeasurementPersistentConfigData
+{
+public:
+  /*!
+   * \brief Constructor of the parser.
+   */
+  ParseMeasurementPersistentConfigData();
+
+  /*!
+   * \brief Parses a tcp sequence to read the persistent configuration of the sensor.
+   *
+   * \param buffer The incoming tcp sequence.
+   * \param field_data Reference to the field data where the information will be set.
+   *
+   * \returns If parsing was successful.
+   */
+  bool parseTCPSequence(const datastructure::PacketBuffer& buffer,
+                        datastructure::FieldData& field_data) const;
+
+private:
+  std::shared_ptr<sick::data_processing::ReadWriteHelper> m_reader_ptr;
+
+  uint32_t readStartAngle(const uint8_t* data_ptr) const;
+};
+
+} // namespace data_processing
+} // namespace sick
+
+#endif // SICK_SAFETYSCANNERS_DATA_PROCESSING_PARSEMEASUREMENTPERSISTENTCONFIGDATA_H

src/SickSafetyscanners.cpp

@@ -165,10 +165,14 @@ void SickSafetyscanners::requestFieldDataInColaSession(
   sick::datastructure::FieldData common_field_data;
 
   sick::cola2::Cola2Session::CommandPtr command_ptr =
-    std::make_shared<sick::cola2::MeasurementCurrentConfigVariableCommand>(
+    std::make_shared<sick::cola2::MeasurementPersistentConfigVariableCommand>(
       boost::ref(*m_session_ptr), common_field_data);
   m_session_ptr->executeCommand(command_ptr);
 
+  command_ptr = std::make_shared<sick::cola2::MeasurementCurrentConfigVariableCommand>(
+    boost::ref(*m_session_ptr), common_field_data);
+  m_session_ptr->executeCommand(command_ptr);
+
   command_ptr = std::make_shared<sick::cola2::MonitoringCaseTableHeaderVariableCommand>(
     boost::ref(*m_session_ptr), common_field_data);
   m_session_ptr->executeCommand(command_ptr);

src/cola2/MeasurementPersistentConfigVariableCommand.cpp

@@ -0,0 +1,76 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+
+/*!
+*  Copyright (C) 2018, SICK AG, Waldkirch
+*  Copyright (C) 2018, FZI Forschungszentrum Informatik, Karlsruhe, Germany
+*
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*    http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+
+*/
+
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!
+ * \file MeasurementPersistentConfigVariableCommand.cpp
+ *
+ * \author  Lennart Puck <puck@fzi.de>
+ * \date    2019-01-07
+ */
+//----------------------------------------------------------------------
+
+#include <sick_safetyscanners/cola2/MeasurementPersistentConfigVariableCommand.h>
+
+#include <sick_safetyscanners/cola2/Cola2Session.h>
+#include <sick_safetyscanners/cola2/Command.h>
+
+namespace sick {
+namespace cola2 {
+
+MeasurementPersistentConfigVariableCommand::MeasurementPersistentConfigVariableCommand(
+  Cola2Session& session, datastructure::FieldData& field_data)
+  : VariableCommand(session, 177)
+  , m_field_data(field_data)
+{
+  m_writer_ptr = std::make_shared<sick::data_processing::ReadWriteHelper>();
+  m_measurement_persistent_config_parser_ptr =
+    std::make_shared<sick::data_processing::ParseMeasurementPersistentConfigData>();
+}
+
+void MeasurementPersistentConfigVariableCommand::addTelegramData(
+  sick::datastructure::PacketBuffer::VectorBuffer& telegram) const
+{
+  base_class::addTelegramData(telegram);
+}
+
+bool MeasurementPersistentConfigVariableCommand::canBeExecutedWithoutSessionID() const
+{
+  return true;
+}
+
+bool MeasurementPersistentConfigVariableCommand::processReply()
+{
+  if (!base_class::processReply())
+  {
+    return false;
+  }
+  m_measurement_persistent_config_parser_ptr->parseTCPSequence(getDataVector(), m_field_data);
+  return true;
+}
+
+
+} // namespace cola2
+} // namespace sick

src/data_processing/ParseMeasurementCurrentConfigData.cpp

@@ -49,16 +49,10 @@ bool ParseMeasurementCurrentConfigData::parseTCPSequence(const datastructure::Pa
                                                          datastructure::FieldData& field_data) const
 {
   const uint8_t* data_ptr(buffer.getBuffer().data());
-  field_data.setStartAngle(readStartAngle(data_ptr));
   field_data.setAngularBeamResolution(readAngularBeamResolution(data_ptr));
   return true;
 }
 
-uint32_t ParseMeasurementCurrentConfigData::readStartAngle(const uint8_t* data_ptr) const
-{
-  return m_reader_ptr->readuint32_tLittleEndian(data_ptr, 36);
-}
-
 uint32_t ParseMeasurementCurrentConfigData::readAngularBeamResolution(const uint8_t* data_ptr) const
 {
   return m_reader_ptr->readuint32_tLittleEndian(data_ptr, 40);