16 #ifndef PSEN_SCAN_V2_ROS_SCANNER_NODE_H
17 #define PSEN_SCAN_V2_ROS_SCANNER_NODE_H
26 #include <fmt/format.h>
28 #include <gtest/gtest_prod.h>
31 #include <std_msgs/UInt8.h>
32 #include <sensor_msgs/LaserScan.h>
47 using namespace std::chrono_literals;
53 template <
typename S = ScannerV2>
67 const std::string& topic,
68 const std::string& tf_prefix,
69 const double& x_axis_rotation,
82 void publishChangedIOStates(
const std::vector<psen_scan_v2_standalone::IOState>& io_states);
92 std::atomic_bool terminate_{
false };
96 friend class RosScannerNodeTests;
97 FRIEND_TEST(RosScannerNodeTests, shouldStartAndStopSuccessfullyIfScannerRespondsToRequests);
98 FRIEND_TEST(RosScannerNodeTests, shouldPublishScansWhenLaserScanCallbackIsInvoked);
99 FRIEND_TEST(RosScannerNodeTests, shouldPublishActiveZonesetWhenLaserScanCallbackIsInvoked);
100 FRIEND_TEST(RosScannerNodeTests, shouldWaitWhenStopRequestResponseIsMissing);
101 FRIEND_TEST(RosScannerNodeTests, shouldProvideScanTopic);
102 FRIEND_TEST(RosScannerNodeTests, shouldProvideActiveZonesetTopic);
103 FRIEND_TEST(RosScannerNodeTests, shouldPublishScanEqualToConversionOfSuppliedLaserScan);
104 FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStartFuture);
105 FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStopFuture);
106 FRIEND_TEST(RosScannerNodeTests, shouldPublishChangedIOStatesEqualToConversionOfSuppliedStandaloneIOStates);
107 FRIEND_TEST(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic);
108 FRIEND_TEST(RosScannerNodeTests, shouldLogChangedIOStates);
113 template <
typename S>
115 const std::string& topic,
116 const std::string& tf_prefix,
117 const double& x_axis_rotation,
120 , tf_prefix_(tf_prefix)
121 , x_axis_rotation_(x_axis_rotation)
129 template <
typename S>
134 const auto laser_scan_msg =
toLaserScanMsg(scan, tf_prefix_, x_axis_rotation_);
137 "Publishing laser scan with angle_min={:.1f} angle_max={:.1f} angle_increment={:.1f} degrees. {} angle values.",
141 laser_scan_msg.ranges.size());
142 pub_scan_.publish(laser_scan_msg);
144 std_msgs::UInt8 active_zoneset;
146 pub_zone_.publish(active_zoneset);
148 publishChangedIOStates(scan.
ioStates());
151 catch (
const std::invalid_argument&
e)
158 template <
typename S>
161 for (
const auto& io : io_states)
163 if (last_io_state_ != io)
168 "IOs changed, new input: {}, new output: {}",
176 template <
typename S>
182 template <
typename S>
186 auto start_future = scanner_.start();
187 const auto start_status = start_future.wait_for(3
s);
188 if (start_status == std::future_status::ready)
193 while (
ros::ok() && !terminate_)
197 auto stop_future = scanner_.stop();
199 const auto stop_status = stop_future.wait_for(3
s);
200 if (stop_status == std::future_status::ready)
208 #endif // PSEN_SCAN_V2_ROS_SCANNER_NODE_H