From d834ba8240015e919052fcb662dfd1d4e379d4dc Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Fri, 5 Aug 2022 14:58:34 +0200 Subject: [PATCH 01/27] first version for encoder data --- CMakeLists.txt | 1 + include/psen_scan_v2/ros_scanner_node.h | 45 +++++++++++++++++++ launch/bringup.launch | 4 ++ launch/psen_scan_v2.launch | 4 ++ msg/EncoderData.msg | 1 + src/psen_scan_driver.cpp | 7 +++ .../configuration/default_parameters.h | 1 + .../laserscan_conversions.h | 9 ++++ .../monitoring_frame_deserialization.h | 2 + .../monitoring_frame_msg.h | 4 ++ .../monitoring_frame_msg_builder.h | 6 +++ .../data_conversion_layer/start_request.h | 13 ++++-- .../psen_scan_v2_standalone/laserscan.h | 12 +++++ .../scanner_config_builder.h | 7 +++ .../scanner_configuration.h | 8 ++++ .../monitoring_frame_deserialization.cpp | 13 ++++++ .../monitoring_frame_msg.cpp | 17 +++++++ .../data_conversion_layer/start_request.cpp | 2 +- .../start_request_serialization.cpp | 4 +- standalone/src/laserscan.cpp | 20 +++++++++ 20 files changed, 175 insertions(+), 5 deletions(-) create mode 100644 msg/EncoderData.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index f7c556c1a..4ff57c355 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,7 @@ add_message_files( IOState.msg ZoneSet.msg ZoneSetConfiguration.msg + EncoderData.msg ) generate_messages( diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index 07fd5f294..511efe4dc 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -33,6 +33,7 @@ #include "psen_scan_v2_standalone/scanner_v2.h" +#include "psen_scan_v2/EncoderData.h" #include "psen_scan_v2/laserscan_ros_conversions.h" #include "psen_scan_v2/io_state_ros_conversion.h" #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" @@ -80,12 +81,14 @@ class ROSScannerNodeT private: void laserScanCallback(const LaserScan& scan); void publishChangedIOStates(const std::vector& io_states); + void publishEncoderData(const std::vector& encoder_data); private: ros::NodeHandle nh_; ros::Publisher pub_scan_; ros::Publisher pub_zone_; ros::Publisher pub_io_; + ros::Publisher pub_encoder_; std::string tf_prefix_; double x_axis_rotation_; S scanner_; @@ -124,6 +127,7 @@ ROSScannerNodeT::ROSScannerNodeT(ros::NodeHandle& nh, pub_scan_ = nh_.advertise(topic, 1); pub_zone_ = nh_.advertise("active_zoneset", 1); pub_io_ = nh_.advertise("io_state", 6, true /* latched */); + pub_encoder_ = nh.advertise("encoder_data", 6, true); } template @@ -146,6 +150,9 @@ void ROSScannerNodeT::laserScanCallback(const LaserScan& scan) pub_zone_.publish(active_zoneset); publishChangedIOStates(scan.ioStates()); + + if (!scan.encoderData().empty()) + publishEncoderData(scan.encoderData()); } // LCOV_EXCL_START catch (const std::invalid_argument& e) @@ -173,6 +180,44 @@ void ROSScannerNodeT::publishChangedIOStates(const std::vector +void ROSScannerNodeT::publishEncoderData(const std::vector& encoder_data) +{ + psen_scan_v2::EncoderData ros_message; + + //for (auto value : encoder_data) + //{ + // ros_message.encodeData.push_back(value); + //} + + ros_message.encodeData.insert(ros_message.encodeData.begin(), encoder_data.cbegin(), encoder_data.cend()); + + pub_encoder_.publish(ros_message); + + //ros_message.intensities.insert( + //ros_message.intensities.begin(), laserscan.intensities().cbegin(), laserscan.intensities().cend()); + + //if (encoder_data.timestamp() < 0) + //{ + // throw std::invalid_argument("IOState of Laserscan message has an invalid timestamp: " + + // std::to_string(io_state.timestamp())); + //} + // + //for (const auto& io : io_states) + //{ + // if (last_io_state_ != io) + // { + // pub_io_.publish(toIOStateMsg(io, tf_prefix_)); + // + // PSENSCAN_INFO("RosScannerNode", + // "IOs changed, new input: {}, new output: {}", + // formatPinStates(io.changedInputStates(last_io_state_)), + // formatPinStates(io.changedOutputStates(last_io_state_))); + // last_io_state_ = io; + // } + //} +} + template void ROSScannerNodeT::terminate() { diff --git a/launch/bringup.launch b/launch/bringup.launch index 9108766d1..9f032b5b9 100644 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -48,6 +48,9 @@ along with this program. If not, see . + + + @@ -60,6 +63,7 @@ along with this program. If not, see . + diff --git a/launch/psen_scan_v2.launch b/launch/psen_scan_v2.launch index 1e6d0abcc..59e103c62 100644 --- a/launch/psen_scan_v2.launch +++ b/launch/psen_scan_v2.launch @@ -48,6 +48,9 @@ along with this program. If not, see . + + + @@ -66,6 +69,7 @@ along with this program. If not, see . + diff --git a/msg/EncoderData.msg b/msg/EncoderData.msg new file mode 100644 index 000000000..b6d12e006 --- /dev/null +++ b/msg/EncoderData.msg @@ -0,0 +1 @@ +float64[] encodeData diff --git a/src/psen_scan_driver.cpp b/src/psen_scan_driver.cpp index e5c29e810..08c662f1c 100644 --- a/src/psen_scan_driver.cpp +++ b/src/psen_scan_driver.cpp @@ -49,6 +49,7 @@ const std::string PARAM_X_AXIS_ROTATION{ "x_axis_rotation" }; const std::string PARAM_FRAGMENTED_SCANS{ "fragmented_scans" }; const std::string PARAM_INTENSITIES{ "intensities" }; const std::string PARAM_RESOLUTION{ "resolution" }; +const std::string PARAM_ENCODER_DATA{ "encoder_data" }; static const std::string DEFAULT_TF_PREFIX = "laser_1"; @@ -97,6 +98,7 @@ int main(int argc, char** argv) .enableIntensities(getOptionalParamFromServer(pnh, PARAM_INTENSITIES, configuration::INTENSITIES)) .scanResolution(util::TenthOfDegree::fromRad( getOptionalParamFromServer(pnh, PARAM_RESOLUTION, configuration::DEFAULT_SCAN_ANGLE_RESOLUTION))) + .enableEncoder(getOptionalParamFromServer(pnh, PARAM_ENCODER_DATA, configuration::ENCODER)) .build() }; @@ -104,6 +106,11 @@ int main(int argc, char** argv) { ROS_INFO("Using fragmented scans."); } + + if (scanner_configuration.encoderEnabled()) + { + ROS_INFO("Reading data from encoders"); + } ROSScannerNode ros_scanner_node(pnh, DEFAULT_PUBLISH_TOPIC, diff --git a/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h b/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h index 34c886a85..e16a1338f 100644 --- a/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h +++ b/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h @@ -35,6 +35,7 @@ static constexpr unsigned short CONTROL_PORT_OF_HOST_DEVICE{ 55116 }; static constexpr bool FRAGMENTED_SCANS{ false }; static constexpr bool INTENSITIES{ false }; static constexpr bool DIAGNOSTICS{ false }; +static constexpr bool ENCODER{ false }; //! @brief Start angle of measurement. static constexpr double DEFAULT_ANGLE_START(-data_conversion_layer::degreeToRadian(137.4)); diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h index a676bb472..87da9768a 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h @@ -102,6 +102,7 @@ inline LaserScan LaserScanConverter::toLaserScan( std::vector measurements; std::vector intensities; std::vector io_states; + std::vector encoderData; for (auto index : sorted_stamped_msgs_indices) { @@ -131,6 +132,13 @@ inline LaserScan LaserScanConverter::toLaserScan( io_states.emplace_back(single_msg.msg_.iOPinData(), single_msg.stamp_); } + // Create the encoder_data as reception order. + if(single_msg.msg_.hasEncoderDataField()) + { + encoderData.insert(encoderData.end(), + single_msg.msg_.encoderData().begin(), + single_msg.msg_.encoderData().end()); + } } LaserScan scan(stamped_msgs[0].msg_.resolution(), @@ -143,6 +151,7 @@ inline LaserScan LaserScanConverter::toLaserScan( scan.measurements(measurements); scan.intensities(intensities); scan.ioStates(io_states); + scan.encoderData(encoderData); return scan; } diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h index effb0a260..998765546 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h @@ -60,6 +60,7 @@ static constexpr uint16_t NUMBER_OF_BYTES_SINGLE_MEASUREMENT{ 2 }; static constexpr uint16_t NO_SIGNAL_ARRIVED{ 59956 }; static constexpr uint16_t SIGNAL_TOO_LATE{ 59958 }; static constexpr uint16_t NUMBER_OF_BYTES_SINGLE_INTENSITY{ 2 }; +static constexpr uint16_t NUMBER_OF_BYTES_ENCODER_DATA{ 2 }; /** * @brief The information included in every single monitoring frame. @@ -140,6 +141,7 @@ enum class AdditionalFieldHeaderID : AdditionalFieldHeader::Id diagnostics = 0x04, measurements = 0x05, intensities = 0x06, + encoder = 0x07, end_of_frame = 0x09 }; diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h index 0a0128d4e..c31d3ad3d 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h @@ -75,6 +75,8 @@ class Message const std::vector& measurements() const; //! @throw AdditionalFieldMissing if intensities were missing during deserialization of a Message. const std::vector& intensities() const; + //! @throw AdditionalFieldMissing if encoder data were missing during deserialization of a Message. + const std::vector& encoderData() const; //! @throw AdditionalFieldMissing if diagnostic_messages were missing during deserialization of a Message. std::vector diagnosticMessages() const; @@ -83,6 +85,7 @@ class Message bool hasIOPinField() const; bool hasMeasurementsField() const; bool hasIntensitiesField() const; + bool hasEncoderDataField() const; bool hasDiagnosticMessagesField() const; private: @@ -96,6 +99,7 @@ class Message boost::optional io_pin_data_; boost::optional> measurements_; boost::optional> intensities_; + boost::optional> encoder_data_; boost::optional> diagnostic_messages_; public: diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h index 5b91d9c8e..63bcd9211 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h @@ -46,6 +46,7 @@ class MessageBuilder MessageBuilder& intensities(const std::vector& intensities); MessageBuilder& diagnosticMessages(const std::vector& diagnostic_messages); MessageBuilder& iOPinData(const io::PinData& io_pin_data); + MessageBuilder& encoderData(const std::vector& encoder_data); private: Message msg_; @@ -114,6 +115,11 @@ inline MessageBuilder& MessageBuilder::iOPinData(const io::PinData& io_pin_data) msg_.io_pin_data_ = io_pin_data; return *this; } +inline MessageBuilder& MessageBuilder::encoderData(const std::vector& encoder_data) +{ + msg_.encoder_data_ = encoder_data; + return *this; +} } // namespace monitoring_frame } // namespace data_conversion_layer } // namespace psen_scan_v2_standalone diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h index be05a0b78..076901927 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h @@ -74,15 +74,17 @@ class Message class DeviceSettings { public: - constexpr DeviceSettings(const bool diagnostics_enabled, const bool intensities_enabled); + constexpr DeviceSettings(const bool diagnostics_enabled, const bool intensities_enabled, const bool encoder_data_enabled); public: constexpr bool diagnosticsEnabled() const; constexpr bool intensitiesEnabled() const; + constexpr bool encoderDataEnabled() const; private: const bool diagnostics_enabled_; const bool intensities_enabled_; + const bool encoder_data_enabled_; }; private: @@ -114,8 +116,8 @@ constexpr util::TenthOfDegree Message::LaserScanSettings::resolution() const return resolution_; }; -constexpr Message::DeviceSettings::DeviceSettings(const bool diagnostics_enabled, const bool intensities_enabled) - : diagnostics_enabled_(diagnostics_enabled), intensities_enabled_(intensities_enabled) +constexpr Message::DeviceSettings::DeviceSettings(const bool diagnostics_enabled, const bool intensities_enabled, const bool encoder_data_enabled) + : diagnostics_enabled_(diagnostics_enabled), intensities_enabled_(intensities_enabled), encoder_data_enabled_(encoder_data_enabled) { } @@ -129,6 +131,11 @@ constexpr bool Message::DeviceSettings::intensitiesEnabled() const return intensities_enabled_; }; +constexpr bool Message::DeviceSettings::encoderDataEnabled() const +{ + return encoder_data_enabled_; +}; + } // namespace start_request } // namespace data_conversion_layer } // namespace psen_scan_v2_standalone diff --git a/standalone/include/psen_scan_v2_standalone/laserscan.h b/standalone/include/psen_scan_v2_standalone/laserscan.h index d9333b7b1..0ad25c249 100644 --- a/standalone/include/psen_scan_v2_standalone/laserscan.h +++ b/standalone/include/psen_scan_v2_standalone/laserscan.h @@ -49,6 +49,7 @@ class LaserScan using MeasurementData = std::vector; using IntensityData = std::vector; using IOData = std::vector; + using EncoderData = std::vector; public: LaserScan(const util::TenthOfDegree& resolution, @@ -117,6 +118,15 @@ class LaserScan [[deprecated("use void ioStates(const IOData& io_states) instead")]] void setIOStates(const IOData& io_states); void ioStates(const IOData& io_states); + /*! deprecated: use const EncoderData& encoderData() instead */ + [[deprecated("use const EncoderData& encoderData() const instead")]] const EncoderData& getEncoderData() const; + const EncoderData& encoderData() const; + + /*! deprecated: use void encoderData(const EncoderData& encoderData) instead */ + [[deprecated("use void encoderData(const EncoderData& encoderData) instead")]] void + setEncoderData(const EncoderData& encoderData); + void encoderData(const EncoderData& encoderData); + private: //! Measurement data of the laserscan (in Millimeters). MeasurementData measurements_; @@ -136,6 +146,8 @@ class LaserScan const uint8_t active_zoneset_; //! Time of the first ray in this scan round (or fragment if fragmented_scans is enabled). const int64_t timestamp_; + //! Stores the received normalized encoder signals. + EncoderData encoderData_; }; std::ostream& operator<<(std::ostream& os, const LaserScan& scan); diff --git a/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h b/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h index ec1e64b3e..ec0c18e89 100644 --- a/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h +++ b/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h @@ -49,6 +49,7 @@ class ScannerConfigurationBuilder ScannerConfigurationBuilder& enableDiagnostics(const bool& enable); ScannerConfigurationBuilder& enableIntensities(const bool& enable); ScannerConfigurationBuilder& enableFragmentedScans(const bool& enable); + ScannerConfigurationBuilder& enableEncoder(const bool& enable); operator ScannerConfiguration(); private: @@ -151,6 +152,12 @@ inline ScannerConfigurationBuilder& ScannerConfigurationBuilder::enableFragmente return *this; } +inline ScannerConfigurationBuilder& ScannerConfigurationBuilder::enableEncoder(const bool& enable = true) +{ + config_.encoder_enabled_ = enable; + return *this; +} + ScannerConfigurationBuilder::operator ScannerConfiguration() { return build(); diff --git a/standalone/include/psen_scan_v2_standalone/scanner_configuration.h b/standalone/include/psen_scan_v2_standalone/scanner_configuration.h index f6ca6e644..8c4e46d42 100644 --- a/standalone/include/psen_scan_v2_standalone/scanner_configuration.h +++ b/standalone/include/psen_scan_v2_standalone/scanner_configuration.h @@ -53,6 +53,8 @@ class ScannerConfiguration bool fragmentedScansEnabled() const; + bool encoderEnabled() const; + /*! deprecated: use void hostIp(const uint32_t& host_ip) instead */ [[deprecated("use void hostIp(const uint32_t& host_ip) instead")]] void setHostIp(const uint32_t& host_ip); void hostIp(const uint32_t& host_ip); @@ -79,6 +81,7 @@ class ScannerConfiguration bool diagnostics_enabled_{ configuration::DIAGNOSTICS }; bool intensities_enabled_{ configuration::INTENSITIES }; bool fragmented_scans_{ configuration::FRAGMENTED_SCANS }; + bool encoder_enabled_{ configuration::ENCODER }; }; inline bool ScannerConfiguration::isComplete() const @@ -151,6 +154,11 @@ inline bool ScannerConfiguration::fragmentedScansEnabled() const return fragmented_scans_; } +inline bool ScannerConfiguration::encoderEnabled() const +{ + return encoder_enabled_; +} + inline void ScannerConfiguration::hostIp(const uint32_t& host_ip) { host_ip_ = host_ip; diff --git a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp index 9022f70e9..a2dd5640c 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -76,6 +76,11 @@ static constexpr double toIntensities(const uint16_t& value) return static_cast(retval & 0b0011111111111111); } +static constexpr double toEncoder(const uint16_t& value) +{ + return static_cast(value); +} + monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data, const std::size_t& num_bytes) { data_conversion_layer::monitoring_frame::MessageBuilder msg_builder; @@ -153,6 +158,14 @@ monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data msg_builder.intensities(intensities); break; } + case AdditionalFieldHeaderID::encoder: { + const size_t num_encoder_values{ static_cast(additional_header.length()) / + NUMBER_OF_BYTES_ENCODER_DATA }; + std::vector encoder_data; + raw_processing::readArray(ss, encoder_data, num_encoder_values, toEncoder); + msg_builder.encoderData(encoder_data); + break; + } default: throw DecodingFailure( fmt::format("Header Id {:#04x} unknown. Cannot read additional field of monitoring frame on position {}.", diff --git a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp index 0052cace3..aeeaa61ef 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp @@ -103,6 +103,18 @@ const std::vector& Message::intensities() const } } +const std::vector& Message::encoderData() const +{ + if (encoder_data_.is_initialized()) + { + return encoder_data_.get(); + } + else + { + throw AdditionalFieldMissing("Encoder data"); + } +} + std::vector Message::diagnosticMessages() const { if (diagnostic_messages_.is_initialized()) @@ -144,6 +156,11 @@ bool Message::hasDiagnosticMessagesField() const { return diagnostic_messages_.is_initialized(); } + +bool Message::hasEncoderDataField() const +{ + return encoder_data_.is_initialized(); +} } // namespace monitoring_frame } // namespace data_conversion_layer } // namespace psen_scan_v2_standalone diff --git a/standalone/src/data_conversion_layer/start_request.cpp b/standalone/src/data_conversion_layer/start_request.cpp index 24960eba4..f2446b1ba 100644 --- a/standalone/src/data_conversion_layer/start_request.cpp +++ b/standalone/src/data_conversion_layer/start_request.cpp @@ -26,7 +26,7 @@ namespace start_request Message::Message(const ScannerConfiguration& scanner_configuration) : host_ip_(*scanner_configuration.hostIp()) , host_udp_port_data_(scanner_configuration.hostUDPPortData()) // Write is deduced by the scanner - , master_device_settings_(scanner_configuration.diagnosticsEnabled(), scanner_configuration.intensitiesEnabled()) + , master_device_settings_(scanner_configuration.diagnosticsEnabled(), scanner_configuration.intensitiesEnabled(), scanner_configuration.encoderEnabled()) , master_(scanner_configuration.scanRange(), scanner_configuration.scanResolution()) { } diff --git a/standalone/src/data_conversion_layer/start_request_serialization.cpp b/standalone/src/data_conversion_layer/start_request_serialization.cpp index 47632d6b9..a9d89b933 100644 --- a/standalone/src/data_conversion_layer/start_request_serialization.cpp +++ b/standalone/src/data_conversion_layer/start_request_serialization.cpp @@ -85,7 +85,9 @@ RawData data_conversion_layer::start_request::serialize(const data_conversion_la const uint8_t active_zone_set_enabled{ 0b00001000 }; const uint8_t io_pin_data_enabled{ 0b00001000 }; const uint8_t scan_counter_enabled{ 0b00001000 }; - const uint8_t speed_encoder_enabled{ 0 }; /**< 0000000bin disabled, 00001111bin enabled.*/ + //const uint8_t speed_encoder_enabled{ 0 }; /**< 0000000bin disabled, 00001111bin enabled.*/ + const uint8_t speed_encoder_enabled{ static_cast( + msg.master_device_settings_.encoderDataEnabled() ? 0b00001000 : 0b00000000) }; const uint8_t diagnostics_enabled{ static_cast( msg.master_device_settings_.diagnosticsEnabled() ? 0b00001000 : 0b00000000) }; diff --git a/standalone/src/laserscan.cpp b/standalone/src/laserscan.cpp index 0156db2cd..fb492b725 100644 --- a/standalone/src/laserscan.cpp +++ b/standalone/src/laserscan.cpp @@ -118,6 +118,16 @@ void LaserScan::setIntensities(const IntensityData& intensities) { this->intensities(intensities); } + +const LaserScan::IntensityData& LaserScan::getEncoderData() const +{ + return this->encoderData(); +} + +void LaserScan::setEncoderData(const EncoderData& encoderData) +{ + this->encoderData(encoderData); +} // LCOV_EXCL_STOP const util::TenthOfDegree& LaserScan::minScanAngle() const @@ -170,6 +180,16 @@ void LaserScan::intensities(const IntensityData& intensities) intensities_ = intensities; } +const LaserScan::EncoderData& LaserScan::encoderData() const +{ + return encoderData_; +} + +void LaserScan::encoderData(const EncoderData& encoderData) +{ + encoderData_ = encoderData; +} + // LCOV_EXCL_START void LaserScan::setIOStates(const IOData& io_states) { From 3247915331a0770dc29d5077af63c8d0b7aa2ab5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Cm-sanchez-rico=E2=80=9D?= Date: Thu, 11 Aug 2022 11:19:54 +0200 Subject: [PATCH 02/27] refactor in the encoder reading feature --- CMakeLists.txt | 3 +- .../encoder_state_ros_conversion.h | 49 ++++++++++++++++ include/psen_scan_v2/ros_scanner_node.h | 49 ++++------------ launch/psen_scan_v2.launch | 6 +- msg/EncoderData.msg | 1 - msg/EncoderState.msg | 3 + standalone/CMakeLists.txt | 1 + .../data_conversion_layer/encoder_data.h | 57 +++++++++++++++++++ .../laserscan_conversions.h | 21 ++++--- .../monitoring_frame_deserialization.h | 2 +- .../monitoring_frame_msg.h | 5 +- .../monitoring_frame_msg_builder.h | 5 +- .../psen_scan_v2_standalone/encoder_state.h | 47 +++++++++++++++ .../psen_scan_v2_standalone/laserscan.h | 14 +++-- .../monitoring_frame_deserialization.cpp | 22 +++++-- .../monitoring_frame_msg.cpp | 2 +- standalone/src/encoder_state.cpp | 48 ++++++++++++++++ standalone/src/laserscan.cpp | 32 ++++++----- 18 files changed, 284 insertions(+), 83 deletions(-) create mode 100644 include/psen_scan_v2/encoder_state_ros_conversion.h delete mode 100644 msg/EncoderData.msg create mode 100644 msg/EncoderState.msg create mode 100644 standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h create mode 100644 standalone/include/psen_scan_v2_standalone/encoder_state.h create mode 100644 standalone/src/encoder_state.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ff57c355..8404445e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,7 +57,7 @@ add_message_files( IOState.msg ZoneSet.msg ZoneSetConfiguration.msg - EncoderData.msg + EncoderState.msg ) generate_messages( @@ -137,6 +137,7 @@ include_directories( set(${PROJECT_NAME}_standalone_sources standalone/src/scanner_v2.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp standalone/src/data_conversion_layer/monitoring_frame_msg.cpp standalone/src/data_conversion_layer/start_request.cpp diff --git a/include/psen_scan_v2/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h new file mode 100644 index 000000000..9244deda0 --- /dev/null +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef PSEN_SCAN_V2_ENCODER_STATE_ROS_CONVERSIONS_H +#define PSEN_SCAN_V2_ENCODER_STATE_ROS_CONVERSIONS_H + +#include +#include + +#include "psen_scan_v2/EncoderState.h" + +namespace psen_scan_v2 +{ + +psen_scan_v2::EncoderState toEncoderStateMsg(const psen_scan_v2_standalone::EncoderState& encoder_state, + const std::string& frame_id) +{ + psen_scan_v2::EncoderState ros_message; + + if (encoder_state.timestamp() < 0) + { + throw std::invalid_argument("IOState of Laserscan message has an invalid timestamp: " + + std::to_string(encoder_state.timestamp())); + } + + ros_message.header.stamp = ros::Time{}.fromNSec(encoder_state.timestamp()); + ros_message.header.frame_id = frame_id; + + ros_message.encoder_1 = encoder_state.getEncoder_1(); + ros_message.encoder_2 = encoder_state.getEncoder_2(); + + return ros_message; +} + +} // namespace psen_scan_v2 + +#endif // PSEN_SCAN_V2_IO_STATE_ROS_CONVERSIONS_H diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index 511efe4dc..9be085788 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -33,9 +33,10 @@ #include "psen_scan_v2_standalone/scanner_v2.h" -#include "psen_scan_v2/EncoderData.h" +#include "psen_scan_v2/EncoderState.h" #include "psen_scan_v2/laserscan_ros_conversions.h" #include "psen_scan_v2/io_state_ros_conversion.h" +#include "psen_scan_v2/encoder_state_ros_conversion.h" #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" #include "psen_scan_v2_standalone/util/format_range.h" @@ -81,7 +82,7 @@ class ROSScannerNodeT private: void laserScanCallback(const LaserScan& scan); void publishChangedIOStates(const std::vector& io_states); - void publishEncoderData(const std::vector& encoder_data); + void publishEncoderData(const std::vector& encoder_states); private: ros::NodeHandle nh_; @@ -127,7 +128,7 @@ ROSScannerNodeT::ROSScannerNodeT(ros::NodeHandle& nh, pub_scan_ = nh_.advertise(topic, 1); pub_zone_ = nh_.advertise("active_zoneset", 1); pub_io_ = nh_.advertise("io_state", 6, true /* latched */); - pub_encoder_ = nh.advertise("encoder_data", 6, true); + pub_encoder_ = nh.advertise("encoder_estate", 6, true); } template @@ -151,8 +152,7 @@ void ROSScannerNodeT::laserScanCallback(const LaserScan& scan) publishChangedIOStates(scan.ioStates()); - if (!scan.encoderData().empty()) - publishEncoderData(scan.encoderData()); + publishEncoderData(scan.encoderStates()); } // LCOV_EXCL_START catch (const std::invalid_argument& e) @@ -181,41 +181,12 @@ void ROSScannerNodeT::publishChangedIOStates(const std::vector -void ROSScannerNodeT::publishEncoderData(const std::vector& encoder_data) +void ROSScannerNodeT::publishEncoderData(const std::vector& encoder_states) { - psen_scan_v2::EncoderData ros_message; - - //for (auto value : encoder_data) - //{ - // ros_message.encodeData.push_back(value); - //} - - ros_message.encodeData.insert(ros_message.encodeData.begin(), encoder_data.cbegin(), encoder_data.cend()); - - pub_encoder_.publish(ros_message); - - //ros_message.intensities.insert( - //ros_message.intensities.begin(), laserscan.intensities().cbegin(), laserscan.intensities().cend()); - - //if (encoder_data.timestamp() < 0) - //{ - // throw std::invalid_argument("IOState of Laserscan message has an invalid timestamp: " + - // std::to_string(io_state.timestamp())); - //} - // - //for (const auto& io : io_states) - //{ - // if (last_io_state_ != io) - // { - // pub_io_.publish(toIOStateMsg(io, tf_prefix_)); - // - // PSENSCAN_INFO("RosScannerNode", - // "IOs changed, new input: {}, new output: {}", - // formatPinStates(io.changedInputStates(last_io_state_)), - // formatPinStates(io.changedOutputStates(last_io_state_))); - // last_io_state_ = io; - // } - //} + for (const auto& single_state : encoder_states) + { + pub_encoder_.publish(toEncoderStateMsg(single_state, tf_prefix_)); + } } template diff --git a/launch/psen_scan_v2.launch b/launch/psen_scan_v2.launch index 59e103c62..bb70107f3 100644 --- a/launch/psen_scan_v2.launch +++ b/launch/psen_scan_v2.launch @@ -27,7 +27,7 @@ along with this program. If not, see . - + @@ -48,8 +48,8 @@ along with this program. If not, see . - - + + diff --git a/msg/EncoderData.msg b/msg/EncoderData.msg deleted file mode 100644 index b6d12e006..000000000 --- a/msg/EncoderData.msg +++ /dev/null @@ -1 +0,0 @@ -float64[] encodeData diff --git a/msg/EncoderState.msg b/msg/EncoderState.msg new file mode 100644 index 000000000..443c31212 --- /dev/null +++ b/msg/EncoderState.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +float64 encoder_1 +float64 encoder_2 \ No newline at end of file diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index 4846a5f97..c995d103a 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -58,6 +58,7 @@ set(${PROJECT_NAME}_sources src/scanner_v2.cpp src/io_state.cpp src/laserscan.cpp + src/encoder_state.cpp src/data_conversion_layer/monitoring_frame_msg.cpp src/data_conversion_layer/start_request.cpp src/data_conversion_layer/start_request_serialization.cpp diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h new file mode 100644 index 000000000..c2b603182 --- /dev/null +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -0,0 +1,57 @@ +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H +#define PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H + +#include +#include + +#include +#include + +#include "psen_scan_v2_standalone/util/format_range.h" + +namespace psen_scan_v2_standalone +{ +namespace data_conversion_layer +{ +namespace monitoring_frame +{ +/** + * @brief Contains all types, etc. needed to describe the encoder information contained + * in a data_conversion_layer::monitoring_frame::Message. + */ +namespace encoder +{ + +struct EncoderData +{ + double encoder_1; + double encoder_2; +}; + +inline std::ostream& operator<<(std::ostream& os, const EncoderData& ed) +{ + return os << fmt::format("EncoderData(encoder_1 = {}, encoder_2 = {})", + ed.encoder_1, + ed.encoder_2); +} + +} // namespace encoder +} // namespace monitoring_frame +} // namespace data_conversion_layer +} // namespace psen_scan_v2_standalone +#endif // PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h index 87da9768a..4d032a32e 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h @@ -102,7 +102,7 @@ inline LaserScan LaserScanConverter::toLaserScan( std::vector measurements; std::vector intensities; std::vector io_states; - std::vector encoderData; + std::vector encoder_states; for (auto index : sorted_stamped_msgs_indices) { @@ -132,13 +132,18 @@ inline LaserScan LaserScanConverter::toLaserScan( io_states.emplace_back(single_msg.msg_.iOPinData(), single_msg.stamp_); } - // Create the encoder_data as reception order. - if(single_msg.msg_.hasEncoderDataField()) + // Fill the data with the encoder information as reception order. + if (single_msg.msg_.hasEncoderDataField()) { - encoderData.insert(encoderData.end(), - single_msg.msg_.encoderData().begin(), - single_msg.msg_.encoderData().end()); - } + PSENSCAN_DEBUG("encoder_states: ", + "stamp_: {} fromTheta: {} encoder_1: {} encoder_2: {}", + single_msg.stamp_, + std::to_string(single_msg.msg_.fromTheta().toRad()), + single_msg.msg_.encoderData().encoder_1, + single_msg.msg_.encoderData().encoder_2); + + encoder_states.emplace_back(single_msg.msg_.encoderData(), single_msg.stamp_); + } } LaserScan scan(stamped_msgs[0].msg_.resolution(), @@ -151,7 +156,7 @@ inline LaserScan LaserScanConverter::toLaserScan( scan.measurements(measurements); scan.intensities(intensities); scan.ioStates(io_states); - scan.encoderData(encoderData); + scan.encoderStates(encoder_states); return scan; } diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h index 998765546..c9e33d6ba 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h @@ -60,7 +60,7 @@ static constexpr uint16_t NUMBER_OF_BYTES_SINGLE_MEASUREMENT{ 2 }; static constexpr uint16_t NO_SIGNAL_ARRIVED{ 59956 }; static constexpr uint16_t SIGNAL_TOO_LATE{ 59958 }; static constexpr uint16_t NUMBER_OF_BYTES_SINGLE_INTENSITY{ 2 }; -static constexpr uint16_t NUMBER_OF_BYTES_ENCODER_DATA{ 2 }; +static constexpr uint16_t NUMBER_OF_BYTES_ENCODER_DATA{ 4 }; /** * @brief The information included in every single monitoring frame. diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h index c31d3ad3d..9c800513f 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h @@ -26,6 +26,7 @@ #include "psen_scan_v2_standalone/configuration/scanner_ids.h" #include "psen_scan_v2_standalone/data_conversion_layer/diagnostics.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" namespace psen_scan_v2_standalone @@ -76,7 +77,7 @@ class Message //! @throw AdditionalFieldMissing if intensities were missing during deserialization of a Message. const std::vector& intensities() const; //! @throw AdditionalFieldMissing if encoder data were missing during deserialization of a Message. - const std::vector& encoderData() const; + const encoder::EncoderData& encoderData() const; //! @throw AdditionalFieldMissing if diagnostic_messages were missing during deserialization of a Message. std::vector diagnosticMessages() const; @@ -99,7 +100,7 @@ class Message boost::optional io_pin_data_; boost::optional> measurements_; boost::optional> intensities_; - boost::optional> encoder_data_; + boost::optional encoder_data_; boost::optional> diagnostic_messages_; public: diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h index 63bcd9211..a45961310 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h @@ -46,7 +46,7 @@ class MessageBuilder MessageBuilder& intensities(const std::vector& intensities); MessageBuilder& diagnosticMessages(const std::vector& diagnostic_messages); MessageBuilder& iOPinData(const io::PinData& io_pin_data); - MessageBuilder& encoderData(const std::vector& encoder_data); + MessageBuilder& encoderData(const encoder::EncoderData& encoder_data); private: Message msg_; @@ -115,7 +115,8 @@ inline MessageBuilder& MessageBuilder::iOPinData(const io::PinData& io_pin_data) msg_.io_pin_data_ = io_pin_data; return *this; } -inline MessageBuilder& MessageBuilder::encoderData(const std::vector& encoder_data) + +inline MessageBuilder& MessageBuilder::encoderData(const encoder::EncoderData& encoder_data) { msg_.encoder_data_ = encoder_data; return *this; diff --git a/standalone/include/psen_scan_v2_standalone/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h new file mode 100644 index 000000000..428daa9e3 --- /dev/null +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -0,0 +1,47 @@ +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_STATE_H +#define PSEN_SCAN_V2_STANDALONE_ENCODER_STATE_H + +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" + +namespace psen_scan_v2_standalone +{ +//! @brief Represents the set of encoder reading states. +class EncoderState +{ +public: + EncoderState() = default; + EncoderState(data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data, const int64_t& timestamp); + //! @return double containing the reading of the encoder 1. + double getEncoder_1() const; + //! @return double containing the reading of the encoder 2. + double getEncoder_2() const; + //! @return time[ns] of the monitoring frame this state is linked to. + int64_t timestamp() const; + +private: + data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data_{}; + int64_t timestamp_{}; + +public: + friend std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state); +}; + +std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state); +} // namespace psen_scan_v2_standalone + +#endif // PSEN_SCAN_V2_STANDALONE_IO_STATE_H diff --git a/standalone/include/psen_scan_v2_standalone/laserscan.h b/standalone/include/psen_scan_v2_standalone/laserscan.h index 0ad25c249..f1d81361f 100644 --- a/standalone/include/psen_scan_v2_standalone/laserscan.h +++ b/standalone/include/psen_scan_v2_standalone/laserscan.h @@ -21,6 +21,7 @@ #include #include "psen_scan_v2_standalone/io_state.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" namespace psen_scan_v2_standalone @@ -39,6 +40,7 @@ namespace psen_scan_v2_standalone * - ID of the currently active zoneset. * - Time of the first scan ray. * - All states of the I/O pins recorded during the scan. + * - All states of the encoders reading during the scan. * * The measures use the target frame defined as \. * @see https://github.com/PilzDE/psen_scan_v2_standalone/blob/main/README.md#tf-frames @@ -49,7 +51,7 @@ class LaserScan using MeasurementData = std::vector; using IntensityData = std::vector; using IOData = std::vector; - using EncoderData = std::vector; + using EncoderData = std::vector; public: LaserScan(const util::TenthOfDegree& resolution, @@ -119,13 +121,13 @@ class LaserScan void ioStates(const IOData& io_states); /*! deprecated: use const EncoderData& encoderData() instead */ - [[deprecated("use const EncoderData& encoderData() const instead")]] const EncoderData& getEncoderData() const; - const EncoderData& encoderData() const; + [[deprecated("use const EncoderData& encoderData() const instead")]] const EncoderData& getEncoderStates() const; + const EncoderData& encoderStates() const; /*! deprecated: use void encoderData(const EncoderData& encoderData) instead */ [[deprecated("use void encoderData(const EncoderData& encoderData) instead")]] void - setEncoderData(const EncoderData& encoderData); - void encoderData(const EncoderData& encoderData); + setEncoderStates(const EncoderData& encoder_states); + void encoderStates(const EncoderData& encoder_states); private: //! Measurement data of the laserscan (in Millimeters). @@ -147,7 +149,7 @@ class LaserScan //! Time of the first ray in this scan round (or fragment if fragmented_scans is enabled). const int64_t timestamp_; //! Stores the received normalized encoder signals. - EncoderData encoderData_; + EncoderData encoder_states_; }; std::ostream& operator<<(std::ostream& os, const LaserScan& scan); diff --git a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp index a2dd5640c..dd1796673 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -23,12 +23,14 @@ #include "psen_scan_v2_standalone/data_conversion_layer/diagnostics.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h" #include "psen_scan_v2_standalone/data_conversion_layer/raw_processing.h" #include "psen_scan_v2_standalone/data_conversion_layer/raw_scanner_data.h" #include "psen_scan_v2_standalone/io_state.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/util/logging.h" namespace psen_scan_v2_standalone @@ -159,10 +161,22 @@ monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data break; } case AdditionalFieldHeaderID::encoder: { - const size_t num_encoder_values{ static_cast(additional_header.length()) / - NUMBER_OF_BYTES_ENCODER_DATA }; - std::vector encoder_data; - raw_processing::readArray(ss, encoder_data, num_encoder_values, toEncoder); + if (additional_header.length() != NUMBER_OF_BYTES_ENCODER_DATA) + { + throw AdditionalFieldUnexpectedSize(fmt::format("Length of zone set field is {}, but should be {}.", + additional_header.length(), + NUMBER_OF_BYTES_ENCODER_DATA)); + } + uint16_t encoder_1_read_buffer; + raw_processing::read(ss, encoder_1_read_buffer); + + uint16_t encoder_2_read_buffer; + raw_processing::read(ss, encoder_2_read_buffer); + + encoder::EncoderData encoder_data; + encoder_data.encoder_1 = (double)encoder_1_read_buffer; + encoder_data.encoder_2 = (double)encoder_2_read_buffer; + msg_builder.encoderData(encoder_data); break; } diff --git a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp index aeeaa61ef..1d0cbf4d8 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp @@ -103,7 +103,7 @@ const std::vector& Message::intensities() const } } -const std::vector& Message::encoderData() const +const encoder::EncoderData& Message::encoderData() const { if (encoder_data_.is_initialized()) { diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp new file mode 100644 index 000000000..b35300423 --- /dev/null +++ b/standalone/src/encoder_state.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#include "psen_scan_v2_standalone/encoder_state.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" + +namespace psen_scan_v2_standalone +{ +EncoderState::EncoderState(data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data, + const int64_t& timestamp) + : encoder_data_(encoder_data), timestamp_(timestamp) +{ +} + +double EncoderState::getEncoder_1() const +{ + return encoder_data_.encoder_1; +} + +double EncoderState::getEncoder_2() const +{ + return encoder_data_.encoder_2; +} + +int64_t EncoderState::timestamp() const +{ + return timestamp_; +} + +std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state) +{ + return os << "EncoderState(timestamp = " << encoder_state.timestamp_ << " nsec, " << encoder_state.encoder_data_ + << ")"; +} + +} // namespace psen_scan_v2_standalone diff --git a/standalone/src/laserscan.cpp b/standalone/src/laserscan.cpp index fb492b725..8e5b44b61 100644 --- a/standalone/src/laserscan.cpp +++ b/standalone/src/laserscan.cpp @@ -119,15 +119,6 @@ void LaserScan::setIntensities(const IntensityData& intensities) this->intensities(intensities); } -const LaserScan::IntensityData& LaserScan::getEncoderData() const -{ - return this->encoderData(); -} - -void LaserScan::setEncoderData(const EncoderData& encoderData) -{ - this->encoderData(encoderData); -} // LCOV_EXCL_STOP const util::TenthOfDegree& LaserScan::minScanAngle() const @@ -180,14 +171,24 @@ void LaserScan::intensities(const IntensityData& intensities) intensities_ = intensities; } -const LaserScan::EncoderData& LaserScan::encoderData() const +const LaserScan::EncoderData& LaserScan::getEncoderStates() const +{ + return encoderStates(); +} + +const LaserScan::EncoderData& LaserScan::encoderStates() const +{ + return encoder_states_; +} + +void LaserScan::setEncoderStates(const EncoderData& encoder_states) { - return encoderData_; + encoderStates(encoder_states); } -void LaserScan::encoderData(const EncoderData& encoderData) +void LaserScan::encoderStates(const EncoderData& encoder_states) { - encoderData_ = encoderData; + encoder_states_ = encoder_states; } // LCOV_EXCL_START @@ -215,7 +216,7 @@ const LaserScan::IOData& LaserScan::ioStates() const std::ostream& operator<<(std::ostream& os, const LaserScan& scan) { os << fmt::format("LaserScan(timestamp = {} nsec, scanCounter = {}, minScanAngle = {} deg, maxScanAngle = {} deg, " - "resolution = {} deg, active_zoneset = {}, measurements = {}, intensities = {}, io_states = {})", + "resolution = {} deg, active_zoneset = {}, measurements = {}, intensities = {}, io_states = {}, encoder_states = {})", scan.timestamp(), scan.scanCounter(), scan.minScanAngle().value() / 10., @@ -224,7 +225,8 @@ std::ostream& operator<<(std::ostream& os, const LaserScan& scan) scan.activeZoneset(), util::formatRange(scan.measurements()), util::formatRange(scan.intensities()), - util::formatRange(scan.ioStates())); + util::formatRange(scan.ioStates()), + util::formatRange(scan.encoderStates())); return os; } From 71fb0aac6ab326132d54e5f00c0fec818b6aa207 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Wed, 17 Aug 2022 09:52:39 +0200 Subject: [PATCH 03/27] touch up some files --- CMakeLists.txt | 2 +- include/psen_scan_v2/encoder_state_ros_conversion.h | 4 ++-- include/psen_scan_v2/ros_scanner_node.h | 2 +- launch/bringup.launch | 4 ++-- src/psen_scan_driver.cpp | 2 +- standalone/CMakeLists.txt | 2 +- .../configuration/default_parameters.h | 2 +- .../data_conversion_layer/encoder_data.h | 2 +- .../data_conversion_layer/monitoring_frame_msg.h | 2 +- .../monitoring_frame_msg_builder.h | 2 +- .../data_conversion_layer/start_request.h | 2 +- .../include/psen_scan_v2_standalone/encoder_state.h | 4 ++-- .../include/psen_scan_v2_standalone/laserscan.h | 7 +------ .../psen_scan_v2_standalone/scanner_config_builder.h | 2 +- .../psen_scan_v2_standalone/scanner_configuration.h | 2 +- .../monitoring_frame_deserialization.cpp | 4 ++-- .../data_conversion_layer/monitoring_frame_msg.cpp | 2 +- .../src/data_conversion_layer/start_request.cpp | 2 +- .../start_request_serialization.cpp | 2 +- standalone/src/encoder_state.cpp | 2 +- standalone/src/laserscan.cpp | 12 +----------- 21 files changed, 25 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8404445e1..097cc1f85 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (c) 2020-2021 Pilz GmbH & Co. KG +# Copyright (c) 2020-2022 Pilz GmbH & Co. KG # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU Lesser General Public License as published by diff --git a/include/psen_scan_v2/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h index 9244deda0..554691d87 100644 --- a/include/psen_scan_v2/encoder_state_ros_conversion.h +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// Copyright (c) 2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by @@ -46,4 +46,4 @@ psen_scan_v2::EncoderState toEncoderStateMsg(const psen_scan_v2_standalone::Enco } // namespace psen_scan_v2 -#endif // PSEN_SCAN_V2_IO_STATE_ROS_CONVERSIONS_H +#endif // PSEN_SCAN_V2_ENCODER_STATE_ROS_CONVERSIONS_H diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index 9be085788..3d8fd8230 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -128,7 +128,7 @@ ROSScannerNodeT::ROSScannerNodeT(ros::NodeHandle& nh, pub_scan_ = nh_.advertise(topic, 1); pub_zone_ = nh_.advertise("active_zoneset", 1); pub_io_ = nh_.advertise("io_state", 6, true /* latched */); - pub_encoder_ = nh.advertise("encoder_estate", 6, true); + pub_encoder_ = nh.advertise("encoder_state", 6, true); } template diff --git a/launch/bringup.launch b/launch/bringup.launch index 9f032b5b9..e042829df 100644 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -1,5 +1,5 @@ - + diff --git a/src/psen_scan_driver.cpp b/src/psen_scan_driver.cpp index 08c662f1c..de54bffea 100644 --- a/src/psen_scan_driver.cpp +++ b/src/psen_scan_driver.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index c995d103a..662069387 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (c) 2020-2021 Pilz GmbH & Co. KG +# Copyright (c) 2020-2022 Pilz GmbH & Co. KG # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h b/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h index e16a1338f..973a23c3b 100644 --- a/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h +++ b/standalone/include/psen_scan_v2_standalone/configuration/default_parameters.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h index c2b603182..633a52ab4 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// Copyright (c) 2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h index 9c800513f..1eb6ad45b 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h index a45961310..9aa7a4867 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Pilz GmbH & Co. KG +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h index 076901927..7df97f871 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/start_request.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h index 428daa9e3..259d1acf4 100644 --- a/standalone/include/psen_scan_v2_standalone/encoder_state.h +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// Copyright (c) 2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by @@ -44,4 +44,4 @@ class EncoderState std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state); } // namespace psen_scan_v2_standalone -#endif // PSEN_SCAN_V2_STANDALONE_IO_STATE_H +#endif // PSEN_SCAN_V2_STANDALONE_ENCODER_STATE_H diff --git a/standalone/include/psen_scan_v2_standalone/laserscan.h b/standalone/include/psen_scan_v2_standalone/laserscan.h index f1d81361f..1ed7720b2 100644 --- a/standalone/include/psen_scan_v2_standalone/laserscan.h +++ b/standalone/include/psen_scan_v2_standalone/laserscan.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by @@ -120,13 +120,8 @@ class LaserScan [[deprecated("use void ioStates(const IOData& io_states) instead")]] void setIOStates(const IOData& io_states); void ioStates(const IOData& io_states); - /*! deprecated: use const EncoderData& encoderData() instead */ - [[deprecated("use const EncoderData& encoderData() const instead")]] const EncoderData& getEncoderStates() const; const EncoderData& encoderStates() const; - /*! deprecated: use void encoderData(const EncoderData& encoderData) instead */ - [[deprecated("use void encoderData(const EncoderData& encoderData) instead")]] void - setEncoderStates(const EncoderData& encoder_states); void encoderStates(const EncoderData& encoder_states); private: diff --git a/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h b/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h index ec0c18e89..77d5d6bdd 100644 --- a/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h +++ b/standalone/include/psen_scan_v2_standalone/scanner_config_builder.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/include/psen_scan_v2_standalone/scanner_configuration.h b/standalone/include/psen_scan_v2_standalone/scanner_configuration.h index 8c4e46d42..d70f03de1 100644 --- a/standalone/include/psen_scan_v2_standalone/scanner_configuration.h +++ b/standalone/include/psen_scan_v2_standalone/scanner_configuration.h @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp index dd1796673..93d2fa3d2 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -174,8 +174,8 @@ monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data raw_processing::read(ss, encoder_2_read_buffer); encoder::EncoderData encoder_data; - encoder_data.encoder_1 = (double)encoder_1_read_buffer; - encoder_data.encoder_2 = (double)encoder_2_read_buffer; + encoder_data.encoder_1 = toEncoder(encoder_1_read_buffer); + encoder_data.encoder_2 = toEncoder(encoder_2_read_buffer); msg_builder.encoderData(encoder_data); break; diff --git a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp index 1d0cbf4d8..7b63f3390 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/src/data_conversion_layer/start_request.cpp b/standalone/src/data_conversion_layer/start_request.cpp index f2446b1ba..d319fe848 100644 --- a/standalone/src/data_conversion_layer/start_request.cpp +++ b/standalone/src/data_conversion_layer/start_request.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/src/data_conversion_layer/start_request_serialization.cpp b/standalone/src/data_conversion_layer/start_request_serialization.cpp index a9d89b933..6b63f15df 100644 --- a/standalone/src/data_conversion_layer/start_request_serialization.cpp +++ b/standalone/src/data_conversion_layer/start_request_serialization.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp index b35300423..c6c6627e8 100644 --- a/standalone/src/encoder_state.cpp +++ b/standalone/src/encoder_state.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021-2022 Pilz GmbH & Co. KG +// Copyright (c) 2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by diff --git a/standalone/src/laserscan.cpp b/standalone/src/laserscan.cpp index 8e5b44b61..b3f56b435 100644 --- a/standalone/src/laserscan.cpp +++ b/standalone/src/laserscan.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020-2021 Pilz GmbH & Co. KG +// Copyright (c) 2020-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by @@ -171,21 +171,11 @@ void LaserScan::intensities(const IntensityData& intensities) intensities_ = intensities; } -const LaserScan::EncoderData& LaserScan::getEncoderStates() const -{ - return encoderStates(); -} - const LaserScan::EncoderData& LaserScan::encoderStates() const { return encoder_states_; } -void LaserScan::setEncoderStates(const EncoderData& encoder_states) -{ - encoderStates(encoder_states); -} - void LaserScan::encoderStates(const EncoderData& encoder_states) { encoder_states_ = encoder_states; From 66fc87d762344fa26d8390865e2997535eca8f8f Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Thu, 25 Aug 2022 11:38:02 +0200 Subject: [PATCH 04/27] Initial version of tests made for the encoder development --- CMakeLists.txt | 27 ++++++++++ include/psen_scan_v2/ros_scanner_node.h | 1 + standalone/CMakeLists.txt | 12 +++++ .../psen_scan_v2_standalone/encoder_state.h | 2 + standalone/src/encoder_state.cpp | 6 +++ .../util/integrationtest_helper.h | 3 ++ .../util/matchers_and_actions.h | 15 +++++- .../api/integrationtest_scanner_api.cpp | 2 + .../monitoring_frame_serialization.cpp | 13 +++++ .../unit_tests/api/unittest_encoder_state.cpp | 52 +++++++++++++++++++ .../unit_tests/api/unittest_laserscan.cpp | 29 ++++++++++- .../unittest_scanner_configuration.cpp | 16 ++++++ .../unittest_laserscan_conversions.cpp | 42 ++++++++++++++- .../unittest_monitoring_frame_msg.cpp | 20 +++++++ .../unittest_monitoring_frame_msg_stamped.cpp | 5 +- ...ng_frame_serialization_deserialization.cpp | 4 ++ .../psen_scan_v2/ros_integrationtest_helper.h | 8 ++- .../integrationtest_ros_scanner_node.cpp | 41 +++++++++++++++ .../unittest_encoder_state_rosconversions.cpp | 46 ++++++++++++++++ 19 files changed, 337 insertions(+), 7 deletions(-) create mode 100644 standalone/test/unit_tests/api/unittest_encoder_state.cpp create mode 100644 test/unit_tests/unittest_encoder_state_rosconversions.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 097cc1f85..2de1abcae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -342,6 +342,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(unittest_laserscan standalone/test/unit_tests/api/unittest_laserscan.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp ) target_link_libraries(unittest_laserscan @@ -367,6 +368,15 @@ if(CATKIN_ENABLE_TESTING) fmt::fmt ) + catkin_add_gtest(unittest_encoder_state + standalone/test/unit_tests/api/unittest_encoder_state.cpp + standalone/src/encoder_state.cpp + ) + target_link_libraries(unittest_encoder_state + ${catkin_LIBRARIES} + fmt::fmt + ) + catkin_add_gtest(unittest_io_state_conversions standalone/test/unit_tests/data_conversion_layer/unittest_io_state_conversions.cpp standalone/src/io_state.cpp @@ -387,6 +397,7 @@ if(CATKIN_ENABLE_TESTING) standalone/test/unit_tests/data_conversion_layer/unittest_laserscan_conversions.cpp standalone/src/io_state.cpp standalone/src/laserscan.cpp + standalone/src/encoder_state.cpp standalone/src/data_conversion_layer/monitoring_frame_msg.cpp standalone/src/data_conversion_layer/diagnostics.cpp ) @@ -398,6 +409,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(unittest_laserscan_ros_conversions test/unit_tests/unittest_laserscan_ros_conversions.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp standalone/src/data_conversion_layer/monitoring_frame_msg.cpp standalone/src/data_conversion_layer/diagnostics.cpp @@ -507,6 +519,16 @@ if(CATKIN_ENABLE_TESTING) ) add_dependencies(unittest_io_state_rosconversions ${${PROJECT_NAME}_EXPORTED_TARGETS}) + catkin_add_gmock(unittest_encoder_state_rosconversions + test/unit_tests/unittest_encoder_state_rosconversions.cpp + standalone/src/encoder_state.cpp + ) + target_link_libraries(unittest_encoder_state_rosconversions + ${catkin_LIBRARIES} + fmt::fmt + ) + add_dependencies(unittest_encoder_state_rosconversions ${${PROJECT_NAME}_EXPORTED_TARGETS}) + catkin_add_gmock(unittest_zoneset_to_marker_conversion test/unit_tests/unittest_zoneset_to_marker_conversion.cpp ) @@ -536,6 +558,7 @@ if(CATKIN_ENABLE_TESTING) standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp standalone/src/scanner_v2.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp standalone/src/data_conversion_layer/monitoring_frame_msg.cpp standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -554,6 +577,7 @@ if(CATKIN_ENABLE_TESTING) test/integration_tests/integrationtest_ros_scanner_node.test test/integration_tests/integrationtest_ros_scanner_node.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp standalone/src/data_conversion_layer/monitoring_frame_msg.cpp standalone/src/data_conversion_layer/diagnostics.cpp @@ -720,6 +744,7 @@ if(CATKIN_ENABLE_TESTING) test/hw_tests/hwtest_scan_compare.test test/hw_tests/hwtest_scan_compare.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp ) add_dependencies(hwtest_scan_compare @@ -732,6 +757,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_executable_with_gtest(hwtest_scan_compare test/hw_tests/hwtest_scan_compare.cpp standalone/src/io_state.cpp + standalone/src/encoder_state.cpp standalone/src/laserscan.cpp EXCLUDE_FROM_ALL ) @@ -790,6 +816,7 @@ if(CATKIN_ENABLE_TESTING) "*/OutputPinState.h" # generated message "*/ZoneSet.h" # generated message "*/ZoneSetConfiguration.h" # generated message + "*/EncoderState.h" # generated message ) add_code_coverage( NAME ${PROJECT_NAME}_coverage diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index 3d8fd8230..c83a35412 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -109,6 +109,7 @@ class ROSScannerNodeT FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStopFuture); FRIEND_TEST(RosScannerNodeTests, shouldPublishChangedIOStatesEqualToConversionOfSuppliedStandaloneIOStates); FRIEND_TEST(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic); + FRIEND_TEST(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedLaserScan); FRIEND_TEST(RosScannerNodeTests, shouldLogChangedIOStates); }; diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index 662069387..24c430d46 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -371,5 +371,17 @@ target_link_libraries(integrationtest_udp_client add_test(NAME integrationtest_udp_client COMMAND integrationtest_udp_client) +ADD_EXECUTABLE(unittest_encoder_state + test/unit_tests/api/unittest_encoder_state.cpp + src/io_state.cpp) + +TARGET_LINK_LIBRARIES(unittest_encoder_state + ${PROJECT_NAME} + gtest +) + +ADD_TEST(NAME unittest_encoder_state + COMMAND unittest_encoder_state) + endif () endif () diff --git a/standalone/include/psen_scan_v2_standalone/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h index 259d1acf4..70e40e8ff 100644 --- a/standalone/include/psen_scan_v2_standalone/encoder_state.h +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -33,6 +33,8 @@ class EncoderState //! @return time[ns] of the monitoring frame this state is linked to. int64_t timestamp() const; + bool operator==(const EncoderState& encoder_state) const; + private: data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data_{}; int64_t timestamp_{}; diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp index c6c6627e8..cd1ce36d8 100644 --- a/standalone/src/encoder_state.cpp +++ b/standalone/src/encoder_state.cpp @@ -45,4 +45,10 @@ std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state) << ")"; } +bool EncoderState::operator==(const EncoderState& encoder_state) const +{ + return encoder_data_.encoder_1 == encoder_state.getEncoder_1() && + encoder_data_.encoder_2 == encoder_state.getEncoder_2(); +} + } // namespace psen_scan_v2_standalone diff --git a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h index 102d415a2..fe9be7a37 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h @@ -34,6 +34,7 @@ #include "psen_scan_v2_standalone/util/format_range.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" #include "psen_scan_v2_standalone/io_state.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/laserscan.h" #include "psen_scan_v2_standalone/scan_range.h" @@ -97,6 +98,8 @@ createMonitoringFrameMsgBuilderWithoutDiagnostics(const util::TenthOfDegree star msg_builder.iOPinData(createPinData()); + msg_builder.encoderData({ 12.005, 25.876 }); + return msg_builder; } diff --git a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h index aeb644853..4f9c6b637 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h @@ -76,6 +76,12 @@ MATCHER_P(IOStateEq, io_state, "") io_state.timestamp() == arg.timestamp(); } +MATCHER_P(EncoderStateEq, encoder_state, "") +{ + return encoder_state.getEncoder_1() == arg.getEncoder_1() && encoder_state.getEncoder_2() == arg.getEncoder_2() && + encoder_state.timestamp() == arg.timestamp(); +} + MATCHER_P(PointwiseIOStateEq, vec, "") { return std::equal(vec.begin(), vec.end(), arg.begin(), arg.end(), [result_listener](const auto& a, const auto& b) { @@ -120,6 +126,12 @@ MATCHER_P(IOPinDataEq, ref_pin, "") Matches(Pointwise(Eq(), ref_pin.output_state))(arg.output_state); } +MATCHER_P(EncoderDataEq, ref_encoder, "") +{ + return ref_encoder.encoder_1 == arg.encoder_1 && + ref_encoder.encoder_2 == arg.encoder_2; +} + MATCHER_P(MonitoringFrameEq, reference_msg, "") { return arg.fromTheta() == reference_msg.fromTheta() && arg.resolution() == reference_msg.resolution() && @@ -127,7 +139,8 @@ MATCHER_P(MonitoringFrameEq, reference_msg, "") Matches(PointwiseDoubleEq(reference_msg.measurements()))(arg.measurements()) && Matches(PointwiseDoubleEq(reference_msg.intensities()))(arg.intensities()) && Matches(Pointwise(Eq(), reference_msg.diagnosticMessages()))(arg.diagnosticMessages()) && - Matches(IOPinDataEq(reference_msg.iOPinData()))(arg.iOPinData()); + Matches(IOPinDataEq(reference_msg.iOPinData()))(arg.iOPinData()) && + Matches(EncoderDataEq(reference_msg.encoderData()))(arg.encoderData()); } } // namespace psen_scan_v2_standalone_test diff --git a/standalone/test/integration_tests/api/integrationtest_scanner_api.cpp b/standalone/test/integration_tests/api/integrationtest_scanner_api.cpp index bb6feaec1..62920733d 100644 --- a/standalone/test/integration_tests/api/integrationtest_scanner_api.cpp +++ b/standalone/test/integration_tests/api/integrationtest_scanner_api.cpp @@ -53,6 +53,7 @@ static const bool FRAGMENTED_SCAN{ true }; static const bool UNFRAGMENTED_SCAN{ false }; static const std::string HOST_IP_ADDRESS{ "127.0.0.1" }; static const std::string SCANNER_IP_ADDRESS{ "127.0.0.1" }; +static const bool ENCODER_ENABLED{ true }; static constexpr std::chrono::milliseconds FUTURE_WAIT_TIMEOUT{ 10 }; @@ -148,6 +149,7 @@ ScannerConfiguration ScannerAPITests::generateScannerConfig(const std::string& h .scanRange(DEFAULT_SCAN_RANGE) .scanResolution(DEFAULT_SCAN_RESOLUTION) .enableIntensities() + .enableEncoder(ENCODER_ENABLED) .enableFragmentedScans(fragmented); } diff --git a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp index cd55e1b76..6f936b600 100644 --- a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp +++ b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp @@ -100,6 +100,19 @@ RawData serialize(const data_conversion_layer::monitoring_frame::Message& msg) os, msg.intensities(), [](double elem) { return (static_cast(std::round(elem))); }); } + if (msg.hasEncoderDataField()) + { + AdditionalFieldHeader encoder_header(static_cast(AdditionalFieldHeaderID::encoder), + NUMBER_OF_BYTES_ENCODER_DATA); + write(os, encoder_header); + + uint16_t encoder_1_payload = static_cast(msg.encoderData().encoder_1); + raw_processing::write(os, encoder_1_payload); + + uint16_t encoder_2_payload = static_cast(msg.encoderData().encoder_2); + raw_processing::write(os, encoder_2_payload); + } + AdditionalFieldHeader::Id end_of_frame_header_id = static_cast(AdditionalFieldHeaderID::end_of_frame); data_conversion_layer::raw_processing::write(os, end_of_frame_header_id); diff --git a/standalone/test/unit_tests/api/unittest_encoder_state.cpp b/standalone/test/unit_tests/api/unittest_encoder_state.cpp new file mode 100644 index 000000000..b52400163 --- /dev/null +++ b/standalone/test/unit_tests/api/unittest_encoder_state.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#include + +#include "psen_scan_v2_standalone/encoder_state.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" + +namespace psen_scan_v2_standalone_test +{ +using namespace psen_scan_v2_standalone; +using data_conversion_layer::monitoring_frame::encoder::EncoderData; + +TEST(EncoderStateTests, shouldReturnCorrectTimestamp) +{ + const EncoderState encoder_state{ EncoderData{}, 42 /*timestamp*/ }; + EXPECT_EQ(42, encoder_state.timestamp()); +} + +TEST(EncoderStateTests, shouldReturnCorrectValueForEncoder1) +{ + const EncoderState encoder_state{ EncoderData{ 12.0005 /*encoder_1*/, 25.786 /*encoder_2*/ }, 42 /*timestamp*/ }; + const auto encoder_1 = encoder_state.getEncoder_1(); + EXPECT_DOUBLE_EQ(encoder_1, 12.0005); +} + +TEST(EncoderStateTests, shouldReturnCorrectValueForEncoder2) +{ + const EncoderState encoder_state{ EncoderData{ 12.0005 /*encoder_1*/, 25.786 /*encoder_2*/ }, 42 /*timestamp*/ }; + const auto encoder_2 = encoder_state.getEncoder_2(); + EXPECT_DOUBLE_EQ(encoder_2, 25.786); +} + +} // namespace psen_scan_v2_standalone_test + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/standalone/test/unit_tests/api/unittest_laserscan.cpp b/standalone/test/unit_tests/api/unittest_laserscan.cpp index e6860dfde..769660183 100644 --- a/standalone/test/unit_tests/api/unittest_laserscan.cpp +++ b/standalone/test/unit_tests/api/unittest_laserscan.cpp @@ -22,8 +22,10 @@ #include #include "psen_scan_v2_standalone/io_state.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/laserscan.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data_helper.h" @@ -174,6 +176,21 @@ TEST(LaserScanTest, testSetAndGetIOStates) EXPECT_EQ(laser_scan->ioStates()[0].timestamp(), 42); } +TEST(LaserScanTest, testSetAndGetEncoderStates) +{ + LaserScanBuilder laser_scan_builder; + std::unique_ptr laser_scan; + ASSERT_NO_THROW(laser_scan.reset(new LaserScan(laser_scan_builder.build()));); + + EncoderState state{ { 12.005, 25.876 }, 65 /*timestamp*/ }; + std::vector encoder_states{ state }; + laser_scan->encoderStates(encoder_states); + + EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_1(), state.getEncoder_1()); + EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_2(), state.getEncoder_2()); + EXPECT_EQ(laser_scan->encoderStates()[0].timestamp(), 65); +} + TEST(LaserScanTest, testPrintMessageSuccess) { LaserScanBuilder laser_scan_builder; @@ -183,6 +200,10 @@ TEST(LaserScanTest, testPrintMessageSuccess) laser_scan->measurements({ 45.0, 44.0, 43.0, 42.0 }); laser_scan->ioStates({ IOState(createPinData(), 41 /*timestamp*/) }); + EncoderState state{ { 12.005, 25.876 }, 41 /*timestamp*/ }; + std::vector encoder_states{ state }; + laser_scan->encoderStates(encoder_states); + // For compatibility with different ubuntu versions (resp. fmt), we need to take account of changes in // the default formatting of floating point numbers #if (FMT_VERSION >= 60000 && FMT_VERSION < 70100) @@ -191,14 +212,18 @@ TEST(LaserScanTest, testPrintMessageSuccess) "LaserScan(timestamp = 1 nsec, scanCounter = 1, minScanAngle = 0.1 deg, maxScanAngle = 0.2 deg, resolution " "= 0.1 deg, active_zoneset = 2, measurements = {45.0, 44.0, 43.0, 42.0}, intensities = {}, io_states = " "{IOState(timestamp = 41 nsec, io::PinData(input = {01001101, 00000000, 00000000, 00000000, 10011010, 00000000, " - "00000000, 00000000}, output = {01010101, 00000000, 00000000, 00000000}))})"); + "00000000, 00000000}, output = {01010101, 00000000, 00000000, 00000000}))}, encoder_states = " + "{EncoderState(timestamp " + "= 41 nsec, EncoderData(encoder_1 = 12.005, encoder_2 = 25.876))})"); #else EXPECT_EQ( fmt::format("{}", *laser_scan), "LaserScan(timestamp = 1 nsec, scanCounter = 1, minScanAngle = 0.1 deg, maxScanAngle = 0.2 deg, resolution " "= 0.1 deg, active_zoneset = 2, measurements = {45, 44, 43, 42}, intensities = {}, io_states = " "{IOState(timestamp = 41 nsec, io::PinData(input = {01001101, 00000000, 00000000, 00000000, 10011010, 00000000, " - "00000000, 00000000}, output = {01010101, 00000000, 00000000, 00000000}))})"); + "00000000, 00000000}, output = {01010101, 00000000, 00000000, 00000000}))}, encoder_states = " + "{EncoderState(timestamp " + "= 41 nsec, EncoderData(encoder_1 = 12.005, encoder_2 = 25.876))})"); #endif } diff --git a/standalone/test/unit_tests/configuration/unittest_scanner_configuration.cpp b/standalone/test/unit_tests/configuration/unittest_scanner_configuration.cpp index 61ad11c38..f15dcac56 100644 --- a/standalone/test/unit_tests/configuration/unittest_scanner_configuration.cpp +++ b/standalone/test/unit_tests/configuration/unittest_scanner_configuration.cpp @@ -59,6 +59,7 @@ static ScannerConfiguration createValidConfig() .scanResolution(SCAN_RESOLUTION) .enableDiagnostics() .enableIntensities() + .enableEncoder(true) .build(); } @@ -75,6 +76,7 @@ static ScannerConfiguration createValidConfig(const std::string& host_ip) .enableDiagnostics() .enableIntensities() .enableFragmentedScans(true) + .enableEncoder(true) .build(); } @@ -316,6 +318,20 @@ TEST_F(ScannerConfigurationTest, shouldThrowInvalidArgumentWithResolutionViolati EXPECT_THROW(sb.scanResolution(util::TenthOfDegree{ 101u }), std::invalid_argument); } +TEST_F(ScannerConfigurationTest, shouldReturnCorrectEncoderFlagAfterDefaultConstruction) +{ + const ScannerConfiguration config{ createValidDefaultConfig() }; + + EXPECT_FALSE(config.encoderEnabled()); +} + +TEST_F(ScannerConfigurationTest, shouldReturnCorrectEncoderFlagAfterValidConstruction) +{ + const ScannerConfiguration config{ createValidConfig()}; + + EXPECT_TRUE(config.encoderEnabled()); +} + } // namespace psen_scan_v2_standalone_test int main(int argc, char* argv[]) diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_laserscan_conversions.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_laserscan_conversions.cpp index e2e2c8fa0..135e80cc6 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_laserscan_conversions.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_laserscan_conversions.cpp @@ -22,6 +22,7 @@ #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" #include "psen_scan_v2_standalone/io_state.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/laserscan.h" #include "psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h" @@ -42,6 +43,7 @@ .activeZoneset(msg.activeZoneset()) \ .measurements(msg.measurements()) \ .intensities(msg.intensities()) \ + .encoderData(msg.encoderData()) \ .property(msg.property() + offset) using namespace psen_scan_v2_standalone; @@ -78,7 +80,8 @@ static double addOffsetToMsgMeasurement(data_conversion_layer::monitoring_frame: .activeZoneset(msg.activeZoneset()) .iOPinData(msg.iOPinData()) .measurements(measurements_copy) - .intensities(msg.intensities()); + .intensities(msg.intensities()) + .encoderData(msg.encoderData()); return measurements_copy.at(index); } @@ -91,7 +94,8 @@ static MessageBuilder createDefaultMsgBuilder() .activeZoneset(0) .iOPinData(createPinData()) .measurements({ 1., 2., 3., 4.5, 5., 42., .4 }) - .intensities({ 0., 4., 3., 1007., 508., 14000., .4 }); + .intensities({ 0., 4., 3., 1007., 508., 14000., .4 }) + .encoderData({ 12.005, 25.876 }); } static MessageStamped createDefaultStampedMsg(const int64_t timestamp = DEFAULT_TIMESTAMP, uint32_t msg_nr = 0) @@ -219,6 +223,24 @@ TEST(LaserScanConversionsTest, laserScanShouldContainCorrectIOStateAfterConversi EXPECT_THAT(scan_ptr->ioStates().at(0), IOStateFromStampedMsg(stamped_msg)); } +MATCHER_P(EncoderStateFromStampedMsg, stamped_msg, "") +{ + return ExplainMatchResult( + EncoderStateEq(EncoderState(stamped_msg.msg_.encoderData(), stamped_msg.stamp_)), arg, result_listener); +} + +TEST(LaserScanConversionsTest, laserScanShouldContainCorrectEncoderStateAfterConversion) +{ + const auto stamped_msg{ createDefaultStampedMsg() }; + + std::unique_ptr scan_ptr; + ASSERT_NO_THROW( + scan_ptr.reset(new LaserScan{ data_conversion_layer::LaserScanConverter::toLaserScan({ stamped_msg }) });); + + ASSERT_EQ(scan_ptr->encoderStates().size(), 1u); + EXPECT_THAT(scan_ptr->encoderStates().at(0), EncoderStateFromStampedMsg(stamped_msg)); +} + ///////////////////////////////////////// // Test Cases with Multiple Messages // ///////////////////////////////////////// @@ -317,6 +339,22 @@ TEST(LaserScanConversionsTest, laserScanShouldContainAllIOStates) } } +TEST(LaserScanConversionsTest, laserScanShouldContainAllEncoderStates) +{ + const size_t msg_count = 6; + const auto stamped_msgs = createValidStampedMsgs(msg_count); + + std::unique_ptr scan_ptr; + ASSERT_NO_THROW( + scan_ptr.reset(new LaserScan{ data_conversion_layer::LaserScanConverter::toLaserScan(stamped_msgs) });); + + ASSERT_EQ(scan_ptr->encoderStates().size(), msg_count); + for (size_t i = 0; i < msg_count; i++) + { + EXPECT_THAT(scan_ptr->encoderStates().at(i), EncoderStateFromStampedMsg(stamped_msgs.at(i))); + } +} + TEST(LaserScanConversionsTest, laserScanShouldContainAllIOStatesInCorrectOrder) { const size_t msg_count = 3; diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp index 0c0a58758..64d83ffe6 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp @@ -68,6 +68,12 @@ TEST(MonitoringFrameMsgTest, shouldThrowAdditionalFieldMissingWhenTryingToGetUns FrameMessage().iOPinData(), AdditionalFieldMissing, ("IO pin data" + ADDITIONAL_FIELD_MISSING_TEXT).c_str()); } +TEST(MonitoringFrameMsgTest, shouldThrowAdditionalFieldMissingWhenTryingToGetUnsetEncoderData) +{ + EXPECT_THROW_AND_WHAT( + FrameMessage().encoderData(), AdditionalFieldMissing, ("Encoder data" + ADDITIONAL_FIELD_MISSING_TEXT).c_str()); +} + TEST(MonitoringFrameMsgTest, shouldThrowAdditionalFieldMissingWhenTryingToGetUnsetDiagnosticMessages) { EXPECT_THROW_AND_WHAT(FrameMessage().diagnosticMessages(), @@ -111,6 +117,12 @@ TEST(MonitoringFrameMsgTest, shouldReturnCorrectStateOfIOPin) EXPECT_TRUE(MessageBuilder().iOPinData(io::PinData()).build().hasIOPinField()); } +TEST(MonitoringFrameMsgTest, shouldReturnCorrectStateOfEncoderData) +{ + EXPECT_FALSE(MessageBuilder().build().hasEncoderDataField()); + EXPECT_TRUE(MessageBuilder().encoderData(encoder::EncoderData()).build().hasEncoderDataField()); +} + TEST(MonitoringFrameMsgTest, shouldReturnCorrectScannerId) { const auto scanner_id{ configuration::ScannerId::subscriber0 }; @@ -171,6 +183,14 @@ TEST(MonitoringFrameMsgTest, shouldReturnCorrectIOPin) EXPECT_THAT(io_pin_data, IOPinDataEq(expected_io_pin_data)); } +TEST(MonitoringFrameMsgTest, shouldReturnCorrectEncoderData) +{ + encoder::EncoderData expected_encoder_data{ 12.005, 25.876 }; + encoder::EncoderData encoder_data; + ASSERT_NO_THROW(encoder_data = MessageBuilder().encoderData(expected_encoder_data).build().encoderData()); + EXPECT_THAT(encoder_data, EncoderDataEq(expected_encoder_data)); +} + TEST(MonitoringFrameMsgTest, shouldReturnCorrectDiagnosticMessages) { const std::vector expected_diagnostic_messages{ { diagnostic::Message( diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp index 936962bc6..2adcb87c3 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp @@ -39,6 +39,8 @@ static data_conversion_layer::monitoring_frame::Message createMsg() io_pin_data.input_state.at(0).set(3); io_pin_data.output_state.at(0).reset(1); + data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data{ 12.005, 25.876 }; + return data_conversion_layer::monitoring_frame::MessageBuilder() .fromTheta(util::TenthOfDegree{ 10 }) .resolution(util::TenthOfDegree{ 90 }) @@ -49,7 +51,8 @@ static data_conversion_layer::monitoring_frame::Message createMsg() .diagnosticMessages({ data_conversion_layer::monitoring_frame::diagnostic::Message{ configuration::ScannerId::master, data_conversion_layer::monitoring_frame::diagnostic::ErrorLocation(1, 7) } }) - .iOPinData(io_pin_data); + .iOPinData(io_pin_data) + .encoderData(encoder_data); } TEST(MonitoringFrameMsgStampedTest, testMsg) diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_serialization_deserialization.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_serialization_deserialization.cpp index eb88d5ad5..a99fd8603 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_serialization_deserialization.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_serialization_deserialization.cpp @@ -144,6 +144,8 @@ TEST(MonitoringFrameSerializationTest, shouldSerializeAndDeserializeFrameConsist pin_data.input_state.at(5).set(4); pin_data.output_state.at(0).set(5); + data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data{ 12, 25 }; + auto msg = monitoring_frame::MessageBuilder() .fromTheta(util::TenthOfDegree(25)) @@ -153,6 +155,7 @@ TEST(MonitoringFrameSerializationTest, shouldSerializeAndDeserializeFrameConsist .measurements({ 10, 20, std::numeric_limits::infinity(), 40 }) .intensities({ 15, 25, 35, 45 }) .iOPinData(pin_data) + .encoderData(encoder_data) .diagnosticMessages( { monitoring_frame::diagnostic::Message(configuration::ScannerId::master, error_locations.at(0)), monitoring_frame::diagnostic::Message(configuration::ScannerId::master, error_locations.at(1)), @@ -174,6 +177,7 @@ TEST(MonitoringFrameSerializationTest, shouldFailOnSerializeAndDeserializeFrameW .activeZoneset(0) .measurements({ 0 }) .intensities({ 70045 }) + .encoderData({ 0, 0 }) .diagnosticMessages({}) .build(); diff --git a/test/include/psen_scan_v2/ros_integrationtest_helper.h b/test/include/psen_scan_v2/ros_integrationtest_helper.h index 7ffced10f..ef10a0382 100644 --- a/test/include/psen_scan_v2/ros_integrationtest_helper.h +++ b/test/include/psen_scan_v2/ros_integrationtest_helper.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Pilz GmbH & Co. KG +// Copyright (c) 2021-2022 Pilz GmbH & Co. KG // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by @@ -103,6 +103,12 @@ MATCHER_P(IOStateMsgEq, msg, "") arg.output == msg.output; } +MATCHER_P(EncoderStateMsgEq, msg, "") +{ + return ::testing::Matches(StdMsgsHeaderEq(msg.header))(arg.header) && arg.encoder_1 == msg.encoder_1 && + arg.encoder_2 == msg.encoder_2; +} + MATCHER_P(LaserScanMsgEq, msg, "") { return ::testing::Matches(StdMsgsHeaderEq(msg.header))(arg.header) && arg.angle_min == msg.angle_min && diff --git a/test/integration_tests/integrationtest_ros_scanner_node.cpp b/test/integration_tests/integrationtest_ros_scanner_node.cpp index 3c773130a..bf9b7ba97 100644 --- a/test/integration_tests/integrationtest_ros_scanner_node.cpp +++ b/test/integration_tests/integrationtest_ros_scanner_node.cpp @@ -62,6 +62,7 @@ static constexpr ScanRange SCAN_RANGE{ util::TenthOfDegree(1), util::TenthOfDegr static constexpr std::chrono::seconds DEFAULT_TIMEOUT{ 3 }; static constexpr std::chrono::seconds LOOP_END_TIMEOUT{ 4 }; static constexpr std::chrono::seconds STOP_TIMEOUT{ 1 }; +static const bool ENABLE_ENCODER{ true }; static const psen_scan_v2_standalone::LaserScan::IOData IO_DATA1{ { psen_scan_v2_standalone::IOState(createPinData({ 0, 0, 0, 0, 0, 0, 0, 0 }, { 6, 0, 0, 0 }), 0 /*timestamp*/), @@ -72,6 +73,10 @@ static const psen_scan_v2_standalone::LaserScan::IOData IO_DATA2{ psen_scan_v2_standalone::IOState(createPinData({ 0, 0, 0, 0, 64, 0, 0, 0 }, { 1, 0, 0, 0 }), 0 /*timestamp*/) } }; +static const psen_scan_v2_standalone::LaserScan::EncoderData ENCODER_DATA{ psen_scan_v2_standalone::EncoderState( + psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::encoder::EncoderData{ 12.00, 25 }, + 41 /*timestamp*/) }; + static void setDefaultActions(ScannerMock& mock, util::Barrier& start_barrier) { ON_CALL(mock, start()).WillByDefault(DoAll(OpenBarrier(&start_barrier), ReturnReadyVoidFuture())); @@ -86,6 +91,7 @@ static ScannerConfiguration createValidConfig() .hostControlPort(HOST_UDP_PORT_CONTROL) .scannerDataPort(configuration::DATA_PORT_OF_SCANNER_DEVICE) .scannerControlPort(configuration::CONTROL_PORT_OF_SCANNER_DEVICE) + .enableEncoder(ENABLE_ENCODER) .scanRange(SCAN_RANGE); } @@ -174,6 +180,12 @@ TEST_F(RosScannerNodeTests, shouldProvideIOTopic) EXPECT_TRUE(TopicExists("/integrationtest_ros_scanner_node/io_state")); } +TEST_F(RosScannerNodeTests, shouldProvideEncoderTopic) +{ + ROSScannerNodeT ros_scanner_node(nh_priv_, "scan", "scanner", 1.0 /*x_axis_rotation*/, scanner_config_); + EXPECT_TRUE(TopicExists("/integrationtest_ros_scanner_node/encoder_state")); +} + const std::string SCAN_TOPICNAME{ "scan" }; TEST_F(RosScannerNodeTests, shouldPublishScansWhenLaserScanCallbackIsInvoked) @@ -232,6 +244,35 @@ TEST_F(RosScannerNodeTests, shouldPublishActiveZonesetWhenLaserScanCallbackIsInv loop.wait_for(LOOP_END_TIMEOUT); } +const std::string ENCODER_TOPICNAME{ "encoder_state" }; + +TEST_F(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedLaserScan) +{ + const std::string prefix = "scanner"; + ROSScannerNodeT ros_scanner_node( + nh_priv_, SCAN_TOPICNAME, prefix, 1.0 /*x_axis_rotation*/, scanner_config_); + + util::Barrier start_barrier; + setDefaultActions(ros_scanner_node.scanner_, start_barrier); + + util::Barrier encoder_topic_barrier; + SubscriberMock subscriber(nh_priv_, ENCODER_TOPICNAME, QUEUE_SIZE); + auto scan = createValidLaserScan(); + EXPECT_CALL(subscriber, callback(EncoderStateMsgEq(toEncoderStateMsg(ENCODER_DATA.at(0), prefix)))) + .WillOnce(OpenBarrier(&encoder_topic_barrier)); + + std::future loop = std::async(std::launch::async, [&ros_scanner_node]() { ros_scanner_node.run(); }); + + ASSERT_BARRIER_OPENS(start_barrier, DEFAULT_TIMEOUT) << "Scanner start was not called"; + + scan.encoderStates(ENCODER_DATA); + ros_scanner_node.scanner_.invokeLaserScanCallback(scan); + encoder_topic_barrier.waitTillRelease(DEFAULT_TIMEOUT); + + ros_scanner_node.terminate(); + loop.wait_for(LOOP_END_TIMEOUT); +} + TEST_F(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic) { ROSScannerNodeT ros_scanner_node(nh_priv_, "scan", "scanner", 1.0 /*x_axis_rotation*/, scanner_config_); diff --git a/test/unit_tests/unittest_encoder_state_rosconversions.cpp b/test/unit_tests/unittest_encoder_state_rosconversions.cpp new file mode 100644 index 000000000..2dd10ba18 --- /dev/null +++ b/test/unit_tests/unittest_encoder_state_rosconversions.cpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Pilz GmbH & Co. KG +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#include + +#include + +#include "psen_scan_v2/EncoderState.h" +#include "psen_scan_v2_standalone/encoder_state.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" + +#include "psen_scan_v2/encoder_state_ros_conversion.h" + +using namespace psen_scan_v2; +using namespace psen_scan_v2_standalone; + +namespace psen_scan_v2_test +{ +TEST(EncoderStateROSConversionsTest, shouldConvertSuccessfully) +{ + psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data{ 12.00, 25.876 }; + psen_scan_v2_standalone::EncoderState encoder_state{ encoder_data, 56 /*timestamp*/ }; + + EXPECT_NO_THROW(psen_scan_v2::EncoderState ros_message = toEncoderStateMsg(encoder_state, "some_frame")); +} + +} // namespace psen_scan_v2_test + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + ros::Time::init(); + return RUN_ALL_TESTS(); +} From 5efbf6d95974a7815b900448459f8dd182e86ef9 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:37:35 +0100 Subject: [PATCH 05/27] Update include/psen_scan_v2/encoder_state_ros_conversion.h I agree. Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- include/psen_scan_v2/encoder_state_ros_conversion.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/psen_scan_v2/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h index 554691d87..27a2a7ea2 100644 --- a/include/psen_scan_v2/encoder_state_ros_conversion.h +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -20,6 +20,7 @@ #include #include "psen_scan_v2/EncoderState.h" +#include "psen_scan_v2_standalone/encoder_state.h" namespace psen_scan_v2 { From 555ce76be93994f5551cfe86bf4633943fdfb617 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:41:00 +0100 Subject: [PATCH 06/27] Sure Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- include/psen_scan_v2/encoder_state_ros_conversion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/psen_scan_v2/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h index 27a2a7ea2..e28a69194 100644 --- a/include/psen_scan_v2/encoder_state_ros_conversion.h +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -32,7 +32,7 @@ psen_scan_v2::EncoderState toEncoderStateMsg(const psen_scan_v2_standalone::Enco if (encoder_state.timestamp() < 0) { - throw std::invalid_argument("IOState of Laserscan message has an invalid timestamp: " + + throw std::invalid_argument("EncoderState of Laserscan message has an invalid timestamp: " + std::to_string(encoder_state.timestamp())); } From e3b44b92eb1a6c895c970c6665a6dc268900f3c4 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:47:36 +0100 Subject: [PATCH 07/27] include header missed Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- include/psen_scan_v2/ros_scanner_node.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index c83a35412..e1d22841e 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -37,6 +37,7 @@ #include "psen_scan_v2/laserscan_ros_conversions.h" #include "psen_scan_v2/io_state_ros_conversion.h" #include "psen_scan_v2/encoder_state_ros_conversion.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" #include "psen_scan_v2_standalone/util/format_range.h" From 160e44ba1cc5e73aad34c3e47aca571760167217 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:51:27 +0100 Subject: [PATCH 08/27] Update launch/psen_scan_v2.launch Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- launch/psen_scan_v2.launch | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/psen_scan_v2.launch b/launch/psen_scan_v2.launch index bb70107f3..fe0bc1383 100644 --- a/launch/psen_scan_v2.launch +++ b/launch/psen_scan_v2.launch @@ -27,7 +27,6 @@ along with this program. If not, see . - From c1513873e0b0f635bd99a3e81fb50db92d94f044 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:51:41 +0100 Subject: [PATCH 09/27] Update include/psen_scan_v2/ros_scanner_node.h Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- include/psen_scan_v2/ros_scanner_node.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/psen_scan_v2/ros_scanner_node.h b/include/psen_scan_v2/ros_scanner_node.h index e1d22841e..3af8cbd33 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -110,7 +110,7 @@ class ROSScannerNodeT FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStopFuture); FRIEND_TEST(RosScannerNodeTests, shouldPublishChangedIOStatesEqualToConversionOfSuppliedStandaloneIOStates); FRIEND_TEST(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic); - FRIEND_TEST(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedLaserScan); + FRIEND_TEST(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedStandaloneEncoderData); FRIEND_TEST(RosScannerNodeTests, shouldLogChangedIOStates); }; From 7ec7090b6f3ff6a3a245fd003ac89acf4c2fcbb5 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:52:10 +0100 Subject: [PATCH 10/27] Update msg/EncoderState.msg Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- msg/EncoderState.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/EncoderState.msg b/msg/EncoderState.msg index 443c31212..94a2eb5ff 100644 --- a/msg/EncoderState.msg +++ b/msg/EncoderState.msg @@ -1,3 +1,3 @@ std_msgs/Header header float64 encoder_1 -float64 encoder_2 \ No newline at end of file +float64 encoder_2 From 30b1acaaa90cbe50028efd9fe2f5a566ebc4acd8 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:53:28 +0100 Subject: [PATCH 11/27] Update src/psen_scan_driver.cpp Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- src/psen_scan_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/psen_scan_driver.cpp b/src/psen_scan_driver.cpp index de54bffea..78e3e5dc3 100644 --- a/src/psen_scan_driver.cpp +++ b/src/psen_scan_driver.cpp @@ -109,7 +109,7 @@ int main(int argc, char** argv) if (scanner_configuration.encoderEnabled()) { - ROS_INFO("Reading data from encoders"); + ROS_INFO("Reading data from encoders."); } ROSScannerNode ros_scanner_node(pnh, From 546c19d24626033d97d6f898af0cd8f345e2a7a9 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:54:19 +0100 Subject: [PATCH 12/27] Update standalone/CMakeLists.txt Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- standalone/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index 24c430d46..71e952864 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -372,8 +372,7 @@ add_test(NAME integrationtest_udp_client COMMAND integrationtest_udp_client) ADD_EXECUTABLE(unittest_encoder_state - test/unit_tests/api/unittest_encoder_state.cpp - src/io_state.cpp) + test/unit_tests/api/unittest_encoder_state.cpp) TARGET_LINK_LIBRARIES(unittest_encoder_state ${PROJECT_NAME} From d2b962041e4bee79eb3491b25fca436b1b24c47c Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:55:56 +0100 Subject: [PATCH 13/27] Update standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- .../psen_scan_v2_standalone/data_conversion_layer/encoder_data.h | 1 - 1 file changed, 1 deletion(-) diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h index 633a52ab4..ecb6a9e65 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -16,7 +16,6 @@ #ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H #define PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H -#include #include #include From 8b8853886350f31884d8e822854ce44486ca67d3 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:56:28 +0100 Subject: [PATCH 14/27] Update standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- .../psen_scan_v2_standalone/data_conversion_layer/encoder_data.h | 1 - 1 file changed, 1 deletion(-) diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h index ecb6a9e65..14e46fe7b 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -21,7 +21,6 @@ #include #include -#include "psen_scan_v2_standalone/util/format_range.h" namespace psen_scan_v2_standalone { From 5a11f5cc6b6f18dd0e3f0093d32a6de5c02b41a5 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:57:43 +0100 Subject: [PATCH 15/27] Update standalone/include/psen_scan_v2_standalone/laserscan.h Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- standalone/include/psen_scan_v2_standalone/laserscan.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/standalone/include/psen_scan_v2_standalone/laserscan.h b/standalone/include/psen_scan_v2_standalone/laserscan.h index 1ed7720b2..7971f24d5 100644 --- a/standalone/include/psen_scan_v2_standalone/laserscan.h +++ b/standalone/include/psen_scan_v2_standalone/laserscan.h @@ -40,7 +40,7 @@ namespace psen_scan_v2_standalone * - ID of the currently active zoneset. * - Time of the first scan ray. * - All states of the I/O pins recorded during the scan. - * - All states of the encoders reading during the scan. + * - All states of the encoders read during the scan. * * The measures use the target frame defined as \. * @see https://github.com/PilzDE/psen_scan_v2_standalone/blob/main/README.md#tf-frames From 629b26be6132aabbc42c73b30ad56c82f2879ff5 Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 11:58:40 +0100 Subject: [PATCH 16/27] Update standalone/src/data_conversion_layer/start_request_serialization.cpp Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- .../src/data_conversion_layer/start_request_serialization.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/standalone/src/data_conversion_layer/start_request_serialization.cpp b/standalone/src/data_conversion_layer/start_request_serialization.cpp index 6b63f15df..f88e9eaaa 100644 --- a/standalone/src/data_conversion_layer/start_request_serialization.cpp +++ b/standalone/src/data_conversion_layer/start_request_serialization.cpp @@ -85,9 +85,8 @@ RawData data_conversion_layer::start_request::serialize(const data_conversion_la const uint8_t active_zone_set_enabled{ 0b00001000 }; const uint8_t io_pin_data_enabled{ 0b00001000 }; const uint8_t scan_counter_enabled{ 0b00001000 }; - //const uint8_t speed_encoder_enabled{ 0 }; /**< 0000000bin disabled, 00001111bin enabled.*/ const uint8_t speed_encoder_enabled{ static_cast( - msg.master_device_settings_.encoderDataEnabled() ? 0b00001000 : 0b00000000) }; + msg.master_device_settings_.encoderDataEnabled() ? 0b00001111 : 0b00000000) }; const uint8_t diagnostics_enabled{ static_cast( msg.master_device_settings_.diagnosticsEnabled() ? 0b00001000 : 0b00000000) }; From 35dbfcb81a77e566ad2e174fdf97b6682c23ddbe Mon Sep 17 00:00:00 2001 From: PilzES <46567535+PilzES@users.noreply.github.com> Date: Fri, 11 Nov 2022 12:01:51 +0100 Subject: [PATCH 17/27] Update standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h Co-authored-by: Immanuel Martini <35950815+martiniil@users.noreply.github.com> --- .../data_conversion_layer/monitoring_frame_deserialization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h index c9e33d6ba..45e016b53 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h @@ -141,7 +141,7 @@ enum class AdditionalFieldHeaderID : AdditionalFieldHeader::Id diagnostics = 0x04, measurements = 0x05, intensities = 0x06, - encoder = 0x07, + encoder = 0x07, end_of_frame = 0x09 }; From df7398718164ffc2788489012201763b84addc10 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Fri, 11 Nov 2022 12:44:12 +0100 Subject: [PATCH 18/27] update integrationtest_ros_scanner_node.cpp --- test/integration_tests/integrationtest_ros_scanner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration_tests/integrationtest_ros_scanner_node.cpp b/test/integration_tests/integrationtest_ros_scanner_node.cpp index bf9b7ba97..a518d3cd5 100644 --- a/test/integration_tests/integrationtest_ros_scanner_node.cpp +++ b/test/integration_tests/integrationtest_ros_scanner_node.cpp @@ -246,7 +246,7 @@ TEST_F(RosScannerNodeTests, shouldPublishActiveZonesetWhenLaserScanCallbackIsInv const std::string ENCODER_TOPICNAME{ "encoder_state" }; -TEST_F(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedLaserScan) +TEST_F(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedStandaloneEncoderData) { const std::string prefix = "scanner"; ROSScannerNodeT ros_scanner_node( From 6c993ef618f2a046fa8d666559928db29a1c7a1a Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Mon, 14 Nov 2022 12:47:43 +0100 Subject: [PATCH 19/27] Solve minor things of feature encoder data --- standalone/CMakeLists.txt | 21 ++++++++++--------- .../data_conversion_layer/encoder_data.h | 6 +++--- .../psen_scan_v2_standalone/encoder_state.h | 4 ++-- standalone/src/encoder_state.cpp | 8 ++----- .../util/integrationtest_helper.h | 2 +- .../unit_tests/api/unittest_laserscan.cpp | 1 + .../unittest_monitoring_frame_msg.cpp | 1 + .../unittest_monitoring_frame_msg_stamped.cpp | 1 + .../integrationtest_ros_scanner_node.cpp | 2 ++ 9 files changed, 24 insertions(+), 22 deletions(-) diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index 71e952864..f343654da 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -344,6 +344,17 @@ ADD_TEST(NAME unittest_udp_client COMMAND unittest_udp_client) +ADD_EXECUTABLE(unittest_encoder_state/test/unit_tests/api/unittest_encoder_state.cpp) + +TARGET_LINK_LIBRARIES(unittest_encoder_state + ${PROJECT_NAME} + gtest +) + +ADD_TEST(NAME unittest_encoder_state + COMMAND unittest_encoder_state) + + add_executable(integrationtest_scanner_api test/integration_tests/api/integrationtest_scanner_api.cpp test/src/communication_layer/mock_udp_server.cpp @@ -371,16 +382,6 @@ target_link_libraries(integrationtest_udp_client add_test(NAME integrationtest_udp_client COMMAND integrationtest_udp_client) -ADD_EXECUTABLE(unittest_encoder_state - test/unit_tests/api/unittest_encoder_state.cpp) - -TARGET_LINK_LIBRARIES(unittest_encoder_state - ${PROJECT_NAME} - gtest -) - -ADD_TEST(NAME unittest_encoder_state - COMMAND unittest_encoder_state) endif () endif () diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h index 14e46fe7b..8da0777f4 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -13,8 +13,8 @@ // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . -#ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H -#define PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H +#ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_DATA_H +#define PSEN_SCAN_V2_STANDALONE_ENCODER_DATA_H #include @@ -52,4 +52,4 @@ inline std::ostream& operator<<(std::ostream& os, const EncoderData& ed) } // namespace monitoring_frame } // namespace data_conversion_layer } // namespace psen_scan_v2_standalone -#endif // PSEN_SCAN_V2_STANDALONE_ENCODER_PIN_DATA_H +#endif // PSEN_SCAN_V2_STANDALONE_ENCODER_DATA_H diff --git a/standalone/include/psen_scan_v2_standalone/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h index 70e40e8ff..277f55729 100644 --- a/standalone/include/psen_scan_v2_standalone/encoder_state.h +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -16,6 +16,8 @@ #ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_STATE_H #define PSEN_SCAN_V2_STANDALONE_ENCODER_STATE_H +#include + #include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" namespace psen_scan_v2_standalone @@ -33,8 +35,6 @@ class EncoderState //! @return time[ns] of the monitoring frame this state is linked to. int64_t timestamp() const; - bool operator==(const EncoderState& encoder_state) const; - private: data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data_{}; int64_t timestamp_{}; diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp index cd1ce36d8..f64fc3837 100644 --- a/standalone/src/encoder_state.cpp +++ b/standalone/src/encoder_state.cpp @@ -13,6 +13,8 @@ // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . +#include + #include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" @@ -45,10 +47,4 @@ std::ostream& operator<<(std::ostream& os, const EncoderState& encoder_state) << ")"; } -bool EncoderState::operator==(const EncoderState& encoder_state) const -{ - return encoder_data_.encoder_1 == encoder_state.getEncoder_1() && - encoder_data_.encoder_2 == encoder_state.getEncoder_2(); -} - } // namespace psen_scan_v2_standalone diff --git a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h index fe9be7a37..88239f65b 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h @@ -31,10 +31,10 @@ #include "psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/util/format_range.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" #include "psen_scan_v2_standalone/io_state.h" -#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/laserscan.h" #include "psen_scan_v2_standalone/scan_range.h" diff --git a/standalone/test/unit_tests/api/unittest_laserscan.cpp b/standalone/test/unit_tests/api/unittest_laserscan.cpp index 769660183..d681934e3 100644 --- a/standalone/test/unit_tests/api/unittest_laserscan.cpp +++ b/standalone/test/unit_tests/api/unittest_laserscan.cpp @@ -186,6 +186,7 @@ TEST(LaserScanTest, testSetAndGetEncoderStates) std::vector encoder_states{ state }; laser_scan->encoderStates(encoder_states); + ASSERT_EQ(laser_scan->encoderStates().size(), 1u); EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_1(), state.getEncoder_1()); EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_2(), state.getEncoder_2()); EXPECT_EQ(laser_scan->encoderStates()[0].timestamp(), 65); diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp index 64d83ffe6..3a8f1afdb 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg.cpp @@ -20,6 +20,7 @@ #include "psen_scan_v2_standalone/configuration/scanner_ids.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" diff --git a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp index 2adcb87c3..a1105e09f 100644 --- a/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp +++ b/standalone/test/unit_tests/data_conversion_layer/unittest_monitoring_frame_msg_stamped.cpp @@ -21,6 +21,7 @@ #include "psen_scan_v2_standalone/configuration/scanner_ids.h" #include "psen_scan_v2_standalone/data_conversion_layer/diagnostics.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h" #include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg_builder.h" #include "psen_scan_v2_standalone/util/tenth_of_degree.h" diff --git a/test/integration_tests/integrationtest_ros_scanner_node.cpp b/test/integration_tests/integrationtest_ros_scanner_node.cpp index a518d3cd5..2caf73f09 100644 --- a/test/integration_tests/integrationtest_ros_scanner_node.cpp +++ b/test/integration_tests/integrationtest_ros_scanner_node.cpp @@ -27,6 +27,8 @@ #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" #include "psen_scan_v2_standalone/data_conversion_layer/io_pin_data_helper.h" +#include "psen_scan_v2_standalone/data_conversion_layer/encoder_data.h" +#include "psen_scan_v2_standalone/encoder_state.h" #include "psen_scan_v2_standalone/scanner_configuration.h" #include "psen_scan_v2_standalone/scanner_config_builder.h" #include "psen_scan_v2_standalone/configuration/default_parameters.h" From 073181ea4f0a3cf5386d73999e3c6cc15d3a94e4 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Mon, 14 Nov 2022 16:37:40 +0100 Subject: [PATCH 20/27] Add new unit tests for encoder state rosconversions. --- .../unittest_encoder_state_rosconversions.cpp | 31 +++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/test/unit_tests/unittest_encoder_state_rosconversions.cpp b/test/unit_tests/unittest_encoder_state_rosconversions.cpp index 2dd10ba18..7c4576ac7 100644 --- a/test/unit_tests/unittest_encoder_state_rosconversions.cpp +++ b/test/unit_tests/unittest_encoder_state_rosconversions.cpp @@ -25,17 +25,44 @@ using namespace psen_scan_v2; using namespace psen_scan_v2_standalone; - +using data_conversion_layer::monitoring_frame::encoder::EncoderData; namespace psen_scan_v2_test { TEST(EncoderStateROSConversionsTest, shouldConvertSuccessfully) { - psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data{ 12.00, 25.876 }; + EncoderData encoder_data{ 12.00 /*encoder 1*/, 25.876 /*encoder 2*/}; psen_scan_v2_standalone::EncoderState encoder_state{ encoder_data, 56 /*timestamp*/ }; EXPECT_NO_THROW(psen_scan_v2::EncoderState ros_message = toEncoderStateMsg(encoder_state, "some_frame")); } +TEST(EncoderStateRosConversionsTest, shouldSetCorrectHeaderData) +{ + EncoderData encoder_data{ 16.40 /*encoder 1*/, 35.647 /*encoder 2*/}; + psen_scan_v2_standalone::EncoderState encoder_state(encoder_data , 10 /*timestamp*/); + psen_scan_v2::EncoderState ros_message = toEncoderStateMsg(encoder_state, "some_frame"); + + EXPECT_EQ(ros_message.header.stamp, ros::Time{}.fromNSec(10)); + EXPECT_EQ(ros_message.header.frame_id, "some_frame"); +} + +TEST(EncoderStateRosConversionsTest, shouldContainCorrectStates) +{ + EncoderData encoder_data{ 1.00 /*encoder 1*/, 45.876 /*encoder 2*/}; + psen_scan_v2_standalone::EncoderState encoder_state{encoder_data, 56 /*timestamp*/ }; + psen_scan_v2::EncoderState ros_message = toEncoderStateMsg(encoder_state, "some_frame"); + + EXPECT_EQ(ros_message.encoder_1, encoder_state.getEncoder_1()); + EXPECT_EQ(ros_message.encoder_2, encoder_state.getEncoder_2()); +} + +TEST(EncoderStateRosConversionsTest, shouldThrowOnNegativeTime) +{ + EncoderData encoder_data{ 12.00 /*encoder 1*/, 25.876 /*encoder 2*/}; + psen_scan_v2_standalone::EncoderState encoder_state(encoder_data, -10 /*timestamp*/); + EXPECT_THROW(toEncoderStateMsg(encoder_state, "some_frame"), std::invalid_argument); +} + } // namespace psen_scan_v2_test int main(int argc, char* argv[]) From 55257b0359f5d4db8ab971017f2cea4881df83d2 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 15 Nov 2022 12:04:39 +0100 Subject: [PATCH 21/27] Add encoder states at ScanDataEqual. --- .../util/integrationtest_helper.h | 2 +- .../util/matchers_and_actions.h | 14 +++++++++++--- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h index 88239f65b..6c45a4149 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/integrationtest_helper.h @@ -98,7 +98,7 @@ createMonitoringFrameMsgBuilderWithoutDiagnostics(const util::TenthOfDegree star msg_builder.iOPinData(createPinData()); - msg_builder.encoderData({ 12.005, 25.876 }); + msg_builder.encoderData({ 12, 25 }); return msg_builder; } diff --git a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h index 4f9c6b637..1ea7c62f3 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h @@ -78,8 +78,8 @@ MATCHER_P(IOStateEq, io_state, "") MATCHER_P(EncoderStateEq, encoder_state, "") { - return encoder_state.getEncoder_1() == arg.getEncoder_1() && encoder_state.getEncoder_2() == arg.getEncoder_2() && - encoder_state.timestamp() == arg.timestamp(); + return Matches(DoubleEq(encoder_state.getEncoder_1()))(arg.getEncoder_1()) && + Matches(DoubleEq(encoder_state.getEncoder_2()))(arg.getEncoder_2()); } MATCHER_P(PointwiseIOStateEq, vec, "") @@ -89,13 +89,21 @@ MATCHER_P(PointwiseIOStateEq, vec, "") }); } +MATCHER_P(PointwiseEncoderStateEq, vec, "") +{ + return std::equal(vec.begin(), vec.end(), arg.begin(), arg.end(), [&](const auto& a, const auto& b) { + return Matches(EncoderStateEq(b))(a); + }); +} + MATCHER_P(ScanDataEqual, scan, "") { return arg.scanCounter() == scan.scanCounter() && arg.scanResolution() == scan.scanResolution() && arg.minScanAngle() == scan.minScanAngle() && arg.maxScanAngle() == scan.maxScanAngle() && Matches(PointwiseDoubleEq(scan.measurements()))(arg.measurements()) && Matches(PointwiseDoubleEq(scan.intensities()))(arg.intensities()) && - ExplainMatchResult(PointwiseIOStateEq(scan.ioStates()), arg.ioStates(), result_listener); + ExplainMatchResult(PointwiseIOStateEq(scan.ioStates()), arg.ioStates(), result_listener) && + Matches(PointwiseEncoderStateEq(scan.encoderStates()))(arg.encoderStates()); } MATCHER_P2(IOTimestampsInExpectedTimeframe, reference_ios, reference_timestamp, "") From fa4d3efbac3612b2d87a06f010a4430d607da8b8 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 15 Nov 2022 12:21:43 +0100 Subject: [PATCH 22/27] Small format change --- .../include/psen_scan_v2_standalone/util/matchers_and_actions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h index 1ea7c62f3..54d527a9b 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h @@ -91,7 +91,7 @@ MATCHER_P(PointwiseIOStateEq, vec, "") MATCHER_P(PointwiseEncoderStateEq, vec, "") { - return std::equal(vec.begin(), vec.end(), arg.begin(), arg.end(), [&](const auto& a, const auto& b) { + return std::equal(vec.begin(), vec.end(), arg.begin(), arg.end(), [&](const auto& a, const auto& b) { return Matches(EncoderStateEq(b))(a); }); } From f64b77edb5d255a3c68854147834befdf71d8f99 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 15 Nov 2022 12:40:48 +0100 Subject: [PATCH 23/27] Solve invalid case style. --- include/psen_scan_v2/encoder_state_ros_conversion.h | 4 ++-- standalone/include/psen_scan_v2_standalone/encoder_state.h | 4 ++-- standalone/src/encoder_state.cpp | 4 ++-- .../psen_scan_v2_standalone/util/matchers_and_actions.h | 4 ++-- standalone/test/unit_tests/api/unittest_encoder_state.cpp | 4 ++-- standalone/test/unit_tests/api/unittest_laserscan.cpp | 4 ++-- test/unit_tests/unittest_encoder_state_rosconversions.cpp | 4 ++-- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/psen_scan_v2/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h index e28a69194..083b6cb83 100644 --- a/include/psen_scan_v2/encoder_state_ros_conversion.h +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -39,8 +39,8 @@ psen_scan_v2::EncoderState toEncoderStateMsg(const psen_scan_v2_standalone::Enco ros_message.header.stamp = ros::Time{}.fromNSec(encoder_state.timestamp()); ros_message.header.frame_id = frame_id; - ros_message.encoder_1 = encoder_state.getEncoder_1(); - ros_message.encoder_2 = encoder_state.getEncoder_2(); + ros_message.encoder_1 = encoder_state.getEncoder1(); + ros_message.encoder_2 = encoder_state.getEncoder2(); return ros_message; } diff --git a/standalone/include/psen_scan_v2_standalone/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h index 277f55729..01549c21a 100644 --- a/standalone/include/psen_scan_v2_standalone/encoder_state.h +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -29,9 +29,9 @@ class EncoderState EncoderState() = default; EncoderState(data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data, const int64_t& timestamp); //! @return double containing the reading of the encoder 1. - double getEncoder_1() const; + double getEncoder1() const; //! @return double containing the reading of the encoder 2. - double getEncoder_2() const; + double getEncoder2() const; //! @return time[ns] of the monitoring frame this state is linked to. int64_t timestamp() const; diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp index f64fc3837..5368ba6ac 100644 --- a/standalone/src/encoder_state.cpp +++ b/standalone/src/encoder_state.cpp @@ -26,12 +26,12 @@ EncoderState::EncoderState(data_conversion_layer::monitoring_frame::encoder::Enc { } -double EncoderState::getEncoder_1() const +double EncoderState::getEncoder1() const { return encoder_data_.encoder_1; } -double EncoderState::getEncoder_2() const +double EncoderState::getEncoder2() const { return encoder_data_.encoder_2; } diff --git a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h index 54d527a9b..340ebea2f 100644 --- a/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h +++ b/standalone/test/include/psen_scan_v2_standalone/util/matchers_and_actions.h @@ -78,8 +78,8 @@ MATCHER_P(IOStateEq, io_state, "") MATCHER_P(EncoderStateEq, encoder_state, "") { - return Matches(DoubleEq(encoder_state.getEncoder_1()))(arg.getEncoder_1()) && - Matches(DoubleEq(encoder_state.getEncoder_2()))(arg.getEncoder_2()); + return Matches(DoubleEq(encoder_state.getEncoder1()))(arg.getEncoder1()) && + Matches(DoubleEq(encoder_state.getEncoder2()))(arg.getEncoder2()); } MATCHER_P(PointwiseIOStateEq, vec, "") diff --git a/standalone/test/unit_tests/api/unittest_encoder_state.cpp b/standalone/test/unit_tests/api/unittest_encoder_state.cpp index b52400163..22e8acaa4 100644 --- a/standalone/test/unit_tests/api/unittest_encoder_state.cpp +++ b/standalone/test/unit_tests/api/unittest_encoder_state.cpp @@ -32,14 +32,14 @@ TEST(EncoderStateTests, shouldReturnCorrectTimestamp) TEST(EncoderStateTests, shouldReturnCorrectValueForEncoder1) { const EncoderState encoder_state{ EncoderData{ 12.0005 /*encoder_1*/, 25.786 /*encoder_2*/ }, 42 /*timestamp*/ }; - const auto encoder_1 = encoder_state.getEncoder_1(); + const auto encoder_1 = encoder_state.getEncoder1(); EXPECT_DOUBLE_EQ(encoder_1, 12.0005); } TEST(EncoderStateTests, shouldReturnCorrectValueForEncoder2) { const EncoderState encoder_state{ EncoderData{ 12.0005 /*encoder_1*/, 25.786 /*encoder_2*/ }, 42 /*timestamp*/ }; - const auto encoder_2 = encoder_state.getEncoder_2(); + const auto encoder_2 = encoder_state.getEncoder2(); EXPECT_DOUBLE_EQ(encoder_2, 25.786); } diff --git a/standalone/test/unit_tests/api/unittest_laserscan.cpp b/standalone/test/unit_tests/api/unittest_laserscan.cpp index d681934e3..f888e0275 100644 --- a/standalone/test/unit_tests/api/unittest_laserscan.cpp +++ b/standalone/test/unit_tests/api/unittest_laserscan.cpp @@ -187,8 +187,8 @@ TEST(LaserScanTest, testSetAndGetEncoderStates) laser_scan->encoderStates(encoder_states); ASSERT_EQ(laser_scan->encoderStates().size(), 1u); - EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_1(), state.getEncoder_1()); - EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder_2(), state.getEncoder_2()); + EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder1(), state.getEncoder1()); + EXPECT_EQ(laser_scan->encoderStates()[0].getEncoder2(), state.getEncoder2()); EXPECT_EQ(laser_scan->encoderStates()[0].timestamp(), 65); } diff --git a/test/unit_tests/unittest_encoder_state_rosconversions.cpp b/test/unit_tests/unittest_encoder_state_rosconversions.cpp index 7c4576ac7..2e1e5be61 100644 --- a/test/unit_tests/unittest_encoder_state_rosconversions.cpp +++ b/test/unit_tests/unittest_encoder_state_rosconversions.cpp @@ -52,8 +52,8 @@ TEST(EncoderStateRosConversionsTest, shouldContainCorrectStates) psen_scan_v2_standalone::EncoderState encoder_state{encoder_data, 56 /*timestamp*/ }; psen_scan_v2::EncoderState ros_message = toEncoderStateMsg(encoder_state, "some_frame"); - EXPECT_EQ(ros_message.encoder_1, encoder_state.getEncoder_1()); - EXPECT_EQ(ros_message.encoder_2, encoder_state.getEncoder_2()); + EXPECT_EQ(ros_message.encoder_1, encoder_state.getEncoder1()); + EXPECT_EQ(ros_message.encoder_2, encoder_state.getEncoder2()); } TEST(EncoderStateRosConversionsTest, shouldThrowOnNegativeTime) From 842f074f984b5981bee481046b7f418a3e929b51 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 15 Nov 2022 15:45:02 +0100 Subject: [PATCH 24/27] Byte order review, swap between Big-Indian and Little-Indian. --- .../monitoring_frame_deserialization.h | 5 +++ .../data_conversion_layer/raw_processing.h | 8 +++++ .../monitoring_frame_deserialization.cpp | 34 +++++++++++++------ .../monitoring_frame_serialization.cpp | 2 ++ 4 files changed, 38 insertions(+), 11 deletions(-) diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h index 45e016b53..5df2591bf 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h @@ -154,6 +154,11 @@ namespace diagnostic std::vector deserializeMessages(std::istream& is); } +namespace encoder +{ + EncoderData deserializeEncoderData(std::istream& is); +} + namespace io { template diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h index e29c85ba4..76fe7029d 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h @@ -119,6 +119,14 @@ inline T toArray(std::ostringstream& os) return raw_data; } +// Reverse to change Little-endian to Big-endian +template +inline void endswap(T *objp) +{ + unsigned char *memp = reinterpret_cast(objp); + std::reverse(memp, memp + sizeof(T)); +} + inline StringStreamFailure::StringStreamFailure(const std::string& msg) : std::runtime_error(msg) { } diff --git a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp index 93d2fa3d2..455d901fe 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -167,17 +167,7 @@ monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data additional_header.length(), NUMBER_OF_BYTES_ENCODER_DATA)); } - uint16_t encoder_1_read_buffer; - raw_processing::read(ss, encoder_1_read_buffer); - - uint16_t encoder_2_read_buffer; - raw_processing::read(ss, encoder_2_read_buffer); - - encoder::EncoderData encoder_data; - encoder_data.encoder_1 = toEncoder(encoder_1_read_buffer); - encoder_data.encoder_2 = toEncoder(encoder_2_read_buffer); - - msg_builder.encoderData(encoder_data); + msg_builder.encoderData(encoder::deserializeEncoderData(ss)); break; } default: @@ -207,6 +197,28 @@ AdditionalFieldHeader readAdditionalField(std::istream& is, const std::size_t& m return AdditionalFieldHeader(id, length); } +namespace encoder +{ +EncoderData deserializeEncoderData(std::istream& is) +{ + uint16_t encoder_1_read_buffer; + raw_processing::read(is, encoder_1_read_buffer); + + uint16_t encoder_2_read_buffer; + raw_processing::read(is, encoder_2_read_buffer); + + // Change to Big-endian, please check protocol + raw_processing::endswap(&encoder_1_read_buffer); + raw_processing::endswap(&encoder_2_read_buffer); + + encoder::EncoderData encoder_data; + encoder_data.encoder_1 = toEncoder(encoder_1_read_buffer); + encoder_data.encoder_2 = toEncoder(encoder_2_read_buffer); + + return encoder_data; +} +} // namespace encoder + namespace io { PinData deserializePins(std::istream& is) diff --git a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp index 6f936b600..7118bc8bb 100644 --- a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp +++ b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp @@ -107,9 +107,11 @@ RawData serialize(const data_conversion_layer::monitoring_frame::Message& msg) write(os, encoder_header); uint16_t encoder_1_payload = static_cast(msg.encoderData().encoder_1); + raw_processing::endswap(&encoder_1_payload); //Change to Big-endian raw_processing::write(os, encoder_1_payload); uint16_t encoder_2_payload = static_cast(msg.encoderData().encoder_2); + raw_processing::endswap(&encoder_2_payload); //Change to Big-endian raw_processing::write(os, encoder_2_payload); } From 6f7cfba8aff55ffd13449bb0e3b7016097204c25 Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 29 Nov 2022 08:43:06 +0100 Subject: [PATCH 25/27] Aling code and rename function for endianSwap --- .../data_conversion_layer/raw_processing.h | 6 +++--- .../monitoring_frame_deserialization.cpp | 4 ++-- .../monitoring_frame_serialization.cpp | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h index 76fe7029d..87ca12601 100644 --- a/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h @@ -121,10 +121,10 @@ inline T toArray(std::ostringstream& os) // Reverse to change Little-endian to Big-endian template -inline void endswap(T *objp) +inline void endianSwap(T& data) { - unsigned char *memp = reinterpret_cast(objp); - std::reverse(memp, memp + sizeof(T)); + unsigned char *data_ptr = reinterpret_cast(&data); + std::reverse(data_ptr, data_ptr + sizeof(T)); } inline StringStreamFailure::StringStreamFailure(const std::string& msg) : std::runtime_error(msg) diff --git a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp index 455d901fe..b468881b7 100644 --- a/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp +++ b/standalone/src/data_conversion_layer/monitoring_frame_deserialization.cpp @@ -208,8 +208,8 @@ EncoderData deserializeEncoderData(std::istream& is) raw_processing::read(is, encoder_2_read_buffer); // Change to Big-endian, please check protocol - raw_processing::endswap(&encoder_1_read_buffer); - raw_processing::endswap(&encoder_2_read_buffer); + raw_processing::endianSwap(encoder_1_read_buffer); + raw_processing::endianSwap(encoder_2_read_buffer); encoder::EncoderData encoder_data; encoder_data.encoder_1 = toEncoder(encoder_1_read_buffer); diff --git a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp index 7118bc8bb..ec161e631 100644 --- a/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp +++ b/standalone/test/src/data_conversion_layer/monitoring_frame_serialization.cpp @@ -107,11 +107,11 @@ RawData serialize(const data_conversion_layer::monitoring_frame::Message& msg) write(os, encoder_header); uint16_t encoder_1_payload = static_cast(msg.encoderData().encoder_1); - raw_processing::endswap(&encoder_1_payload); //Change to Big-endian + raw_processing::endianSwap(encoder_1_payload); //Change to Big-endian raw_processing::write(os, encoder_1_payload); uint16_t encoder_2_payload = static_cast(msg.encoderData().encoder_2); - raw_processing::endswap(&encoder_2_payload); //Change to Big-endian + raw_processing::endianSwap(encoder_2_payload); //Change to Big-endian raw_processing::write(os, encoder_2_payload); } From 897db601bcd53ec88ae7c0d394ff5319f7a0479c Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Tue, 29 Nov 2022 15:09:17 +0100 Subject: [PATCH 26/27] Fix error on standalone tests --- standalone/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index f343654da..6390e2295 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -344,7 +344,7 @@ ADD_TEST(NAME unittest_udp_client COMMAND unittest_udp_client) -ADD_EXECUTABLE(unittest_encoder_state/test/unit_tests/api/unittest_encoder_state.cpp) +ADD_EXECUTABLE(unittest_encoder_state test/unit_tests/api/unittest_encoder_state.cpp) TARGET_LINK_LIBRARIES(unittest_encoder_state ${PROJECT_NAME} From a3f5e57559a99160dc024839e618e41937e52e5c Mon Sep 17 00:00:00 2001 From: m-sanchez-rico Date: Fri, 2 Dec 2022 14:04:37 +0100 Subject: [PATCH 27/27] Modify the UDP dumps to fix serialize/deserialize tests --- .../communication_layer/udp_frame_dumps.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/standalone/test/include/psen_scan_v2_standalone/communication_layer/udp_frame_dumps.h b/standalone/test/include/psen_scan_v2_standalone/communication_layer/udp_frame_dumps.h index 5e02cca2e..f6510aada 100644 --- a/standalone/test/include/psen_scan_v2_standalone/communication_layer/udp_frame_dumps.h +++ b/standalone/test/include/psen_scan_v2_standalone/communication_layer/udp_frame_dumps.h @@ -130,7 +130,8 @@ class WithIntensitiesAndDiagnostics .measurements(readMeasurements(hex_dump, 143, 250)) .intensities(readIntensities(hex_dump, intensities_offset, 250)) .diagnosticMessages({ diagnostic::Message(configuration::ScannerId::master, diagnostic::ErrorLocation(2, 0)), - diagnostic::Message(configuration::ScannerId::master, diagnostic::ErrorLocation(4, 3)) }); + diagnostic::Message(configuration::ScannerId::master, diagnostic::ErrorLocation(4, 3)) }) + .encoderData({256, 183}); expected_msg_ = msg_builder.build(); }; @@ -138,7 +139,7 @@ class WithIntensitiesAndDiagnostics // This is a dump read with wireshark and transformed with the script test/scripts/parse_dump.py. // clang-format off - const std::array hex_dump = { + const std::array hex_dump = { 0x00, 0x00, 0x00, 0x00, 0xca, 0x00, // 0020 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0xe9, 0x03, 0x02, 0x00, 0x01, // 0030 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0040 @@ -211,7 +212,7 @@ class WithIntensitiesAndDiagnostics 0x0e, 0x44, 0xd6, 0x44, 0xa3, 0x45, 0x70, 0x46, 0x6a, 0x47, 0x41, 0x47, 0xd4, 0x45, 0xfd, 0x44, // 0470 0xca, 0x45, 0xeb, 0x46, 0xa3, 0x46, 0x05, 0x45, 0x86, 0x43, 0x33, 0x43, 0x39, 0x43, 0x27, 0x43, // 0480 0x2a, 0x43, 0x22, 0x43, 0x12, 0x43, 0x1a, 0x43, 0x1e, 0x43, 0x33, 0x43, 0x3e, 0x43, 0x1f, 0x43, // 0490 - 0x1c, 0x43, 0x2a, 0x43, 0x09, 0x00, 0x00, 0x00, // 04a0 + 0x1c, 0x43, 0x2a, 0x43, 0x07, 0x05, 0x00, 0x01, 0x00, 0x00, 0xb7, 0x09, 0x00, 0x00, 0x00, // 04a0 }; // clang-format on