├── VISensor_Factsheet_web.pdf ├── install_libvisensor.sh ├── pkgconfig └── visensor.pc ├── .gitignore ├── package.xml ├── cmake ├── FindVISensor.cmake └── FindEigen.cmake ├── src ├── sensors │ ├── led_flasher.cpp │ ├── corner_detector.cpp │ ├── camera.cpp │ ├── imu.cpp │ ├── external_trigger.cpp │ ├── sensor.cpp │ ├── imu_mpu9150.cpp │ ├── corner_mt9v034.cpp │ ├── imu_adis16488.cpp │ └── camera_mt9v034.cpp ├── visensor │ ├── visensor_datatypes.cpp │ └── visensor_api.cpp ├── work │ ├── camera_tau320.hpp │ ├── imu_mpu6000.hpp │ ├── brisk_mt9v034.hpp │ ├── sensor_calibration.cpp │ ├── calib_mpu6000.cpp │ └── brisk_mt9v034.cpp ├── synchronization │ ├── frame_corner_synchronizer.cpp │ └── time_synchronizer.cpp ├── serial_bridge │ └── SerialHost.cpp └── networking │ └── file_transfer.cpp ├── include ├── config │ ├── config.hpp │ ├── debug.hpp │ ├── assert.hpp │ └── compiler.hpp ├── visensor │ ├── visensor_config.hpp │ ├── visensor_version.hpp │ ├── visensor.hpp │ ├── visensor_exceptions.hpp │ ├── visensor_constants.hpp │ └── visensor_api.hpp ├── sensors │ ├── sensor_factory.hpp │ ├── led_flasher.hpp │ ├── imu.hpp │ ├── imu_adis16488.hpp │ ├── imu_adis16448.hpp │ ├── camera.hpp │ ├── corner_detector.hpp │ ├── corner_mt9v034.hpp │ ├── camera_tau640.hpp │ ├── imu_mpu9150.hpp │ ├── dense_matcher.hpp │ ├── sensor.hpp │ └── external_trigger.hpp ├── networking │ ├── file_transfer.hpp │ ├── auto_discovery.hpp │ ├── config_connection.hpp │ └── connection.hpp ├── helpers │ └── stereo_homography.hpp ├── synchronization │ ├── frame_corner_synchronizer.hpp │ ├── time_synchronizer.hpp │ └── concurrent_queue.hpp ├── serial_bridge │ └── SerialHost.hpp └── visensor_impl.hpp ├── CMakeLists.txt └── README.md /VISensor_Factsheet_web.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/libvisensor/HEAD/VISensor_Factsheet_web.pdf -------------------------------------------------------------------------------- /install_libvisensor.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cmake -H. -Bbuild -DDONT_USE_CATKIN=1 3 | cd build 4 | make -j8 5 | sudo make install 6 | -------------------------------------------------------------------------------- /pkgconfig/visensor.pc: -------------------------------------------------------------------------------- 1 | Name: visensor 2 | Description: Interface library for the VI-Sensor 3 | Requires: 4 | Version: 5 | Libs: -lvisensor 6 | Cflags: -I/usr/include/visensor 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | libvisensor 4 | 1.1.0 5 | 6 | 7 | Driver for the communication with the VI-Sensor 8 | 9 | 10 | http://www.skybotix.com 11 | Michael Burri, Pascal Gohl and Thomas Schneider 12 | Pascal Gohl 13 | Propriatary 14 | catkin 15 | 16 | eigen 17 | 18 | 19 | -------------------------------------------------------------------------------- /cmake/FindVISensor.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find ImageMagick++ 2 | # Once done, this will define 3 | # 4 | # Magick++_FOUND - system has Magick++ 5 | # Magick++_INCLUDE_DIRS - the Magick++ include directories 6 | # Magick++_LIBRARIES - link these to use Magick++ 7 | 8 | include(LibFindMacros) 9 | 10 | # Use pkg-config to get hints about paths 11 | libfind_pkg_check_modules(VISensorDriver_PKGCONF visensor) 12 | 13 | # Include dir 14 | find_path(VISensorDriver_INCLUDE_DIR 15 | NAMES aslam_sensor_driver.hpp 16 | PATHS ${VISensorDriver_PKGCONF_INCLUDE_DIRS} 17 | ) 18 | 19 | 20 | # Finally the library itself 21 | find_library(VISensorDriver_LIBRARY 22 | NAMES libvisensor.so 23 | PATHS ${VISensorDriver_PKGCONF_LIBRARY_DIRS} 24 | ) 25 | 26 | # Set the include dir variables and the libraries and let libfind_process do the rest. 27 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 28 | set(VISensorDriver_PROCESS_INCLUDES VISensorDriver_INCLUDE_DIR) 29 | set(VISensorDriver_PROCESS_LIBS VISensorDriver_LIBRARY) 30 | libfind_process(VISensorDriver) 31 | -------------------------------------------------------------------------------- /src/sensors/led_flasher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * camera_mt9v034.cpp 3 | * 4 | * Created on: Apr 18, 2012 5 | * Author: burrimi 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "sensors/led_flasher.hpp" 13 | #include "visensor_impl.hpp" // needed by forward declaration 14 | 15 | namespace visensor { 16 | LedFlasher::LedFlasher(SensorId::SensorId sensor_id, 17 | ConfigConnection::WeakPtr config_connection) 18 | : Sensor(SensorSettings(sensor_id, 19 | SensorType::SensorType::LIGHT_CONTROL, 20 | 0, 21 | LedFlasherDefaults::NUM_OF_MSGS_IN_PACKAGE, 22 | LedFlasherDefaults::USE_CONST_PACKAGE_SIZE, 23 | LedFlasherDefaults::CALIBRATION_SIZE), 24 | config_connection), 25 | config_(sensor_id) { 26 | } 27 | 28 | LedFlasher::~LedFlasher() { 29 | } 30 | 31 | void LedFlasher::init() { 32 | } 33 | 34 | ViConfigMsg LedFlasher::getConfigParam(std::string cmd, uint32_t value) { 35 | return config_.getConfigParam(cmd, value); 36 | } 37 | 38 | } //namespace visensor 39 | -------------------------------------------------------------------------------- /include/config/config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CONFIG_HPP_ 33 | #define CONFIG_HPP_ 34 | 35 | //global includes 36 | #include //uint32, ... types 37 | 38 | //include debug helpers for library builds 39 | #include "compiler.hpp" 40 | #include "debug.hpp" 41 | #include "assert.hpp" 42 | 43 | 44 | #endif /* CONFIG_HPP_ */ 45 | -------------------------------------------------------------------------------- /include/visensor/visensor_config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_CONFIG_HPP_ 33 | #define VISENSOR_CONFIG_HPP_ 34 | 35 | //Control symbol exports 36 | #ifdef VISENSOR_EXPORT 37 | # include 38 | #else 39 | # define DSO_EXPORT 40 | # define DSO_IMPORT 41 | #endif 42 | 43 | 44 | #endif /* VISENSOR_CONFIG_HPP_ */ 45 | -------------------------------------------------------------------------------- /include/visensor/visensor_version.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_VERSION_HPP_ 33 | #define VISENSOR_VERSION_HPP_ 34 | 35 | #include 36 | 37 | namespace visensor { 38 | 39 | static const unsigned int LIBRARY_VERSION_MAJOR = 1; 40 | static const unsigned int LIBRARY_VERSION_MINOR = 1; 41 | static const unsigned int LIBRARY_VERSION_PATCH = 0; 42 | 43 | } //namespace visensor 44 | 45 | #endif /* VISENSOR_VERSION_HPP_ */ 46 | -------------------------------------------------------------------------------- /include/visensor/visensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Redistribution and non-commercial use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * Redistributions in binary form must reproduce the above copyright notice, this 14 | * list of conditions and the following disclaimer in the documentation and/or 15 | * other materials provided with the distribution. 16 | * 17 | * Neither the name of the {organization} nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 23 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 25 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 28 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef VISENSOR_HPP_ 35 | #define VISENSOR_HPP_ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #endif //VISENSOR_HPP_ 44 | -------------------------------------------------------------------------------- /src/sensors/corner_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | 34 | #include "visensor/visensor_datatypes.hpp" 35 | #include "sensors/corner_detector.hpp" 36 | 37 | namespace visensor { 38 | 39 | void CornerDetector::setCornerCallback(boost::function callback) { 40 | user_callback_ = callback; 41 | } 42 | 43 | void CornerDetector::publishCornerData(ViCorner::Ptr& frame, ViErrorCode error) { 44 | if (user_callback_) 45 | user_callback_(frame, error); 46 | } 47 | 48 | } // namespace visensor 49 | -------------------------------------------------------------------------------- /include/sensors/sensor_factory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef SENSOR_FACTORY_HPP_ 33 | #define SENSOR_FACTORY_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "networking/connection.hpp" 40 | #include "sensors/sensor.hpp" 41 | 42 | namespace visensor { 43 | class SensorFactory { 44 | public: 45 | static const void createSensors( 46 | IpConnection::WeakPtr config_connection, 47 | Sensor::IdMap* sensor_map, 48 | boost::thread_group* threads); 49 | }; 50 | 51 | } //namespace visensor 52 | 53 | #endif /* SENSOR_FACTORY_HPP_ */ 54 | -------------------------------------------------------------------------------- /include/networking/file_transfer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef FILEHANDLER_HPP_ 33 | #define FILEHANDLER_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | class FileTransfer { 39 | public: 40 | FileTransfer(boost::asio::ip::tcp::socket& socket); 41 | virtual ~FileTransfer(); 42 | 43 | void sendFile(std::string& local_path, std::string& remote_path); 44 | void receiveFile(std::string& local_path, std::string& remote_path); 45 | 46 | private: 47 | std::string sendPathString(std::string string); 48 | 49 | boost::asio::ip::tcp::socket& socket_; 50 | }; 51 | 52 | #endif /* FILEHANDLER_HPP_ */ 53 | -------------------------------------------------------------------------------- /src/sensors/camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace visensor { 38 | 39 | void Camera::setFrameCallback(boost::function callback) { 40 | user_callbacks_.push_back(callback); 41 | } 42 | 43 | void Camera::publishFrameData(ViFrame::Ptr& frame, ViErrorCode error) { 44 | if (user_callbacks_.empty() == 0) { 45 | BOOST_FOREACH( 46 | boost::function callback, user_callbacks_) { 47 | callback(frame, error); 48 | } 49 | } 50 | } 51 | 52 | } // namespace visensor 53 | -------------------------------------------------------------------------------- /include/config/debug.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef DEBUG_HPP_ 33 | #define DEBUG_HPP_ 34 | 35 | #include 36 | 37 | #ifdef NDEBUG 38 | #define DEBUG_TEST 0 39 | #else 40 | #define DEBUG_TEST 1 41 | #endif 42 | 43 | 44 | /***************************************************/ 45 | /* DEBUG TERMINAL PRINT */ 46 | /***************************************************/ 47 | 48 | struct Sink { template Sink(Args const& ... ) {} }; 49 | #if DEBUG_TEST 50 | #define VISENSOR_DEBUG(...) \ 51 | do { printf("[libvisensor]: "); printf(__VA_ARGS__); } while (0) 52 | #else 53 | #define VISENSOR_DEBUG(...) Sink { __VA_ARGS__ } 54 | #endif 55 | 56 | 57 | 58 | 59 | #endif /* DEBUG_HPP_ */ 60 | -------------------------------------------------------------------------------- /include/sensors/led_flasher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * camera_mt9v034.hpp 3 | * 4 | * Created on: Apr 18, 2012 5 | * Author: burrimi 6 | */ 7 | 8 | #ifndef LED_FLASHER_HPP_ 9 | #define LED_FLASHER_HPP_ 10 | 11 | #include 12 | #include 13 | 14 | #include "networking/param_server.hpp" 15 | #include "sensors/camera.hpp" 16 | 17 | namespace visensor { 18 | 19 | namespace LedFlasherDefaults { 20 | const int I2C_ADRESS = 0x48, 21 | RATE = CAMERA_FREQUENCY, 22 | NUM_OF_MSGS_IN_PACKAGE = 1, 23 | CALIBRATION_SIZE = 0; 24 | const uint8_t COM_TYPE = ViComType::FPGA_32; 25 | 26 | const bool USE_CONST_PACKAGE_SIZE = true; 27 | } 28 | 29 | class LedFlasherConfig { 30 | public: 31 | LedFlasherConfig(const SensorId::SensorId sensorId) 32 | : sensor_id_(sensorId) { 33 | // name, type, register, default, mask, min, max 34 | param_server_.addParam("strobe", param_server::UINT_T, 0X04, 2, 0, 0, 12287); 35 | param_server_.addParam("strobe_mode", param_server::BOOL_T, 0X00, true, 0x01); 36 | } 37 | 38 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 39 | ViConfigMsg msg; 40 | msg.valChanged = param_server_.getConfigParam(paramName, value, msg.reg, 41 | msg.val); 42 | msg.sensorId = sensor_id_; 43 | msg.devAdress = LedFlasherDefaults::I2C_ADRESS; 44 | msg.comType = LedFlasherDefaults::COM_TYPE; 45 | return msg; 46 | } 47 | 48 | ViConfigMsg getConfigParam(const std::string paramName) { 49 | ViConfigMsg msg; 50 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 51 | msg.sensorId = sensor_id_; 52 | msg.devAdress = LedFlasherDefaults::I2C_ADRESS; 53 | msg.comType = LedFlasherDefaults::COM_TYPE; 54 | return msg; 55 | } 56 | 57 | const SensorId::SensorId sensor_id_; 58 | 59 | private: 60 | param_server::ParamServer param_server_; 61 | }; 62 | 63 | class LedFlasher : public Sensor { 64 | public: 65 | typedef boost::shared_ptr Ptr; 66 | 67 | LedFlasher(SensorId::SensorId sensor_id, 68 | ConfigConnection::WeakPtr config_connection); 69 | virtual ~LedFlasher(); 70 | 71 | virtual void init(); 72 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 73 | 74 | private: 75 | LedFlasherConfig config_; 76 | }; 77 | 78 | } //namespace visensor 79 | 80 | #endif /* LED_FLASHER_HPP_ */ 81 | -------------------------------------------------------------------------------- /include/networking/auto_discovery.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef AutoDiscovery_HPP_ 33 | #define AutoDiscovery_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "visensor/visensor_datatypes.hpp" 41 | 42 | namespace visensor 43 | { 44 | 45 | class AutoDiscovery { 46 | public: 47 | 48 | AutoDiscovery(unsigned short int port); 49 | virtual ~AutoDiscovery(); 50 | 51 | ViDeviceList findSensor(); 52 | 53 | private: 54 | typedef std::set SensorSet; 55 | std::vector getIpList(); 56 | 57 | boost::asio::io_service io_service_; 58 | boost::asio::ip::udp::socket socket_; 59 | unsigned short int port_; 60 | char* data_; 61 | boost::asio::ip::udp::endpoint sensor_endpoint_; 62 | }; 63 | 64 | } //namespace visensor 65 | 66 | #endif /* AutoDiscovery_HPP_ */ 67 | -------------------------------------------------------------------------------- /src/visensor/visensor_datatypes.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | 34 | 35 | namespace visensor 36 | { 37 | 38 | ViFrame::ViFrame() 39 | { 40 | allocated_image_bytes=0; 41 | height=0; 42 | width=0; 43 | camera_id=0; 44 | timestamp=0; 45 | timestamp_host=0; 46 | id=0; 47 | image_type=ViImageType::NOT_SET; 48 | useCorners=0; 49 | // allocated_image_bytes=image_size; 50 | // image = new unsigned char[allocated_image_bytes]; 51 | } 52 | 53 | ViFrame::~ViFrame() 54 | { 55 | } 56 | 57 | int ViFrame::getBufferSize() 58 | { 59 | return allocated_image_bytes; 60 | } 61 | 62 | void ViFrame::setImageRawPtr(boost::shared_ptr buffer, 63 | uint32_t buffer_size) { 64 | image=buffer; 65 | allocated_image_bytes=buffer_size; 66 | } 67 | 68 | uint8_t* ViFrame::getImageRawPtr() 69 | { 70 | return image.get(); 71 | } 72 | 73 | } //namespace visensor 74 | 75 | -------------------------------------------------------------------------------- /include/networking/config_connection.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CONFIGCONNECTION_HPP_ 33 | #define CONFIGCONNECTION_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | namespace visensor { 41 | 42 | class ConfigConnection { 43 | public: 44 | 45 | typedef boost::shared_ptr Ptr; 46 | typedef boost::weak_ptr WeakPtr; 47 | 48 | ConfigConnection() {}; 49 | virtual ~ConfigConnection() {}; 50 | 51 | virtual void writeConfig(SensorId::SensorId sensor_id, uint8_t dev_adress, uint8_t reg, 52 | uint32_t val, uint8_t comType) = 0; 53 | virtual void readConfig(SensorId::SensorId sensor_id, uint8_t dev_adress, uint8_t reg, 54 | uint32_t &val, uint8_t comType) = 0; 55 | }; 56 | 57 | } /* namespace visensor */ 58 | 59 | #endif /* CONFIGCONNECTION_HPP_ */ 60 | -------------------------------------------------------------------------------- /include/config/assert.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef ASSERT_HPP_ 33 | #define ASSERT_HPP_ 34 | 35 | /***************************************************/ 36 | /* ASSERT MACROS */ 37 | /***************************************************/ 38 | //asserts if cond is true 39 | #define VISENSOR_ASSERT_COND(cond, ...) \ 40 | do { \ 41 | if ((cond)) { \ 42 | printf("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n\tcond = %s\n", __FILE__, __LINE__, #cond); \ 43 | do { printf("\tmsg: "); printf(__VA_ARGS__); printf("\n");} while (0); \ 44 | exit(1); \ 45 | } \ 46 | } while (0) 47 | 48 | 49 | #define VISENSOR_ASSERT(...) \ 50 | do { \ 51 | printf("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n", __FILE__, __LINE__); \ 52 | do { printf("\tmsg: "); printf(__VA_ARGS__); printf("\n");} while (0); \ 53 | exit(1); \ 54 | } while (0) 55 | 56 | 57 | 58 | #endif /* ASSERT_HPP_ */ 59 | -------------------------------------------------------------------------------- /include/helpers/stereo_homography.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef STEREOHOMOGRAPHY_HPP_ 33 | #define STEREOHOMOGRAPHY_HPP_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | class StereoHomography { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | StereoHomography(const visensor::ViCameraCalibration& calib_cam0, 44 | const visensor::ViCameraCalibration& calib_cam1); 45 | 46 | //Computes homography for stereo rectification 47 | void getHomography(Eigen::Matrix3d& H0, Eigen::Matrix3d& H1, double& f_new, Eigen::Vector2d& p0_new, 48 | Eigen::Vector2d& p1_new); 49 | 50 | private: 51 | double r0_[9]; 52 | double t0_[3]; 53 | double f0_[2]; 54 | double p0_[2]; 55 | double d0_[5]; 56 | double r1_[9]; 57 | double t1_[3]; 58 | double f1_[2]; 59 | double p1_[2]; 60 | double d1_[5]; 61 | int image_width_; 62 | int image_height_; 63 | }; 64 | 65 | #endif /* STEREOHOMOGRAPHY_HPP_ */ 66 | -------------------------------------------------------------------------------- /include/synchronization/frame_corner_synchronizer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef FRAMECORNERSYNCHRONIZER_HPP_ 33 | #define FRAMECORNERSYNCHRONIZER_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "visensor/visensor_datatypes.hpp" 42 | 43 | using namespace visensor; 44 | 45 | class FrameCornerSynchronizer { 46 | public: 47 | FrameCornerSynchronizer(); 48 | ~FrameCornerSynchronizer(); 49 | 50 | void addFrame(ViFrame::Ptr frame); 51 | void addCorner(ViCorner::Ptr corner); 52 | void setUserCallback( 53 | boost::function callback); 54 | 55 | private: 56 | boost::function< void (ViFrame::Ptr, ViCorner::Ptr)> user_callback_; 57 | mutable boost::mutex mutex_; 58 | boost::condition_variable cond_; 59 | std::queue corner_queue_; 60 | }; 61 | 62 | #endif /* FRAMECORNERSYNCHRONIZER_HPP_ */ 63 | -------------------------------------------------------------------------------- /include/config/compiler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef COMPILER_HPP_ 33 | #define COMPILER_HPP_ 34 | 35 | #ifndef VISENSOR_PRIVATE_API_ACCESS 36 | 37 | #if defined _WIN32 || defined __CYGWIN__ 38 | #ifdef BUILDING_DSO 39 | #ifdef __GNUC__ 40 | #define DSO_EXPORT __attribute__ ((DSOexport)) 41 | #else 42 | #define DSO_EXPORT __declspec(DSOexport) // Note: actually gcc seems to also supports this syntax. 43 | #endif 44 | #else 45 | #ifdef __GNUC__ 46 | #define DSO_EXPORT __attribute__ ((DSOimport)) 47 | #else 48 | #define DSO_EXPORT __declspec(DSOimport) // Note: actually gcc seems to also supports this syntax. 49 | #endif 50 | #endif 51 | #define DSO_HIDDEN 52 | #else 53 | #if __GNUC__ >= 4 54 | #define DSO_EXPORT __attribute__ ((visibility ("default"))) 55 | #define DSO_HIDDEN __attribute__ ((visibility ("hidden"))) 56 | #else 57 | #define DSO_EXPORT 58 | #define DSO_HIDDEN 59 | #endif 60 | #endif 61 | 62 | #endif 63 | 64 | #endif /* COMPILER_HPP_ */ 65 | -------------------------------------------------------------------------------- /include/visensor/visensor_exceptions.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_EXCEPTIONS_HPP_ 33 | #define VISENSOR_EXCEPTIONS_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | namespace visensor { 39 | 40 | class DSO_EXPORT exceptions : public std::runtime_error 41 | { 42 | public: 43 | exceptions(std::string msg): std::runtime_error(msg) {} 44 | 45 | class ConnectionException; 46 | class SensorException; 47 | class FirmwareException; 48 | }; 49 | 50 | class DSO_EXPORT exceptions::ConnectionException : public exceptions 51 | { 52 | public: 53 | ConnectionException(std::string msg): exceptions(msg) {} 54 | }; 55 | 56 | class DSO_EXPORT exceptions::SensorException : public exceptions 57 | { 58 | public: 59 | SensorException(std::string msg): exceptions(msg) {} 60 | }; 61 | 62 | class DSO_EXPORT exceptions::FirmwareException : public exceptions 63 | { 64 | public: 65 | FirmwareException(std::string msg): exceptions(msg) {} 66 | }; 67 | 68 | 69 | } //namespace visensor 70 | 71 | #endif /* VISENSOR_EXCEPTIONS_HPP_ */ 72 | -------------------------------------------------------------------------------- /include/sensors/imu.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef IMU_HPP_ 33 | #define IMU_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "sensors/sensor.hpp" 40 | 41 | namespace visensor { 42 | #define STANDARD_GRAVITY 9.80665 43 | 44 | class Imu : public Sensor { 45 | public: 46 | typedef boost::shared_ptr Ptr; 47 | 48 | Imu(SensorId::SensorId sensor_id, SensorSettings sensorSettings, 49 | ConfigConnection::WeakPtr config_connection) 50 | : Sensor(sensorSettings, config_connection), 51 | imu_id_(sensor_id) { 52 | } 53 | virtual ~Imu() { 54 | } 55 | ; 56 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value)=0; 57 | 58 | void publishImuData(ViImuMsg::Ptr imu, ViErrorCode error); 59 | void setUserCallback(boost::function callback); 60 | virtual void init() = 0; 61 | 62 | protected: 63 | boost::function user_callback_; 64 | const SensorId::SensorId imu_id_; 65 | }; 66 | } //namespace visensor 67 | 68 | #endif /* IMU_HPP_ */ 69 | -------------------------------------------------------------------------------- /include/serial_bridge/SerialHost.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef SERIALHOST_HPP_ 33 | #define SERIALHOST_HPP_ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "synchronization/concurrent_queue.hpp" 43 | #include "visensor/visensor_datatypes.hpp" 44 | 45 | namespace visensor { 46 | 47 | class SerialHost { 48 | 49 | private: 50 | void processSerialData(); 51 | void publishSerialData(ViSerialData::Ptr& data_ptr); 52 | 53 | boost::shared_ptr worker_thread; 54 | 55 | std::vector > user_callbacks_; 56 | concurrent_queue > data_queue_; 57 | 58 | public: 59 | SerialHost(); 60 | virtual ~SerialHost(); 61 | 62 | void setSerialDataCallback(boost::function callback); 63 | void addDataToPublishQueue(ViSerialData::Ptr data_ptr); 64 | 65 | typedef boost::shared_ptr Ptr; 66 | typedef boost::weak_ptr WeakPtr; 67 | }; 68 | 69 | } /* namespace visensor */ 70 | #endif /* SERIALHOST_HPP_ */ 71 | -------------------------------------------------------------------------------- /src/work/camera_tau320.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * camera_tau320.hpp 3 | * 4 | * Created on: Apr 18, 2012 5 | * Author: burrimi 6 | */ 7 | 8 | #ifndef CAMERA_TAU320_HPP_ 9 | #define CAMERA_TAU320_HPP_ 10 | 11 | #include "camera.hpp" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "param_server.hpp" 18 | 19 | namespace visensor { 20 | 21 | namespace CameraTau320Defaults { 22 | const int I2C_ADRESS = 0x48, WIDTH = 324, HEIGHT = 256, RATE = CAMERA_FREQUENCY, 23 | NUM_OF_MSGS_IN_PACKAGE = 1, CALIBRATION_SIZE = 0; 24 | const ViImageType TYPE = MONO16; 25 | const bool USE_CONST_PACKAGE_SIZE = true; 26 | } 27 | 28 | class CameraTau320Config { 29 | public: 30 | CameraTau320Config(const uint8_t sensorId) 31 | : sensor_id_(sensorId) { 32 | } 33 | 34 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 35 | ViConfigMsg msg; 36 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 37 | msg.sensorId = sensor_id_; 38 | msg.devAdress = CameraTau320Defaults::I2C_ADRESS; 39 | msg.comType = ViComType::I2C_16; 40 | return msg; 41 | } 42 | 43 | ViConfigMsg getConfigParam(const std::string paramName) { 44 | ViConfigMsg msg; 45 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 46 | msg.sensorId = sensor_id_; 47 | msg.devAdress = CameraTau320Defaults::I2C_ADRESS; 48 | msg.comType = ViComType::I2C_16; 49 | return msg; 50 | } 51 | 52 | private: 53 | param_server::ParamServer param_server_; 54 | const uint8_t sensor_id_; 55 | }; 56 | 57 | class CameraTau320 : public Camera { 58 | public: 59 | CameraTau320(SensorId::SensorId sensor_id, int stream_id, 60 | IpConnection::WeakPtr config_connection); 61 | virtual ~CameraTau320(); 62 | 63 | virtual ViConfigMsg getConfigParam(std::string cmd, uint16_t value); 64 | //the threaded function that works on the queue of finished measurements 65 | void processMeasurements(); 66 | virtual bool init(); 67 | virtual bool setCalibration(CalibBase::Ptr calibration); 68 | static uint32_t calculateBufferSize(); 69 | 70 | private: 71 | void allocateBuffer(); 72 | void checkTimestamp(uint64_t timestamp); 73 | void bitReshuffle(uint8_t *in /*10 bit values*/, 74 | uint8_t *out /*8 bit values*/, 75 | int size_in /*number of 10 bit values*/, int offset); 76 | void imageReshuffle(uint8_t *data_in /*10 bit raw data*/, uint8_t *image_out, 77 | int image_height, int image_width); 78 | 79 | uint32_t frame_size_; /* size of one frame */ 80 | int width_; 81 | int height_; 82 | CameraTau320Config config_; 83 | }; 84 | 85 | } //namespace visensor 86 | 87 | #endif /* CAMERA_TAU320_HPP_ */ 88 | -------------------------------------------------------------------------------- /src/sensors/imu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | 34 | #include 35 | 36 | namespace visensor 37 | { 38 | 39 | void Imu::setUserCallback(boost::function callback) 40 | { 41 | user_callback_=callback; 42 | } 43 | 44 | void Imu::publishImuData(ViImuMsg::Ptr imu, ViErrorCode error) 45 | { 46 | ///////////////////////////// 47 | // 48 | // WORKAROUND FOR FPGA BUG 49 | // 50 | // some bug in the imu pipeline causes ~4 old imu measurements to be stuck 51 | // in some buffer. these measurements are released at reconnect. 52 | // 53 | // therefore we drop the first 4 messages, until we have an fpga fix 54 | // 55 | ///////////////////////////// 56 | 57 | //discard the first 4 real imu messages (1 imu message = 10 imu measurements) 58 | static int drop_counter = 41; 59 | if(drop_counter > 0) 60 | { 61 | //only real frames count (= no error) 62 | if(error==visensor::ViErrorCodes::NO_ERROR) 63 | drop_counter--; 64 | return; 65 | } 66 | 67 | // END WORKAROUND FOR FPGA BUG 68 | ///////////////////////////// 69 | 70 | //publish 71 | if(user_callback_) 72 | user_callback_(imu, error); 73 | } 74 | 75 | } //namespace visensor 76 | -------------------------------------------------------------------------------- /include/synchronization/time_synchronizer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef TIME_SYNCHRONIZER_HPP_ 33 | #define TIME_SYNCHRONIZER_HPP_ 34 | 35 | #include 36 | 37 | namespace TimeSynchronizerValues { 38 | const double TIME_KALMAN_GAIN = 0.001; 39 | const uint32_t NUM_OF_MEASUREMENTS = 5; 40 | const uint32_t SKIP_FIRST_N = 20; 41 | } 42 | 43 | class TimeSynchronizer { 44 | private: 45 | 46 | int64_t _offset_tracker; 47 | uint64_t _initial_offset; 48 | uint64_t _previous_timestamp; 49 | uint32_t _counter; 50 | int64_t _offset; 51 | uint32_t unusable_frames_counter_; 52 | 53 | uint64_t _timeFpgaAtUpdate; // this time is needed to assign consistent timestamps to messages that 54 | // arrive after the update, but were captured by the fpga before the update 55 | uint64_t _offsetBeforeUpdate; 56 | 57 | public: 58 | TimeSynchronizer(); 59 | void init(uint64_t time_fpga); 60 | void init(uint64_t time_pc, uint64_t time_fpga); 61 | bool _synchronized; 62 | void updateTime(uint64_t time_fpga); 63 | void updateTime(uint64_t time_pc, uint64_t time_fpga); 64 | uint64_t getSynchronizedTime(uint64_t time_fpga); 65 | uint64_t getSystemTime(); 66 | bool isSynchronized() { 67 | return _synchronized; 68 | } 69 | ; 70 | }; 71 | 72 | #endif /* TIME_SYNCHRONIZER_HPP_ */ 73 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #Thomas Schneider, 2/12/2013 2 | #Pascal Gohl, 21/08/2014 3 | cmake_minimum_required(VERSION 2.8.0) 4 | 5 | ############ 6 | # CATKIN STUFF 7 | ############ 8 | project(libvisensor) 9 | 10 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 11 | find_package(Eigen REQUIRED) 12 | 13 | if(NOT DEFINED DONT_USE_CATKIN) 14 | find_package(catkin QUIET) 15 | catkin_package( 16 | INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} 17 | CATKIN_DEPENDS 18 | LIBRARIES visensor 19 | ) 20 | endif() 21 | 22 | ############ 23 | # SETTINGS 24 | ############ 25 | set(PKGNAME visensor) 26 | 27 | SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) 28 | SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) 29 | 30 | ########### 31 | # BUILD 32 | ########### 33 | FILE( 34 | GLOB SRCS 35 | src/*.cpp 36 | src/sensors/*.cpp 37 | src/networking/*.cpp 38 | src/synchronization/*.cpp 39 | src/serial_bridge/*.cpp 40 | src/visensor/*.cpp 41 | src/helpers/*.cpp 42 | ) 43 | INCLUDE_DIRECTORIES("include") 44 | include_directories(${EIGEN_INCLUDE_DIR}) 45 | 46 | #Release / debug build 47 | 48 | ADD_DEFINITIONS (-DVISENSOR_EXPORT -fPIC -fmessage-length=0 -MMD -MP -Wall -pedantic -std=c++0x -fvisibility=hidden) 49 | if(NOT DEFINED CMAKE_BUILD_TYPE) 50 | set(CMAKE_BUILD_TYPE Release) 51 | ADD_DEFINITIONS (-march=native -O3) 52 | endif(NOT DEFINED CMAKE_BUILD_TYPE) 53 | 54 | #shared library 55 | ADD_LIBRARY(${PKGNAME} SHARED ${SRCS}) 56 | SET_TARGET_PROPERTIES(${PKGNAME} PROPERTIES OUTPUT_NAME ${PKGNAME}) 57 | TARGET_LINK_LIBRARIES(${PKGNAME} boost_thread boost_system) 58 | 59 | #static library 60 | ADD_LIBRARY(${PKGNAME}_static STATIC ${SRCS}) 61 | SET_TARGET_PROPERTIES(${PKGNAME}_static PROPERTIES OUTPUT_NAME ${PKGNAME}) 62 | TARGET_LINK_LIBRARIES(${PKGNAME}_static boost_thread boost_system) 63 | 64 | ########### 65 | # INSTALL 66 | ########### 67 | #shared library 68 | INSTALL( 69 | FILES lib/lib${PKGNAME}.so 70 | DESTINATION lib 71 | COMPONENT shared-lib 72 | ) 73 | 74 | #static library + headers (-dev package) 75 | INSTALL( 76 | FILES lib/lib${PKGNAME}.a 77 | DESTINATION lib 78 | COMPONENT static-lib 79 | ) 80 | 81 | INSTALL( 82 | FILES include/visensor/visensor_api.hpp 83 | include/visensor/visensor_config.hpp 84 | include/visensor/visensor_constants.hpp 85 | include/visensor/visensor_datatypes.hpp 86 | include/visensor/visensor_exceptions.hpp 87 | include/visensor/visensor_version.hpp 88 | include/visensor/visensor.hpp 89 | DESTINATION include/visensor 90 | COMPONENT static-lib 91 | ) 92 | 93 | INSTALL( 94 | FILES cmake/FindVISensor.cmake 95 | DESTINATION lib/visensor 96 | COMPONENT static-lib 97 | ) 98 | 99 | INSTALL( 100 | FILES pkgconfig/visensor.pc 101 | DESTINATION share/pkgconfig 102 | COMPONENT 103 | static-lib 104 | ) 105 | -------------------------------------------------------------------------------- /include/sensors/imu_adis16488.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef IMU_ADIS16488_HPP_ 33 | #define IMU_ADIS16488_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/connection.hpp" 39 | #include "sensors/imu.hpp" 40 | 41 | namespace visensor 42 | { 43 | namespace ImuAdis16488Defaults 44 | { 45 | const int 46 | RATE=IMU_FREQUENCY, 47 | MSG_SIZE=(7*4+4*2), 48 | NUM_OF_MSGS_IN_PACKAGE=10, 49 | CALIBRATION_SIZE=0; 50 | const bool USE_CONST_PACKAGE_SIZE=true; 51 | } 52 | 53 | class ImuAdis16488 : public Imu { 54 | public: 55 | typedef boost::shared_ptr Ptr; 56 | 57 | ImuAdis16488(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection); 58 | virtual ~ImuAdis16488(){}; 59 | 60 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 61 | void processMeasurements(); 62 | virtual void init(); 63 | static uint32_t calculateBufferSize(); 64 | 65 | private: 66 | void allocateBuffer(); 67 | int getNumOfNewMsgs(); 68 | void resetBuffer(); 69 | void getGyro(uint8_t * buffer, double * gyro); 70 | void getAcc(uint8_t * buffer, double * acc); 71 | void getMag(uint8_t * buffer, double * mag); 72 | void getBaro(uint8_t * buffer, double * baro); 73 | void getTemp(uint8_t * buffer, double * temp); 74 | void newImuMsgs(); 75 | uint64_t getTimestamp(); 76 | }; 77 | } //namespace visensor 78 | 79 | #endif /* IMU_ADIS16488_HPP_ */ 80 | -------------------------------------------------------------------------------- /include/sensors/imu_adis16448.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef IMU_ADIS16448_HPP_ 33 | #define IMU_ADIS16448_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/connection.hpp" 39 | #include "sensors/imu.hpp" 40 | 41 | namespace visensor 42 | { 43 | namespace ImuAdis16448Defaults 44 | { 45 | const int 46 | RATE=IMU_FREQUENCY, 47 | MSG_SIZE=(10+6*2), 48 | NUM_OF_MSGS_IN_PACKAGE=10, 49 | CALIBRATION_SIZE=0; 50 | const bool USE_CONST_PACKAGE_SIZE=true; 51 | } 52 | 53 | class ImuAdis16448 : public Imu { 54 | public: 55 | typedef boost::shared_ptr Ptr; 56 | 57 | ImuAdis16448(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection); 58 | virtual ~ImuAdis16448(){}; 59 | 60 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 61 | //the threaded function that works on the queue of finished measurements 62 | void processMeasurements(); 63 | virtual void init(); 64 | static uint32_t calculateBufferSize(); 65 | 66 | private: 67 | void allocateBuffer(); 68 | int getNumOfNewMsgs(); 69 | void resetBuffer(); 70 | void getGyro(uint8_t * buffer, double * gyro); 71 | void getAcc(uint8_t * buffer, double * acc); 72 | void getMag(uint8_t * buffer, double * mag); 73 | void getBaro(uint8_t * buffer, double * baro); 74 | void getTemp(uint8_t * buffer, double * temp); 75 | void newImuMsgs(); 76 | uint64_t getTimestamp(); 77 | }; 78 | } //namespace visensor 79 | 80 | #endif /* IMU_ADIS16448_HPP_ */ 81 | -------------------------------------------------------------------------------- /src/work/imu_mpu6000.hpp: -------------------------------------------------------------------------------- 1 | #ifndef IMU_MPU6000_HPP_ 2 | #define IMU_MPU6000_HPP_ 3 | 4 | #include "imu.hpp" 5 | #include "param_server.hpp" 6 | 7 | namespace visensor { 8 | 9 | namespace ImuMpu6000Defaults { 10 | const int I2C_ADRESS = 0x68, RATE = IMU_FREQUENCY, MSG_SIZE = (6 * 2 + 6 * 2), 11 | NUM_OF_MSGS_IN_PACKAGE = 50, CALIBRATION_SIZE = 0; 12 | const uint8_t COM_TYPE = ViComType::I2C_8; 13 | const bool USE_CONST_PACKAGE_SIZE = true; 14 | } 15 | 16 | class ImuMpu6000Config { 17 | public: 18 | ImuMpu6000Config(const uint8_t sensorId) 19 | : sensor_id_(sensorId) { 20 | // name, type, register, default, mask, min, max 21 | 22 | param_server_.addParam("enable", param_server::CONST_T, 0x6B, 0x0000, 23 | 0x00FF); // enable mpu 6000 page 41 24 | param_server_.addParam("digital_low_pass_filter_config", 25 | param_server::UINT_T, 0x1A, 0x0002, 0x000F, 0, 6); // config parameters page 13 26 | // settings: 0 Bandwith: 260 Hz, Delay 0 ms 27 | // settings: 1 Bandwith: 184 Hz, Delay 2.0 ms 28 | // settings: 2 Bandwith: 94 Hz, Delay 3.0 ms 29 | // settings: 3 Bandwith: 44 Hz, Delay 4.9 ms 30 | // settings: 4 Bandwith: 21 Hz, Delay 8.5 ms 31 | // settings: 5 Bandwith: 10 Hz, Delay 13.8 ms 32 | // settings: 6 Bandwith: 5 Hz, Delay 19.0 ms 33 | 34 | } 35 | 36 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 37 | ViConfigMsg msg; 38 | msg.valChanged = param_server_.getConfigParam(paramName, value, msg.reg, 39 | msg.val); 40 | msg.sensorId = sensor_id_; 41 | msg.devAdress = ImuMpu6000Defaults::I2C_ADRESS; 42 | msg.comType = ImuMpu6000Defaults::COM_TYPE; 43 | return msg; 44 | } 45 | 46 | ViConfigMsg getConfigParam(const std::string paramName) { 47 | ViConfigMsg msg; 48 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 49 | msg.sensorId = sensor_id_; 50 | msg.devAdress = ImuMpu6000Defaults::I2C_ADRESS; 51 | msg.comType = ImuMpu6000Defaults::COM_TYPE; 52 | return msg; 53 | } 54 | 55 | private: 56 | param_server::ParamServer param_server_; 57 | const uint8_t sensor_id_; 58 | }; 59 | 60 | class ImuMpu6000 : public Imu { 61 | public: 62 | ImuMpu6000(int sensor_id, int imu_id, 63 | ViSensorDriver::Impl* ViSensorDriver); 64 | virtual ~ImuMpu6000() { 65 | } 66 | ; 67 | 68 | virtual ViConfigMsg getConfigParam(std::string cmd, uint16_t value); 69 | //the threaded function that works on the queue of finished measurements 70 | void processMeasurements(); 71 | virtual bool init(); 72 | virtual bool setCalibration(CalibBase::Ptr calibration); 73 | static uint32_t calculateBufferSize(); 74 | 75 | private: 76 | void allocateBuffer(); 77 | void getGyro(uint8_t* buffer, double * gyro); 78 | void getAcc(uint8_t* buffer, double * acc); 79 | void getGyro2(uint8_t* buffer, double * gyro); 80 | void getAcc2(uint8_t* buffer, double * acc); 81 | // void getMag(double * mag); 82 | // void getBaro(double * baro); 83 | void incrementMsgCounter(); 84 | void newImuMsgs(); 85 | 86 | ImuMpu6000Config config_; 87 | CalibImuMpu6000 calibration_; 88 | }; 89 | } //namespace visensor 90 | 91 | #endif /* IMU_MPU6000_HPP_ */ 92 | -------------------------------------------------------------------------------- /src/synchronization/frame_corner_synchronizer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "synchronization/frame_corner_synchronizer.hpp" 38 | 39 | FrameCornerSynchronizer::FrameCornerSynchronizer() { 40 | } 41 | FrameCornerSynchronizer::~FrameCornerSynchronizer() { 42 | } 43 | 44 | void FrameCornerSynchronizer::addFrame(ViFrame::Ptr frame) { 45 | boost::mutex::scoped_lock lock(mutex_); 46 | while (corner_queue_.empty()) 47 | cond_.wait(lock); 48 | 49 | if (frame->timestamp < corner_queue_.front()->timestamp) { 50 | VISENSOR_DEBUG( 51 | "no matching corners found, publishing image without corners!!!\n"); 52 | frame->useCorners = false; 53 | return; 54 | } 55 | 56 | while (frame->timestamp > corner_queue_.front()->timestamp) { 57 | corner_queue_.pop(); 58 | if (corner_queue_.empty()) 59 | cond_.wait(lock); 60 | } 61 | 62 | if (frame->timestamp != corner_queue_.front()->timestamp) { 63 | VISENSOR_DEBUG("pair NOT found: this should not happen!!!\n"); 64 | return; 65 | } 66 | ViCorner::Ptr corner = corner_queue_.front(); 67 | corner_queue_.pop(); 68 | 69 | if (user_callback_) 70 | user_callback_(frame, corner); 71 | } 72 | 73 | void FrameCornerSynchronizer::addCorner(ViCorner::Ptr corner) { 74 | { 75 | boost::mutex::scoped_lock lock(mutex_); 76 | corner_queue_.push(corner); 77 | } 78 | cond_.notify_one(); 79 | } 80 | 81 | void FrameCornerSynchronizer::setUserCallback( 82 | boost::function< 83 | void(ViFrame::Ptr, ViCorner::Ptr)> callback) { 84 | user_callback_ = callback; 85 | } 86 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | -------------------------------------------------------------------------------- /include/sensors/camera.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CAMERA_HPP_ 33 | #define CAMERA_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "sensor.hpp" 40 | 41 | namespace visensor { 42 | 43 | struct ViCameraConfig { 44 | ViCameraConfig(uint32_t width_, uint32_t height_, uint32_t frame_rate_) 45 | : width(width_), 46 | height(height_), 47 | frame_rate(frame_rate_) 48 | {}; 49 | 50 | uint32_t width; /* the image width */ 51 | uint32_t height; /* the image height */ 52 | uint32_t frame_rate; /* the camera frame rate */ 53 | }; 54 | 55 | class Camera : public Sensor { 56 | public: 57 | Camera(SensorId::SensorId sensor_id, ViCameraConfig config, 58 | SensorSettings sensorSettings, 59 | ConfigConnection::WeakPtr config_connection) 60 | : Sensor(sensorSettings, config_connection), 61 | config_(config), 62 | camera_id_(sensor_id) {}; 63 | 64 | virtual ~Camera() {}; 65 | 66 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value) = 0; 67 | void publishFrameData(ViFrame::Ptr& frame, ViErrorCode error); 68 | void setFrameCallback(boost::function callback); 69 | 70 | //the threaded function that works on the queue of finished measurements 71 | virtual void processMeasurements() = 0; 72 | virtual void init() = 0; 73 | 74 | typedef boost::shared_ptr Ptr; 75 | 76 | public: 77 | ViCameraConfig config_; /* the camera configuration */ 78 | 79 | protected: 80 | std::vector > user_callbacks_; 81 | const SensorId::SensorId camera_id_; 82 | }; 83 | } //namespace visensor 84 | 85 | #endif /* CAMERA_HPP_ */ 86 | -------------------------------------------------------------------------------- /include/sensors/corner_detector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef Corner_DETECTOR_HPP_ 33 | #define Corner_DETECTOR_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "sensors/sensor.hpp" 40 | 41 | namespace visensor { 42 | 43 | struct ViCornerConfig { 44 | ViCornerConfig(uint32_t width_, uint32_t height_, uint32_t frame_rate_) 45 | : width(width_), 46 | height(height_), 47 | frame_rate(frame_rate_) { 48 | } 49 | ; 50 | uint32_t width; /* the image width */ 51 | uint32_t height; /* the image height */ 52 | uint32_t frame_rate; /* the Corner frame rate */ 53 | }; 54 | 55 | class CornerDetector : public Sensor { 56 | public: 57 | typedef boost::shared_ptr Ptr; 58 | 59 | CornerDetector(SensorId::SensorId sensor_id, ViCornerConfig config, 60 | SensorSettings sensorSettings, 61 | ConfigConnection::WeakPtr config_connection) 62 | : Sensor(sensorSettings, config_connection), 63 | config_(config), 64 | camera_id_(sensor_id){ 65 | } 66 | ; 67 | 68 | virtual ~CornerDetector() { 69 | } 70 | ; 71 | 72 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value) = 0; 73 | void publishCornerData(ViCorner::Ptr& frame, ViErrorCode error); 74 | void setCornerCallback(boost::function callback); 75 | //the threaded function that works on the queue of finished measurements 76 | virtual void processMeasurements() = 0; 77 | virtual void init() = 0; 78 | 79 | protected: 80 | boost::function user_callback_; 81 | ViCornerConfig config_; /* the Corner configuration */ 82 | const uint32_t camera_id_; 83 | }; 84 | } //namespace visensor 85 | 86 | #endif /* Corner_HPP_ */ 87 | -------------------------------------------------------------------------------- /src/serial_bridge/SerialHost.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "visensor/visensor_datatypes.hpp" 39 | #include "serial_bridge/SerialHost.hpp" 40 | 41 | namespace visensor { 42 | 43 | SerialHost::SerialHost() 44 | { 45 | //start processing thread 46 | worker_thread = boost::make_shared(&SerialHost::processSerialData, this); 47 | } 48 | 49 | SerialHost::~SerialHost() { 50 | //stop thread 51 | try { 52 | if(worker_thread) 53 | worker_thread->interrupt(); 54 | } catch (const std::exception &ex) { 55 | VISENSOR_DEBUG("SerialHost exception in destructor: %s\n", ex.what()); 56 | } 57 | 58 | //wait for it to stop 59 | worker_thread->join(); 60 | } 61 | 62 | //the threaded function that works on the queue of finished measurements 63 | void SerialHost::processSerialData() { 64 | 65 | while (1) { 66 | boost::this_thread::interruption_point(); 67 | 68 | //get the newest measurement (waits if no new msgs available..) 69 | ViSerialData::Ptr data_ptr = data_queue_.pop(); 70 | 71 | //send out to user application 72 | publishSerialData(data_ptr); 73 | } 74 | } 75 | 76 | void SerialHost::setSerialDataCallback(boost::function callback) { 77 | user_callbacks_.push_back(callback); 78 | } 79 | 80 | void SerialHost::publishSerialData(ViSerialData::Ptr& data_ptr) { 81 | if (user_callbacks_.empty() == 0) { 82 | BOOST_FOREACH( boost::function callback, user_callbacks_) 83 | { callback(data_ptr); } 84 | } 85 | } 86 | 87 | void SerialHost::addDataToPublishQueue(ViSerialData::Ptr data_ptr) 88 | { 89 | data_queue_.push(data_ptr); 90 | } 91 | 92 | } /* namespace visensor */ 93 | -------------------------------------------------------------------------------- /src/work/brisk_mt9v034.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * corner_mt9v034.hpp 3 | * 4 | * Created on: Apr 18, 2012 5 | * Author: burrimi 6 | */ 7 | 8 | #ifndef BRISK_MT9V034_HPP_ 9 | #define BRISK_MT9V034_HPP_ 10 | 11 | #include "camera.hpp" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "param_server.hpp" 18 | 19 | namespace visensor { 20 | 21 | namespace BriskMt9v034Defaults { 22 | const int I2C_ADRESS = 0x48, WIDTH = 752, HEIGHT = 480, MAX_FEATURES = 30000, 23 | RATE = CAMERA_FREQUENCY, SCORE_BYTES = 4, BRISK_BYTES = 48, 24 | NUM_OF_MSGS_IN_PACKAGE = 1, CALIBRATION_SIZE = 0; 25 | const uint8_t COM_TYPE = ViComType::I2C_16; 26 | const bool USE_CONST_PACKAGE_SIZE = false; 27 | } 28 | 29 | class BriskMt9v034Config { 30 | private: 31 | param_server::ParamServer _paramServer; 32 | const uint8_t _sensorId; 33 | public: 34 | BriskMt9v034Config(const uint8_t sensorId) 35 | : _sensorId(sensorId) { 36 | // name, type, register, default, mask, min, max 37 | 38 | //_paramServer.addParam("coarse_shutter_width",param_server::UINT_T,0x0B, 480, 0x7FFF,2,32765); //page 27 39 | } 40 | 41 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 42 | ViConfigMsg msg; 43 | msg.valChanged = _paramServer.getConfigParam(paramName, value, msg.reg, 44 | msg.val); 45 | msg.sensorId = _sensorId; 46 | msg.devAdress = BriskMt9v034Defaults::I2C_ADRESS; 47 | msg.comType = BriskMt9v034Defaults::COM_TYPE; 48 | return msg; 49 | } 50 | 51 | ViConfigMsg getConfigParam(const std::string paramName) { 52 | ViConfigMsg msg; 53 | msg.valChanged = _paramServer.getConfigParam(paramName, msg.reg, msg.val); 54 | msg.sensorId = _sensorId; 55 | msg.devAdress = BriskMt9v034Defaults::I2C_ADRESS; 56 | msg.comType = BriskMt9v034Defaults::COM_TYPE; 57 | return msg; 58 | } 59 | 60 | }; 61 | 62 | class BriskMt9v034 : public Camera { 63 | public: 64 | BriskMt9v034(int sensor_id, int camera_id, 65 | ViSensorDriver::Impl* ViSensorDriver); 66 | virtual ~BriskMt9v034(); 67 | // virtual int processData(uint8_t const * data,int nBytesReceived); //this function will get called by the ASLCamDriver 68 | // virtual uint8_t* getBufferPtr(); //return raw pointer to the buffer 69 | // virtual int getBytesInBuffer(); //return the number of bytes in the buffer 70 | // virtual int getBufferSize(); // return the buffer size 71 | 72 | virtual ViConfigMsg getConfigParam(std::string cmd, uint16_t value); 73 | 74 | //the threaded function that works on the queue of finished measurements 75 | void processMeasurements(); 76 | 77 | bool getBriskElem(uint8_t* buf, ViCornerElem& corner); 78 | 79 | virtual bool init(); 80 | 81 | // set calibration 82 | virtual bool setCalibration(CalibBase::Ptr calibration); 83 | 84 | static uint32_t calculateBufferSize(); 85 | 86 | private: 87 | //ViCameraConfig config; /* the camera configuration */ 88 | // uint8_t * _buffer; /* camera frame buffer */ 89 | // uint32_t _buffer_size; /* size of the frame buffer */ 90 | // uint32_t _bytes_in_buffer; /* number of bytes stored in the frame buffer */ 91 | // bool _active; /* sets if camera is active or not */ 92 | uint32_t _frame_size; /* size of one frame */ 93 | 94 | int _width; 95 | int _height; 96 | 97 | BriskMt9v034Config _config; 98 | 99 | }; 100 | 101 | } //namespace visensor 102 | 103 | #endif /* BRISK_MT9V034_HPP_ */ 104 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | libvisensor 2 | ============= 3 | 4 | Sensor driver library to the Visual-Inertial Sensor developed by the [Autonomous Systems Lab (ASL), ETH Zurich](http://www.asl.ethz.ch) and [Skybotix AG](http://www.skybotix.com). This sensor provides fully time-synchronized and factory calibrated IMU- and stereo-camera datastreams. A detailed spec sheet of the sensor can be found [here](VISensor_Factsheet_web.pdf). The ROS frontend of the VI-Sensor library can be found [here](https://github.com/ethz-asl/visensor_node). 5 | 6 | ![alt text](http://wiki.ros.org/vi_sensor?action=AttachFile&do=get&target=vi-sensor-front.jpg "Sensor Photo") 7 | 8 | A detailed description on how the sensor can be used in ROS is found in the corresponding [ROS-Wiki](http://wiki.ros.org/vi_sensor). 9 | 10 | ##Authors 11 | * Michael Burri [michael.burri at mavt.ethz.ch] 12 | * Janosch Nikolic [janosch.nikolic at mavt.ethz.ch] 13 | * Jörn Rehder [joern.rehder at mavt.ethz.ch] 14 | * Stefan Leutenegger [s.leutenegger at imperial.ac.uk] 15 | * Thomas Schneider [schneith at ethz.ch] 16 | * Pascal Gohl [pascal.gohl at mavt.ethz.ch] 17 | * Sammy Omari [sammy.omari at mavt.ethz.ch] 18 | 19 | ## Reference 20 | Please cite our paper when using the VI-Sensor in an academic publication. 21 | 22 | 1. Nikolic, J., Rehder, J., Burri, M., Gohl, P., Leutenegger, S., Furgale, P. T., & Siegwart, R. (2014). *A Synchronized Visual-Inertial Sensor System with FPGA Pre-Processing for Accurate Real-Time SLAM.* IEEE International Conference on Robotics and Automation (ICRA), Hongkong, China. ( [video] (http://youtu.be/jcjB_Pflu5A) ) 23 | 24 | as bibtex: 25 | ``` 26 | @inproceedings{nikolic2014synchronized, 27 | title={A synchronized visual-inertial sensor system with FPGA pre-processing for accurate real-time SLAM}, 28 | author={Nikolic, Janosch and Rehder, Joern and Burri, Michael and Gohl, Pascal and Leutenegger, Stefan and Furgale, Paul T and Siegwart, Roland}, 29 | booktitle={Robotics and Automation (ICRA), 2014 IEEE International Conference on}, 30 | pages={431--437}, 31 | year={2014}, 32 | organization={IEEE} 33 | } 34 | ``` 35 | 36 | ## Installation Instructions ROS 37 | 38 | Check out the sensor library and this node to your catkin workspace: 39 | 40 | ``` 41 | cd your_catkin_workspace 42 | git clone https://github.com/ethz-asl/libvisensor.git 43 | ``` 44 | 45 | Make sure that you installed all necessary Ubuntu packages 46 | 47 | ``` 48 | sudo apt-get install libeigen3-dev libboost-dev 49 | ``` 50 | 51 | Build the package using catkin_make 52 | 53 | ``` 54 | catkin_make 55 | ``` 56 | 57 | 58 | ## Installation Instructions Stand-alone (No ROS) 59 | Check out the sensor library 60 | ``` 61 | git clone https://github.com/ethz-asl/libvisensor.git 62 | ``` 63 | Make sure that you installed all necessary Ubuntu packages 64 | 65 | ``` 66 | sudo apt-get install libeigen3-dev libboost-dev 67 | ``` 68 | 69 | Build the package and install it system-wide. 70 | 71 | ``` 72 | cd libvisensor 73 | ./install_libvisensor 74 | ``` 75 | 76 | In case you don't want to install the library system-wide, you can also simply build it using 77 | ``` 78 | cd libvisensor 79 | mkdir build 80 | cd build 81 | cmake .. 82 | make 83 | ``` 84 | In this case, you have to ensure that your applications can find the library files and headers. 85 | 86 | To see sample (stand-alone) projects, please refer to https://github.com/skybotix/visensor_sample_applications 87 | 88 | This repo contains a few sample standalone applications for the VI-Sensor 89 | 90 | * **vi_sensor_interface**: A very basic interface to the VI-Sensor. Receives images and IMU messages and displays it using openCV. 91 | * **vi_sensor_stereo_block_matcher**: Applies rectification to Vi-Sensor images and subsequently computes the disparity image using standard openCV block matcher. Intrinsic & extrinsic calibration parameters for rectification are downloaded from VI-Sensor. 92 | -------------------------------------------------------------------------------- /src/sensors/external_trigger.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "networking/connection.hpp" 38 | #include "sensors/external_trigger.hpp" 39 | 40 | namespace visensor { 41 | 42 | ExternalTrigger::ExternalTrigger(SensorId::SensorId sensor_id, 43 | IpConnection::WeakPtr config_connection) 44 | : Sensor( 45 | SensorSettings(sensor_id, 46 | SensorType::SensorType::EXTERNAL_TRIGGER, 47 | calculateBufferSize(), 48 | ExternalTriggerDefaults::NUM_OF_MSGS_IN_PACKAGE, 0, 0), 49 | config_connection), 50 | config_(sensor_id) { 51 | } 52 | 53 | void ExternalTrigger::setUserCallback( 54 | boost::function callback) { 55 | user_callback_ = callback; 56 | } 57 | 58 | ViConfigMsg ExternalTrigger::getConfigParam(std::string cmd, uint32_t value) { 59 | return config_.getConfigParam(cmd, value); 60 | } 61 | 62 | void ExternalTrigger::publishExternalTriggerData( 63 | ViExternalTriggerMsg::Ptr msg) { 64 | if (user_callback_) 65 | user_callback_(msg); 66 | } 67 | 68 | uint32_t ExternalTrigger::calculateBufferSize() { 69 | return (ExternalTriggerDefaults::MSG_SIZE); 70 | } 71 | 72 | void ExternalTrigger::init() { 73 | } 74 | 75 | void ExternalTrigger::processMeasurements() { 76 | while (1) { 77 | 78 | boost::this_thread::interruption_point(); 79 | //get the newest measurement 80 | Measurement::Ptr meas = Sensor::measurement_queue_.pop(); 81 | 82 | // create new shared pointer for the message 83 | ViExternalTriggerMsg::Ptr trigger_msg_ptr = boost::make_shared(); 84 | 85 | uint8_t* buffer = meas->data.get(); 86 | uint8_t id = static_cast(buffer[0]); 87 | 88 | trigger_msg_ptr->trigger_id = id; 89 | trigger_msg_ptr->timestamp = meas->timestamp; 90 | trigger_msg_ptr->timestamp_host = meas->timestamp_host; 91 | trigger_msg_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter; 92 | trigger_msg_ptr->event_id = *(uint32_t*)(meas->data.get() + 1); //counts all trigger events (allows for event missed checking) 93 | publishExternalTriggerData(trigger_msg_ptr); 94 | 95 | } 96 | } 97 | } //namespace visensor 98 | -------------------------------------------------------------------------------- /include/visensor/visensor_constants.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_CONSTANTS_HPP_ 33 | #define VISENSOR_CONSTANTS_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | namespace visensor { 39 | #define CAMERA_FREQUENCY 20 // in Hz 40 | #define IMU_FREQUENCY 200 // in Hz 41 | #define FPGA_FREQUENCY 125000000 42 | #define FPGA_TIME_COUNTER_FREQUENCY 100000.0 //fpga timestamp counter frequency 43 | 44 | namespace ViComType { 45 | static const int SPI_8 = 0, SPI_16 = 1, SPI_32 = 2, I2C_8 = 3, I2C_16 = 4, I2C_32 = 5, FPGA_32 = 6; 46 | } 47 | 48 | namespace SensorType { 49 | enum SensorType { 50 | CAMERA_MT9V034 = 1, 51 | IMU_ADIS16488 = 2, 52 | IMU_MPU6000 = 3, // mpu of prototype 0 53 | IMU_ADIS16375 = 4, 54 | IMU_MPU6000_P1 = 5, // mpu of prototype 1 55 | CAMERA_TAU320 = 6, 56 | CORNER_MT9V034 = 7, 57 | IMU_ADXRS450 = 8, 58 | BRISK_MT9V034 = 9, 59 | TIMING_BLOCK = 10, 60 | IMU_ADIS16448 = 11, 61 | CAMERA_TAU640 = 12, 62 | LIGHT_CONTROL = 13, 63 | ZYNQ_STATUS = 14, 64 | DENSE_MATCHER = 15, 65 | EXTERNAL_TRIGGER = 16, 66 | MPU_9150 = 17 67 | }; 68 | } //namespace SensorType 69 | 70 | namespace SensorId { 71 | enum SensorId { 72 | CAM0 = 0, 73 | CAM1 = 1, 74 | CAM2 = 2, 75 | CAM3 = 3, 76 | IMU0 = 4, 77 | IMU_CAM0 = 5, 78 | IMU_CAM1 = 6, 79 | IMU_CAM2 = 7, 80 | IMU_CAM3 = 8, 81 | CORNER_CAM0 = 9, 82 | CORNER_CAM1 = 10, 83 | CORNER_CAM2 = 11, 84 | CORNER_CAM3 = 12, 85 | DENSE_MATCHER0 = 13, 86 | EXTERNAL_TRIGGER0 = 14, 87 | SENSOR_STATUS = 15, 88 | SENSOR_CLOCK = 16, 89 | FLIR0 = 17, 90 | FLIR1 = 18, 91 | FLIR2 = 19, 92 | FLIR3 = 20, 93 | LED_FLASHER0 = 21, 94 | NOT_SPECIFIED 95 | }; 96 | 97 | DSO_EXPORT inline SensorId getCamId(SensorId id) { 98 | switch (id) { 99 | case (CORNER_CAM0): 100 | case (DENSE_MATCHER0): 101 | return CAM0; 102 | case (CORNER_CAM1): 103 | return CAM1; 104 | case (CORNER_CAM2): 105 | return CAM2; 106 | case (CORNER_CAM3): 107 | return CAM3; 108 | default: 109 | return NOT_SPECIFIED; 110 | } 111 | } 112 | } //namespace SensorPort 113 | 114 | } //namespace visensor 115 | 116 | #endif /* VISENSOR_CONSTANTS_HPP_ */ 117 | -------------------------------------------------------------------------------- /src/work/sensor_calibration.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | *******************************************************************************/ 30 | /* 31 | * sensor_calibration.cpp 32 | * 33 | * Created on: Jan 31, 2013 34 | * Author: root 35 | */ 36 | #include "sensor.hpp" 37 | 38 | 39 | namespace visensor 40 | { 41 | 42 | CalibProvider::CalibProvider(uint32_t fpgaId) 43 | { 44 | 45 | createCalibrationMaps(); 46 | 47 | if(_calibrationMaps.find(fpgaId)==_calibrationMaps.end()) 48 | { 49 | VISENSOR_DEBUG("no calibration found for fpgaId %u\n",fpgaId); 50 | return; 51 | } 52 | _calibrationMap=_calibrationMaps.at(fpgaId); 53 | } 54 | 55 | void CalibProvider::setCalibration(Sensor::Ptr sensor) 56 | { 57 | uint32_t sensorId=sensor->getId(); 58 | 59 | if(_calibrationMap.find(sensorId)==_calibrationMap.end()) 60 | { 61 | VISENSOR_DEBUG("no calibration found for sensorId %u\n",sensorId); 62 | return; 63 | } 64 | 65 | sensor->setCalibration(_calibrationMap.at(sensorId)); 66 | } 67 | 68 | void CalibProvider::createCalibrationMaps() 69 | { 70 | boost::shared_ptr tempCalib (new CalibImuMpu6000); 71 | 72 | tempCalib->setMisalignementG(0.00109,0.00360,0.01633); 73 | tempCalib->setMisalignementA(-0.00115,0.00137,0.00092); 74 | tempCalib->setScaleG(1.00377,0.99688,1.00473); 75 | tempCalib->setScaleA(1.00218,1.00008,1.01080); 76 | 77 | addCalibrationToMaps(2,5,tempCalib); 78 | 79 | boost::shared_ptr tempCalib1 (new CalibImuMpu6000); 80 | 81 | tempCalib1->setMisalignementG(0,0,0); 82 | tempCalib1->setMisalignementA(0,0,0); 83 | tempCalib1->setScaleG(1,1,1); 84 | tempCalib1->setScaleA(1,1,1); 85 | 86 | addCalibrationToMaps(3,5,tempCalib1); 87 | 88 | } 89 | 90 | // this function creates a calibration map for each fpga id and adds the calibration to the submap of the fpga 91 | void CalibProvider::addCalibrationToMaps(uint32_t fpgaId, uint32_t sensorId, CalibBase::Ptr calib) 92 | { 93 | if(_calibrationMaps.find(fpgaId)==_calibrationMaps.end()) 94 | { 95 | // create new calibration map for the fpgaId 96 | std::map tempCalibrationMap; 97 | tempCalibrationMap.insert( std::pair (sensorId, calib)); 98 | _calibrationMaps.insert( std::pair >(fpgaId, tempCalibrationMap)); 99 | }else{ 100 | // if fpgaId exists add calib to the corresponding calibration map 101 | _calibrationMaps.at(fpgaId).insert( std::pair (sensorId, calib)); 102 | } 103 | } 104 | 105 | } //namespace visensor 106 | -------------------------------------------------------------------------------- /include/sensors/corner_mt9v034.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CORNER_MT9V034_HPP_ 33 | #define CORNER_MT9V034_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/connection.hpp" 39 | #include "networking/param_server.hpp" 40 | #include "sensors/corner_detector.hpp" 41 | 42 | namespace visensor 43 | { 44 | 45 | namespace CornerMt9v034Defaults 46 | { 47 | const int 48 | I2C_ADRESS=0x48, 49 | WIDTH=752, 50 | HEIGHT=480, 51 | MAX_FEATURES=30000, 52 | RATE=CAMERA_FREQUENCY, 53 | DEPTH=32, 54 | NUM_OF_MSGS_IN_PACKAGE=1, 55 | CALIBRATION_SIZE=0; 56 | 57 | const uint8_t COM_TYPE = ViComType::I2C_16; 58 | const bool USE_CONST_PACKAGE_SIZE=false; 59 | } 60 | 61 | class CornerMt9v034Config 62 | { 63 | public: 64 | CornerMt9v034Config(const SensorId::SensorId sensorId) 65 | : sensor_id_(sensorId) { 66 | } 67 | 68 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) 69 | { 70 | ViConfigMsg msg; 71 | msg.valChanged=param_server_.getConfigParam(paramName,value,msg.reg,msg.val); 72 | msg.sensorId=sensor_id_; 73 | msg.devAdress=CornerMt9v034Defaults::I2C_ADRESS; 74 | msg.comType=CornerMt9v034Defaults::COM_TYPE; 75 | return msg; 76 | } 77 | 78 | ViConfigMsg getConfigParam(const std::string paramName) 79 | { 80 | ViConfigMsg msg; 81 | msg.valChanged=param_server_.getConfigParam(paramName,msg.reg,msg.val); 82 | msg.sensorId=sensor_id_; 83 | msg.devAdress=CornerMt9v034Defaults::I2C_ADRESS; 84 | msg.comType=CornerMt9v034Defaults::COM_TYPE; 85 | return msg; 86 | } 87 | 88 | private: 89 | param_server::ParamServer param_server_; 90 | const SensorId::SensorId sensor_id_; 91 | }; 92 | 93 | class CornerMt9v034 : public CornerDetector{ 94 | public: 95 | typedef boost::shared_ptr Ptr; 96 | 97 | CornerMt9v034(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection); 98 | virtual ~CornerMt9v034(); 99 | 100 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 101 | //the threaded function that works on the queue of finished measurements 102 | void processMeasurements(); 103 | bool getCornerElem(uint8_t* buf, ViCornerElem& corner); 104 | virtual void init(); 105 | static uint32_t calculateBufferSize(); 106 | 107 | private: 108 | void bitReshuffle(uint8_t *in /*10 bit values*/, uint8_t *out /*8 bit values*/, int size_in /*number of 10 bit values*/, int offset); 109 | void imageReshuffle(uint8_t *data_in /*10 bit raw data*/, uint8_t *image_out, int image_height, int image_width); 110 | 111 | CornerMt9v034Config config_; 112 | }; 113 | 114 | 115 | } //namespace visensor 116 | 117 | 118 | #endif /* CORNER_MT9V034_HPP_ */ 119 | -------------------------------------------------------------------------------- /src/sensors/sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | 34 | #include "networking/config_connection.hpp" 35 | #include "sensors/sensor.hpp" 36 | 37 | namespace visensor 38 | { 39 | 40 | Sensor::Sensor(SensorSettings sensor_settings, ConfigConnection::WeakPtr config_connection): 41 | config_connection_(config_connection), 42 | settings_(sensor_settings), 43 | prev_timestamp_(0), 44 | allowed_timediff_(0), 45 | measurement_miss_counter_(0) 46 | { 47 | } 48 | 49 | Sensor::~Sensor() 50 | { 51 | } 52 | 53 | //lock weak pointer for usage 54 | ConfigConnection::Ptr Sensor::getConfigConnection() { 55 | return config_connection_.lock(); 56 | } 57 | 58 | void Sensor::writeRequest(const ViConfigMsg configMsg) 59 | { 60 | getConfigConnection()->writeConfig(configMsg.sensorId,configMsg.devAdress,configMsg.reg,configMsg.val, configMsg.comType); 61 | return; 62 | } 63 | 64 | void Sensor::readRequest(ViConfigMsg& configMsg) 65 | { 66 | getConfigConnection()->readConfig(configMsg.sensorId,configMsg.devAdress,configMsg.reg, configMsg.val, configMsg.comType); 67 | return; 68 | } 69 | 70 | bool Sensor::checkTimestamp(uint64_t timestamp) 71 | { 72 | bool frameMissed = false; 73 | 74 | //init with first timestamp 75 | if(prev_timestamp_==0) 76 | { 77 | prev_timestamp_=timestamp; 78 | return frameMissed; 79 | } 80 | 81 | if(timestamp-prev_timestamp_>allowed_timediff_) 82 | { 83 | measurement_miss_counter_++; 84 | VISENSOR_DEBUG("sensor %d frame missed (total misses: %u), timediff: %ld ns\n", id(), measurement_miss_counter_, timestamp-prev_timestamp_); 85 | frameMissed = true; 86 | } 87 | 88 | prev_timestamp_=timestamp; 89 | return frameMissed; 90 | } 91 | 92 | int Sensor::getMeasurementBufferSize() const 93 | { 94 | return settings_.measurementBufferSize; 95 | } 96 | 97 | void Sensor::addMeasurement(const Measurement::Ptr meas){ 98 | measurement_queue_.push(meas); 99 | } 100 | 101 | //return time between msgs in nanoseconds 102 | uint64_t Sensor::getTimeBetweenMsgs() const 103 | { 104 | // avoid division by 0, just return 0 if rate is not set yet 105 | if(settings_.rate==0) 106 | return 0; 107 | 108 | return 1000000000/settings_.rate; 109 | } 110 | 111 | int Sensor::getNumOfMsgsInPackage() const 112 | { 113 | return settings_.numOfMsgsInPackage; 114 | } 115 | 116 | // start sensor with given rate 117 | bool Sensor::startSensor(uint32_t rate) 118 | { 119 | settings_.rate=rate; 120 | settings_.active=true; 121 | allowed_timediff_=1.5*getTimeBetweenMsgs(); 122 | return true; 123 | } 124 | 125 | // stop sensor 126 | bool Sensor::stopSensor() 127 | { 128 | settings_.active=false; 129 | return true; 130 | } 131 | 132 | }//namespace visensor 133 | -------------------------------------------------------------------------------- /include/sensors/camera_tau640.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CAMERA_TAU640_HPP_ 33 | #define CAMERA_TAU640_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/connection.hpp" 39 | #include "sensors/camera.hpp" 40 | #include "networking/param_server.hpp" 41 | 42 | namespace visensor { 43 | 44 | namespace CameraTau640Defaults { 45 | const int I2C_ADRESS = 0x48, 46 | WIDTH = 640, 47 | HEIGHT = 512, 48 | RATE = CAMERA_FREQUENCY, 49 | NUM_OF_MSGS_IN_PACKAGE = 1, 50 | CALIBRATION_SIZE = 0; 51 | 52 | const ViImageType TYPE = ViImageType::MONO16; 53 | const bool USE_CONST_PACKAGE_SIZE = true; 54 | } 55 | 56 | class CameraTau640Config { 57 | private: 58 | param_server::ParamServer _paramServer; 59 | const SensorId::SensorId _sensorId; 60 | public: 61 | CameraTau640Config(const SensorId::SensorId sensorId) 62 | : _sensorId(sensorId) { 63 | } 64 | 65 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 66 | ViConfigMsg msg; 67 | msg.valChanged = _paramServer.getConfigParam(paramName, msg.reg, msg.val); 68 | msg.sensorId = _sensorId; 69 | msg.devAdress = CameraTau640Defaults::I2C_ADRESS; 70 | msg.comType = ViComType::I2C_16; 71 | return msg; 72 | } 73 | 74 | ViConfigMsg getConfigParam(const std::string paramName) { 75 | ViConfigMsg msg; 76 | msg.valChanged = _paramServer.getConfigParam(paramName, msg.reg, msg.val); 77 | msg.sensorId = _sensorId; 78 | msg.devAdress = CameraTau640Defaults::I2C_ADRESS; 79 | msg.comType = ViComType::I2C_16; 80 | return msg; 81 | } 82 | }; 83 | 84 | class CameraTau640 : public Camera { 85 | public: 86 | CameraTau640(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection); 87 | virtual ~CameraTau640(); 88 | 89 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 90 | void processMeasurements(); 91 | virtual void init(); 92 | static uint32_t calculateBufferSize(); 93 | // virtual bool setCalibration(CalibBase::Ptr calibration); 94 | 95 | typedef boost::shared_ptr Ptr; 96 | 97 | private: 98 | void allocateBuffer(); 99 | void bitReshuffle(uint8_t *in /*10 bit values*/, 100 | uint8_t *out /*8 bit values*/, 101 | int size_in /*number of 10 bit values*/, int offset); 102 | void imageReshuffle(uint8_t *data_in /*10 bit raw data*/, uint8_t *image_out, 103 | int image_height, int image_width); 104 | 105 | private: 106 | uint32_t frame_size_; /* size of one frame */ 107 | int _width; 108 | int _height; 109 | CameraTau640Config _config; 110 | uint64_t _prevTimestamp; 111 | uint32_t _allowedTimediff; 112 | }; 113 | 114 | } //namespace visensor 115 | 116 | #endif /* CAMERA_TAU640_HPP_ */ 117 | -------------------------------------------------------------------------------- /src/networking/file_transfer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "networking/file_transfer.hpp" 37 | 38 | namespace 39 | { 40 | const std::size_t CHUNK_SIZE = 8096; 41 | } 42 | 43 | FileTransfer::FileTransfer(boost::asio::ip::tcp::socket& socket) 44 | : socket_(socket){ 45 | } 46 | 47 | FileTransfer::~FileTransfer() { 48 | } 49 | 50 | void FileTransfer::sendFile(std::string& local_path, std::string& remote_path) { 51 | 52 | sendPathString(remote_path); 53 | 54 | std::ifstream stream(local_path.c_str(), std::ios::binary); 55 | if (!stream) 56 | { 57 | VISENSOR_DEBUG("could not open file: %s\n", local_path.c_str()); 58 | return; 59 | } 60 | 61 | stream.seekg(0, std::ios::end); 62 | uint64_t length = stream.tellg(); 63 | stream.seekg(0); 64 | 65 | // file length 66 | boost::asio::write(socket_, boost::asio::buffer(&length, sizeof(length))); 67 | 68 | // file content 69 | boost::array chunk; 70 | 71 | uint64_t transferred = 0; 72 | 73 | while (transferred != length) 74 | { 75 | uint64_t remaining = length - transferred; 76 | std::size_t chunk_size = (remaining > CHUNK_SIZE) ? CHUNK_SIZE : static_cast(remaining); 77 | stream.read(&chunk[0], chunk_size); 78 | boost::asio::write(socket_, boost::asio::buffer(chunk, chunk_size)); 79 | transferred += chunk_size; 80 | } 81 | } 82 | 83 | void FileTransfer::receiveFile(std::string& local_path, std::string& remote_path) { 84 | 85 | sendPathString(remote_path); 86 | 87 | std::ofstream stream(local_path.c_str(), std::ios::binary); 88 | if (!stream) 89 | { 90 | VISENSOR_DEBUG( "could not save file: %s\n", local_path.c_str()); 91 | return; 92 | } 93 | 94 | // file length 95 | uint64_t length = 0; 96 | boost::asio::read(socket_, boost::asio::buffer(&length, sizeof(length))); 97 | 98 | // file content 99 | boost::array chunk; 100 | 101 | uint64_t transferred = 0; 102 | 103 | while (transferred != length) 104 | { 105 | uint64_t remaining = length - transferred; 106 | std::size_t chunk_size = (remaining > CHUNK_SIZE) ? CHUNK_SIZE : static_cast(remaining); 107 | boost::asio::read(socket_, boost::asio::buffer(chunk, chunk_size)); 108 | stream.write(&chunk[0], chunk_size); 109 | transferred += chunk_size; 110 | } 111 | } 112 | 113 | std::string FileTransfer::sendPathString(std::string string) { 114 | // // send string length 115 | // boost::uint64_t string_size = string.size(); 116 | // boost::asio::write(socket_, boost::asio::buffer(&string_size, sizeof(string_size))); 117 | // 118 | // // send string 119 | // boost::asio::write(socket_, boost::asio::buffer(&string, sizeof(string_size))); 120 | 121 | boost::asio::streambuf buff; 122 | std::ostream os(&buff); 123 | os << string; 124 | 125 | socket_.send(buff.data()); // for example 126 | 127 | return string; 128 | } 129 | -------------------------------------------------------------------------------- /src/sensors/imu_mpu9150.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace visensor { 39 | 40 | ImuMpu9150::ImuMpu9150(SensorId::SensorId sensor_id, 41 | IpConnection::WeakPtr config_connection) 42 | : Imu(sensor_id, 43 | SensorSettings(sensor_id, SensorType::SensorType::MPU_9150, 44 | calculateBufferSize(), 45 | ImuMpu9150Defaults::NUM_OF_MSGS_IN_PACKAGE, 46 | ImuMpu9150Defaults::USE_CONST_PACKAGE_SIZE, 47 | ImuMpu9150Defaults::CALIBRATION_SIZE), 48 | config_connection), 49 | config_(sensor_id) { 50 | } 51 | 52 | uint32_t ImuMpu9150::calculateBufferSize() { 53 | return (ImuMpu9150Defaults::MSG_SIZE); 54 | } 55 | 56 | void ImuMpu9150::init() { 57 | writeRequest(config_.getConfigParam("enable")); 58 | writeRequest(config_.getConfigParam("digital_low_pass_filter_config")); 59 | } 60 | 61 | void ImuMpu9150::processMeasurements() { 62 | while (1) { 63 | boost::this_thread::interruption_point(); 64 | 65 | //get the newest measurement 66 | boost::shared_ptr meas = Sensor::measurement_queue_.pop(); 67 | 68 | //checkTimestamp(meas.timestamp); 69 | 70 | ViImuMsg::Ptr imu_msg_ptr = boost::make_shared(); 71 | 72 | // Add imu information to the msg 73 | imu_msg_ptr->imu_id = Imu::imu_id_; 74 | imu_msg_ptr->timestamp = meas->timestamp; 75 | getGyro(meas->data.get(), &imu_msg_ptr->gyro[0]); 76 | getTemperature(meas->data.get(), &imu_msg_ptr->temperature); 77 | getAcc(meas->data.get(), &imu_msg_ptr->acc[0]); 78 | 79 | publishImuData(imu_msg_ptr, ViErrorCodes::NO_ERROR); 80 | } 81 | } 82 | 83 | ViConfigMsg ImuMpu9150::getConfigParam(std::string cmd, uint32_t value) { 84 | return config_.getConfigParam(cmd, value); 85 | } 86 | 87 | void ImuMpu9150::getAcc(uint8_t* buffer, double * acc) { 88 | int i, current_pos; 89 | int16_t temp; 90 | 91 | for (i = 0; i < 3; i++) { 92 | current_pos = 2 * i; 93 | temp = (int) ((buffer[current_pos] << 8) | (buffer[current_pos + 1] << 0)); 94 | acc[i] = (double) temp; 95 | acc[i] = acc[i] / 16384 * STANDARD_GRAVITY; 96 | } 97 | } 98 | 99 | void ImuMpu9150::getGyro(uint8_t* buffer, double * gyro) { 100 | int i, current_pos; 101 | int16_t temp; 102 | for (i = 0; i < 3; i++) { 103 | current_pos = 2 * 3 + 2 + 2 * i; 104 | temp = (int) ((buffer[current_pos] << 8) | (buffer[current_pos + 1] << 0)); 105 | gyro[i] = (double) temp; 106 | gyro[i] = gyro[i] / 131 * M_PI / 180; 107 | } 108 | } 109 | 110 | void ImuMpu9150::getTemperature(uint8_t* buffer, double * temperature) { 111 | int current_pos; 112 | int16_t temp; 113 | current_pos = 2 * 3; 114 | temp = 115 | (int16_t) ((buffer[current_pos] << 8) | (buffer[current_pos + 1] << 0)); 116 | *temperature = (double) temp; 117 | *temperature = *temperature / 340.0 + 35.0; 118 | } 119 | 120 | } //namespace visensor 121 | -------------------------------------------------------------------------------- /src/work/calib_mpu6000.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | *******************************************************************************/ 30 | /* 31 | * calib_mpu6000.cpp 32 | * 33 | * Created on: Feb 4, 2013 34 | * Author: burrimi 35 | */ 36 | 37 | 38 | #include "sensor_calibration.hpp" 39 | 40 | 41 | namespace visensor 42 | { 43 | void CalibImuMpu6000::setScaleA(double a0,double a1, double a2) 44 | { 45 | _scaleA.at(0)=a0; 46 | _scaleA.at(1)=a1; 47 | _scaleA.at(2)=a2; 48 | } 49 | 50 | void CalibImuMpu6000::setScaleG(double a0,double a1, double a2) 51 | { 52 | _scaleG.at(0)=a0; 53 | _scaleG.at(1)=a1; 54 | _scaleG.at(2)=a2; 55 | } 56 | 57 | void CalibImuMpu6000::setMisalignementA(double a0,double a1, double a2) 58 | { 59 | _misalignementA.at(0)=a0; 60 | _misalignementA.at(1)=a1; 61 | _misalignementA.at(2)=a2; 62 | } 63 | 64 | void CalibImuMpu6000::setMisalignementG(double a0,double a1, double a2) 65 | { 66 | _misalignementG.at(0)=a0; 67 | _misalignementG.at(1)=a1; 68 | _misalignementG.at(2)=a2; 69 | } 70 | 71 | std::vector > CalibImuMpu6000::getScaleMisalignementInverseG() 72 | { 73 | std::vector temp(3,0.0); 74 | std::vector > scaleMisalignementInverseG(3,temp); 75 | 76 | //scaleMisalignementInverse = inv(Scale*Misalignement) 77 | scaleMisalignementInverseG.at(0).at(0)=1/_scaleG[0]; 78 | scaleMisalignementInverseG.at(1).at(0)=-_misalignementG[0]/_scaleG[0]; 79 | scaleMisalignementInverseG.at(1).at(1)=1/_scaleG[1]; 80 | scaleMisalignementInverseG.at(2).at(0)=(_misalignementG[0]*_misalignementG[2]-_misalignementG[1])/_scaleG[0]; 81 | scaleMisalignementInverseG.at(2).at(1)=-_misalignementG[2]/_scaleG[1]; 82 | scaleMisalignementInverseG.at(2).at(2)=1/_scaleG[2]; 83 | 84 | return scaleMisalignementInverseG; 85 | } 86 | 87 | std::vector > CalibImuMpu6000::getScaleMisalignementInverseA() 88 | { 89 | 90 | std::vector temp(3,0.0); 91 | std::vector > scaleMisalignementInverseA(3,temp); 92 | 93 | //scaleMisalignementInverse = inv(Scale*Misalignement) 94 | scaleMisalignementInverseA.at(0).at(0)=1/_scaleA[0]; 95 | scaleMisalignementInverseA.at(1).at(0)=-_misalignementA[0]/_scaleA[0]; 96 | scaleMisalignementInverseA.at(1).at(1)=1/_scaleA[1]; 97 | scaleMisalignementInverseA.at(2).at(0)=(_misalignementA[0]*_misalignementA[2]-_misalignementA[1])/_scaleA[0]; 98 | scaleMisalignementInverseA.at(2).at(1)=-_misalignementA[2]/_scaleA[1]; 99 | scaleMisalignementInverseA.at(2).at(2)=1/_scaleA[2]; 100 | 101 | // std::vector > scaleMisalignementInverseG={{1/_scaleA[0],0,0}, 102 | // {-_misalignementA[0]/_scaleA[0],1/_scaleA[1],0}, 103 | // {(_misalignementA[0]*_misalignementA[2]-_misalignementA[1])/_scaleA[0],-_misalignementA[2]/_scaleA[1],1/_scaleA[2]}}; 104 | return scaleMisalignementInverseA; 105 | } 106 | 107 | 108 | 109 | // todo implement this 110 | void CalibImuMpu6000::getCalibratedMeasurement(ViImuMsg::Ptr imu_ptr) 111 | { 112 | 113 | } 114 | 115 | 116 | } //namespace visensor 117 | -------------------------------------------------------------------------------- /include/sensors/imu_mpu9150.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef IMU_MPU9150_HPP_ 33 | #define IMU_MPU9150_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/connection.hpp" 39 | #include "networking/param_server.hpp" 40 | #include "sensors/imu.hpp" 41 | 42 | namespace visensor { 43 | 44 | namespace ImuMpu9150Defaults { 45 | const int I2C_ADRESS = 0x68, 46 | RATE = IMU_FREQUENCY, 47 | MSG_SIZE = (3 * 2 /*Acc*/ + 2 /*temperature*/ + 3 * 2 /*gyro*/), 48 | NUM_OF_MSGS_IN_PACKAGE = 10, 49 | CALIBRATION_SIZE = 0; 50 | const bool USE_CONST_PACKAGE_SIZE = true; 51 | const uint8_t COM_TYPE = ViComType::I2C_8; 52 | } 53 | 54 | class ImuMpu9150Config { 55 | public: 56 | ImuMpu9150Config(const SensorId::SensorId sensorId) 57 | : sensor_id_(sensorId) { 58 | // name, type, register, default, mask, min, max 59 | param_server_.addParam("enable", param_server::CONST_T, 0x6B, 0x0000, 60 | 0x00FF); // enable mpu 9150 page 41 61 | param_server_.addParam("digital_low_pass_filter_config", 62 | param_server::UINT_T, 0x1A, 0x0002, 0x000F, 0, 6); // config parameters page 13 63 | // settings: 0 Bandwith: 260 Hz, Delay 0 ms 64 | // settings: 1 Bandwith: 184 Hz, Delay 2.0 ms 65 | // settings: 2 Bandwith: 94 Hz, Delay 3.0 ms 66 | // settings: 3 Bandwith: 44 Hz, Delay 4.9 ms 67 | // settings: 4 Bandwith: 21 Hz, Delay 8.5 ms 68 | // settings: 5 Bandwith: 10 Hz, Delay 13.8 ms 69 | // settings: 6 Bandwith: 5 Hz, Delay 19.0 ms 70 | } 71 | 72 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 73 | ViConfigMsg msg; 74 | msg.valChanged = param_server_.getConfigParam(paramName, value, msg.reg, 75 | msg.val); 76 | msg.sensorId = sensor_id_; 77 | msg.devAdress = ImuMpu9150Defaults::I2C_ADRESS; 78 | msg.comType = ImuMpu9150Defaults::COM_TYPE; 79 | return msg; 80 | } 81 | 82 | ViConfigMsg getConfigParam(const std::string paramName) { 83 | ViConfigMsg msg; 84 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 85 | msg.sensorId = sensor_id_; 86 | msg.devAdress = ImuMpu9150Defaults::I2C_ADRESS; 87 | msg.comType = ImuMpu9150Defaults::COM_TYPE; 88 | return msg; 89 | } 90 | 91 | private: 92 | param_server::ParamServer param_server_; 93 | const SensorId::SensorId sensor_id_; 94 | }; 95 | 96 | class ImuMpu9150 : public Imu { 97 | public: 98 | typedef boost::shared_ptr Ptr; 99 | 100 | ImuMpu9150(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection); 101 | virtual ~ImuMpu9150() {}; 102 | 103 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 104 | //the threaded function that works on the queue of finished measurements 105 | void processMeasurements(); 106 | virtual void init(); 107 | static uint32_t calculateBufferSize(); 108 | 109 | private: 110 | void getGyro(uint8_t* buffer, double * gyro); 111 | void getAcc(uint8_t* buffer, double * acc); 112 | void getTemperature(uint8_t* buffer, double * acc); 113 | // void getMag(double * mag); 114 | // void getBaro(double * baro); 115 | 116 | ImuMpu9150Config config_; 117 | }; 118 | } //namespace visensor 119 | 120 | #endif /* IMU_MPU9150_HPP_ */ 121 | -------------------------------------------------------------------------------- /include/sensors/dense_matcher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef DENSE_MATCHER_HPP_ 33 | #define DENSE_MATCHER_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "networking/param_server.hpp" 39 | #include "sensors/camera.hpp" 40 | 41 | namespace visensor { 42 | 43 | namespace DenseMatcherDefaults { 44 | const int I2C_ADRESS = 0x48, 45 | WIDTH = 752, 46 | HEIGHT = 480, 47 | RATE = CAMERA_FREQUENCY, 48 | NUM_OF_MSGS_IN_PACKAGE = 1, 49 | CALIBRATION_SIZE = 0; 50 | const uint8_t COM_TYPE = ViComType::FPGA_32; 51 | 52 | const ViImageType IMAGE_TYPE = ViImageType::MONO8; 53 | const bool USE_CONST_PACKAGE_SIZE = true; 54 | } 55 | 56 | class DenseMatcherConfig { 57 | public: 58 | DenseMatcherConfig(const SensorId::SensorId sensorId) 59 | : sensor_id_(sensorId) { 60 | // name, type, register, default, mask, min, max 61 | param_server_.addParam("penalty_1", param_server::UINT_T, 0X04 * 40, 2, 0xFFFF, 0, 32765); 62 | param_server_.addParam("penalty_2", param_server::UINT_T, 0X04 * 41, 60, 0xFFFF, 0, 32765); 63 | param_server_.addParam("threshold", param_server::UINT_T, 0X04 * 42, 255, 0xFFFF, 0, 32765); 64 | param_server_.addParam("lr_check", param_server::UINT_T, 0X04 * 43, 5, 0xFFFF, 0, 32765); 65 | } 66 | 67 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 68 | ViConfigMsg msg; 69 | msg.valChanged = param_server_.getConfigParam(paramName, value, msg.reg, 70 | msg.val); 71 | msg.sensorId = sensor_id_; 72 | msg.devAdress = DenseMatcherDefaults::I2C_ADRESS; 73 | msg.comType = DenseMatcherDefaults::COM_TYPE; 74 | return msg; 75 | } 76 | 77 | ViConfigMsg getConfigParam(const std::string paramName) { 78 | ViConfigMsg msg; 79 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 80 | msg.sensorId = sensor_id_; 81 | msg.devAdress = DenseMatcherDefaults::I2C_ADRESS; 82 | msg.comType = DenseMatcherDefaults::COM_TYPE; 83 | return msg; 84 | } 85 | 86 | const SensorId::SensorId sensor_id_; 87 | 88 | private: 89 | param_server::ParamServer param_server_; 90 | }; 91 | 92 | class DenseMatcher : public Camera { 93 | public: 94 | typedef boost::shared_ptr Ptr; 95 | 96 | DenseMatcher(SensorId::SensorId sensor_id, 97 | ConfigConnection::WeakPtr config_connection); 98 | virtual ~DenseMatcher(); 99 | 100 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 101 | void setCalibration(ViCameraCalibration& calib_cam0, ViCameraCalibration& calib_cam1); 102 | 103 | //the threaded function that works on the queue of finished measurements 104 | void processMeasurements(); 105 | virtual void init(); 106 | static uint32_t calculateBufferSize(); 107 | 108 | private: 109 | void bitReshuffle(uint8_t *in /*10 bit values*/, 110 | uint8_t *out /*8 bit values*/, 111 | int size_in /*number of 10 bit values*/, int offset); 112 | void imageReshuffle(uint8_t *data_in /*10 bit raw data*/, uint8_t *image_out, 113 | int image_height, int image_width); 114 | 115 | private: 116 | uint32_t frame_size_; /* size of one frame */ 117 | int width_; 118 | int height_; 119 | DenseMatcherConfig config_; 120 | }; 121 | 122 | } //namespace visensor 123 | 124 | #endif /* CAMERA_MT9V034_HPP_ */ 125 | -------------------------------------------------------------------------------- /src/synchronization/time_synchronizer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include "synchronization/time_synchronizer.hpp" 34 | 35 | using namespace TimeSynchronizerValues; 36 | 37 | TimeSynchronizer::TimeSynchronizer():_synchronized(false) 38 | { 39 | _offset_tracker=0; 40 | _initial_offset=0; 41 | _previous_timestamp=0; 42 | _counter=0; 43 | _offset=0; 44 | unusable_frames_counter_=0; 45 | _timeFpgaAtUpdate=0; 46 | _offsetBeforeUpdate=0; 47 | } 48 | 49 | void TimeSynchronizer::init(uint64_t time_fpga) 50 | { 51 | init(getSystemTime(), time_fpga); 52 | } 53 | 54 | 55 | void TimeSynchronizer::init(uint64_t time_pc, uint64_t time_fpga) 56 | { 57 | _initial_offset=time_pc-time_fpga; 58 | _synchronized=true; 59 | } 60 | 61 | void TimeSynchronizer::updateTime(uint64_t time_fpga) 62 | { 63 | updateTime(getSystemTime(), time_fpga); 64 | } 65 | 66 | void TimeSynchronizer::updateTime(uint64_t time_pc, uint64_t time_fpga) 67 | { 68 | //WARNING(schneith): leica code assumes that the clock offset/skew is fixed for all msgs 69 | 70 | // detect fpga time wrap around 71 | //if(time_fpga<_previous_timestamp) 72 | //{ 73 | // _initial_offset += (uint64_t)0xFFFFFFFF+1; 74 | //} 75 | //_previous_timestamp = time_fpga; 76 | 77 | // // ignore first 10 frames 78 | // if(unusable_frames_counter_ handle this here? 121 | return time_fpga+_initial_offset; 122 | /* 123 | // take old offset for messages captured before the update 124 | if(time_fpga>_timeFpgaAtUpdate) 125 | return time_fpga+_initial_offset+_offset; 126 | else 127 | return time_fpga+_initial_offset+_offsetBeforeUpdate; 128 | */ 129 | } 130 | 131 | // get system time in nanoseconds 132 | uint64_t TimeSynchronizer::getSystemTime() 133 | { 134 | //timespec ts; 135 | // clock_gettime(CLOCK_MONOTONIC, &ts); // Works on FreeBSD 136 | //clock_gettime(CLOCK_REALTIME, &ts); // Works on Linux 137 | //return (uint64_t)(ts.tv_sec) * 1000000000 + (uint64_t)(ts.tv_nsec); 138 | 139 | timeval tv; 140 | gettimeofday(&tv,NULL); 141 | return (uint64_t)tv.tv_sec * 1000000000 + (uint64_t)tv.tv_usec * 1000; 142 | } 143 | -------------------------------------------------------------------------------- /include/visensor/visensor_api.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_API_HPP_ 33 | #define VISENSOR_API_HPP_ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | namespace visensor { 46 | 47 | class DSO_EXPORT ViSensorDriver { 48 | 49 | public: 50 | typedef boost::shared_ptr Ptr; 51 | 52 | ViSensorDriver(); 53 | ~ViSensorDriver(); 54 | 55 | //get list of all ViSensors connected to the network 56 | void getAutoDiscoveryDeviceList(ViDeviceList &hostname_list); 57 | 58 | //init using autodiscovery (first sensor that responded, returns its ip) 59 | std::string init(); 60 | 61 | //connect to specific host/ip address 62 | void init(std::string host_ip); 63 | 64 | void startSensor(visensor::SensorId::SensorId sensor_id, uint32_t rate = 65 | CAMERA_FREQUENCY); 66 | void stopSensor(visensor::SensorId::SensorId sensor_id); 67 | void setSensorConfigParam(visensor::SensorId::SensorId sensor_id, const std::string cmd, 68 | int value); 69 | 70 | void startAllCameras(uint32_t rate = CAMERA_FREQUENCY); 71 | void setCameraCallback(boost::function callback); 72 | 73 | void startAllCorners(); 74 | void setCornerCallback(boost::function callback); 75 | 76 | void startAllImus(uint32_t rate = IMU_FREQUENCY); 77 | void setImuCallback(boost::function callback); 78 | 79 | void startAllExternalTriggers(uint32_t rate); 80 | void setExternalTriggerCallback(boost::function callback); 81 | void setExternalTriggerConfig(const ViExternalTriggerConfig config); 82 | 83 | void startAllDenseMatchers(); 84 | void setDenseMatcherCallback(boost::function callback); 85 | 86 | // is called with synchronized images and corresponding corners 87 | void setFramesCornersCallback(boost::function callback); 88 | 89 | std::vector getListOfSensorIDs() const; 90 | std::vector getListOfCameraIDs() const; 91 | std::vector getListOfDenseIDs() const; 92 | std::vector getListOfCornerIDs() const; 93 | std::vector getListOfImuIDs() const; 94 | std::vector getListOfTriggerIDs() const; 95 | uint32_t getFpgaId() const; 96 | 97 | //slot 0 holds the factory calibration (and can't be overwritten using the public API, use the calibration tool instead) 98 | bool getCameraCalibration(SensorId::SensorId cam_id, ViCameraCalibration& calib, bool* is_camera_flipped = NULL, int slot_id=0) const; 99 | bool setCameraCalibration(SensorId::SensorId cam_id, const ViCameraCalibration calib, int slot_id=1) const; 100 | bool setCameraFactoryCalibration(SensorId::SensorId cam_id, const ViCameraCalibration calib, bool flip_camera = false) const; 101 | void setCameraCalibrationSlot(int slot_id); 102 | int getCameraCalibrationSlot(); 103 | bool isStereoCameraFlipped() const; 104 | 105 | 106 | //serial port bridge 107 | void sendSerialData(ViSerialData::Ptr data) const; 108 | void setSerialCallback(boost::function callback); 109 | void setSerialDelimiter(const char serial_id, const std::string delimiter) const; 110 | void setSerialBaudrate(const char serial_id, const unsigned int baudrate) const; 111 | 112 | //file transfer 113 | void downloadFile(std::string local_path, std::string remote_path); 114 | void uploadFile(std::string local_path, std::string remote_path); 115 | 116 | private: 117 | class Impl; 118 | Impl* pImpl_; 119 | 120 | public: 121 | //private API access 122 | Impl* getPrivateApiAccess() { return pImpl_; } 123 | }; 124 | } //namespace visensor 125 | 126 | #endif /* VISENSOR_API_HPP_ */ 127 | -------------------------------------------------------------------------------- /src/sensors/corner_mt9v034.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "sensors/corner_mt9v034.hpp" 37 | 38 | namespace visensor { 39 | CornerMt9v034::CornerMt9v034(SensorId::SensorId sensor_id, 40 | IpConnection::WeakPtr config_connection) 41 | : CornerDetector( 42 | sensor_id, 43 | ViCornerConfig(CornerMt9v034Defaults::WIDTH, 44 | CornerMt9v034Defaults::HEIGHT, 45 | CornerMt9v034Defaults::RATE), 46 | SensorSettings(sensor_id, SensorType::SensorType::CORNER_MT9V034, 47 | calculateBufferSize(), 48 | CornerMt9v034Defaults::NUM_OF_MSGS_IN_PACKAGE, 49 | CornerMt9v034Defaults::USE_CONST_PACKAGE_SIZE, 50 | CornerMt9v034Defaults::CALIBRATION_SIZE), 51 | config_connection), 52 | config_(sensor_id) { 53 | 54 | } 55 | 56 | void CornerMt9v034::init() { 57 | 58 | } 59 | 60 | uint32_t CornerMt9v034::calculateBufferSize() { 61 | //only use const variables here 62 | return (CornerMt9v034Defaults::MAX_FEATURES 63 | * (CornerMt9v034Defaults::DEPTH / 8 + 4)); 64 | } 65 | 66 | CornerMt9v034::~CornerMt9v034() { 67 | 68 | } 69 | 70 | //the threaded function that works on the queue of finished measurements 71 | void CornerMt9v034::processMeasurements() { 72 | while (1) { 73 | 74 | boost::this_thread::interruption_point(); 75 | //get the newest measurement 76 | Measurement::Ptr meas = Sensor::measurement_queue_.pop(); 77 | 78 | // VISENSOR_DEBUG("in corner thread %d\n", Corner::_cameraId); 79 | 80 | //check for missed frames 81 | // TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...) 82 | if( checkTimestamp(meas->timestamp) ) 83 | { 84 | //publish an empty missed frame 85 | ViCorner::Ptr missed_corner_ptr = boost::make_shared(); 86 | missed_corner_ptr->camera_id = camera_id_; 87 | publishCornerData(missed_corner_ptr, ViErrorCodes::MEASUREMENT_DROPPED); 88 | } 89 | 90 | // create new shared pointer for the corners 91 | ViCorner::Ptr corner_ptr = boost::make_shared(); 92 | 93 | corner_ptr->camera_id = camera_id_; 94 | corner_ptr->timestamp = meas->timestamp; 95 | corner_ptr->timestamp_host = meas->timestamp_host; 96 | corner_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter; 97 | 98 | // process corners 99 | uint32_t pos = 0; 100 | while (pos < meas->buffer_size) { 101 | ViCornerElem corner; 102 | if (getCornerElem(meas->data.get() + pos, corner)) 103 | corner_ptr->corners.push_back(corner); 104 | // else 105 | // VISENSOR_DEBUG("pos in buffer: %u \n",meas->_bytesInBuffer/8-pos/8); 106 | pos += CornerMt9v034Defaults::DEPTH / 8 + 4; 107 | } 108 | 109 | VISENSOR_DEBUG("corners size %lu\n",corner_ptr->corners.size()); 110 | 111 | publishCornerData(corner_ptr, ViErrorCodes::NO_ERROR); 112 | } 113 | } 114 | 115 | bool CornerMt9v034::getCornerElem(uint8_t* buf, ViCornerElem& corner) { 116 | 117 | corner.x = ((buf[0] << 8) | (buf[1] << 0)); 118 | corner.y = ((buf[2] << 8) | (buf[3] << 0)); 119 | corner.score = (buf[4] << 24) | (buf[5] << 16) | (buf[6] << 8) 120 | | (buf[7] << 0); 121 | 122 | if (corner.x >= CornerDetector::config_.width || corner.y >= CornerDetector::config_.height) { 123 | VISENSOR_DEBUG( 124 | "Sensor::Corner::CornerMt9v034 - invalid corner position x:%u y:%u\n", 125 | corner.x, corner.y); 126 | return false; 127 | } 128 | 129 | // if(corner.x==0 && corner.y==0) 130 | // { 131 | // VISENSOR_DEBUG("Sensor::Corner::CornerMt9v034 - zero corner received score:%u\n",corner.score); 132 | // return false; 133 | // } 134 | 135 | return true; 136 | } 137 | 138 | ViConfigMsg CornerMt9v034::getConfigParam(std::string cmd, uint32_t value) { 139 | return config_.getConfigParam(cmd, value); 140 | } 141 | 142 | } //namespace visensor 143 | -------------------------------------------------------------------------------- /src/work/brisk_mt9v034.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | *******************************************************************************/ 30 | /* 31 | * brisk_mt9v034.cpp 32 | * 33 | * Created on: Apr 18, 2012 34 | * Author: burrimi 35 | */ 36 | 37 | #include "brisk_mt9v034.hpp" 38 | #include "visensor_impl.hpp" // needed by forward declaration 39 | 40 | namespace visensor 41 | { 42 | BriskMt9v034::BriskMt9v034(int sensor_id,int camera_id, ViSensorDriver::Impl* ViSensorDriver): 43 | Camera(camera_id,ViCameraConfig( 44 | BriskMt9v034Defaults::WIDTH, BriskMt9v034Defaults::HEIGHT, BriskMt9v034Defaults::RATE), 45 | ViSensorDriver, 46 | SensorSettings(sensor_id,SensorType::SensorType::CORNER_MT9V034,calculateBufferSize(), 47 | BriskMt9v034Defaults::NUM_OF_MSGS_IN_PACKAGE, 48 | BriskMt9v034Defaults::USE_CONST_PACKAGE_SIZE, 49 | BriskMt9v034Defaults::CALIBRATION_SIZE)), 50 | _config(sensor_id) 51 | { 52 | 53 | 54 | 55 | } 56 | 57 | bool BriskMt9v034::init() 58 | { 59 | return true; 60 | } 61 | 62 | // set calibration 63 | bool BriskMt9v034::setCalibration(CalibBase::Ptr calibration) 64 | { 65 | // TODO set calibration 66 | return true; 67 | } 68 | 69 | 70 | uint32_t BriskMt9v034::calculateBufferSize(){ 71 | //only use const variables here 72 | return (BriskMt9v034Defaults::MAX_FEATURES*(BriskMt9v034Defaults::SCORE_BYTES+BriskMt9v034Defaults::BRISK_BYTES+4)); 73 | } 74 | 75 | BriskMt9v034::~BriskMt9v034() 76 | { 77 | 78 | } 79 | 80 | //the threaded function that works on the queue of finished measurements 81 | void BriskMt9v034::processMeasurements(){ 82 | while(1){ 83 | 84 | 85 | boost::this_thread::interruption_point(); 86 | //get the newest measurement 87 | Measurement meas = Sensor::measurement_queue_.pop(); 88 | 89 | // VISENSOR_DEBUG("in corner thread %d\n", Camera::_cameraId); 90 | 91 | Sensor::checkTimestamp(meas.timestamp); 92 | 93 | 94 | // // create new shared pointer for the corners 95 | ViCorner::Ptr corner_ptr(new ViCorner()); 96 | 97 | corner_ptr->camera_id=Camera::camera_id_; 98 | corner_ptr->timestamp=meas.timestamp; 99 | 100 | // process corners 101 | uint32_t pos=0; 102 | while(poscorners.push_back(corner); 107 | // else 108 | // VISENSOR_DEBUG("pos in buffer: %u \n",meas._bytesInBuffer/8-pos/8); 109 | pos+=BriskMt9v034Defaults::SCORE_BYTES+BriskMt9v034Defaults::BRISK_BYTES+4; 110 | } 111 | 112 | // VISENSOR_DEBUG("corners size %lu\n",corner_ptr->corners.size()); 113 | 114 | delete[] meas.data; 115 | 116 | Sensor::publishCorner(corner_ptr); 117 | } 118 | } 119 | 120 | bool BriskMt9v034::getBriskElem(uint8_t* buf, ViCornerElem& corner) 121 | { 122 | 123 | corner.x=((buf[0]<<8) | (buf[1]<<0)); 124 | corner.y=((buf[2]<<8) | (buf[3]<<0)); 125 | corner.score=(buf[4]<<24) | (buf[5]<<16) | (buf[6]<<8) | (buf[7]<<0); 126 | 127 | memcpy(corner.descriptor,&buf[8],BriskMt9v034Defaults::BRISK_BYTES); 128 | 129 | if(corner.x>=Camera::config_.width || corner.y>=Camera::config_.height) 130 | { 131 | VISENSOR_DEBUG("Sensor::Camera::BriskMt9v034 - invalid corner position x:%u y:%u\n",corner.x,corner.y); 132 | return false; 133 | } 134 | 135 | // if(corner.x==0 && corner.y==0) 136 | // { 137 | // VISENSOR_DEBUG("Sensor::Camera::CornerMt9v034 - zero corner received score:%u\n",corner.score); 138 | // return false; 139 | // } 140 | 141 | return true; 142 | } 143 | 144 | ViConfigMsg BriskMt9v034::getConfigParam(std::string cmd, uint16_t value) 145 | { 146 | return _config.getConfigParam(cmd,value); 147 | } 148 | 149 | 150 | //void BriskMt9v034::checkTimestamp(uint64_t timestamp) 151 | //{ 152 | //// //VISENSOR_DEBUG("camera %d frame missed, timediff: %ld ns\n",_camera_id,Sensor::_timestamp); 153 | //// 154 | // if(_prevTimestamp==0) 155 | // { 156 | // _prevTimestamp=timestamp; 157 | // return; 158 | // } 159 | // 160 | // if(timestamp-_prevTimestamp>_allowedTimediff) 161 | // VISENSOR_DEBUG("corner %d frame missed, timediff: %ld ns\n",Camera::_cameraId,timestamp-_prevTimestamp); 162 | // _prevTimestamp=timestamp; 163 | //} 164 | 165 | 166 | } //namespace visensor 167 | -------------------------------------------------------------------------------- /include/visensor_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_IMPL_HPP_ 33 | #define VISENSOR_IMPL_HPP_ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "visensor/visensor.hpp" 41 | #include "visensor/visensor_datatypes.hpp" 42 | #include "visensor/visensor_exceptions.hpp" 43 | 44 | #include "networking/connection.hpp" 45 | #include "networking/file_transfer.hpp" 46 | #include "synchronization/frame_corner_synchronizer.hpp" 47 | 48 | // sensors 49 | #include "sensors/sensor_factory.hpp" 50 | #include "sensors/sensor.hpp" 51 | #include "sensors/camera_mt9v034.hpp" 52 | #include "sensors/camera_tau640.hpp" 53 | #include "sensors/imu_adis16448.hpp" 54 | #include "sensors/imu_adis16488.hpp" 55 | #include "sensors/corner_mt9v034.hpp" 56 | 57 | 58 | namespace visensor { 59 | 60 | class ViSensorDriver::Impl { 61 | public: 62 | Impl(); 63 | virtual ~Impl(); 64 | 65 | void getAutoDiscoveryDeviceList(ViDeviceList &hostname_list); 66 | 67 | void init(std::string hostname); 68 | std::string initAutodiscovery(); 69 | 70 | void startSensor(SensorId::SensorId sensor_id, uint32_t rate = CAMERA_FREQUENCY); 71 | void stopSensor(SensorId::SensorId sensor_id); 72 | void setSensorConfigParam(SensorId::SensorId sensor_id, const std::string cmd, uint16_t value); 73 | 74 | void startAllCameras(uint32_t rate = CAMERA_FREQUENCY); 75 | void setCameraCallback(boost::function callback); 76 | 77 | void startAllCorners(); 78 | void setCornerCallback(boost::function callback); 79 | 80 | void startAllImus(uint32_t rate = IMU_FREQUENCY); 81 | void setImuCallback(boost::function callback); 82 | 83 | void startAllExternalTriggers(uint32_t rate); 84 | void setExternalTriggerCallback( 85 | boost::function callback); 86 | void setExternalTriggerConfig(const ViExternalTriggerConfig config); 87 | 88 | void startAllDenseMatchers(); 89 | void setDenseMatcherCallback(boost::function callback); 90 | 91 | bool getCameraCalibration(SensorId::SensorId cam_id, ViCameraCalibration &calib, bool* is_camera_flipped = NULL); 92 | bool getCameraCalibration(SensorId::SensorId cam_id, int slot_id, ViCameraCalibration &calib, bool* is_camera_flipped = NULL); 93 | bool setCameraCalibration(SensorId::SensorId cam_id, int slot_id, const ViCameraCalibration calib, bool flip_camera = false); 94 | bool setCameraFactoryCalibration(SensorId::SensorId cam_id, const ViCameraCalibration calib, bool flip_camera = false); 95 | bool isStereoCameraFlipped(); 96 | // is called with synchronized images and corresponding corners 97 | void setFramesCornersCallback( 98 | boost::function< 99 | void(ViFrame::Ptr, ViCorner::Ptr)> callback); 100 | 101 | std::vector getListOfSensorIDs() const; 102 | std::vector getListOfCameraIDs() const; 103 | std::vector getListOfDenseIDs() const; 104 | std::vector getListOfCornerIDs() const; 105 | std::vector getListOfImuIDs() const; 106 | std::vector getListOfTriggerIDs() const; 107 | uint32_t getFpgaId() const; 108 | 109 | void setCameraCalibrationSlot(int slot_id = 0); // slot 0 is factory calibration 110 | int getCameraCalibrationSlot(); 111 | void downloadFile(std::string& local_path, std::string& remote_path); 112 | void uploadFile(std::string& local_path, std::string& remote_path); 113 | 114 | void sendSerialData(ViSerialData::Ptr data); 115 | void setSerialCallback(boost::function callback); 116 | void setSerialDelimiter(const char serial_id, const std::string delimiter); 117 | void setSerialBaudrate(const char serial_id, const unsigned int baudrate); 118 | 119 | private: 120 | bool isInitialized(); 121 | 122 | IpConnection::Ptr ip_connection_; 123 | SerialHost::Ptr serial_host_; 124 | 125 | Sensor::IdMap sensors_; 126 | boost::thread_group sensor_threads_; 127 | 128 | bool initialized_; 129 | 130 | int current_camera_calibration_slot_; 131 | 132 | std::vector cam_corner_syncronizer_; 133 | }; 134 | } //namespace visensor 135 | 136 | #endif 137 | -------------------------------------------------------------------------------- /include/sensors/sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef SENSOR_HPP_ 33 | #define SENSOR_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "networking/config_connection.hpp" 41 | #include "synchronization/concurrent_queue.hpp" 42 | #include "networking/config_connection.hpp" 43 | #include "visensor/visensor_constants.hpp" 44 | #include "visensor/visensor_datatypes.hpp" 45 | 46 | namespace visensor { 47 | 48 | // used to clean up memory in shared pointers 49 | template 50 | struct array_deleter { 51 | void operator ()(T const * p) { 52 | delete[] p; 53 | } 54 | }; 55 | 56 | struct Measurement { 57 | typedef boost::shared_ptr Ptr; 58 | 59 | Measurement() 60 | : data(), 61 | buffer_size(0), 62 | bytes_in_buffer(0), 63 | timestamp(0), 64 | timestamp_host(0) { 65 | } 66 | ~Measurement() { 67 | } 68 | 69 | boost::shared_ptr data; 70 | uint32_t buffer_size; 71 | uint32_t bytes_in_buffer; 72 | uint64_t timestamp; 73 | uint64_t timestamp_host; 74 | uint32_t timestamp_fpga_counter; //raw fpga counter timestamp (1e-5 s) 75 | }; 76 | 77 | struct SensorSettings { 78 | SensorSettings(SensorId::SensorId id, SensorType::SensorType type, 79 | uint32_t measurementBufferSize, uint32_t numOfMsgsInPackage, 80 | bool constBufferSize, uint32_t calibrationBufferSize) 81 | : sensor_id(id), 82 | type(type), 83 | measurementBufferSize(measurementBufferSize), 84 | numOfMsgsInPackage(numOfMsgsInPackage), 85 | constBufferSize(constBufferSize), 86 | calibrationBufferSize(calibrationBufferSize), 87 | active(false), 88 | rate(0) { 89 | } 90 | const SensorId::SensorId sensor_id; 91 | const SensorType::SensorType type; 92 | const uint32_t measurementBufferSize; 93 | const uint32_t numOfMsgsInPackage; 94 | const bool constBufferSize; 95 | const uint32_t calibrationBufferSize; 96 | bool active; 97 | uint32_t rate; 98 | }; 99 | 100 | class Sensor { 101 | public: 102 | typedef boost::shared_ptr Ptr; 103 | typedef std::map IdMap; 104 | 105 | Sensor(SensorSettings sensorSettings, 106 | ConfigConnection::WeakPtr config_connection); 107 | virtual ~Sensor(); 108 | 109 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value)=0; 110 | virtual void init() = 0; 111 | virtual void setUserCallback(boost::function callback){}; 112 | virtual void setFrameCallback(boost::function callback){}; 113 | virtual void setCornerCallback(boost::function callback){}; 114 | 115 | virtual void setUserCallback(boost::function callback){}; 116 | virtual void setUserCallback(boost::function callback){}; 117 | 118 | bool startSensor(uint32_t rate); 119 | bool stopSensor(); 120 | uint64_t getTimeBetweenMsgs() const; 121 | int getNumOfMsgsInPackage() const; 122 | int getMeasurementBufferSize() const; 123 | Measurement getEmptyCalibrationMsg() const; 124 | void addMeasurement(const Measurement::Ptr meas); 125 | 126 | inline bool getSensorActive() const { 127 | return settings_.active; 128 | } 129 | 130 | inline bool getIsConstBufferSize() const { 131 | return settings_.constBufferSize; 132 | } 133 | 134 | inline SensorType::SensorType type() const { 135 | return settings_.type; 136 | } 137 | 138 | inline const SensorId::SensorId id() const { 139 | return settings_.sensor_id; 140 | } 141 | 142 | const SensorSettings& settings() const { 143 | return settings_; 144 | } 145 | 146 | protected: 147 | void writeRequest(const ViConfigMsg configMsg); 148 | void readRequest(ViConfigMsg& configMsg); 149 | bool checkTimestamp(uint64_t timestamp); 150 | ConfigConnection::Ptr getConfigConnection(); 151 | 152 | public: 153 | concurrent_queue > measurement_queue_; 154 | 155 | private: 156 | //config server conenction 157 | ConfigConnection::WeakPtr config_connection_; 158 | 159 | SensorSettings settings_; 160 | uint64_t prev_timestamp_; 161 | uint32_t allowed_timediff_; 162 | uint32_t measurement_miss_counter_; 163 | }; 164 | 165 | } //namespace visensor 166 | 167 | #endif /* SENSOR_HPP_ */ 168 | -------------------------------------------------------------------------------- /src/sensors/imu_adis16488.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace visensor { 39 | 40 | ImuAdis16488::ImuAdis16488(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection) 41 | : Imu( 42 | sensor_id, 43 | SensorSettings(sensor_id, SensorType::SensorType::IMU_ADIS16488, calculateBufferSize(), 44 | ImuAdis16488Defaults::NUM_OF_MSGS_IN_PACKAGE, ImuAdis16488Defaults::USE_CONST_PACKAGE_SIZE, 45 | ImuAdis16488Defaults::CALIBRATION_SIZE), 46 | config_connection) { 47 | } 48 | 49 | uint32_t ImuAdis16488::calculateBufferSize() { 50 | return (ImuAdis16488Defaults::MSG_SIZE); 51 | } 52 | 53 | void ImuAdis16488::init() { 54 | } 55 | 56 | void ImuAdis16488::processMeasurements() { 57 | while (1) { 58 | boost::this_thread::interruption_point(); 59 | Measurement::Ptr meas = Sensor::measurement_queue_.pop(); 60 | 61 | //check for missed frames 62 | // TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...) 63 | if (checkTimestamp(meas->timestamp)) { 64 | //publish an empty missed frame 65 | ViImuMsg::Ptr missed_imu_ptr = boost::make_shared(); 66 | missed_imu_ptr->imu_id = Imu::imu_id_; 67 | publishImuData(missed_imu_ptr, ViErrorCodes::MEASUREMENT_DROPPED); 68 | } 69 | 70 | ViImuMsg::Ptr imu_msg_ptr = boost::make_shared(); 71 | 72 | // Add imu information to the msg 73 | imu_msg_ptr->imu_id = Imu::imu_id_; 74 | imu_msg_ptr->timestamp = meas->timestamp; 75 | imu_msg_ptr->timestamp_host = meas->timestamp_host; 76 | imu_msg_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter; 77 | 78 | // get imu data 79 | getGyro(meas->data.get(), &imu_msg_ptr->gyro[0]); 80 | getAcc(meas->data.get(), &imu_msg_ptr->acc[0]); 81 | getMag(meas->data.get(), &imu_msg_ptr->mag[0]); 82 | getBaro(meas->data.get(), &imu_msg_ptr->baro); 83 | 84 | publishImuData(imu_msg_ptr, ViErrorCodes::NO_ERROR); 85 | } 86 | } 87 | 88 | ViConfigMsg ImuAdis16488::getConfigParam(std::string cmd, uint32_t value) { 89 | // TODO: implement this 90 | ViConfigMsg config; 91 | config.sensorId = SensorId::SensorId::NOT_SPECIFIED; 92 | config.devAdress = 0; 93 | config.reg = 0; 94 | config.val = 0; 95 | config.comType = 0; 96 | config.valChanged = false; 97 | return config; 98 | } 99 | 100 | void ImuAdis16488::getGyro(uint8_t * buffer, double * gyro) { 101 | int32_t temp; 102 | int current_pos; 103 | for (int i = 0; i < 3; i++) { 104 | current_pos = 4 * i; 105 | temp = (int32_t) ((buffer[current_pos] << 24) | (buffer[current_pos + 1] << 16) 106 | | (buffer[current_pos + 2] << 8) | (buffer[current_pos + 3] << 0)); 107 | gyro[i] = (double) temp; 108 | gyro[i] = gyro[i] * 450.0 / 1474560000.0 / 180.0 * M_PI; 109 | } 110 | } 111 | 112 | void ImuAdis16488::getAcc(uint8_t * buffer, double * acc) { 113 | int i, current_pos; 114 | int32_t temp; 115 | 116 | for (i = 0; i < 3; i++) { 117 | current_pos = 3 * 4 + 4 * i; 118 | temp = (int32_t) ((buffer[current_pos] << 24) | (buffer[current_pos + 1] << 16) 119 | | (buffer[current_pos + 2] << 8) | (buffer[current_pos + 3] << 0)); 120 | 121 | if ((temp / 81920000.0 * STANDARD_GRAVITY) < -200.0) 122 | VISENSOR_DEBUG("wrong imu acc value id: %i value raw: %i value: %f \n", i, temp, 123 | (temp /81920000*STANDARD_GRAVITY)); 124 | 125 | acc[i] = (double) temp; 126 | acc[i] = acc[i] / 81920000.0 * STANDARD_GRAVITY; //m^2/s 127 | } 128 | } 129 | 130 | void ImuAdis16488::getMag(uint8_t * buffer, double * mag) { 131 | for (int i = 0; i < 3; i++) { 132 | int current_pos = 6 * 4 + 2 * i; 133 | int temp = (int16_t) ((buffer[current_pos + 0] << 8) | (buffer[current_pos + 1] << 0)); 134 | 135 | mag[i] = static_cast(temp) * 3.2767 / 2147418112.0; 136 | } 137 | } 138 | 139 | void ImuAdis16488::getBaro(uint8_t * buffer, double * baro) { 140 | int current_pos = 6 * 4 + 3 * 2; 141 | int temp = (uint32_t) ((buffer[current_pos + 0] << 24) | (buffer[current_pos + 1] << 16) 142 | | (buffer[current_pos + 2] << 8) | (buffer[current_pos + 3] << 0)); 143 | 144 | *baro = static_cast(temp) * 1.31068 / 2147418112.0 * 1000.0; //hPa 145 | } 146 | 147 | void ImuAdis16488::getTemp(uint8_t * buffer, double * temp) { 148 | int current_pos = 7 * 4 + 3 * 2; 149 | int temp_int = (int) ((buffer[current_pos + 0] << 8) | (buffer[current_pos + 1] << 0)); 150 | *temp = static_cast(temp_int) * 0.00565 + 25; // C; 151 | } 152 | 153 | } //namespace visensor 154 | -------------------------------------------------------------------------------- /include/sensors/external_trigger.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef ExternalTrigger_HPP_ 33 | #define ExternalTrigger_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "networking/param_server.hpp" 40 | #include "sensors/sensor.hpp" 41 | 42 | namespace visensor { 43 | 44 | namespace ExternalTriggerDefaults { 45 | const int MSG_SIZE = 1, NUM_OF_MSGS_IN_PACKAGE = 1; 46 | const int I2C_ADRESS = 0x48; 47 | const uint8_t COM_TYPE = ViComType::FPGA_32; 48 | 49 | const uint8_t R_EX_TRIGGER_CONTROL = 0x00; 50 | const uint8_t R_EX_TRIGGER_DIRECTION = 0x04; 51 | const uint8_t R_EX_TRIGGER_POLARITY = 0x08; // 0=active high, 1=low active 52 | const uint8_t R_EX_TRIGGER_LENGHT = 0x0C; // defines the impulse length in fpga zyklen (125MHz) 53 | const uint8_t R_EX_TRIGGER0_START_OFFSET = 0x10; // 0=OFF, 1=ON 54 | const uint8_t R_EX_TRIGGER0_INTERVAL = 0x14; // 0=out, 1=in 55 | const uint8_t R_EX_TRIGGER1_START_OFFSET = 0x18; // 0=active high, 1=low active 56 | const uint8_t R_EX_TRIGGER1_INTERVAL = 0x1C; // defines the impulse length in fpga zyklen (125MHz) 57 | const uint8_t R_EX_TRIGGER2_START_OFFSET = 0x20; 58 | const uint8_t R_EX_TRIGGER2_INTERVAL = 0x24; 59 | } 60 | 61 | class ExternalTriggerConfig { 62 | public: 63 | ExternalTriggerConfig(const SensorId::SensorId sensorId) 64 | : sensor_id_(sensorId) { 65 | // name, type, register, default, mask, min, max 66 | param_server_.addParam("enable_trigger_0", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_CONTROL, 0, 0b1); 67 | param_server_.addParam("enable_trigger_1", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_CONTROL, 0, 0b10); 68 | param_server_.addParam("enable_trigger_2", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_CONTROL, 0, 0b100); 69 | 70 | param_server_.addParam("trigger_0_rate", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER0_INTERVAL, 100, 0, 0, 100000); 71 | param_server_.addParam("trigger_1_rate", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER1_INTERVAL, 100, 0, 0, 100000); 72 | param_server_.addParam("trigger_2_rate", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER2_INTERVAL, 100, 0, 0, 100000); 73 | 74 | param_server_.addParam("trigger_0_offset", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER0_START_OFFSET, 0, 0, 0, 100000); 75 | param_server_.addParam("trigger_1_offset", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER1_START_OFFSET, 0, 0, 0, 100000); 76 | param_server_.addParam("trigger_2_offset", param_server::UINT_T, ExternalTriggerDefaults::R_EX_TRIGGER2_START_OFFSET, 0, 0, 0, 100000); 77 | 78 | param_server_.addParam("trigger_0_direction", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_DIRECTION, 0, 0b1); //0 is output, 1 is input 79 | param_server_.addParam("trigger_1_direction", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_DIRECTION, 0, 0b10); //0 is output, 1 is input 80 | param_server_.addParam("trigger_2_direction", param_server::BOOL_T, ExternalTriggerDefaults::R_EX_TRIGGER_DIRECTION, 0, 0b100); //0 is output, 1 is input 81 | } 82 | 83 | ViConfigMsg getConfigParam(const std::string paramName, uint16_t value) { 84 | ViConfigMsg msg; 85 | msg.valChanged = param_server_.getConfigParam(paramName, value, msg.reg, 86 | msg.val); 87 | msg.sensorId = sensor_id_; 88 | msg.devAdress = ExternalTriggerDefaults::I2C_ADRESS; 89 | msg.comType = ExternalTriggerDefaults::COM_TYPE; 90 | return msg; 91 | } 92 | 93 | ViConfigMsg getConfigParam(const std::string paramName) { 94 | ViConfigMsg msg; 95 | msg.valChanged = param_server_.getConfigParam(paramName, msg.reg, msg.val); 96 | msg.sensorId = sensor_id_; 97 | msg.devAdress = ExternalTriggerDefaults::I2C_ADRESS; 98 | msg.comType = ExternalTriggerDefaults::COM_TYPE; 99 | return msg; 100 | } 101 | 102 | const SensorId::SensorId sensor_id_; 103 | 104 | private: 105 | param_server::ParamServer param_server_; 106 | }; 107 | 108 | class ExternalTrigger : public Sensor { 109 | public: 110 | typedef boost::shared_ptr Ptr; 111 | 112 | ExternalTrigger(SensorId::SensorId sensor_id, 113 | IpConnection::WeakPtr config_connection); 114 | virtual ~ExternalTrigger() { 115 | } 116 | ; 117 | 118 | virtual ViConfigMsg getConfigParam(std::string cmd, uint32_t value); 119 | void publishExternalTriggerData( 120 | ViExternalTriggerMsg::Ptr ExternalTrigger); 121 | void setUserCallback( 122 | boost::function callback); 123 | virtual void init(); 124 | 125 | void processMeasurements(); 126 | static uint32_t calculateBufferSize(); 127 | 128 | protected: 129 | boost::function user_callback_; 130 | 131 | private: 132 | ExternalTriggerConfig config_; 133 | }; 134 | } //namespace visensor 135 | 136 | #endif /* ExternalTrigger_HPP_ */ 137 | -------------------------------------------------------------------------------- /src/sensors/camera_mt9v034.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "sensors/camera_mt9v034.hpp" 37 | #include "visensor_impl.hpp" // needed by forward declaration 38 | 39 | namespace visensor { 40 | CameraMt9v034::CameraMt9v034(SensorId::SensorId sensor_id, IpConnection::WeakPtr config_connection) 41 | : Camera( 42 | sensor_id, 43 | ViCameraConfig(CameraMt9v034Defaults::WIDTH, 44 | CameraMt9v034Defaults::HEIGHT, 45 | CameraMt9v034Defaults::RATE), 46 | SensorSettings(sensor_id, SensorType::SensorType::CAMERA_MT9V034, 47 | calculateBufferSize(), 48 | CameraMt9v034Defaults::NUM_OF_MSGS_IN_PACKAGE, 49 | CameraMt9v034Defaults::USE_CONST_PACKAGE_SIZE, 50 | CameraMt9v034Defaults::CALIBRATION_SIZE), 51 | config_connection), 52 | config_(sensor_id) { 53 | 54 | exposure_ = 80; 55 | gain_ = 16; 56 | 57 | //allocateBuffer(); 58 | } 59 | 60 | void CameraMt9v034::init() { 61 | writeRequest(config_.getConfigParam("chip_control")); 62 | writeRequest(config_.getConfigParam("min_coarse_shutter_width")); 63 | writeRequest(config_.getConfigParam("max_coarse_shutter_width")); 64 | writeRequest(config_.getConfigParam("lvds_shift_clk_ctrl")); 65 | writeRequest(config_.getConfigParam("lvds_payload_control")); 66 | writeRequest(config_.getConfigParam("lvds_master_control")); 67 | writeRequest(config_.getConfigParam("dangerous_reserved_register")); 68 | writeRequest(config_.getConfigParam("soft_reset")); 69 | if (camera_id_ == 1) { 70 | writeRequest(config_.getConfigParam("row_flip", 1)); 71 | writeRequest(config_.getConfigParam("column_flip", 1)); 72 | } else { 73 | writeRequest(config_.getConfigParam("row_flip", 0)); 74 | writeRequest(config_.getConfigParam("column_flip", 0)); 75 | } 76 | } 77 | 78 | uint32_t CameraMt9v034::calculateBufferSize() { 79 | return (CameraMt9v034Defaults::WIDTH * CameraMt9v034Defaults::HEIGHT); 80 | } 81 | 82 | CameraMt9v034::~CameraMt9v034() { 83 | 84 | } 85 | 86 | void CameraMt9v034::controlExposure() { 87 | // ++_exposure_update_frequency; 88 | // if (_sensorId == 4 && _exposure_update_frequency > 1) { 89 | // _exposure_update_frequency = 0; 90 | // ViConfigMsg msg; 91 | // uint32_t desired_bin, current_bin, max_exp; 92 | // desired_bin = 0x3A; 93 | // msg = _config.getConfigParam("agc_current_bin"); 94 | // readRequest(msg); 95 | // current_bin = msg.val; 96 | // msg = _config.getConfigParam("max_coarse_shutter_width"); 97 | // readRequest(msg); 98 | // max_exp = msg.val; 99 | // if (_gain == 16) { 100 | // if (current_bin < 7) 101 | // _exposure = _exposure * 2; 102 | // else if (current_bin == 64) 103 | // _exposure = _exposure * 3 / 4; 104 | // else 105 | // _exposure = (_exposure * desired_bin) / current_bin; 106 | // 107 | // if (_exposure > max_exp) 108 | // _exposure = max_exp; 109 | // 110 | // writeRequest(_config.getConfigParam("coarse_shutter_width", _exposure)); 111 | // } else { 112 | // _exposure = max_exp; 113 | // if (current_bin < 7) 114 | // _gain = _gain * 2; 115 | // else if (current_bin == 64) 116 | // _gain = _gain * 3 / 4; 117 | // else 118 | // _gain = (_gain * desired_bin) / current_bin; 119 | // 120 | // if (_gain > 64) 121 | // _gain = 64; 122 | // 123 | // if (_gain < 16) 124 | // _gain = 16; 125 | // 126 | // writeRequest(_config.getConfigParam("global_analog_gain", _gain)); 127 | // } 128 | // VISENSOR_DEBUG("exposure_new %X gain_new: %X \n", _exposure, _gain); 129 | // } 130 | // VISENSOR_DEBUG("in camera thread %d\n", Camera::_cameraId); 131 | } 132 | 133 | //the threaded function that works on the queue of finished measurements 134 | void CameraMt9v034::processMeasurements() { 135 | while (1) { 136 | 137 | boost::this_thread::interruption_point(); 138 | 139 | //get the newest measurement 140 | boost::shared_ptr meas = Sensor::measurement_queue_.pop(); 141 | 142 | //cam exposure control 143 | controlExposure(); 144 | 145 | //check for missed frames 146 | // TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...) 147 | if( checkTimestamp(meas->timestamp) ) 148 | { 149 | //publish an empty missed frame 150 | ViFrame::Ptr missed_frame_ptr = boost::make_shared(); 151 | missed_frame_ptr->camera_id = Camera::camera_id_; 152 | publishFrameData(missed_frame_ptr, ViErrorCodes::MEASUREMENT_DROPPED); 153 | } 154 | 155 | 156 | ViFrame::Ptr frame_ptr = boost::make_shared(); 157 | 158 | // Add camera information to the frame 159 | frame_ptr->width = Camera::config_.width; 160 | frame_ptr->height = Camera::config_.height; 161 | frame_ptr->camera_id = Camera::camera_id_; 162 | frame_ptr->timestamp = meas->timestamp; 163 | frame_ptr->timestamp_host = meas->timestamp_host; 164 | frame_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter; 165 | 166 | frame_ptr->image_type = CameraMt9v034Defaults::IMAGE_TYPE; 167 | 168 | frame_ptr->setImageRawPtr(meas->data, meas->bytes_in_buffer); 169 | 170 | publishFrameData(frame_ptr, ViErrorCodes::NO_ERROR); 171 | } 172 | } 173 | 174 | ViConfigMsg CameraMt9v034::getConfigParam(std::string cmd, uint32_t value) { 175 | // msg->sensorId=Sensor::_sensorId; 176 | return config_.getConfigParam(cmd, value); 177 | } 178 | 179 | } //namespace visensor 180 | -------------------------------------------------------------------------------- /src/visensor/visensor_api.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include "visensor/visensor_api.hpp" 33 | #include "visensor_impl.hpp" 34 | 35 | namespace visensor 36 | { 37 | 38 | ViSensorDriver::ViSensorDriver() 39 | { 40 | pImpl_ = new Impl(); 41 | } 42 | 43 | void ViSensorDriver::getAutoDiscoveryDeviceList(ViDeviceList &hostname_list) 44 | { 45 | pImpl_->getAutoDiscoveryDeviceList(hostname_list); 46 | } 47 | 48 | std::string ViSensorDriver::init() 49 | { 50 | return pImpl_->initAutodiscovery(); 51 | } 52 | 53 | void ViSensorDriver::init(std::string hostname) 54 | { 55 | pImpl_->init(hostname); 56 | } 57 | 58 | ViSensorDriver::~ViSensorDriver()//:_fpga(&getSensorFromID){ 59 | { 60 | delete pImpl_; 61 | } 62 | 63 | void ViSensorDriver::setCameraCallback(boost::function callback) 64 | { 65 | pImpl_->setCameraCallback(callback); 66 | } 67 | 68 | void ViSensorDriver::setCornerCallback(boost::function callback) { 69 | pImpl_->setCornerCallback(callback); 70 | } 71 | 72 | void ViSensorDriver::setFramesCornersCallback(boost::function callback) { 73 | pImpl_->setFramesCornersCallback(callback); 74 | } 75 | 76 | void ViSensorDriver::setImuCallback(boost::function, ViErrorCode)> callback) 77 | { 78 | pImpl_->setImuCallback(callback); 79 | } 80 | 81 | void ViSensorDriver::startAllCameras(uint32_t rate) { 82 | pImpl_->startAllCameras(rate); 83 | } 84 | 85 | void ViSensorDriver::startSensor(SensorId::SensorId id, uint32_t rate) 86 | { 87 | pImpl_->startSensor(id, rate); 88 | } 89 | 90 | void ViSensorDriver::stopSensor(SensorId::SensorId id) 91 | { 92 | pImpl_->stopSensor(id); 93 | } 94 | 95 | void ViSensorDriver::setSensorConfigParam(SensorId::SensorId sensor_id, 96 | std::string cmd, 97 | int value) { 98 | pImpl_->setSensorConfigParam(sensor_id, cmd, value); 99 | } 100 | 101 | void ViSensorDriver::startAllCorners() { 102 | pImpl_->startAllCorners(); 103 | } 104 | 105 | void ViSensorDriver::startAllImus(uint32_t rate) { 106 | pImpl_->startAllImus(rate); 107 | } 108 | 109 | std::vector ViSensorDriver::getListOfSensorIDs() const 110 | { 111 | return pImpl_->getListOfSensorIDs(); 112 | } 113 | 114 | std::vector ViSensorDriver::getListOfCameraIDs() const 115 | { 116 | return pImpl_->getListOfCameraIDs(); 117 | } 118 | 119 | std::vector ViSensorDriver::getListOfDenseIDs() const 120 | { 121 | return pImpl_->getListOfDenseIDs(); 122 | } 123 | 124 | std::vector ViSensorDriver::getListOfCornerIDs() const { 125 | return pImpl_->getListOfCornerIDs(); 126 | } 127 | 128 | std::vector ViSensorDriver::getListOfImuIDs() const 129 | { 130 | return pImpl_->getListOfImuIDs(); 131 | } 132 | 133 | std::vector ViSensorDriver::getListOfTriggerIDs() const 134 | { 135 | return pImpl_->getListOfTriggerIDs(); 136 | } 137 | 138 | uint32_t ViSensorDriver::getFpgaId() const 139 | { 140 | return pImpl_->getFpgaId(); 141 | } 142 | 143 | void ViSensorDriver::downloadFile(std::string local_path, std::string remote_path) { 144 | pImpl_->downloadFile(local_path, remote_path); 145 | } 146 | 147 | void ViSensorDriver::uploadFile(std::string local_path, std::string remote_path) { 148 | pImpl_->uploadFile(local_path, remote_path); 149 | } 150 | 151 | void ViSensorDriver::startAllExternalTriggers(uint32_t rate) { 152 | pImpl_->startAllExternalTriggers(rate); 153 | } 154 | 155 | void ViSensorDriver::setExternalTriggerCallback( 156 | boost::function callback) { 157 | pImpl_->setExternalTriggerCallback(callback); 158 | } 159 | 160 | void ViSensorDriver::setExternalTriggerConfig(const ViExternalTriggerConfig config) { 161 | pImpl_->setExternalTriggerConfig(config); 162 | } 163 | 164 | void ViSensorDriver::startAllDenseMatchers() { 165 | pImpl_->startAllDenseMatchers(); 166 | } 167 | 168 | void ViSensorDriver::setDenseMatcherCallback(boost::function callback) { 169 | pImpl_->setDenseMatcherCallback(callback); 170 | } 171 | 172 | void ViSensorDriver::setCameraCalibrationSlot(int slot_id){ 173 | pImpl_->setCameraCalibrationSlot(slot_id); 174 | } 175 | 176 | int ViSensorDriver::getCameraCalibrationSlot(){ 177 | return pImpl_->getCameraCalibrationSlot(); 178 | } 179 | 180 | bool ViSensorDriver::getCameraCalibration(SensorId::SensorId cam_id, ViCameraCalibration &calib, bool* is_camera_flipped, int slot_id) const 181 | { 182 | return pImpl_->getCameraCalibration(cam_id, slot_id, calib, is_camera_flipped); 183 | } 184 | 185 | bool ViSensorDriver::setCameraCalibration(SensorId::SensorId cam_id, const ViCameraCalibration calib, int slot_id ) const 186 | { 187 | return pImpl_->setCameraCalibration(cam_id, slot_id, calib); 188 | } 189 | 190 | bool ViSensorDriver::setCameraFactoryCalibration(SensorId::SensorId cam_id, const ViCameraCalibration calib, bool flip_camera) const 191 | { 192 | return pImpl_->setCameraFactoryCalibration(cam_id, calib, flip_camera); 193 | } 194 | 195 | bool ViSensorDriver::isStereoCameraFlipped() const 196 | { 197 | return pImpl_->isStereoCameraFlipped(); 198 | } 199 | 200 | 201 | void ViSensorDriver::sendSerialData(ViSerialData::Ptr data) const 202 | { 203 | pImpl_->sendSerialData(data); 204 | } 205 | 206 | void ViSensorDriver::setSerialCallback(boost::function callback) 207 | { 208 | pImpl_->setSerialCallback(callback); 209 | } 210 | 211 | void ViSensorDriver::setSerialDelimiter(const char serial_id, const std::string delimiter) const 212 | { 213 | pImpl_->setSerialDelimiter(serial_id, delimiter); 214 | } 215 | 216 | void ViSensorDriver::setSerialBaudrate(const char serial_id, const unsigned int baudrate) const 217 | { 218 | pImpl_->setSerialBaudrate(serial_id, baudrate); 219 | } 220 | 221 | } //namespace visensor 222 | -------------------------------------------------------------------------------- /include/networking/connection.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef IP_CONNECTION_ 33 | #define IP_CONNECTION_ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "visensor/visensor_datatypes.hpp" 43 | #include "synchronization/time_synchronizer.hpp" 44 | #include "networking/ip_data_definitions.hpp" 45 | #include "networking/file_transfer.hpp" 46 | #include "networking/config_connection.hpp" 47 | #include "serial_bridge/SerialHost.hpp" 48 | #include "sensors/sensor.hpp" 49 | 50 | namespace visensor { 51 | 52 | //Fpga version struct 53 | struct FpgaConfig { 54 | FpgaConfig() { 55 | fpga_id_ = 0; 56 | firmware_version_major = 0; 57 | firmware_version_minor = 0; 58 | firmware_version_patch = 0; 59 | num_of_sensors_ = 0; 60 | } 61 | uint32_t fpga_id_; 62 | uint16_t firmware_version_major; 63 | uint16_t firmware_version_minor; 64 | uint16_t firmware_version_patch; 65 | uint16_t num_of_sensors_; 66 | }; 67 | 68 | class IpConnection : public visensor::ConfigConnection { 69 | public: 70 | typedef boost::shared_ptr Ptr; 71 | typedef boost::weak_ptr WeakPtr; 72 | 73 | IpConnection(); 74 | ~IpConnection(); 75 | 76 | void connect(std::string hostname); 77 | bool sendLedConfig(uint16_t value); 78 | // set lightning mode: 79 | // 0 - strobe 80 | // 1 - continuous 81 | bool sendLedConfig(bool value); 82 | 83 | void sendExternalTriggerConfig(SensorId::SensorId sensor_id, const ViExternalTriggerConfig config); 84 | 85 | void addSensor(SensorId::SensorId sensor_id, Sensor::Ptr sensor); 86 | void startSensor(SensorId::SensorId sensor_id, uint32_t rate); 87 | void stopSensor(SensorId::SensorId sensor_id); 88 | 89 | bool readCameraCalibration(SensorId::SensorId sensor_id, unsigned int slot_id, ViCameraCalibration &calib_out); 90 | bool writeCameraCalibration(SensorId::SensorId sensor_id, unsigned int slot_id, const ViCameraCalibration calib); 91 | 92 | //serial bridge communication 93 | void registerSerialHost(SerialHost::WeakPtr serial_host); 94 | SerialHost::Ptr getSerialHostPointer(); 95 | 96 | void sendSerialData(ViSerialData::Ptr data_ptr); 97 | void setSerialDelimiter(const char sensor_id, const std::string delimiter); 98 | void setSerialBaudrate(const char sensor_id, const unsigned int baudrate); 99 | 100 | uint32_t getId() const { 101 | return fpga_config_.fpga_id_; 102 | } 103 | 104 | bool fpgaInitialized() const { 105 | return connected_; 106 | } 107 | 108 | virtual void writeConfig(SensorId::SensorId sensor_id, uint8_t dev_adress, uint8_t reg, 109 | uint32_t val, uint8_t comType); 110 | virtual void readConfig(SensorId::SensorId sensor_id, uint8_t dev_adress, uint8_t reg, 111 | uint32_t &val, uint8_t comType); 112 | 113 | void downloadFile(std::string& local_path, std::string& remote_path); 114 | void uploadFile(std::string& local_path, std::string& remote_path); 115 | 116 | std::vector getAttachedSensors(); 117 | 118 | private: 119 | void readFpgaInfo(); 120 | void syncTime(); 121 | 122 | bool sendHeader(int identifier); 123 | void sendStartSensor(SensorId::SensorId sensor_id, uint32_t rate); 124 | void sendStopSensor(SensorId::SensorId sensor_id); 125 | 126 | template 127 | bool writeDataUbi(SensorId::SensorId sensor_id, unsigned char reg, T val, 128 | int numBits); 129 | template 130 | bool readDataUbi(SensorId::SensorId sensor_id, unsigned char reg, T *val, 131 | int numBits); 132 | bool writeDataFpga(SensorId::SensorId sensor_id, unsigned char reg, int val); 133 | bool readDataFpga(SensorId::SensorId sensor_id, unsigned char reg, int &val); 134 | bool readInitMsg(unsigned char * msg); 135 | 136 | uint32_t getTimestampFpgaRaw(uint8_t* buffer); 137 | uint64_t getTimestamp(uint8_t* buffer); 138 | 139 | void processPackage(SensorId::SensorId sensor_id, Measurement::Ptr new_measurement); 140 | void read_handler(const boost::system::error_code& e, 141 | std::size_t bytes_transferred); 142 | void imu_read_handler(const boost::system::error_code& e, 143 | std::size_t bytes_transferred); 144 | void serial_read_handler(const boost::system::error_code& e, 145 | std::size_t bytes_transferred); 146 | 147 | bool receiveAck(boost::asio::ip::tcp::socket &socket); 148 | 149 | template 150 | void send_package(IpComm::Header &header, T &data); 151 | template 152 | void receive_package(IpComm::Header &header, T &data, 153 | P payload_type /*only used to know the datatype*/); 154 | template 155 | void send_payload(boost::array& data); 156 | template 157 | void receive_payload(boost::asio::ip::tcp::socket &socket, boost::array& data); 158 | 159 | IpComm::Header readHeader(boost::asio::ip::tcp::socket &socket); 160 | 161 | private: 162 | bool connected_; 163 | TimeSynchronizer time_synchronizer_; 164 | uint8_t number_of_sensors_; 165 | std::map sensors_; 166 | SensorId::SensorId led_sensor_id_; // HACK: either add the light to the cam options or create a led sensor class 167 | FpgaConfig fpga_config_; 168 | 169 | //pointer to serialhost 170 | SerialHost::WeakPtr serial_host_; 171 | 172 | // connection variables 173 | boost::asio::io_service io_service_; 174 | boost::asio::ip::tcp::socket data_socket_; 175 | boost::asio::ip::tcp::socket imu_socket_; 176 | boost::asio::ip::tcp::socket serial_socket_; 177 | boost::asio::ip::tcp::socket config_socket_; 178 | IpComm::HeaderPayload data_header_payload_; 179 | IpComm::HeaderPayload imu_header_payload_; 180 | IpComm::HeaderPayload serial_header_payload_; 181 | 182 | FileTransfer file_transfer_; 183 | }; 184 | } //namespace visensor 185 | 186 | #endif /* IP_CONNECTION_ */ 187 | -------------------------------------------------------------------------------- /include/synchronization/concurrent_queue.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef CONCURRENT_QUEUE_H_ 33 | #define CONCURRENT_QUEUE_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "lifo_nonlocking.h" 40 | 41 | template 42 | class concurrent_heap; 43 | 44 | template 45 | class concurrent_queue; 46 | 47 | template 48 | class concurrent_base 49 | { 50 | friend class concurrent_heap; 51 | friend class concurrent_queue; 52 | protected: 53 | container_T mqueue; 54 | mutable boost::mutex mmutex; 55 | boost::condition_variable mcond; 56 | public: 57 | typedef container_T container_type; 58 | typedef typename container_T::value_type value_type; 59 | concurrent_base(){ 60 | while(!mqueue.empty()) 61 | mqueue.pop(); 62 | } 63 | virtual ~concurrent_base(){}; 64 | virtual __inline__ void clear() = 0; 65 | virtual __inline__ value_type pop(void) = 0; 66 | 67 | __inline__ void push(value_type const& data) 68 | { 69 | //bool const was_empty=mqueue.empty(); 70 | using namespace std; 71 | { //to unlock before notification 72 | boost::mutex::scoped_lock lock(mmutex); 73 | mqueue.push(data); 74 | } 75 | //if(was_empty) 76 | //{ 77 | mcond.notify_one(); 78 | //} 79 | } 80 | __inline__ bool empty() const 81 | { 82 | boost::mutex::scoped_lock lock(mmutex); 83 | return mqueue.empty(); 84 | } 85 | __inline__ typename container_T::size_type size() const 86 | { 87 | boost::mutex::scoped_lock lock(mmutex); 88 | return mqueue.size(); 89 | } 90 | 91 | void notify(){ 92 | mcond.notify_all(); 93 | } 94 | }; 95 | 96 | template 97 | class concurrent_heap:public concurrent_base 98 | { 99 | public: 100 | using concurrent_base::size; 101 | using concurrent_base::push; 102 | using concurrent_base::empty; 103 | 104 | concurrent_heap(){}; 105 | 106 | virtual ~concurrent_heap(){}; 107 | virtual __inline__ typename concurrent_base::value_type pop(void){ 108 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 109 | while(concurrent_base::mqueue.empty()) 110 | { 111 | concurrent_base::mcond.wait(lock); 112 | } 113 | typename container_T::value_type topval = concurrent_base::mqueue.top(); 114 | concurrent_base::mqueue.pop(); 115 | return topval; 116 | } 117 | virtual __inline__ void clear(){ 118 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 119 | concurrent_base::mqueue.clear(); 120 | } 121 | }; 122 | 123 | template 124 | class concurrent_queue:public concurrent_base 125 | { 126 | public: 127 | using concurrent_base::size; 128 | using concurrent_base::push; 129 | using concurrent_base::empty; 130 | concurrent_queue(){}; 131 | virtual ~concurrent_queue(){}; 132 | __inline__ typename concurrent_base::value_type pop(void){ 133 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 134 | while(concurrent_base::mqueue.empty()) 135 | { 136 | concurrent_base::mcond.wait(lock); 137 | } 138 | typename concurrent_base::value_type topval = concurrent_base::mqueue.front(); 139 | concurrent_base::mqueue.pop(); 140 | return topval; 141 | } 142 | virtual __inline__ void clear(){ 143 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 144 | while(!concurrent_base::mqueue.empty()){ 145 | concurrent_base::mqueue.pop(); 146 | } 147 | } 148 | }; 149 | 150 | template 151 | class concurrent_pqueue:public concurrent_base 152 | { 153 | public: 154 | concurrent_pqueue(){}; 155 | virtual ~concurrent_pqueue(){}; 156 | __inline__ typename concurrent_base::value_type pop(void){ 157 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 158 | while(concurrent_base::mqueue.empty()) 159 | { 160 | concurrent_base::mcond.timed_wait(lock, boost::posix_time::microsec(10)); 161 | } 162 | typename concurrent_base::value_type topval = concurrent_base::mqueue.top(); 163 | concurrent_base::mqueue.pop(); 164 | return topval; 165 | } 166 | virtual __inline__ void clear(){ 167 | boost::mutex::scoped_lock lock(concurrent_base::mmutex); 168 | while(!concurrent_base::mqueue.empty()){ 169 | concurrent_base::mqueue.pop(); 170 | } 171 | } 172 | }; 173 | 174 | 175 | template 176 | class nonlocking_lifo 177 | { 178 | private: 179 | struct dataWrapper:public nonlocking::Lifo::Node{ //a wrapper, since nonlocking lifo stores pointers that must inherit from Lifo::Node 180 | value_T data; 181 | dataWrapper(value_T _data):data(_data){}; 182 | }; 183 | 184 | nonlocking::Lifo lifo; //the stack 185 | 186 | public: 187 | typedef value_T value_type; 188 | nonlocking_lifo(){}; 189 | virtual ~nonlocking_lifo(){}; 190 | __inline__ value_T pop(void){ 191 | dataWrapper* dtwrap = NULL; 192 | while(!dtwrap){ //dtwrap will be NULL if queue is empty 193 | dtwrap = (dataWrapper*)lifo.pop(); 194 | boost::this_thread::interruption_point(); 195 | if(!dtwrap) 196 | boost::this_thread::sleep(boost::posix_time::millisec(10)); //if queue is empty wait. Otherwise we hammer the queue too badly 197 | } 198 | value_T val = dtwrap->data; 199 | delete dtwrap; 200 | return val; 201 | } 202 | virtual __inline__ void clear(){ 203 | dataWrapper* topval = NULL; 204 | do{ //topval will be NULL if queue is empty 205 | topval = (dataWrapper*)lifo.pop(); 206 | if(topval) 207 | delete topval; 208 | }while(topval); 209 | } 210 | 211 | __inline__ void push(value_T const& data) 212 | { 213 | dataWrapper* val = new dataWrapper(data); 214 | boost::this_thread::interruption_point(); 215 | lifo.push(val); 216 | } 217 | __inline__ bool empty() const 218 | { 219 | return (lifo.head == NULL); 220 | } 221 | __inline__ int sync() const 222 | { 223 | return lifo.sync; 224 | } 225 | 226 | //no size() implemented, because this would require a mutex. Simply counting unsave the nodes from head to tail w/o a mutex is not an option! 227 | }; 228 | 229 | #endif /* CONCURRENT_QUEUE_H_ */ 230 | --------------------------------------------------------------------------------