io_state_ros_conversion.h
Go to the documentation of this file.
1 // Copyright (c) 2021-2022 Pilz GmbH & Co. KG
2 //
3 // This program is free software: you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation, either version 3 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program. If not, see <https://www.gnu.org/licenses/>.
15 
16 #ifndef PSEN_SCAN_V2_IO_STATE_ROS_CONVERSIONS_H
17 #define PSEN_SCAN_V2_IO_STATE_ROS_CONVERSIONS_H
18 
19 #include <string>
20 #include <algorithm>
21 
22 #include "psen_scan_v2/IOState.h"
23 #include "psen_scan_v2/InputPinState.h"
24 #include "psen_scan_v2/OutputPinState.h"
26 
27 namespace psen_scan_v2
28 {
29 template <typename PinStateType>
31 {
32  PinStateType pin_msg;
33  pin_msg.pin_id.id = pin.id();
34  pin_msg.name = pin.name();
35  pin_msg.state = pin.state();
36  return pin_msg;
37 }
38 
39 psen_scan_v2::IOState toIOStateMsg(const psen_scan_v2_standalone::IOState& io_state, const std::string& frame_id)
40 {
41  psen_scan_v2::IOState ros_message;
42  if (io_state.timestamp() < 0)
43  {
44  throw std::invalid_argument("IOState of Laserscan message has an invalid timestamp: " +
45  std::to_string(io_state.timestamp()));
46  }
47  ros_message.header.stamp = ros::Time{}.fromNSec(io_state.timestamp());
48  ros_message.header.frame_id = frame_id;
49 
50  auto input = io_state.input();
51  std::transform(input.begin(), input.end(), std::back_inserter(ros_message.input), [](const auto& pin) {
52  return toPinStateMsg<InputPinState>(pin);
53  });
54 
55  auto output = io_state.output();
56  std::transform(output.begin(), output.end(), std::back_inserter(ros_message.output), [](const auto& pin) {
57  return toPinStateMsg<OutputPinState>(pin);
58  });
59 
60  return ros_message;
61 }
62 
63 } // namespace psen_scan_v2
64 
65 #endif // PSEN_SCAN_V2_IO_STATE_ROS_CONVERSIONS_H
psen_scan_v2_standalone::PinState::name
std::string name() const
Definition: io_state.cpp:49
psen_scan_v2_standalone::PinState
Represents a single I/O pin.
Definition: io_state.h:29
psen_scan_v2_standalone::IOState::input
std::vector< PinState > input() const
Definition: io_state.cpp:80
psen_scan_v2_standalone::PinState::id
uint32_t id() const
Definition: io_state.cpp:44
psen_scan_v2::toIOStateMsg
psen_scan_v2::IOState toIOStateMsg(const psen_scan_v2_standalone::IOState &io_state, const std::string &frame_id)
Definition: io_state_ros_conversion.h:39
psen_scan_v2_standalone::IOState::output
std::vector< PinState > output() const
Definition: io_state.cpp:85
psen_scan_v2
Root namespace for the ROS part.
Definition: active_zoneset_node.h:29
psen_scan_v2_standalone::PinState::state
bool state() const
Definition: io_state.cpp:54
psen_scan_v2_standalone::IOState::timestamp
int64_t timestamp() const
Definition: io_state.cpp:90
io_state.h
psen_scan_v2_standalone::IOState
Represents the set of all I/Os of the scanner and their states.
Definition: io_state.h:76
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
ros::Time
psen_scan_v2::toPinStateMsg
PinStateType toPinStateMsg(const psen_scan_v2_standalone::PinState &pin)
Definition: io_state_ros_conversion.h:30


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Jun 22 2024 02:46:11