diff --git a/CMakeLists.txt b/CMakeLists.txt index f7c556c1a..2de1abcae 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 @@ -57,6 +57,7 @@ add_message_files( IOState.msg ZoneSet.msg ZoneSetConfiguration.msg + EncoderState.msg ) generate_messages( @@ -136,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 @@ -340,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 @@ -365,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 @@ -385,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 ) @@ -396,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 @@ -505,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 ) @@ -534,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 @@ -552,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 @@ -718,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 @@ -730,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 ) @@ -788,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/encoder_state_ros_conversion.h b/include/psen_scan_v2/encoder_state_ros_conversion.h new file mode 100644 index 000000000..083b6cb83 --- /dev/null +++ b/include/psen_scan_v2/encoder_state_ros_conversion.h @@ -0,0 +1,50 @@ +// 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 . + +#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" +#include "psen_scan_v2_standalone/encoder_state.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("EncoderState 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.getEncoder1(); + ros_message.encoder_2 = encoder_state.getEncoder2(); + + return ros_message; +} + +} // namespace psen_scan_v2 + +#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 07fd5f294..3af8cbd33 100644 --- a/include/psen_scan_v2/ros_scanner_node.h +++ b/include/psen_scan_v2/ros_scanner_node.h @@ -33,8 +33,11 @@ #include "psen_scan_v2_standalone/scanner_v2.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/encoder_state.h" #include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h" #include "psen_scan_v2_standalone/util/format_range.h" @@ -80,12 +83,14 @@ class ROSScannerNodeT private: void laserScanCallback(const LaserScan& scan); void publishChangedIOStates(const std::vector& io_states); + void publishEncoderData(const std::vector& encoder_states); 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_; @@ -105,6 +110,7 @@ class ROSScannerNodeT FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStopFuture); FRIEND_TEST(RosScannerNodeTests, shouldPublishChangedIOStatesEqualToConversionOfSuppliedStandaloneIOStates); FRIEND_TEST(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic); + FRIEND_TEST(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedStandaloneEncoderData); FRIEND_TEST(RosScannerNodeTests, shouldLogChangedIOStates); }; @@ -124,6 +130,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_state", 6, true); } template @@ -146,6 +153,8 @@ void ROSScannerNodeT::laserScanCallback(const LaserScan& scan) pub_zone_.publish(active_zoneset); publishChangedIOStates(scan.ioStates()); + + publishEncoderData(scan.encoderStates()); } // LCOV_EXCL_START catch (const std::invalid_argument& e) @@ -173,6 +182,15 @@ void ROSScannerNodeT::publishChangedIOStates(const std::vector +void ROSScannerNodeT::publishEncoderData(const std::vector& encoder_states) +{ + for (const auto& single_state : encoder_states) + { + pub_encoder_.publish(toEncoderStateMsg(single_state, tf_prefix_)); + } +} + template void ROSScannerNodeT::terminate() { diff --git a/launch/bringup.launch b/launch/bringup.launch index 9108766d1..e042829df 100644 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -1,5 +1,5 @@ + + + @@ -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..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 . - @@ -48,6 +47,9 @@ along with this program. If not, see . + + + @@ -66,6 +68,7 @@ along with this program. If not, see . + diff --git a/msg/EncoderState.msg b/msg/EncoderState.msg new file mode 100644 index 000000000..94a2eb5ff --- /dev/null +++ b/msg/EncoderState.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +float64 encoder_1 +float64 encoder_2 diff --git a/src/psen_scan_driver.cpp b/src/psen_scan_driver.cpp index e5c29e810..78e3e5dc3 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 @@ -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/CMakeLists.txt b/standalone/CMakeLists.txt index 4846a5f97..6390e2295 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 @@ -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 @@ -343,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 @@ -370,5 +382,6 @@ target_link_libraries(integrationtest_udp_client add_test(NAME integrationtest_udp_client COMMAND integrationtest_udp_client) + endif () endif () 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..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 @@ -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/encoder_data.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h new file mode 100644 index 000000000..8da0777f4 --- /dev/null +++ b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/encoder_data.h @@ -0,0 +1,55 @@ +// 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 . + +#ifndef PSEN_SCAN_V2_STANDALONE_ENCODER_DATA_H +#define PSEN_SCAN_V2_STANDALONE_ENCODER_DATA_H + +#include + +#include +#include + + +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_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 a676bb472..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,6 +102,7 @@ inline LaserScan LaserScanConverter::toLaserScan( std::vector measurements; std::vector intensities; std::vector io_states; + std::vector encoder_states; for (auto index : sorted_stamped_msgs_indices) { @@ -131,6 +132,18 @@ inline LaserScan LaserScanConverter::toLaserScan( io_states.emplace_back(single_msg.msg_.iOPinData(), single_msg.stamp_); } + // Fill the data with the encoder information as reception order. + if (single_msg.msg_.hasEncoderDataField()) + { + 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(), @@ -143,6 +156,7 @@ inline LaserScan LaserScanConverter::toLaserScan( scan.measurements(measurements); scan.intensities(intensities); scan.ioStates(io_states); + 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 effb0a260..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 @@ -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{ 4 }; /** * @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 }; @@ -152,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/monitoring_frame_msg.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h index 0a0128d4e..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 @@ -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 @@ -75,6 +76,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 encoder::EncoderData& encoderData() const; //! @throw AdditionalFieldMissing if diagnostic_messages were missing during deserialization of a Message. std::vector diagnosticMessages() const; @@ -83,6 +86,7 @@ class Message bool hasIOPinField() const; bool hasMeasurementsField() const; bool hasIntensitiesField() const; + bool hasEncoderDataField() const; bool hasDiagnosticMessagesField() const; private: @@ -96,6 +100,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..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 @@ -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 encoder::EncoderData& encoder_data); private: Message msg_; @@ -114,6 +115,12 @@ inline MessageBuilder& MessageBuilder::iOPinData(const io::PinData& io_pin_data) msg_.io_pin_data_ = io_pin_data; return *this; } + +inline MessageBuilder& MessageBuilder::encoderData(const encoder::EncoderData& 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/raw_processing.h b/standalone/include/psen_scan_v2_standalone/data_conversion_layer/raw_processing.h index e29c85ba4..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 @@ -119,6 +119,14 @@ inline T toArray(std::ostringstream& os) return raw_data; } +// Reverse to change Little-endian to Big-endian +template +inline void endianSwap(T& data) +{ + 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/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..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 @@ -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/encoder_state.h b/standalone/include/psen_scan_v2_standalone/encoder_state.h new file mode 100644 index 000000000..01549c21a --- /dev/null +++ b/standalone/include/psen_scan_v2_standalone/encoder_state.h @@ -0,0 +1,49 @@ +// 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 . + +#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 +{ +//! @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 getEncoder1() const; + //! @return double containing the reading of the encoder 2. + double getEncoder2() 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_ENCODER_STATE_H diff --git a/standalone/include/psen_scan_v2_standalone/laserscan.h b/standalone/include/psen_scan_v2_standalone/laserscan.h index d9333b7b1..7971f24d5 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 @@ -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 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 @@ -49,6 +51,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 +120,10 @@ class LaserScan [[deprecated("use void ioStates(const IOData& io_states) instead")]] void setIOStates(const IOData& io_states); void ioStates(const IOData& io_states); + const EncoderData& encoderStates() const; + + void encoderStates(const EncoderData& encoder_states); + private: //! Measurement data of the laserscan (in Millimeters). MeasurementData measurements_; @@ -136,6 +143,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 encoder_states_; }; 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..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 @@ -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..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 @@ -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..b468881b7 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 @@ -76,6 +78,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 +160,16 @@ monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data msg_builder.intensities(intensities); break; } + case AdditionalFieldHeaderID::encoder: { + 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)); + } + msg_builder.encoderData(encoder::deserializeEncoderData(ss)); + break; + } default: throw DecodingFailure( fmt::format("Header Id {:#04x} unknown. Cannot read additional field of monitoring frame on position {}.", @@ -180,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::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); + 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/src/data_conversion_layer/monitoring_frame_msg.cpp b/standalone/src/data_conversion_layer/monitoring_frame_msg.cpp index 0052cace3..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 @@ -103,6 +103,18 @@ const std::vector& Message::intensities() const } } +const encoder::EncoderData& 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..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 @@ -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..f88e9eaaa 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 @@ -85,7 +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() ? 0b00001111 : 0b00000000) }; const uint8_t diagnostics_enabled{ static_cast( msg.master_device_settings_.diagnosticsEnabled() ? 0b00001000 : 0b00000000) }; diff --git a/standalone/src/encoder_state.cpp b/standalone/src/encoder_state.cpp new file mode 100644 index 000000000..5368ba6ac --- /dev/null +++ b/standalone/src/encoder_state.cpp @@ -0,0 +1,50 @@ +// 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 +{ +EncoderState::EncoderState(data_conversion_layer::monitoring_frame::encoder::EncoderData encoder_data, + const int64_t& timestamp) + : encoder_data_(encoder_data), timestamp_(timestamp) +{ +} + +double EncoderState::getEncoder1() const +{ + return encoder_data_.encoder_1; +} + +double EncoderState::getEncoder2() 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 0156db2cd..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 @@ -118,6 +118,7 @@ void LaserScan::setIntensities(const IntensityData& intensities) { this->intensities(intensities); } + // LCOV_EXCL_STOP const util::TenthOfDegree& LaserScan::minScanAngle() const @@ -170,6 +171,16 @@ void LaserScan::intensities(const IntensityData& intensities) intensities_ = intensities; } +const LaserScan::EncoderData& LaserScan::encoderStates() const +{ + return encoder_states_; +} + +void LaserScan::encoderStates(const EncoderData& encoder_states) +{ + encoder_states_ = encoder_states; +} + // LCOV_EXCL_START void LaserScan::setIOStates(const IOData& io_states) { @@ -195,7 +206,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., @@ -204,7 +215,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; } 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 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..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 @@ -31,6 +31,7 @@ #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" @@ -97,6 +98,8 @@ createMonitoringFrameMsgBuilderWithoutDiagnostics(const util::TenthOfDegree star msg_builder.iOPinData(createPinData()); + 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 aeb644853..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 @@ -76,6 +76,12 @@ MATCHER_P(IOStateEq, io_state, "") io_state.timestamp() == arg.timestamp(); } +MATCHER_P(EncoderStateEq, encoder_state, "") +{ + return Matches(DoubleEq(encoder_state.getEncoder1()))(arg.getEncoder1()) && + Matches(DoubleEq(encoder_state.getEncoder2()))(arg.getEncoder2()); +} + MATCHER_P(PointwiseIOStateEq, vec, "") { return std::equal(vec.begin(), vec.end(), arg.begin(), arg.end(), [result_listener](const auto& a, const auto& b) { @@ -83,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, "") @@ -120,6 +134,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 +147,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..ec161e631 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,21 @@ 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::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::endianSwap(encoder_2_payload); //Change to Big-endian + 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..22e8acaa4 --- /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.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.getEncoder2(); + 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..f888e0275 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,22 @@ 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); + + ASSERT_EQ(laser_scan->encoderStates().size(), 1u); + 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); +} + TEST(LaserScanTest, testPrintMessageSuccess) { LaserScanBuilder laser_scan_builder; @@ -183,6 +201,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 +213,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..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" @@ -68,6 +69,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 +118,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 +184,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..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" @@ -39,6 +40,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 +52,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..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" @@ -62,6 +64,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 +75,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 +93,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 +182,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 +246,35 @@ TEST_F(RosScannerNodeTests, shouldPublishActiveZonesetWhenLaserScanCallbackIsInv loop.wait_for(LOOP_END_TIMEOUT); } +const std::string ENCODER_TOPICNAME{ "encoder_state" }; + +TEST_F(RosScannerNodeTests, shouldPublishEncoderDataEqualToConversionOfSuppliedStandaloneEncoderData) +{ + 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..2e1e5be61 --- /dev/null +++ b/test/unit_tests/unittest_encoder_state_rosconversions.cpp @@ -0,0 +1,73 @@ +// 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; +using data_conversion_layer::monitoring_frame::encoder::EncoderData; +namespace psen_scan_v2_test +{ +TEST(EncoderStateROSConversionsTest, shouldConvertSuccessfully) +{ + 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.getEncoder1()); + EXPECT_EQ(ros_message.encoder_2, encoder_state.getEncoder2()); +} + +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[]) +{ + testing::InitGoogleTest(&argc, argv); + ros::Time::init(); + return RUN_ALL_TESTS(); +}