Feature/publish encoder data

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

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 <https://www.gnu.org/licenses/>.
+
+#ifndef PSEN_SCAN_V2_ENCODER_STATE_ROS_CONVERSIONS_H
+#define PSEN_SCAN_V2_ENCODER_STATE_ROS_CONVERSIONS_H
+
+#include <string>
+#include <algorithm>
+
+#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

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<psen_scan_v2_standalone::IOState>& io_states);
+  void publishEncoderData(const std::vector<psen_scan_v2_standalone::EncoderState>& 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<S>::ROSScannerNodeT(ros::NodeHandle& nh,
   pub_scan_ = nh_.advertise<sensor_msgs::LaserScan>(topic, 1);
   pub_zone_ = nh_.advertise<std_msgs::UInt8>("active_zoneset", 1);
   pub_io_ = nh_.advertise<psen_scan_v2::IOState>("io_state", 6, true /* latched */);
+  pub_encoder_ = nh.advertise<psen_scan_v2::EncoderState>("encoder_state", 6, true);
 }
 
 template <typename S>
@@ -146,6 +153,8 @@ void ROSScannerNodeT<S>::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<S>::publishChangedIOStates(const std::vector<psen_scan_v2_s
   }
 }
 
+template <typename S>
+void ROSScannerNodeT<S>::publishEncoderData(const std::vector<psen_scan_v2_standalone::EncoderState>& encoder_states)
+{
+  for (const auto& single_state : encoder_states)
+  {
+    pub_encoder_.publish(toEncoderStateMsg(single_state, tf_prefix_));
+  }
+}
+
 template <typename S>
 void ROSScannerNodeT<S>::terminate()
 {

launch/bringup.launch

@@ -1,5 +1,5 @@
 <!--
-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
@@ -48,6 +48,9 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
   <!-- Set the following to true in order to publish scan data as soon as a UDP packet is ready, instead of waiting for a full scan -->
   <arg name="fragmented_scans" default="false" />
+
+  <!-- Set the following to true in order to publish encoder data -->
+  <arg name="encoder_data" default="false" />
 
   <node name="$(arg tf_prefix)" type="psen_scan_v2_node" pkg="psen_scan_v2" output="screen" required="true">
     <param name="sensor_ip" value="$(arg sensor_ip)" />
@@ -60,6 +63,7 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
     <param name="host_udp_port_data" value="$(arg host_udp_port_data)" />
     <param name="host_udp_port_control" value="$(arg host_udp_port_control)" />
     <param name="fragmented_scans" value="$(arg fragmented_scans)" />
+    <param name="encoder_data" value="$(arg encoder_data)" />
   </node>
 
 </launch>

launch/psen_scan_v2.launch

@@ -27,7 +27,6 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
   <arg name="angle_start" default="$(eval radians(-137.4))" />
 
   <!-- End angle of measurement in radian -->
-
   <arg name="angle_end" default="$(eval radians(137.4))" />
 
   <!-- Publishing of intensities -->
@@ -48,6 +47,9 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
   <!-- Set the following to true in order to publish scan data as soon as a UDP packet is ready, instead of waiting for a full scan -->
   <arg name="fragmented_scans" default="false" />
 
+  <!-- Set the following to true in order to publish the encoders data -->
+  <arg name="encoder_data" default="false" />
+
   <!-- Load scanner config file to publish zonesets -->
   <arg name="config_file" default="" />
 
@@ -66,6 +68,7 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
     <arg name="host_udp_port_data" value="$(arg host_udp_port_data)" />
     <arg name="host_udp_port_control" value="$(arg host_udp_port_control)" />
     <arg name="fragmented_scans" value="$(arg fragmented_scans)" />
+    <arg name="encoder_data" value="$(arg encoder_data)" />
   </include>
 
   <!-- Publish tf frames for the device -->

msg/EncoderState.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+float64 encoder_1
+float64 encoder_2

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,13 +98,19 @@ int main(int argc, char** argv)
           .enableIntensities(getOptionalParamFromServer<bool>(pnh, PARAM_INTENSITIES, configuration::INTENSITIES))
           .scanResolution(util::TenthOfDegree::fromRad(
               getOptionalParamFromServer<double>(pnh, PARAM_RESOLUTION, configuration::DEFAULT_SCAN_ANGLE_RESOLUTION)))
+          .enableEncoder(getOptionalParamFromServer<bool>(pnh, PARAM_ENCODER_DATA, configuration::ENCODER))
           .build()
     };
 
     if (scanner_configuration.fragmentedScansEnabled())
     {
       ROS_INFO("Using fragmented scans.");
     }
+
+    if (scanner_configuration.encoderEnabled())
+    {
+      ROS_INFO("Reading data from encoders.");
+    }
 
     ROSScannerNode ros_scanner_node(pnh,
                                     DEFAULT_PUBLISH_TOPIC,

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 ()

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));