[136] | 1 | #ifndef LIB3DV_DETAIL_DEVICE_IMPL_H
|
---|
| 2 | #define LIB3DV_DETAIL_DEVICE_IMPL_H
|
---|
| 3 |
|
---|
| 4 | /* lib3dv/detail/device_impl.h
|
---|
| 5 | *
|
---|
| 6 | * Copyright (C) 2013 VisLab
|
---|
| 7 | *
|
---|
| 8 | * This file is part of lib3dv; you can redistribute it and/or modify
|
---|
| 9 | * it under the terms of the GNU Lesser General Public License as published by
|
---|
| 10 | * the Free Software Foundation; either version 3 of the License, or (at
|
---|
| 11 | * your option) any later version.
|
---|
| 12 | *
|
---|
| 13 | * This program is distributed in the hope that it will be useful, but
|
---|
| 14 | * WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
---|
| 16 | * General Public License for more details.
|
---|
| 17 | *
|
---|
| 18 | * You should have received a copy of the GNU Lesser General Public License
|
---|
| 19 | * along with this program; if not, see <http://www.gnu.org/licenses/>.
|
---|
| 20 | */
|
---|
| 21 |
|
---|
| 22 | #include <lib3dv/device.h>
|
---|
| 23 | #include <lib3dv/image.h>
|
---|
| 24 | #include <lib3dv/terrain.h>
|
---|
| 25 | #include <lib3dv/motion.h>
|
---|
| 26 | #include <lib3dv/candidate.h>
|
---|
| 27 | #include <lib3dv/obstacle.h>
|
---|
| 28 | #include <lib3dv/detail/command.h>
|
---|
| 29 | #include <lib3dv/detail/raw_data.h>
|
---|
| 30 | #include <lib3dv/version.h>
|
---|
| 31 |
|
---|
| 32 | #include <boost/array.hpp>
|
---|
| 33 | #include <boost/asio.hpp>
|
---|
| 34 | #include <boost/date_time/time_duration.hpp>
|
---|
| 35 | #include <boost/shared_ptr.hpp>
|
---|
| 36 | #include <boost/signals2.hpp>
|
---|
| 37 | #include <boost/thread/thread.hpp>
|
---|
| 38 | #include <boost/uuid/uuid.hpp>
|
---|
| 39 |
|
---|
| 40 | #include <map>
|
---|
| 41 | #include <stdint.h>
|
---|
| 42 | #include <vector>
|
---|
| 43 | #include <utility>
|
---|
| 44 |
|
---|
| 45 | namespace lib3dv
|
---|
| 46 | {
|
---|
| 47 | namespace detail
|
---|
| 48 | {
|
---|
| 49 | class device_impl
|
---|
| 50 | {
|
---|
| 51 | public:
|
---|
| 52 |
|
---|
| 53 | static const unsigned int MAX_UDP_PAYLOAD_SIZE = 65507; ///< max admissible UDP payload size [bytes].
|
---|
| 54 |
|
---|
| 55 | static std::vector<device> enumerate(const boost::asio::ip::address_v4& local_address, unsigned short remote_commands_port, uint8_t log_level, error& error, const boost::posix_time::time_duration& timeout);
|
---|
| 56 |
|
---|
| 57 | device_impl(const boost::asio::ip::address_v4& local_address,
|
---|
| 58 | const boost::asio::ip::address_v4& remote_address, unsigned short remote_commands_port, unsigned short remote_data_port,
|
---|
| 59 | const boost::uuids::uuid& guid,
|
---|
| 60 | const std::bitset<device::capability::NUM>& capabilities,
|
---|
| 61 | const device::version_info& version,
|
---|
| 62 | uint8_t log_level, error& device_error);
|
---|
| 63 |
|
---|
| 64 | ~device_impl();
|
---|
| 65 |
|
---|
| 66 | inline bool valid() const { return (m_status == device::status::ONLINE) || (m_status == device::status::TRANSMITTING); }
|
---|
| 67 |
|
---|
| 68 | const boost::uuids::uuid& guid() const { return m_guid; }
|
---|
| 69 |
|
---|
| 70 | const std::bitset<device::capability::NUM>& capabilities() const { return m_capabilities; }
|
---|
| 71 |
|
---|
| 72 | const device::version_info& version() const { return m_version; }
|
---|
| 73 |
|
---|
| 74 | void start_transmission(error& error);
|
---|
| 75 |
|
---|
| 76 | void stop_transmission(error& error);
|
---|
| 77 |
|
---|
| 78 | device::status::types status() const { return m_status; }
|
---|
| 79 |
|
---|
| 80 | std::vector<device::property> enumerate_properties(error& error);
|
---|
| 81 |
|
---|
| 82 | void save_properties(error& error);
|
---|
| 83 |
|
---|
| 84 | void reset_properties(error& error);
|
---|
| 85 |
|
---|
| 86 | void poweroff(error& error);
|
---|
| 87 |
|
---|
| 88 | void reboot(error& error);
|
---|
| 89 |
|
---|
| 90 | boost::any get_property_value(uint16_t address, error& error);
|
---|
| 91 |
|
---|
| 92 | void set_property_value(uint16_t address, const boost::any& value, error& error);
|
---|
| 93 |
|
---|
| 94 | uint64_t connect_image_callback(const boost::function<void (boost::shared_ptr<const lib3dv::image>, uint32_t)>& function);
|
---|
| 95 |
|
---|
| 96 | uint64_t connect_terrain_callback(const boost::function<void (boost::shared_ptr<const lib3dv::terrain>, uint32_t)>& function);
|
---|
| 97 |
|
---|
| 98 | uint64_t connect_obstacles_callback(const boost::function<void (boost::shared_ptr<const std::vector<lib3dv::obstacle> >, uint32_t)>& function);
|
---|
| 99 |
|
---|
| 100 | uint64_t connect_motion_callback(const boost::function<void (boost::shared_ptr<const lib3dv::motion>, uint32_t)>& function);
|
---|
| 101 |
|
---|
| 102 | uint64_t connect_classification_callback(const boost::function<void (boost::shared_ptr<const lib3dv::classification>, uint32_t)>& function);
|
---|
| 103 |
|
---|
| 104 | LIB3DV_DEPRECATED uint64_t connect_image_callback(const boost::function<void (boost::shared_ptr<lib3dv::image>, uint32_t)>& function);
|
---|
| 105 |
|
---|
| 106 | LIB3DV_DEPRECATED uint64_t connect_terrain_callback(const boost::function<void (boost::shared_ptr<lib3dv::terrain>, uint32_t)>& function);
|
---|
| 107 |
|
---|
| 108 | LIB3DV_DEPRECATED uint64_t connect_obstacles_callback(const boost::function<void (boost::shared_ptr<std::vector<lib3dv::obstacle> >, uint32_t)>& function);
|
---|
| 109 |
|
---|
| 110 | LIB3DV_DEPRECATED uint64_t connect_motion_callback(const boost::function<void (boost::shared_ptr<lib3dv::motion>, uint32_t)>& function);
|
---|
| 111 |
|
---|
| 112 | LIB3DV_DEPRECATED uint64_t connect_classification_callback(const boost::function<void (boost::shared_ptr<lib3dv::classification>, uint32_t)>& function);
|
---|
| 113 |
|
---|
| 114 | uint64_t connect_timeout_callback(const boost::function<void (void)>& function);
|
---|
| 115 |
|
---|
| 116 |
|
---|
| 117 | void disconnect_callback(uint64_t id);
|
---|
| 118 |
|
---|
| 119 | uint8_t m_log_level;
|
---|
| 120 |
|
---|
| 121 | boost::posix_time::time_duration m_timeout;
|
---|
| 122 |
|
---|
| 123 | friend std::ostream& operator<< (std::ostream& output, const device_impl& device);
|
---|
| 124 |
|
---|
| 125 | private:
|
---|
| 126 |
|
---|
| 127 | typedef raw_data<protocol::image_info_header> raw_image_type;
|
---|
| 128 | typedef raw_data<protocol::terrain_info_header> raw_terrain_type;
|
---|
| 129 | typedef raw_data<protocol::obstacles_map_info_header> raw_obstacles_type;
|
---|
| 130 | typedef raw_data<protocol::motion_info_header> raw_motion_type;
|
---|
| 131 | typedef raw_data<protocol::classifier_info_header> raw_candidate_type;
|
---|
| 132 |
|
---|
| 133 | static const size_t DEFAULT_MAX_REASSEMBLY_CONTAINER_SIZE = 100;
|
---|
| 134 |
|
---|
| 135 | void on_command_received(const boost::system::error_code& error, size_t bytes_received, boost::any& result, lib3dv::error& device_error);
|
---|
| 136 |
|
---|
| 137 | void on_command_sent(const boost::system::error_code& error, size_t bytes_received, boost::any& result, lib3dv::error& device_error);
|
---|
| 138 |
|
---|
| 139 | void on_timeout(lib3dv::error& device_error);
|
---|
| 140 |
|
---|
| 141 | /**
|
---|
| 142 | * @brief Asynchronous UDP data handler.
|
---|
| 143 | *
|
---|
| 144 | * @param [in] error Possible error condition.
|
---|
| 145 | * @param [in] bytes_received The amount of data received during the last read [bytes]
|
---|
| 146 | * @return void
|
---|
| 147 | **/
|
---|
| 148 | void on_data_received(const boost::system::error_code& error, size_t bytes_received);
|
---|
| 149 |
|
---|
| 150 | template<typename C, typename F, typename H>
|
---|
| 151 | typename C::mapped_type init_reassembly_container(C& container, F& fragments_container, const H& data_info_header, uint32_t key);
|
---|
| 152 |
|
---|
| 153 | template<typename C, typename F>
|
---|
| 154 | typename C::mapped_type fill_reassembly_container(C& container, F& fragments_container, size_t bytes_received, uint32_t key, uint32_t fragment, uint32_t total_fragments);
|
---|
| 155 |
|
---|
| 156 | template<typename C, typename F>
|
---|
| 157 | void cleanup_reassembly_container(C& container, F& fragments_container, size_t max_size = DEFAULT_MAX_REASSEMBLY_CONTAINER_SIZE);
|
---|
| 158 |
|
---|
| 159 | void decode_terrain(boost::shared_ptr<raw_terrain_type> terrain, uint32_t guid);
|
---|
| 160 |
|
---|
| 161 | void decode_obstacles(boost::shared_ptr<raw_obstacles_type> obstacles, uint32_t guid);
|
---|
| 162 |
|
---|
| 163 | void decode_motion(boost::shared_ptr<raw_motion_type> pose, uint32_t guid);
|
---|
| 164 |
|
---|
| 165 | void decode_classification(boost::shared_ptr<raw_candidate_type> pose, uint32_t guid);
|
---|
| 166 |
|
---|
| 167 | const uint8_t* decode_property(device::property& property, const uint8_t* data);
|
---|
| 168 |
|
---|
| 169 | void send_command(boost::shared_ptr<lib3dv::detail::command> command, error& error);
|
---|
| 170 |
|
---|
| 171 | std::map<uint32_t, boost::shared_ptr<raw_image_type> > m_images;
|
---|
| 172 | std::map<uint32_t, std::vector<boost::shared_ptr<raw_data_fragment> > > m_image_fragments;
|
---|
| 173 |
|
---|
| 174 | std::map<uint32_t, boost::shared_ptr<raw_terrain_type> > m_terrains;
|
---|
| 175 | std::map<uint32_t, std::vector<boost::shared_ptr<raw_data_fragment> > > m_terrain_fragments;
|
---|
| 176 |
|
---|
| 177 | std::map<uint32_t, boost::shared_ptr<raw_obstacles_type> > m_obstacles;
|
---|
| 178 | std::map<uint32_t, std::vector<boost::shared_ptr<raw_data_fragment> > > m_obstacle_fragments;
|
---|
| 179 |
|
---|
| 180 | std::map<uint32_t, boost::shared_ptr<raw_motion_type> > m_poses;
|
---|
| 181 | std::map<uint32_t, std::vector<boost::shared_ptr<raw_data_fragment> > > m_pose_fragments;
|
---|
| 182 |
|
---|
| 183 | std::map<uint32_t, boost::shared_ptr<raw_candidate_type> > m_candidates;
|
---|
| 184 | std::map<uint32_t, std::vector<boost::shared_ptr<raw_data_fragment> > > m_candidates_fragments;
|
---|
| 185 |
|
---|
| 186 |
|
---|
| 187 | boost::asio::io_service m_data_socket_io_service;
|
---|
| 188 | boost::thread m_data_socket_io_service_thread;
|
---|
| 189 |
|
---|
| 190 | boost::asio::io_service m_commands_socket_io_service;
|
---|
| 191 |
|
---|
| 192 | boost::asio::ip::address_v4 m_local_address;
|
---|
| 193 | boost::asio::ip::address_v4 m_remote_address;
|
---|
| 194 | unsigned short m_remote_commands_port;
|
---|
| 195 | unsigned short m_remote_data_port;
|
---|
| 196 |
|
---|
| 197 | boost::asio::ip::udp::socket m_data_socket;
|
---|
| 198 | boost::asio::ip::udp::socket m_commands_socket;
|
---|
| 199 |
|
---|
| 200 | boost::asio::ip::udp::endpoint m_data_sender_endpoint;
|
---|
| 201 | boost::asio::ip::udp::endpoint m_commands_sender_endpoint;
|
---|
| 202 |
|
---|
| 203 | boost::array<uint8_t, MAX_UDP_PAYLOAD_SIZE> m_data_payload;
|
---|
| 204 | boost::array<uint8_t, MAX_UDP_PAYLOAD_SIZE> m_commands_payload;
|
---|
| 205 |
|
---|
| 206 | boost::asio::deadline_timer m_commands_deadline;
|
---|
| 207 | boost::asio::deadline_timer m_data_deadline;
|
---|
| 208 |
|
---|
| 209 |
|
---|
| 210 | boost::signals2::signal<void (const command&)> m_command_signal;
|
---|
| 211 |
|
---|
| 212 | boost::signals2::signal<void (boost::shared_ptr<const lib3dv::image>, uint32_t)> m_image_signal;
|
---|
| 213 | boost::signals2::signal<void (boost::shared_ptr<const lib3dv::terrain>, uint32_t)> m_terrain_signal;
|
---|
| 214 | boost::signals2::signal<void (boost::shared_ptr<const std::vector<lib3dv::obstacle> >, uint32_t)> m_obstacles_signal;
|
---|
| 215 | boost::signals2::signal<void (boost::shared_ptr<const lib3dv::motion>, uint32_t)> m_motion_signal;
|
---|
| 216 | boost::signals2::signal<void (boost::shared_ptr<const lib3dv::classification>, uint32_t)> m_classification_signal;
|
---|
| 217 |
|
---|
| 218 | LIB3DV_DEPRECATED boost::signals2::signal<void (boost::shared_ptr<lib3dv::image>, uint32_t)> m_deprecated_image_signal;
|
---|
| 219 | LIB3DV_DEPRECATED boost::signals2::signal<void (boost::shared_ptr<lib3dv::terrain>, uint32_t)> m_deprecated_terrain_signal;
|
---|
| 220 | LIB3DV_DEPRECATED boost::signals2::signal<void (boost::shared_ptr<std::vector<lib3dv::obstacle> >, uint32_t)> m_deprecated_obstacles_signal;
|
---|
| 221 | LIB3DV_DEPRECATED boost::signals2::signal<void (boost::shared_ptr<lib3dv::motion>, uint32_t)> m_deprecated_motion_signal;
|
---|
| 222 | LIB3DV_DEPRECATED boost::signals2::signal<void (boost::shared_ptr<lib3dv::classification>, uint32_t)> m_deprecated_classification_signal;
|
---|
| 223 |
|
---|
| 224 | boost::signals2::signal<void (void)> m_timeout_signal;
|
---|
| 225 |
|
---|
| 226 | boost::uuids::uuid m_guid;
|
---|
| 227 | std::bitset<device::capability::NUM> m_capabilities;
|
---|
| 228 | device::version_info m_version;
|
---|
| 229 |
|
---|
| 230 | uint64_t m_max_connection_id;
|
---|
| 231 | std::map<uint64_t, boost::signals2::connection> m_connections;
|
---|
| 232 |
|
---|
| 233 | device::status::types m_status;
|
---|
| 234 | };
|
---|
| 235 | }
|
---|
| 236 | }
|
---|
| 237 |
|
---|
| 238 | #include <lib3dv/detail/device_impl.hpp>
|
---|
| 239 |
|
---|
| 240 | #endif
|
---|