From 553c5df5b77fbd0d436c055c62f3d55d4297114e Mon Sep 17 00:00:00 2001 From: Ramakrishna Kintada Date: Wed, 21 Jun 2017 12:40:07 -0700 Subject: [PATCH 01/62] Updated the VISLAM example for mv_1.0.2 release Signed-off-by: Ramakrishna Kintada --- src/camera/SnapdragonCameraManager.cpp | 4 ++-- src/nodes/SnapdragonRosNodeVislam.cpp | 12 ++++++------ src/vislam/SnapdragonVislamManager.cpp | 2 +- test/test_SnapdragonVislamManager.cpp | 12 ++++++------ 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/camera/SnapdragonCameraManager.cpp b/src/camera/SnapdragonCameraManager.cpp index bc5c763..8caf7dc 100644 --- a/src/camera/SnapdragonCameraManager.cpp +++ b/src/camera/SnapdragonCameraManager.cpp @@ -139,8 +139,8 @@ int32_t Snapdragon::CameraManager::Initialize(){ } } - snap_camera_param_ptr_->mv_cpa_config.startExposure = camera_config_ptr_->exposure; - snap_camera_param_ptr_->mv_cpa_config.startGain = camera_config_ptr_->gain; + snap_camera_param_ptr_->mv_cpa_config.legacyCost.startExposure = camera_config_ptr_->exposure; + snap_camera_param_ptr_->mv_cpa_config.legacyCost.startGain = camera_config_ptr_->gain; if (snap_camera_param_ptr_->enable_cpa) { mvCPA_ptr_ = mvCPA_Initialize(&snap_camera_param_ptr_->mv_cpa_config); diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index bfc9f8b..83f6749 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -153,12 +153,12 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { //set the cpa configuration. mvCPA_Configuration cpaConfig; - cpaConfig.cpaType = 1; - cpaConfig.startExposure = param.camera_config.exposure; - cpaConfig.startGain = param.camera_config.gain; - cpaConfig.filterSize = 1; - cpaConfig.exposureCost = 1.0f; - cpaConfig.gainCost = 0.3333f; + cpaConfig.cpaType = MVCPA_MODE_COST; + cpaConfig.legacyCost.startExposure = param.camera_config.exposure; + cpaConfig.legacyCost.startGain = param.camera_config.gain; + cpaConfig.legacyCost.filterSize = 1; + cpaConfig.legacyCost.exposureCost = 1.0f; + cpaConfig.legacyCost.gainCost = 0.3333f; param.mv_cpa_config = cpaConfig; Snapdragon::VislamManager vislam_man; diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index e840a67..742ff67 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -110,7 +110,7 @@ int32_t Snapdragon::VislamManager::Initialize if( rc == 0 ) { vislam_ptr_ = mvVISLAM_Initialize ( - &(cam_params_.mv_camera_config), + &(cam_params_.mv_camera_config), 0, vislam_params_.tbc, vislam_params_.ombc, vislam_params_.delta, vislam_params_.std0Tbc, vislam_params_.std0Ombc, vislam_params_.std0Delta, vislam_params_.accelMeasRange, vislam_params_.gyroMeasRange, diff --git a/test/test_SnapdragonVislamManager.cpp b/test/test_SnapdragonVislamManager.cpp index bf5b299..ead6f3b 100644 --- a/test/test_SnapdragonVislamManager.cpp +++ b/test/test_SnapdragonVislamManager.cpp @@ -225,12 +225,12 @@ int main( int argc, char** argv ) { //set the cpa configuration. mvCPA_Configuration cpaConfig; - cpaConfig.cpaType = 1; - cpaConfig.startExposure = param.camera_config.exposure; - cpaConfig.startGain = param.camera_config.gain; - cpaConfig.filterSize = 1; - cpaConfig.exposureCost = 1.0f; - cpaConfig.gainCost = 0.3333f; + cpaConfig.cpaType = MVCPA_MODE_COST; + cpaConfig.legacyCost.startExposure = param.camera_config.exposure; + cpaConfig.legacyCost.startGain = param.camera_config.gain; + cpaConfig.legacyCost.filterSize = 1; + cpaConfig.legacyCost.exposureCost = 1.0f; + cpaConfig.legacyCost.gainCost = 0.3333f; param.mv_cpa_config = cpaConfig; From da026cfcb40c092489b680e9f08f9298a5039ce1 Mon Sep 17 00:00:00 2001 From: Ramakrishna Kintada Date: Wed, 21 Jun 2017 12:50:56 -0700 Subject: [PATCH 02/62] Updated the README.md file to add the specifics for mv 1.0.2 release Signed-off-by: Ramakrishna Kintada --- README.md | 40 +++++++++++++--------------------------- 1 file changed, 13 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 591e484..74707c1 100644 --- a/README.md +++ b/README.md @@ -2,9 +2,9 @@ This repo provides the sample code and instructions to run Visual-Inertial Simultaneous Localization And Mapping (VISLAM) as a ROS node on the [Qualcomm Snapdragon Platform](https://developer.qualcomm.com/hardware/snapdragon-flight)TM. -This example code is for MV SDK release [mv_0.9.1](https://developer.qualcomm.com/hardware/snapdragon-flight/tools). +This example code is for MV SDK release [mv_1.0.2](https://developer.qualcomm.com/hardware/snapdragon-flight/tools). -**NOTE**: The older release(mv0.8) instructions are [here](https://github.com/ATLFlight/ros-examples/tree/mv-release-0.8). The mv0.8 release will deprecated after May. 15th 2017. +**NOTE**: The older release(mv0.9.1) instructions are [here](https://github.com/ATLFlight/ros-examples/tree/master). This example assumes that you are familiar with ROS framework. If you are new to ROS, refer to [ROS Start Guide](http://wiki.ros.org/ROS/StartGuide) first to get started. @@ -36,11 +36,11 @@ This example assumes that you are familiar with ROS framework. If you are new t ### Summary of changes from previous release -| Item | Previous release - mv0.8 | Current Release - mv0.9.1 | +| Item | Previous release - mv0.8 | Current Release - mv0.9.1 | Future Release mv 1.0.2 | |----|----|----| -|MV_SDK environment variable| needed | Not needed. The new mv installation puts the library files under /usr/lib | -|MV License file installation | needed. Should be placed in the /opt/qualcomm/mv/lib/mv/bin/lin/8x74/ | needed should be placed at /usr/lib | -|MV link library Name| libmv.so | libmv1.so. Update the respective make files to link against libmv1 instead of libmv.so | +|MV_SDK environment variable| needed | Not needed. The new mv installation puts the library files under /usr/lib | Not needed | +|MV License file installation | needed. Should be placed in the /opt/qualcomm/mv/lib/mv/bin/lin/8x74/ | needed should be placed at /usr/lib | needed should be placed at /opt/qcom-licenses/ | +|MV link library Name| libmv.so | libmv1.so. Update the respective make files to link against libmv1 instead of libmv.so | same as mv 0.9.1 | The current build process is supported only on-target (i.e. on the Snapdragon FlightTM Board). Future release(s) will support off-target cross-compilation on a host computer. @@ -52,7 +52,7 @@ The following is an overall workflow for installation, build and execution of th #### Platform BSP -These instructions were tested with version **Flight_3.1.2**. The latest version of the software can be downloaded from [here](http://support.intrinsyc.com/projects/snapdragon-flight/files) and installed by following the instructions found [here](http://support.intrinsyc.com/projects/snapdragon-flight/wiki) +These instructions were tested with version **Flight_3.1.3**. The latest version of the software can be downloaded from [here](http://support.intrinsyc.com/projects/snapdragon-flight/files) and installed by following the instructions found [here](http://support.intrinsyc.com/projects/snapdragon-flight/wiki) **NOTE**: By default the HOME environment variable is not set. Set this up doing the following: @@ -74,22 +74,6 @@ source /home/linaro/.bashrc Get the latest Snapdragon FlightTM qrlSDK for your Ubuntu 14.04 host computer by following the instructions [here](https://github.com/ATLFlight/ATLFlightDocs/blob/master/AppsGettingStarted.md) -**NOTE**: For this example, you will need the qrlSDK to get the missing files on to the target (see below). - - - 1. Platform build setup for camera headers - - **NOTE**: For on-target camera development there are few header files missing on the target, but are part of the qrlSDK. To fix this, the missing files need to be pushed on to the target. - This is an interim solution and will be addressed in future releases. - - Push the following missing files to the target: - -``` -cd /sysroots/eagle8074/usr/include -adb push camera.h /usr/include -adb push camera_parameters.h /usr/include -adb shell sync -``` #### Install ROS on Snapdragon Platform. @@ -99,7 +83,7 @@ Refer to the following [page](https://github.com/ATLFlight/ATLFlightDocs/blob/ma * Download the latest Snapdragon Machine Vision SDK from [here](https://developer.qualcomm.com/sdflight-tools) * The package name will be mv\.deb. -** Example: *mv0.9.1_8x74.deb* +** Example: *mv1.0.2_8x74.deb* * push the deb package to the target and install it. ``` @@ -115,15 +99,17 @@ dpkg -i /home/linaro/mv.deb The Machine Vision SDK will need a license file to run. Obtain a research and development license file from [here](https://developer.qualcomm.com/sdflight-key-req) -The license file needs to be placed at the same location as the MV SDK library **libmv1.so**. +The license file needs to be placed in the following folder on target: /opt/qcom-licenses/ Push the license file to the target using the following command: ``` -adb push snapdragon-flight-license.bin /usr/lib/ +adb push snapdragon-flight-license.bin /opt/qcom-licenses/ adb shell sync ``` +*NOTE:* Make sure to create the folder /opt/qcom-licenses/ if it is not present on the target. + ### Clone and build sample code #### Setup ROS workspace on target @@ -149,7 +135,7 @@ adb shell source /home/linaro/.bashrc roscd cd ../src -git clone https://github.com/ATLFlight/ros-examples.git snap_ros_examples +git clone -b mv-release-1.0.2 https://github.com/ATLFlight/ros-examples.git snap_ros_examples ``` * Build the code From 1f0d66139a1405bf7e14719e88d472577f85ceb7 Mon Sep 17 00:00:00 2001 From: Ramakrishna Kintada Date: Wed, 21 Jun 2017 12:53:38 -0700 Subject: [PATCH 03/62] Minor Formatting update in the README.md file. Signed-off-by: Ramakrishna Kintada --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 74707c1..5c4d763 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ This example assumes that you are familiar with ROS framework. If you are new t ### Summary of changes from previous release | Item | Previous release - mv0.8 | Current Release - mv0.9.1 | Future Release mv 1.0.2 | -|----|----|----| +|----|----|----|----| |MV_SDK environment variable| needed | Not needed. The new mv installation puts the library files under /usr/lib | Not needed | |MV License file installation | needed. Should be placed in the /opt/qualcomm/mv/lib/mv/bin/lin/8x74/ | needed should be placed at /usr/lib | needed should be placed at /opt/qcom-licenses/ | |MV link library Name| libmv.so | libmv1.so. Update the respective make files to link against libmv1 instead of libmv.so | same as mv 0.9.1 | From 58f0b03eda875096cff2318d1872fbafd6c7bf7c Mon Sep 17 00:00:00 2001 From: Ramakrishna Kintada Date: Wed, 21 Jun 2017 13:02:38 -0700 Subject: [PATCH 04/62] Removed a section that is not needed for mv 1.0.2 release. Signed-off-by: Ramakrishna Kintada --- README.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/README.md b/README.md index 5c4d763..0e480b8 100644 --- a/README.md +++ b/README.md @@ -70,10 +70,6 @@ adb shell source /home/linaro/.bashrc ``` -#### Cross-Compile Build Environment - -Get the latest Snapdragon FlightTM qrlSDK for your Ubuntu 14.04 host computer by following the instructions [here](https://github.com/ATLFlight/ATLFlightDocs/blob/master/AppsGettingStarted.md) - #### Install ROS on Snapdragon Platform. From 05bdc1f87e96e9c632944b19e1f79f5ae57574d5 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 17 May 2017 11:36:19 +0200 Subject: [PATCH 05/62] Changed package name --- CMakeLists.txt | 2 +- package.xml | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a02481e..bd35e62 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(snap_ros_examples) +project(rpg_qualcomm_vislam) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/package.xml b/package.xml index 2f420e0..e39a815 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,16 @@ - snap_ros_examples + rpg_qualcomm_vislam 0.1.1 - This package provides example code as ros nodes for select features of snapdragon flight platfrom(TM) + + Private fork of the Qualcomm Vislam example code from ATLFlight + (https://github.com/ATLFlight/ros-examples) for use with the Snapdragon Flight + - rkintada + potaito From 430fc1aac54a5830b1f79756f4d4a1e1ce21a593 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 22 May 2017 09:18:14 +0200 Subject: [PATCH 06/62] Removed IMUManager dependency completely - still compiling --- src/vislam/SnapdragonVislamManager.cpp | 168 +++++++++++++------------ src/vislam/SnapdragonVislamManager.hpp | 31 +++-- 2 files changed, 107 insertions(+), 92 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 742ff67..ca449c5 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -34,25 +34,34 @@ Snapdragon::VislamManager::VislamManager() { cam_man_ptr_ = nullptr; - imu_man_ptr_ = nullptr; + // imu_man_ptr_ = nullptr; vislam_ptr_ = nullptr; initialized_ = false; image_buffer_size_bytes_ = 0; image_buffer_ = nullptr; + + // Setup publishers/subscribers + // imu_sub_ = nh_.subscribe ("mavros/imu/data_raw", 10, callback_imu); } Snapdragon::VislamManager::~VislamManager() { CleanUp(); } +void Snapdragon::VislamManager::callback_imu( + const sensor_msgs::Imu::ConstPtr& msg) +{ + // TODO: Process IMU message +} + int32_t Snapdragon::VislamManager::CleanUp() { // stop the imu api first. - if( imu_man_ptr_ != nullptr ) { - imu_man_ptr_->RemoveHandler( this ); - imu_man_ptr_->Terminate(); - delete imu_man_ptr_; - imu_man_ptr_ = nullptr; - } + // if( imu_man_ptr_ != nullptr ) { + // imu_man_ptr_->RemoveHandler( this ); + // imu_man_ptr_->Terminate(); + // delete imu_man_ptr_; + // imu_man_ptr_ = nullptr; + // } //stop the camera. if( cam_man_ptr_ != nullptr ) { @@ -95,16 +104,16 @@ int32_t Snapdragon::VislamManager::Initialize } } - if( rc == 0 ) { //initialize the Imu Manager. - imu_man_ptr_ = new Snapdragon::ImuManager(); - if( imu_man_ptr_ != nullptr ) { - rc = imu_man_ptr_->Initialize(); - imu_man_ptr_->AddHandler( this ); - } - else { - rc = -1; - } - } + // if( rc == 0 ) { //initialize the Imu Manager. + // imu_man_ptr_ = new Snapdragon::ImuManager(); + // if( imu_man_ptr_ != nullptr ) { + // rc = imu_man_ptr_->Initialize(); + // imu_man_ptr_->AddHandler( this ); + // } + // else { + // rc = -1; + // } + // } //now intialize the VISLAM module. if( rc == 0 ) { @@ -139,7 +148,7 @@ int32_t Snapdragon::VislamManager::Start() { int32_t rc = 0; if( initialized_ ) { //start the camera - rc = imu_man_ptr_->Start(); + // rc = imu_man_ptr_->Start(); rc |= cam_man_ptr_->Start(); //wait till we get the first frame. @@ -174,67 +183,68 @@ bool Snapdragon::VislamManager::HasUpdatedPointCloud() { return (mv_ret != 0 ); } -int32_t Snapdragon::VislamManager::Imu_IEventListener_ProcessSamples( sensor_imu* imu_samples, uint32_t sample_count ) { - int32_t rc = 0; - for (int ii = 0; ii < sample_count; ++ii) - { - int64_t current_timestamp_ns = (int64_t)imu_samples[ii].timestamp_in_us * 1000; - - float delta = 0.f; - static int64_t last_timestamp = 0; - if (last_timestamp != 0) - { - delta = (current_timestamp_ns - last_timestamp)*1e-6; - const float imu_sample_dt_reasonable_threshold_ms = 2.5; - if (delta > imu_sample_dt_reasonable_threshold_ms) - { - if (cam_params_.verbose) - { - WARN_PRINT("IMU sample dt > %f ms -- %f ms", - imu_sample_dt_reasonable_threshold_ms, - delta); - } - } - } - last_timestamp = current_timestamp_ns; - - float lin_acc[3], ang_vel[3]; - const float kNormG = 9.80665f; - lin_acc[0] = imu_samples[ii].linear_acceleration[0] * kNormG; - lin_acc[1] = imu_samples[ii].linear_acceleration[1] * kNormG; - lin_acc[2] = imu_samples[ii].linear_acceleration[2] * kNormG; - ang_vel[0] = imu_samples[ii].angular_velocity[0]; - ang_vel[1] = imu_samples[ii].angular_velocity[1]; - ang_vel[2] = imu_samples[ii].angular_velocity[2]; - - static uint32_t sequence_number_last = 0; - int num_dropped_samples = 0; - if (sequence_number_last != 0) - { - // The diff should be 1, anything greater means we dropped samples - num_dropped_samples = imu_samples[ii].sequence_number - - sequence_number_last - 1; - if (num_dropped_samples > 0) - { - if (cam_params_.verbose) - { - WARN_PRINT("Current IMU sample = %u, last IMU sample = %u", - imu_samples[ii].sequence_number, sequence_number_last); - } - } - } - sequence_number_last = imu_samples[ii].sequence_number; - - { - std::lock_guard lock( sync_mutex_ ); - mvVISLAM_AddAccel(vislam_ptr_, current_timestamp_ns, - lin_acc[0], lin_acc[1], lin_acc[2]); - mvVISLAM_AddGyro(vislam_ptr_, current_timestamp_ns, - ang_vel[0], ang_vel[1], ang_vel[2]); - } - } - return rc; -} +// TODO: Use mavros instead! +// int32_t Snapdragon::VislamManager::Imu_IEventListener_ProcessSamples( sensor_imu* imu_samples, uint32_t sample_count ) { +// int32_t rc = 0; +// for (int ii = 0; ii < sample_count; ++ii) +// { +// int64_t current_timestamp_ns = (int64_t)imu_samples[ii].timestamp_in_us * 1000; +// +// float delta = 0.f; +// static int64_t last_timestamp = 0; +// if (last_timestamp != 0) +// { +// delta = (current_timestamp_ns - last_timestamp)*1e-6; +// const float imu_sample_dt_reasonable_threshold_ms = 2.5; +// if (delta > imu_sample_dt_reasonable_threshold_ms) +// { +// if (cam_params_.verbose) +// { +// WARN_PRINT("IMU sample dt > %f ms -- %f ms", +// imu_sample_dt_reasonable_threshold_ms, +// delta); +// } +// } +// } +// last_timestamp = current_timestamp_ns; +// +// float lin_acc[3], ang_vel[3]; +// const float kNormG = 9.80665f; +// lin_acc[0] = imu_samples[ii].linear_acceleration[0] * kNormG; +// lin_acc[1] = imu_samples[ii].linear_acceleration[1] * kNormG; +// lin_acc[2] = imu_samples[ii].linear_acceleration[2] * kNormG; +// ang_vel[0] = imu_samples[ii].angular_velocity[0]; +// ang_vel[1] = imu_samples[ii].angular_velocity[1]; +// ang_vel[2] = imu_samples[ii].angular_velocity[2]; +// +// static uint32_t sequence_number_last = 0; +// int num_dropped_samples = 0; +// if (sequence_number_last != 0) +// { +// // The diff should be 1, anything greater means we dropped samples +// num_dropped_samples = imu_samples[ii].sequence_number +// - sequence_number_last - 1; +// if (num_dropped_samples > 0) +// { +// if (cam_params_.verbose) +// { +// WARN_PRINT("Current IMU sample = %u, last IMU sample = %u", +// imu_samples[ii].sequence_number, sequence_number_last); +// } +// } +// } +// sequence_number_last = imu_samples[ii].sequence_number; +// +// { +// std::lock_guard lock( sync_mutex_ ); +// mvVISLAM_AddAccel(vislam_ptr_, current_timestamp_ns, +// lin_acc[0], lin_acc[1], lin_acc[2]); +// mvVISLAM_AddGyro(vislam_ptr_, current_timestamp_ns, +// ang_vel[0], ang_vel[1], ang_vel[2]); +// } +// } +// return rc; +// } int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_frame_id, uint64_t& timestamp_ns ) { int32_t rc = 0; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index 4bc9765..b835c7d 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -31,11 +31,12 @@ ****************************************************************************/ #pragma once #include +#include +#include +#include "sensor_msgs/Imu.h" #include "SnapdragonCameraTypes.hpp" #include "mvVISLAM.h" #include "SnapdragonCameraManager.hpp" -#include "SnapdragonImuManager.hpp" -#include namespace Snapdragon { class VislamManager; @@ -44,12 +45,12 @@ namespace Snapdragon { /** * Class to wrap the mvVISLAM SDK with Camera and IMU Samples. */ -class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { +class Snapdragon::VislamManager{ public: /** * This structure defines all the parameters needed to initialize - * the mvVISLAM_Initialize() method. Refer the API definition in + * the mvVISLAM_Initialize() method. Refer the API definition in * mvVISLAM.h file to get the description for each field. **/ typedef struct { @@ -82,19 +83,19 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { * Initalizes the VISLAM Manager with Camera and VISLAM Parameters * @param params * The structure that holds the VISLAM parameters. - * @return + * @return * 0 = success * otherwise = failure. **/ int32_t Initialize - ( - const Snapdragon::CameraParameters& cam_params, + ( + const Snapdragon::CameraParameters& cam_params, const Snapdragon::VislamManager::InitParams& params ); /** * Start the Camera and Imu modules for the VISLAM functionality. - * @return + * @return * 0 = success * otherwise = failure; **/ @@ -102,7 +103,7 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { /** * Stops the VISLAM engine by stoping the Camera and IMU modules. - * @return + * @return * 0 = success; * otherwise = failure. **/ @@ -131,7 +132,7 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { **/ int32_t GetPose( mvVISLAMPose& pose, int64_t& frame_id, uint64_t& timestamp_ns ); - /** + /** * MV SDK's wrapper to get the PointCloud data. * @param points * The Point Cloud points @@ -160,7 +161,9 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { * 0 = success; * otherwise = false; **/ - int32_t Imu_IEventListener_ProcessSamples( sensor_imu* samples, uint32_t count ); + // int32_t Imu_IEventListener_ProcessSamples( sensor_imu* samples, uint32_t count ); + + void callback_imu(const sensor_msgs::Imu::ConstPtr& msg); /** * Destructor @@ -168,16 +171,18 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { virtual ~VislamManager(); private: - // utility methods + // utility methods int32_t CleanUp(); std::atomic initialized_; Snapdragon::CameraParameters cam_params_; Snapdragon::VislamManager::InitParams vislam_params_; bool verbose_; Snapdragon::CameraManager* cam_man_ptr_; - Snapdragon::ImuManager* imu_man_ptr_; + // Snapdragon::ImuManager* imu_man_ptr_; mvVISLAM* vislam_ptr_; std::mutex sync_mutex_; uint8_t* image_buffer_; size_t image_buffer_size_bytes_; + + // ros::Subscriber imu_sub_; }; From b7701a22a52d579b8587931006c5c01bbf57245a Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 22 May 2017 09:29:23 +0200 Subject: [PATCH 07/62] Temporarily removed tests from Cmake and fixed a missing include --- CMakeLists.txt | 17 +++++++++-------- src/nodes/SnapdragonRosNodeVislam.cpp | 3 +-- src/vislam/SnapdragonVislamManager.cpp | 4 ++-- src/vislam/SnapdragonVislamManager.hpp | 2 +- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bd35e62..a12b9ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -149,14 +149,15 @@ target_include_directories( snap_vislam PUBLIC add_library(mv1 SHARED IMPORTED) set_property(TARGET mv1 PROPERTY IMPORTED_LOCATION $ENV{MV_SDK}/usr/lib/libmv1.so) -add_executable( test_snap_cam_manager test/test_SnapdragonCameraManager.cpp ) -target_link_libraries( test_snap_cam_manager snap_vislam sensor_imu mv1 pthread camera ) - -add_executable( test_imu_manager test/test_SnapdragonImuManager.cpp ) -target_link_libraries( test_imu_manager snap_vislam sensor_imu mv1 pthread camera ) - -add_executable( test_vislam test/test_SnapdragonVislamManager.cpp ) -target_link_libraries( test_vislam snap_vislam sensor_imu mv1 pthread camera ) +# Tests +# add_executable( test_snap_cam_manager test/test_SnapdragonCameraManager.cpp ) +# target_link_libraries( test_snap_cam_manager snap_vislam sensor_imu mv1 pthread camera ) +# +# add_executable( test_imu_manager test/test_SnapdragonImuManager.cpp ) +# target_link_libraries( test_imu_manager snap_vislam sensor_imu mv1 pthread camera ) +# +# add_executable( test_vislam test/test_SnapdragonVislamManager.cpp ) +# target_link_libraries( test_vislam snap_vislam sensor_imu mv1 pthread camera ) ## Declare a C++ executable add_executable(snap_vislam_node src/nodes/SnapdragonRosNodeVislam.cpp src/nodes/SnapdragonRosNodeVislam_main.cpp ) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 83f6749..f5c57cd 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -161,7 +161,7 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { cpaConfig.legacyCost.gainCost = 0.3333f; param.mv_cpa_config = cpaConfig; - Snapdragon::VislamManager vislam_man; + Snapdragon::VislamManager vislam_man(nh_); if( vislam_man.Initialize( param, vislamParams ) != 0 ) { ROS_WARN_STREAM( "Snapdragon::RosNodeVislam::VislamThreadMain: Error initializing the VISLAM Manager " ); thread_started_ = false; @@ -273,4 +273,3 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose static tf2_ros::TransformBroadcaster br; br.sendTransform(transforms); } - diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index ca449c5..8188e4e 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -32,7 +32,7 @@ #include "SnapdragonVislamManager.hpp" #include "SnapdragonDebugPrint.h" -Snapdragon::VislamManager::VislamManager() { +Snapdragon::VislamManager::VislamManager(ros::NodeHandle nh) { cam_man_ptr_ = nullptr; // imu_man_ptr_ = nullptr; vislam_ptr_ = nullptr; @@ -41,7 +41,7 @@ Snapdragon::VislamManager::VislamManager() { image_buffer_ = nullptr; // Setup publishers/subscribers - // imu_sub_ = nh_.subscribe ("mavros/imu/data_raw", 10, callback_imu); + // imu_sub_ = nh.subscribe ("mavros/imu/data_raw", 10, callback_imu); } Snapdragon::VislamManager::~VislamManager() { diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index b835c7d..d6fd1f6 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -77,7 +77,7 @@ class Snapdragon::VislamManager{ /** * Constructor **/ - VislamManager(); + VislamManager(ros::NodeHandle nh); /** * Initalizes the VISLAM Manager with Camera and VISLAM Parameters From a660b07599947bbd6df1a9ba7cbbae76a1b97731 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 22 May 2017 10:00:52 +0200 Subject: [PATCH 08/62] Now using imu ros messages for Vislam - compiling, but untested --- src/vislam/SnapdragonVislamManager.cpp | 126 ++++++++++++------------- src/vislam/SnapdragonVislamManager.hpp | 18 +++- 2 files changed, 77 insertions(+), 67 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 8188e4e..ad8b027 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -41,17 +41,74 @@ Snapdragon::VislamManager::VislamManager(ros::NodeHandle nh) { image_buffer_ = nullptr; // Setup publishers/subscribers - // imu_sub_ = nh.subscribe ("mavros/imu/data_raw", 10, callback_imu); + imu_sub_ = nh.subscribe("mavros/imu/data_raw", 10, + &Snapdragon::VislamManager::ImuCallback, this); } Snapdragon::VislamManager::~VislamManager() { CleanUp(); } -void Snapdragon::VislamManager::callback_imu( +void Snapdragon::VislamManager::ImuCallback( const sensor_msgs::Imu::ConstPtr& msg) { - // TODO: Process IMU message + int64_t current_timestamp_ns = + msg->header.stamp.sec * 1e9 + msg->header.stamp.nsec; + + static int64_t last_timestamp = 0; + float delta = 0.f; + + // Sanity check on IMU timestamp + if (last_timestamp != 0) + { + delta = (current_timestamp_ns - last_timestamp) * 1e-6; + const float imu_sample_dt_reasonable_threshold_ms = 2.5; + if (delta > imu_sample_dt_reasonable_threshold_ms) + { + if (cam_params_.verbose) + { + WARN_PRINT("IMU sample dt > %f ms -- %f ms", + imu_sample_dt_reasonable_threshold_ms, delta); + } + } + } + last_timestamp = current_timestamp_ns; + + // Parse IMU message + float lin_acc[3], ang_vel[3]; + const float kNormGZurich = 9.807f; + lin_acc[0] = msg->linear_acceleration.x * kNormGZurich; + lin_acc[1] = msg->linear_acceleration.y * kNormGZurich; + lin_acc[2] = msg->linear_acceleration.z * kNormGZurich; + ang_vel[0] = msg->angular_velocity.x; + ang_vel[1] = msg->angular_velocity.y; + ang_vel[2] = msg->angular_velocity.z; + + // Check for dropped IMU messages + static uint32_t sequence_number_last = 0; + int num_dropped_samples = 0; + if (sequence_number_last != 0) + { + // The diff should be 1, anything greater means we dropped samples + num_dropped_samples = msg->header.seq - sequence_number_last - 1; + if (num_dropped_samples > 0) + { + if (cam_params_.verbose) + { + WARN_PRINT("Current IMU sample = %u, last IMU sample = %u", + msg->header.seq, sequence_number_last); + } + } + } + // sequence_number_last = imu_samples[ii].sequence_number; + sequence_number_last = msg->header.seq; + + // Feed IMU message to VISLAM + std::lock_guard lock(sync_mutex_); + mvVISLAM_AddAccel(vislam_ptr_, current_timestamp_ns, lin_acc[0], lin_acc[1], + lin_acc[2]); + mvVISLAM_AddGyro(vislam_ptr_, current_timestamp_ns, ang_vel[0], ang_vel[1], + ang_vel[2]); } int32_t Snapdragon::VislamManager::CleanUp() { @@ -183,69 +240,6 @@ bool Snapdragon::VislamManager::HasUpdatedPointCloud() { return (mv_ret != 0 ); } -// TODO: Use mavros instead! -// int32_t Snapdragon::VislamManager::Imu_IEventListener_ProcessSamples( sensor_imu* imu_samples, uint32_t sample_count ) { -// int32_t rc = 0; -// for (int ii = 0; ii < sample_count; ++ii) -// { -// int64_t current_timestamp_ns = (int64_t)imu_samples[ii].timestamp_in_us * 1000; -// -// float delta = 0.f; -// static int64_t last_timestamp = 0; -// if (last_timestamp != 0) -// { -// delta = (current_timestamp_ns - last_timestamp)*1e-6; -// const float imu_sample_dt_reasonable_threshold_ms = 2.5; -// if (delta > imu_sample_dt_reasonable_threshold_ms) -// { -// if (cam_params_.verbose) -// { -// WARN_PRINT("IMU sample dt > %f ms -- %f ms", -// imu_sample_dt_reasonable_threshold_ms, -// delta); -// } -// } -// } -// last_timestamp = current_timestamp_ns; -// -// float lin_acc[3], ang_vel[3]; -// const float kNormG = 9.80665f; -// lin_acc[0] = imu_samples[ii].linear_acceleration[0] * kNormG; -// lin_acc[1] = imu_samples[ii].linear_acceleration[1] * kNormG; -// lin_acc[2] = imu_samples[ii].linear_acceleration[2] * kNormG; -// ang_vel[0] = imu_samples[ii].angular_velocity[0]; -// ang_vel[1] = imu_samples[ii].angular_velocity[1]; -// ang_vel[2] = imu_samples[ii].angular_velocity[2]; -// -// static uint32_t sequence_number_last = 0; -// int num_dropped_samples = 0; -// if (sequence_number_last != 0) -// { -// // The diff should be 1, anything greater means we dropped samples -// num_dropped_samples = imu_samples[ii].sequence_number -// - sequence_number_last - 1; -// if (num_dropped_samples > 0) -// { -// if (cam_params_.verbose) -// { -// WARN_PRINT("Current IMU sample = %u, last IMU sample = %u", -// imu_samples[ii].sequence_number, sequence_number_last); -// } -// } -// } -// sequence_number_last = imu_samples[ii].sequence_number; -// -// { -// std::lock_guard lock( sync_mutex_ ); -// mvVISLAM_AddAccel(vislam_ptr_, current_timestamp_ns, -// lin_acc[0], lin_acc[1], lin_acc[2]); -// mvVISLAM_AddGyro(vislam_ptr_, current_timestamp_ns, -// ang_vel[0], ang_vel[1], ang_vel[2]); -// } -// } -// return rc; -// } - int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_frame_id, uint64_t& timestamp_ns ) { int32_t rc = 0; if( !initialized_ ) { diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index d6fd1f6..241258e 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -33,7 +33,9 @@ #include #include #include -#include "sensor_msgs/Imu.h" +#include +#include +#include #include "SnapdragonCameraTypes.hpp" #include "mvVISLAM.h" #include "SnapdragonCameraManager.hpp" @@ -162,8 +164,12 @@ class Snapdragon::VislamManager{ * otherwise = false; **/ // int32_t Imu_IEventListener_ProcessSamples( sensor_imu* samples, uint32_t count ); +<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a void callback_imu(const sensor_msgs::Imu::ConstPtr& msg); +======= + +>>>>>>> Now using imu ros messages for Vislam - compiling, but untested /** * Destructor @@ -171,7 +177,12 @@ class Snapdragon::VislamManager{ virtual ~VislamManager(); private: +<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a + // utility methods +======= // utility methods + void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg); +>>>>>>> Now using imu ros messages for Vislam - compiling, but untested int32_t CleanUp(); std::atomic initialized_; Snapdragon::CameraParameters cam_params_; @@ -183,6 +194,11 @@ class Snapdragon::VislamManager{ std::mutex sync_mutex_; uint8_t* image_buffer_; size_t image_buffer_size_bytes_; +<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a // ros::Subscriber imu_sub_; +======= + + ros::Subscriber imu_sub_; +>>>>>>> Now using imu ros messages for Vislam - compiling, but untested }; From 4eb65547b005960226bfab978b4a57298295dba3 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 22 May 2017 11:11:44 +0200 Subject: [PATCH 09/62] Added instructions for mavros --- README_MAVROS.md | 24 ++++++++++++++++++++++++ package.xml | 1 + 2 files changed, 25 insertions(+) create mode 100644 README_MAVROS.md diff --git a/README_MAVROS.md b/README_MAVROS.md new file mode 100644 index 0000000..e5de8ad --- /dev/null +++ b/README_MAVROS.md @@ -0,0 +1,24 @@ +# Snapdragon Flight VISLAM-ROS Sample Code +This repository is a copy of the VISLAM from ATLFlight/ros-examples, modified to work with the px4 flight stack. + +## Preparations +Get the mavros sources into your catkin's source directory by following the instructions from the [official px dev page](https://dev.px4.io/en/ros/mavros_installation.html). + +## Build +`catkin build` + +This can easily take 30 minutes on the snapdragon. + + +## Run +Get a roscore running +`roscore` + +Start the px4 flight stack +``` +cd $HOME +./px4 mainapp.config +``` + +Launch rpg_qualcomm_vislam +`roslaunch rpg_qualcomm_vislam mavros_vislam.launch` diff --git a/package.xml b/package.xml index e39a815..7010d3c 100644 --- a/package.xml +++ b/package.xml @@ -50,6 +50,7 @@ roscpp rospy std_msgs + mavros From d4ff9abb668ec27da53ae7bcde015f60f0f721cc Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 22 May 2017 11:12:00 +0200 Subject: [PATCH 10/62] Added launch file for mavros + vislam --- launch/mavros_vislam.launch | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 launch/mavros_vislam.launch diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch new file mode 100644 index 0000000..3f778dd --- /dev/null +++ b/launch/mavros_vislam.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + From 627a4a9024f0e1a75789e5922c8a6a63371877cd Mon Sep 17 00:00:00 2001 From: Potaito Date: Tue, 23 May 2017 08:55:25 +0200 Subject: [PATCH 11/62] Improved (un-)subscribing to mavros imu topic --- src/vislam/SnapdragonVislamManager.cpp | 49 ++++++++++---------------- src/vislam/SnapdragonVislamManager.hpp | 33 +++-------------- 2 files changed, 23 insertions(+), 59 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index ad8b027..e140872 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -32,17 +32,13 @@ #include "SnapdragonVislamManager.hpp" #include "SnapdragonDebugPrint.h" -Snapdragon::VislamManager::VislamManager(ros::NodeHandle nh) { +Snapdragon::VislamManager::VislamManager(ros::NodeHandle nh) : nh_(nh) { cam_man_ptr_ = nullptr; // imu_man_ptr_ = nullptr; vislam_ptr_ = nullptr; initialized_ = false; image_buffer_size_bytes_ = 0; image_buffer_ = nullptr; - - // Setup publishers/subscribers - imu_sub_ = nh.subscribe("mavros/imu/data_raw", 10, - &Snapdragon::VislamManager::ImuCallback, this); } Snapdragon::VislamManager::~VislamManager() { @@ -52,8 +48,7 @@ Snapdragon::VislamManager::~VislamManager() { void Snapdragon::VislamManager::ImuCallback( const sensor_msgs::Imu::ConstPtr& msg) { - int64_t current_timestamp_ns = - msg->header.stamp.sec * 1e9 + msg->header.stamp.nsec; + int64_t current_timestamp_ns = msg->header.stamp.toNSec(); static int64_t last_timestamp = 0; float delta = 0.f; @@ -73,13 +68,18 @@ void Snapdragon::VislamManager::ImuCallback( } } last_timestamp = current_timestamp_ns; + + // Let's print every 10th time stamp + // if(msg->header.seq%10==0) + // { + ROS_INFO_STREAM("IMU time: " << last_timestamp); + // } // Parse IMU message float lin_acc[3], ang_vel[3]; - const float kNormGZurich = 9.807f; - lin_acc[0] = msg->linear_acceleration.x * kNormGZurich; - lin_acc[1] = msg->linear_acceleration.y * kNormGZurich; - lin_acc[2] = msg->linear_acceleration.z * kNormGZurich; + lin_acc[0] = msg->linear_acceleration.x; + lin_acc[1] = msg->linear_acceleration.y; + lin_acc[2] = msg->linear_acceleration.z; ang_vel[0] = msg->angular_velocity.x; ang_vel[1] = msg->angular_velocity.y; ang_vel[2] = msg->angular_velocity.z; @@ -112,14 +112,6 @@ void Snapdragon::VislamManager::ImuCallback( } int32_t Snapdragon::VislamManager::CleanUp() { - // stop the imu api first. - // if( imu_man_ptr_ != nullptr ) { - // imu_man_ptr_->RemoveHandler( this ); - // imu_man_ptr_->Terminate(); - // delete imu_man_ptr_; - // imu_man_ptr_ = nullptr; - // } - //stop the camera. if( cam_man_ptr_ != nullptr ) { WARN_PRINT( "Stopping Camera...." ); @@ -161,17 +153,6 @@ int32_t Snapdragon::VislamManager::Initialize } } - // if( rc == 0 ) { //initialize the Imu Manager. - // imu_man_ptr_ = new Snapdragon::ImuManager(); - // if( imu_man_ptr_ != nullptr ) { - // rc = imu_man_ptr_->Initialize(); - // imu_man_ptr_->AddHandler( this ); - // } - // else { - // rc = -1; - // } - // } - //now intialize the VISLAM module. if( rc == 0 ) { vislam_ptr_ = mvVISLAM_Initialize @@ -216,6 +197,10 @@ int32_t Snapdragon::VislamManager::Start() { image_buffer_size_bytes_ = cam_man_ptr_->GetImageSize(); INFO_PRINT( "image Size: %d frameId: %lld", cam_man_ptr_->GetImageSize(), cam_man_ptr_->GetLatestFrameId() ); image_buffer_ = new uint8_t[ image_buffer_size_bytes_ ]; + + // Setup publishers/subscribers + imu_sub_ = nh_.subscribe("mavros/imu/data_raw", 10, + &Snapdragon::VislamManager::ImuCallback, this); } else { ERROR_PRINT( "Calling Start without calling intialize" ); @@ -226,6 +211,9 @@ int32_t Snapdragon::VislamManager::Start() { int32_t Snapdragon::VislamManager::Stop() { CleanUp(); + + // Unsubscribe from IMU topic + imu_sub_.shutdown(); return 0; } @@ -274,6 +262,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); + ROS_INFO_STREAM("Image time: " << timestamp_ns); } } return rc; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index 241258e..24fba9f 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -150,26 +150,8 @@ class Snapdragon::VislamManager{ * @return int32_t * 0 = success * otherwise = false; + int32_t Reset(); **/ - int32_t Reset(); - - /** - * The IMU callback handler to add the accel/gyro data into VISLAM. - * @param samples - * The imu Samples to be added to VISLAM. - * @param count - * The number of samples in the buffer. - * @return int32_t - * 0 = success; - * otherwise = false; - **/ - // int32_t Imu_IEventListener_ProcessSamples( sensor_imu* samples, uint32_t count ); -<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a - - void callback_imu(const sensor_msgs::Imu::ConstPtr& msg); -======= - ->>>>>>> Now using imu ros messages for Vislam - compiling, but untested /** * Destructor @@ -177,12 +159,9 @@ class Snapdragon::VislamManager{ virtual ~VislamManager(); private: -<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a - // utility methods -======= - // utility methods void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg); ->>>>>>> Now using imu ros messages for Vislam - compiling, but untested + + // utility methods int32_t CleanUp(); std::atomic initialized_; Snapdragon::CameraParameters cam_params_; @@ -194,11 +173,7 @@ class Snapdragon::VislamManager{ std::mutex sync_mutex_; uint8_t* image_buffer_; size_t image_buffer_size_bytes_; -<<<<<<< 3c130b7951dd44edaa7d33b99ccf85fc73d1133a - - // ros::Subscriber imu_sub_; -======= + ros::NodeHandle nh_; ros::Subscriber imu_sub_; ->>>>>>> Now using imu ros messages for Vislam - compiling, but untested }; From eba7ebb1baf4d8731b9e79598d00a5e279ecd45a Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 24 May 2017 08:10:43 +0200 Subject: [PATCH 12/62] Output formatting --- src/vislam/SnapdragonVislamManager.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index e140872..a96108a 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -68,12 +68,8 @@ void Snapdragon::VislamManager::ImuCallback( } } last_timestamp = current_timestamp_ns; - - // Let's print every 10th time stamp - // if(msg->header.seq%10==0) - // { - ROS_INFO_STREAM("IMU time: " << last_timestamp); - // } + + ROS_INFO_STREAM("IMU time: \t" << last_timestamp); // Parse IMU message float lin_acc[3], ang_vel[3]; @@ -262,7 +258,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - ROS_INFO_STREAM("Image time: " << timestamp_ns); + ROS_INFO_STREAM("Image time: \t" << timestamp_ns); } } return rc; From dcbef95cbfaf726d451cd3cc4f9690af81584d24 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 24 May 2017 09:06:12 +0200 Subject: [PATCH 13/62] VISLAM working - Converting IMU messages from ENU to NED frame prior to feeding to VISLAM --- src/nodes/SnapdragonRosNodeVislam.cpp | 13 +++++++++++++ src/vislam/SnapdragonVislamManager.cpp | 21 +++++++++++++-------- src/vislam/SnapdragonVislamManager.hpp | 2 ++ 3 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index f5c57cd..8b42aba 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -189,6 +189,19 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { // Publish Pose Data PublishVislamData( vislamPose, vislamFrameId, timestamp_ns ); } + else + { + switch (vislamPose.poseQuality) + { + case MV_TRACKING_STATE_FAILED: + ROS_INFO_THROTTLE(1, "VISLAM TRACKING FAILED"); + break; + + case MV_TRACKING_STATE_INITIALIZING: + ROS_INFO_THROTTLE(1, "VISLAM INITIALIZING"); + break; + } + } } else { ROS_WARN_STREAM( "Snapdragon::RosNodeVislam::VislamThreadMain: Warning Getting Pose Information" ); diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index a96108a..8aae4a0 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -48,6 +48,9 @@ Snapdragon::VislamManager::~VislamManager() { void Snapdragon::VislamManager::ImuCallback( const sensor_msgs::Imu::ConstPtr& msg) { + // Convert from ENU (mavros) to NED (vislam expectation) frame + sensor_msgs::Imu msg_ned = *msg; + int64_t current_timestamp_ns = msg->header.stamp.toNSec(); static int64_t last_timestamp = 0; @@ -69,16 +72,18 @@ void Snapdragon::VislamManager::ImuCallback( } last_timestamp = current_timestamp_ns; - ROS_INFO_STREAM("IMU time: \t" << last_timestamp); + // ROS_INFO_STREAM("IMU time: \t" << last_timestamp); // Parse IMU message float lin_acc[3], ang_vel[3]; - lin_acc[0] = msg->linear_acceleration.x; - lin_acc[1] = msg->linear_acceleration.y; - lin_acc[2] = msg->linear_acceleration.z; - ang_vel[0] = msg->angular_velocity.x; - ang_vel[1] = msg->angular_velocity.y; - ang_vel[2] = msg->angular_velocity.z; + + // Convert ENU to NED coordinates + lin_acc[0] = msg->linear_acceleration.y; + lin_acc[1] = msg->linear_acceleration.x; + lin_acc[2] = -msg->linear_acceleration.z; + ang_vel[0] = msg->angular_velocity.y; + ang_vel[1] = msg->angular_velocity.x; + ang_vel[2] = -msg->angular_velocity.z; // Check for dropped IMU messages static uint32_t sequence_number_last = 0; @@ -258,7 +263,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - ROS_INFO_STREAM("Image time: \t" << timestamp_ns); + // ROS_INFO_STREAM("Image time: \t" << timestamp_ns); } } return rc; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index 24fba9f..c8631c0 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -152,6 +152,7 @@ class Snapdragon::VislamManager{ * otherwise = false; int32_t Reset(); **/ + int32_t Reset(); /** * Destructor @@ -163,6 +164,7 @@ class Snapdragon::VislamManager{ // utility methods int32_t CleanUp(); + std::atomic initialized_; Snapdragon::CameraParameters cam_params_; Snapdragon::VislamManager::InitParams vislam_params_; From 2dfc921332eadead1c6cfb581bb7b94da425d528 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 24 May 2017 09:27:55 +0200 Subject: [PATCH 14/62] Added logging for changes in mv tracking state --- src/nodes/SnapdragonRosNodeVislam.cpp | 13 ++++++++++++- src/nodes/SnapdragonRosNodeVislam.hpp | 1 + src/vislam/SnapdragonVislamManager.hpp | 1 - 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 8b42aba..2db73b4 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -189,7 +189,9 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { // Publish Pose Data PublishVislamData( vislamPose, vislamFrameId, timestamp_ns ); } - else + + // Log changes in tracking state + if (previous_mv_tracking_state_ != vislamPose.poseQuality) { switch (vislamPose.poseQuality) { @@ -200,8 +202,17 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { case MV_TRACKING_STATE_INITIALIZING: ROS_INFO_THROTTLE(1, "VISLAM INITIALIZING"); break; + + case MV_TRACKING_STATE_HIGH_QUALITY: + ROS_INFO_THROTTLE(1, "VISLAM TRACKING HIGH QUALITY"); + break; + + case MV_TRACKING_STATE_LOW_QUALITY: + ROS_INFO_THROTTLE(1, "VISLAM TRACKING LOW QUALITY"); + break; } } + previous_mv_tracking_state_ = vislamPose.poseQuality; } else { ROS_WARN_STREAM( "Snapdragon::RosNodeVislam::VislamThreadMain: Warning Getting Pose Information" ); diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index 88f3523..c4d0108 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -103,4 +103,5 @@ class Snapdragon::RosNode::Vislam ros::NodeHandle nh_; ros::Publisher pub_vislam_pose_; ros::Publisher pub_vislam_odometry_; + int previous_mv_tracking_state_ = MV_TRACKING_STATE_FAILED; }; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index c8631c0..bd37804 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -170,7 +170,6 @@ class Snapdragon::VislamManager{ Snapdragon::VislamManager::InitParams vislam_params_; bool verbose_; Snapdragon::CameraManager* cam_man_ptr_; - // Snapdragon::ImuManager* imu_man_ptr_; mvVISLAM* vislam_ptr_; std::mutex sync_mutex_; uint8_t* image_buffer_; From 21b82fbb82da68313a61ae290fb5254aa0e43862 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 24 May 2017 11:42:13 +0200 Subject: [PATCH 15/62] Changed mavlink port --- launch/mavros_vislam.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 3f778dd..14b4eee 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -1,7 +1,7 @@ - + From 0fd9b6f0d295df1a11793bc6a7fb1bc959944c79 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 29 May 2017 10:35:57 +0200 Subject: [PATCH 16/62] Fixed IMU coordinate frame transformation for vislam --- src/vislam/SnapdragonVislamManager.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 8aae4a0..2b92ef6 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -34,7 +34,6 @@ Snapdragon::VislamManager::VislamManager(ros::NodeHandle nh) : nh_(nh) { cam_man_ptr_ = nullptr; - // imu_man_ptr_ = nullptr; vislam_ptr_ = nullptr; initialized_ = false; image_buffer_size_bytes_ = 0; @@ -78,11 +77,11 @@ void Snapdragon::VislamManager::ImuCallback( float lin_acc[3], ang_vel[3]; // Convert ENU to NED coordinates - lin_acc[0] = msg->linear_acceleration.y; - lin_acc[1] = msg->linear_acceleration.x; + lin_acc[0] = msg->linear_acceleration.x; + lin_acc[1] = -msg->linear_acceleration.y; lin_acc[2] = -msg->linear_acceleration.z; - ang_vel[0] = msg->angular_velocity.y; - ang_vel[1] = msg->angular_velocity.x; + ang_vel[0] = msg->angular_velocity.x; + ang_vel[1] = -msg->angular_velocity.y; ang_vel[2] = -msg->angular_velocity.z; // Check for dropped IMU messages From 74d5ac86f24b008c0856c9528ba601785e57adb9 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 29 May 2017 10:36:21 +0200 Subject: [PATCH 17/62] Renamed project to snapdragon_mavros_vislam --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a12b9ac..f7d0f25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(rpg_qualcomm_vislam) +project(snapdragon_mavros_vislam) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/package.xml b/package.xml index 7010d3c..1757d87 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - rpg_qualcomm_vislam + snapdragon_mavros_vislam 0.1.1 Private fork of the Qualcomm Vislam example code from ATLFlight From a6cd961fac0bdaed6d4a80c56bac70147684a0b6 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 29 May 2017 10:36:37 +0200 Subject: [PATCH 18/62] Fixed launch file --- launch/mavros_vislam.launch | 10 +- launch/snapdragon_px4_config.yaml | 181 ++++++++++++++++++++++++++++++ 2 files changed, 188 insertions(+), 3 deletions(-) create mode 100644 launch/snapdragon_px4_config.yaml diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 14b4eee..630e058 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -9,8 +9,8 @@ - - + + @@ -18,7 +18,11 @@ - + + + + + diff --git a/launch/snapdragon_px4_config.yaml b/launch/snapdragon_px4_config.yaml new file mode 100644 index 0000000..5362b58 --- /dev/null +++ b/launch/snapdragon_px4_config.yaml @@ -0,0 +1,181 @@ +# Common configuration for PX4 autopilot +# +# node: +startup_px4_usb_quirk: true + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) + +# sys_status +sys: + min_voltage: 10.0 # diagnostics min voltage + disable_diag: false # disable all sys_status diagnostics, except heartbeat + +# sys_time +time: + time_ref_source: "fcu" # time_reference source + timesync_mode: PASSTHROUGH + timesync_avg_alpha: 0.6 # timesync averaging factor + +# --- mavros plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + low_rssi: 40 # raw rssi lower level for diagnostics + +# actuator_control +# None + +# command +cmd: + use_comp_id_system_control: false # quirk for some old FCUs + +# dummy +# None + +# ftp +# None + +# global_position +global_position: + frame_id: "fcu" # pose and fix frame_id + rot_covariance: 99999.0 # covariance for attitude? + tf: + send: false # send TF? + frame_id: "local_origin" # TF frame_id + child_frame_id: "fcu_utm" # TF child_frame_id + +# imu_pub +imu: + frame_id: "fcu" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: !degrees 0.02 + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +# local_position +local_position: + frame_id: "fcu" + tf: + send: false + frame_id: "local_origin" + child_frame_id: "fcu" + send_fcu: false + +# param +# None, used for FCU params + +# rc_io +# None + +# safety_area +safety_area: + p1: {x: 1.0, y: 1.0, z: 1.0} + p2: {x: -1.0, y: -1.0, z: -1.0} + +# setpoint_accel +setpoint_accel: + send_force: false + +# setpoint_attitude +setpoint_attitude: + reverse_throttle: false # allow reversed throttle + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "attitude" + rate_limit: 10.0 + +# setpoint_position +setpoint_position: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "setpoint" + rate_limit: 50.0 + +# setpoint_velocity +# None + +# vfr_hud +# None + +# waypoint +mission: + pull_after_gcs: true # update mission if gcs updates + +# --- mavros extras plugins (same order) --- + +# distance_sensor (PX4 only) +distance_sensor: + hrlv_ez4_pub: + id: 0 + frame_id: "hrlv_ez4_sonar" + orientation: ROLL_180 # RPY:{180.0, 0.0, 0.0} + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + lidarlite_pub: + id: 1 + frame_id: "lidarlite_laser" + orientation: ROLL_180 + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + sonar_1_sub: + subscriber: true + id: 2 + orientation: ROLL_180 + laser_1_sub: + subscriber: true + id: 3 + orientation: ROLL_180 + +## Currently available orientations: +# Check http://wiki.ros.org/mavros/Enumerations +## + +# image_pub +image: + frame_id: "px4flow" + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# px4flow +px4flow: + frame_id: "px4flow" + ranger_fov: !degrees 0.0 # XXX TODO + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters + +# vision_pose_estimate +vision_pose: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "vision" + rate_limit: 10.0 + +# vision_speed_estimate +vision_speed: + listen_twist: false + +# vibration +vibration: + frame_id: "vibration" + +# adsb +# None + +# vim:set ts=2 sw=2 et: From 94c8f8e748fb179422010344171b25764f1e8e32 Mon Sep 17 00:00:00 2001 From: Potaito Date: Mon, 29 May 2017 10:48:15 +0200 Subject: [PATCH 19/62] Readme update --- README_MAVROS.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README_MAVROS.md b/README_MAVROS.md index e5de8ad..bc0edf0 100644 --- a/README_MAVROS.md +++ b/README_MAVROS.md @@ -2,7 +2,9 @@ This repository is a copy of the VISLAM from ATLFlight/ros-examples, modified to work with the px4 flight stack. ## Preparations -Get the mavros sources into your catkin's source directory by following the instructions from the [official px dev page](https://dev.px4.io/en/ros/mavros_installation.html). +Get the mavros sources into your catkin's source directory by following the instructions from the [mavros git page](https://github.com/mavlink/mavros/tree/master/mavros#source-installation). + +Note: Step 3 should also include `--rosdistro kinetic` flag. ## Build `catkin build` From 946c2895300acba152d29037e59a43ffc1605a4d Mon Sep 17 00:00:00 2001 From: Potaito Date: Tue, 30 May 2017 11:43:09 +0200 Subject: [PATCH 20/62] Readme update --- README_MAVROS.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/README_MAVROS.md b/README_MAVROS.md index bc0edf0..1cf02dc 100644 --- a/README_MAVROS.md +++ b/README_MAVROS.md @@ -6,6 +6,19 @@ Get the mavros sources into your catkin's source directory by following the inst Note: Step 3 should also include `--rosdistro kinetic` flag. +Note2: On a freshly flashed snapdragon, I had to install the following things +* python pip +* python future (`pip install future`) +* `sudo apt-get install ros-indigo-diagnostic-updater` +* `sudo apt-get install ros-indigo-angles` +* `sudo apt-get install ros-indigo-eigen-conversions` +* `sudo apt-get install ros-indigo-urdf` +* `sudo apt-get install ros-indigo-tf` +* `sudo apt-get install ros-indigo-control-toolbox` + If `control-toolbox` cannot be installed due to unmet dependencies, you might have to install the missing dependency yourself. In my case overwriting an existing file was necessary for the package `fontconfig-config`. That can be done with + `sudo apt-get -o Dpkg::Options::="--force-overwrite" install fontconfig-config` + + ## Build `catkin build` From 69eb32d5ed1752910e510f5e5ab4155ea5694d5a Mon Sep 17 00:00:00 2001 From: Potaito Date: Tue, 30 May 2017 16:08:00 +0200 Subject: [PATCH 21/62] Fixed bug with time stamp conversion --- src/vislam/SnapdragonVislamManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 2b92ef6..d969a4e 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -71,7 +71,7 @@ void Snapdragon::VislamManager::ImuCallback( } last_timestamp = current_timestamp_ns; - // ROS_INFO_STREAM("IMU time: \t" << last_timestamp); + ROS_INFO_STREAM_THROTTLE(1, "IMU time: \t" << last_timestamp); // Parse IMU message float lin_acc[3], ang_vel[3]; @@ -262,7 +262,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - // ROS_INFO_STREAM("Image time: \t" << timestamp_ns); + ROS_INFO_STREAM_THROTTLE(1, "Image time: \t" << timestamp_ns); } } return rc; From 9b02e0837fb73a52a091d4cbd4c82fcce9b8e19a Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 31 May 2017 17:13:53 +0200 Subject: [PATCH 22/62] Removed the YZ inversion and corrected the vislam IMU-CAM transformation instead --- src/nodes/SnapdragonRosNodeVislam.cpp | 15 ++++++++------- src/vislam/SnapdragonVislamManager.cpp | 12 ++++++------ 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 2db73b4..4ac6648 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -110,13 +110,14 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { Snapdragon::VislamManager::InitParams vislamParams; - vislamParams.tbc[0] = 0.005; - vislamParams.tbc[1] = 0.0150; - vislamParams.tbc[2] = 0.0; - - vislamParams.ombc[0] = 0.0; - vislamParams.ombc[1] = 0.0; - vislamParams.ombc[2] = 1.57; + // Transformation between camera and ROS IMU frame (board frame apparently) + vislamParams.tbc[0] = 0.009; // default: 0.005 + vislamParams.tbc[1] = 0.000; // default 0.015 + vislamParams.tbc[2] = 0.0; // default 0.0 + + vislamParams.ombc[0] = 2.221; // pi / sqrt(2) + vislamParams.ombc[1] = -2.221; // -pi / sqrt(2) + vislamParams.ombc[2] = 0.0; vislamParams.delta = -0.008; diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index d969a4e..cac1b00 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -71,18 +71,18 @@ void Snapdragon::VislamManager::ImuCallback( } last_timestamp = current_timestamp_ns; - ROS_INFO_STREAM_THROTTLE(1, "IMU time: \t" << last_timestamp); + ROS_INFO_STREAM_THROTTLE(1, "IMU timestamp [ns]: \t" << last_timestamp); // Parse IMU message float lin_acc[3], ang_vel[3]; // Convert ENU to NED coordinates lin_acc[0] = msg->linear_acceleration.x; - lin_acc[1] = -msg->linear_acceleration.y; - lin_acc[2] = -msg->linear_acceleration.z; + lin_acc[1] = msg->linear_acceleration.y; + lin_acc[2] = msg->linear_acceleration.z; ang_vel[0] = msg->angular_velocity.x; - ang_vel[1] = -msg->angular_velocity.y; - ang_vel[2] = -msg->angular_velocity.z; + ang_vel[1] = msg->angular_velocity.y; + ang_vel[2] = msg->angular_velocity.z; // Check for dropped IMU messages static uint32_t sequence_number_last = 0; @@ -262,7 +262,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - ROS_INFO_STREAM_THROTTLE(1, "Image time: \t" << timestamp_ns); + ROS_INFO_STREAM_THROTTLE(1, "Image timestamp [ns]: \t\t" << timestamp_ns); } } return rc; From 85089af213a98dd13eb971f203f4e586b4190d65 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 31 May 2017 17:15:22 +0200 Subject: [PATCH 23/62] Added publishers for the estimated camera-imu translation and rotation --- src/nodes/SnapdragonRosNodeVislam.cpp | 30 +++++++++++++++++++++++++++ src/nodes/SnapdragonRosNodeVislam.hpp | 17 +++++++++------ 2 files changed, 41 insertions(+), 6 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 4ac6648..4d0ca53 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -44,6 +44,10 @@ Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) { pub_vislam_pose_ = nh_.advertise("vislam/pose",1); pub_vislam_odometry_ = nh_.advertise("vislam/odometry",1); + pub_vislam_tbc_estimate_ = nh_.advertise("vislam/tbc",1); + pub_vislam_rbc_estimate_x_ = nh_.advertise("vislam/rbc_x", 1); + pub_vislam_rbc_estimate_y_ = nh_.advertise("vislam/rbc_y", 1); + pub_vislam_rbc_estimate_z_ = nh_.advertise("vislam/rbc_z", 1); vislam_initialized_ = false; thread_started_ = false; thread_stop_ = false; @@ -256,6 +260,32 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose pose_msg.pose.orientation.z = q.getZ(); pose_msg.pose.orientation.w = q.getW(); pub_vislam_pose_.publish(pose_msg); + + // Publish translation and rotation estimates + geometry_msgs::Vector3 tbc_msg; + tbc_msg.x = vislamPose.tbc[0]; + tbc_msg.y = vislamPose.tbc[1]; + tbc_msg.z = vislamPose.tbc[2]; + pub_vislam_tbc_estimate_.publish(tbc_msg); + + geometry_msgs::Vector3 rbc_x_msg; + rbc_x_msg.x = vislamPose.Rbc[0][0]; + rbc_x_msg.y = vislamPose.Rbc[0][1]; + rbc_x_msg.z = vislamPose.Rbc[0][2]; + pub_vislam_rbc_estimate_x_.publish(rbc_x_msg); + + geometry_msgs::Vector3 rbc_y_msg; + rbc_y_msg.x = vislamPose.Rbc[1][0]; + rbc_y_msg.y = vislamPose.Rbc[1][1]; + rbc_y_msg.z = vislamPose.Rbc[1][2]; + pub_vislam_rbc_estimate_y_.publish(rbc_y_msg); + + geometry_msgs::Vector3 rbc_z_msg; + rbc_z_msg.x = vislamPose.Rbc[2][0]; + rbc_z_msg.y = vislamPose.Rbc[2][1]; + rbc_z_msg.z = vislamPose.Rbc[2][2]; + pub_vislam_rbc_estimate_z_.publish(rbc_z_msg); + //publish the odometry message. nav_msgs::Odometry odom_msg; diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index c4d0108..0732faa 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -89,19 +89,24 @@ class Snapdragon::RosNode::Vislam * Destructor for the node. */ ~Vislam(); -private: +private: // class methods - int32_t PublishVislamData( mvVISLAMPose& vislamPose, int64_t vislamFrameId, uint64_t timestamp_ns ); + int32_t PublishVislamData(mvVISLAMPose& vislamPose, int64_t vislamFrameId, + uint64_t timestamp_ns); void ThreadMain(); // data members; - std::thread vislam_process_thread_; + std::thread vislam_process_thread_; std::atomic thread_started_; std::atomic thread_stop_; std::atomic vislam_initialized_; - ros::NodeHandle nh_; - ros::Publisher pub_vislam_pose_; - ros::Publisher pub_vislam_odometry_; + ros::NodeHandle nh_; + ros::Publisher pub_vislam_pose_; + ros::Publisher pub_vislam_odometry_; + ros::Publisher pub_vislam_tbc_estimate_; + ros::Publisher pub_vislam_rbc_estimate_x_; + ros::Publisher pub_vislam_rbc_estimate_y_; + ros::Publisher pub_vislam_rbc_estimate_z_; int previous_mv_tracking_state_ = MV_TRACKING_STATE_FAILED; }; From 70b424241e70b9e7fb41ac66720fc506b8256a01 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 31 May 2017 17:46:00 +0200 Subject: [PATCH 24/62] Added PoseWithCovarianceStamped publisher for mavros --- src/nodes/SnapdragonRosNodeVislam.cpp | 10 +++++++++- src/nodes/SnapdragonRosNodeVislam.hpp | 1 + 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 4d0ca53..ec034f8 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) { pub_vislam_pose_ = nh_.advertise("vislam/pose",1); + pub_vislam_pose_cov_ = nh_.advertise("vislam/pose_cov",1); pub_vislam_odometry_ = nh_.advertise("vislam/odometry",1); pub_vislam_tbc_estimate_ = nh_.advertise("vislam/tbc",1); pub_vislam_rbc_estimate_x_ = nh_.advertise("vislam/rbc_x", 1); @@ -305,7 +307,13 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose odom_msg.pose.covariance[ i*6 + j ] = vislamPose.errCovPose[i][j]; } } - pub_vislam_odometry_.publish(odom_msg); + pub_vislam_odometry_.publish(odom_msg); + + // Publish pose with covariance (for mavros) + geometry_msgs::PoseWithCovarianceStamped pose_cov_msg; + pose_cov_msg.header = odom_msg.header; + pose_cov_msg.pose = odom_msg.pose; + pub_vislam_pose_cov_.publish(pose_cov_msg); // compute transforms std::vector transforms; diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index 0732faa..4bd3332 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -103,6 +103,7 @@ class Snapdragon::RosNode::Vislam std::atomic vislam_initialized_; ros::NodeHandle nh_; ros::Publisher pub_vislam_pose_; + ros::Publisher pub_vislam_pose_cov_; ros::Publisher pub_vislam_odometry_; ros::Publisher pub_vislam_tbc_estimate_; ros::Publisher pub_vislam_rbc_estimate_x_; From b20a062da668d842ddc55308ca8cd153a844a34d Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 31 May 2017 17:46:21 +0200 Subject: [PATCH 25/62] Comment and formatting --- src/nodes/SnapdragonRosNodeVislam.cpp | 6 +++++- src/vislam/SnapdragonVislamManager.cpp | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index ec034f8..9a8f135 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -116,7 +116,11 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { Snapdragon::VislamManager::InitParams vislamParams; - // Transformation between camera and ROS IMU frame (board frame apparently) + // Transformation between camera and ROS IMU frame. It seems that mavros + // does not only invert the IMU Y- and Z-Axis, but also transforms the sensor + // readings to the board coordinate frame. I did not confirm that in the mavros + // sources, but the online translation and rotation estimates converge nicely + // nicely to values supporting this hypothesis. vislamParams.tbc[0] = 0.009; // default: 0.005 vislamParams.tbc[1] = 0.000; // default 0.015 vislamParams.tbc[2] = 0.0; // default 0.0 diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index cac1b00..da37b39 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -262,7 +262,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - ROS_INFO_STREAM_THROTTLE(1, "Image timestamp [ns]: \t\t" << timestamp_ns); + ROS_INFO_STREAM_THROTTLE(1, "Image timestamp [ns]: \t" << timestamp_ns); } } return rc; From db499984f5b6a7c51313eee55ebecbeca2072826 Mon Sep 17 00:00:00 2001 From: Potaito Date: Wed, 31 May 2017 17:54:23 +0200 Subject: [PATCH 26/62] Added topic remapping to feed mavros with vislam odometry --- launch/mavros_vislam.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 630e058..ad7de17 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -20,7 +20,8 @@ - + + From 8ba586d2a5a7a1f44455c32ee1e358bc9d5512e3 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 09:27:39 +0200 Subject: [PATCH 27/62] Added px4 configurations files (mainapp.conf and px4.conf) --- px4_confs/mainapp.conf | 20 ++++++++++++ px4_confs/px4.config | 71 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 px4_confs/mainapp.conf create mode 100644 px4_confs/px4.config diff --git a/px4_confs/mainapp.conf b/px4_confs/mainapp.conf new file mode 100644 index 0000000..6c0ddea --- /dev/null +++ b/px4_confs/mainapp.conf @@ -0,0 +1,20 @@ +uorb start +muorb start +logger start -t -b 200 +param set MAV_BROADCAST 1 +param set SDLOG_PRIO_BOOST 3 +# EKF2: fuse external vision pose estimate +param set EKF2_AID_MASK 25 +dataman start +navigator start +mavlink start -u 14556 -r 1000000 +sleep 1 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink stream -u 14556 -s RC_CHANNELS -r 20 +# +# mavlink instance for vislam +# +mavlink start -u 14559 -r 100000 +mavlink stream -u 14559 -s HIGHRES_IMU -r 250 +mavlink boot_complete diff --git a/px4_confs/px4.config b/px4_confs/px4.config new file mode 100644 index 0000000..e9d6105 --- /dev/null +++ b/px4_confs/px4.config @@ -0,0 +1,71 @@ +uorb start +qshell start +param set SYS_AUTOSTART 4001 +param set MAV_TYPE 2 +param set MC_YAW_P 12 +param set MC_YAWRATE_P 0.08 +param set MC_YAWRATE_I 0.0 +param set MC_YAWRATE_D 0 +param set MC_PITCH_P 7.0 +param set MC_PITCHRATE_P 0.08 +param set MC_PITCHRATE_I 0.0 +param set MC_PITCHRATE_D 0.001 +param set MC_ROLL_P 7.0 +param set MC_ROLLRATE_P 0.08 +param set MC_ROLLRATE_I 0.0 +param set MC_ROLLRATE_D 0.001 +param set RC_MAP_THROTTLE 1 +param set RC_MAP_ROLL 2 +param set RC_MAP_PITCH 3 +param set RC_MAP_YAW 4 +param set RC_MAP_MODE_SW 5 +param set RC_MAP_POSCTL_SW 6 +param set RC1_MAX 1900 +param set RC1_MIN 1099 +param set RC1_TRIM 1099 +param set RC1_REV 1 +param set RC2_MAX 1900 +param set RC2_MIN 1099 +param set RC2_TRIM 1500 +param set RC2_REV -1 +param set RC3_MAX 1900 +param set RC3_MIN 1099 +param set RC3_TRIM 1500 +param set RC3_REV 1 +param set RC4_MAX 1900 +param set RC4_MIN 1099 +param set RC4_TRIM 1500 +param set RC4_REV -1 +param set RC5_MAX 1900 +param set RC5_MIN 1099 +param set RC5_TRIM 1500 +param set RC5_REV 1 +param set RC6_MAX 1900 +param set RC6_MIN 1099 +param set RC6_TRIM 1099 +param set RC6_REV 1 +param set ATT_W_MAG 0.00 +param set SENS_BOARD_ROT 0 +param set RC_RECEIVER_TYPE 1 +param set UART_ESC_MODEL 2 +param set UART_ESC_BAUDRTE 250000 +param set UART_ESC_MOTOR1 4 +param set UART_ESC_MOTOR2 2 +param set UART_ESC_MOTOR3 1 +param set UART_ESC_MOTOR4 3 +param set SYS_MC_EST_GROUP 2 +sleep 1 +df_mpu9250_wrapper start +df_bmp280_wrapper start +sensors start +commander start +ekf2 start +#land_detector start multicopter +mc_pos_control start +mc_att_control start +uart_esc start -D /dev/tty-2 +sleep 1 +list_devices +list_files +list_tasks +list_topics From c0a090c17c2f6d73fefe5da28ad9ca268088d6cc Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 11:04:40 +0200 Subject: [PATCH 28/62] Added changelog --- CHANGELOG.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..5d84608 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,15 @@ +# Change Log +All notable changes to this project will be documented in this file. + +The format is based on [Keep a Changelog](http://keepachangelog.com/) +and this project adheres to [Semantic Versioning](http://semver.org/). + +## [Unreleased] + +## 0.0.1 - 2017-06-1 +### Changed +- VISLAM running with IMU measurements coming from mavros + +### Added +- EKF2 sensor fusion of VISLAM odometry on px4. Z is a bit doing whatever it wants (I guess baro) and occasionally the ekf2 output jumps to zero for one time step before it recovers. +- px4 configuration files for snapdragon added to `./px4_confs`. From 9beacdb21dcd02d2299a002e91bc80bda0dbf942 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 11:07:15 +0200 Subject: [PATCH 29/62] Added parameter for ekf2 using vision as main source for height estimation --- CHANGELOG.md | 4 +++- px4_confs/mainapp.conf | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5d84608..025a725 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,11 +5,13 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] +### Changed +- EKF2 now using vision as main source for height estimation instead of the barometer. Height estimate is now as good as `X` and `Y` estimations. ## 0.0.1 - 2017-06-1 ### Changed - VISLAM running with IMU measurements coming from mavros ### Added -- EKF2 sensor fusion of VISLAM odometry on px4. Z is a bit doing whatever it wants (I guess baro) and occasionally the ekf2 output jumps to zero for one time step before it recovers. +- EKF2 sensor fusion of VISLAM odometry on px4. `Z` is a bit doing whatever it wants (I guess baro) and occasionally the ekf2 output jumps to zero for one time step before it recovers. - px4 configuration files for snapdragon added to `./px4_confs`. diff --git a/px4_confs/mainapp.conf b/px4_confs/mainapp.conf index 6c0ddea..6d7b5e4 100644 --- a/px4_confs/mainapp.conf +++ b/px4_confs/mainapp.conf @@ -5,6 +5,8 @@ param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 # EKF2: fuse external vision pose estimate param set EKF2_AID_MASK 25 +# EKF2L Switch from barometer to vision as main source for height estimation +param set EKF2_HGT_MODE 3 dataman start navigator start mavlink start -u 14556 -r 1000000 From 19e0e15a4b4c2b77b4359579fabde233b1ed93c4 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 12:26:24 +0200 Subject: [PATCH 30/62] Trying to get LPE to work --- px4_confs/mainapp.conf | 15 +++++++++++---- px4_confs/px4.config | 6 +++--- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/px4_confs/mainapp.conf b/px4_confs/mainapp.conf index 6d7b5e4..3c011b9 100644 --- a/px4_confs/mainapp.conf +++ b/px4_confs/mainapp.conf @@ -3,10 +3,14 @@ muorb start logger start -t -b 200 param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 -# EKF2: fuse external vision pose estimate -param set EKF2_AID_MASK 25 -# EKF2L Switch from barometer to vision as main source for height estimation -param set EKF2_HGT_MODE 3 +## Use LPE for Vision fusion +param set SYS_MC_EST_GROUP 1 +param set LPE_FUSION 247 +param set ATT_EXT_HDG_M 1 +param set LPE_VIS_XY 0.01 +param set LPE_VIS_Z 0.05 +# +# dataman start navigator start mavlink start -u 14556 -r 1000000 @@ -20,3 +24,6 @@ mavlink stream -u 14556 -s RC_CHANNELS -r 20 mavlink start -u 14559 -r 100000 mavlink stream -u 14559 -s HIGHRES_IMU -r 250 mavlink boot_complete +# +sleep 1 +local_position_estimator start diff --git a/px4_confs/px4.config b/px4_confs/px4.config index e9d6105..0f1f620 100644 --- a/px4_confs/px4.config +++ b/px4_confs/px4.config @@ -53,14 +53,14 @@ param set UART_ESC_MOTOR1 4 param set UART_ESC_MOTOR2 2 param set UART_ESC_MOTOR3 1 param set UART_ESC_MOTOR4 3 -param set SYS_MC_EST_GROUP 2 +# param set SYS_MC_EST_GROUP 2 sleep 1 df_mpu9250_wrapper start df_bmp280_wrapper start sensors start commander start -ekf2 start -#land_detector start multicopter +# ekf2 start +land_detector start multicopter mc_pos_control start mc_att_control start uart_esc start -D /dev/tty-2 From b73f43c5c8eee349c07b58cd41eb3c44ffff89a0 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 14:19:51 +0200 Subject: [PATCH 31/62] Added land detector and rc receiver back to px4.config --- px4_confs/px4.config | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/px4_confs/px4.config b/px4_confs/px4.config index 0f1f620..b3197cc 100644 --- a/px4_confs/px4.config +++ b/px4_confs/px4.config @@ -59,11 +59,12 @@ df_mpu9250_wrapper start df_bmp280_wrapper start sensors start commander start -# ekf2 start +ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start uart_esc start -D /dev/tty-2 +rc_receiver start -D /dev/tty-1 sleep 1 list_devices list_files From bcc2986386cba428f4e2b25c664ba8b6bd95619d Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 14:20:15 +0200 Subject: [PATCH 32/62] Reduced covariance on ekf2 vision --- px4_confs/mainapp.conf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/px4_confs/mainapp.conf b/px4_confs/mainapp.conf index 3c011b9..4fa824f 100644 --- a/px4_confs/mainapp.conf +++ b/px4_confs/mainapp.conf @@ -24,6 +24,6 @@ mavlink stream -u 14556 -s RC_CHANNELS -r 20 mavlink start -u 14559 -r 100000 mavlink stream -u 14559 -s HIGHRES_IMU -r 250 mavlink boot_complete -# +# sleep 1 local_position_estimator start From 4d2d1a00a6a35a92270237937ce05e67f2a0fdee Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 17:03:49 +0200 Subject: [PATCH 33/62] Fusing vislam pose into LPE seems to be working --- px4_confs/{ => lpe}/mainapp.conf | 16 ++++++++-------- px4_confs/{ => lpe}/px4.config | 1 + 2 files changed, 9 insertions(+), 8 deletions(-) rename px4_confs/{ => lpe}/mainapp.conf (80%) rename px4_confs/{ => lpe}/px4.config (98%) diff --git a/px4_confs/mainapp.conf b/px4_confs/lpe/mainapp.conf similarity index 80% rename from px4_confs/mainapp.conf rename to px4_confs/lpe/mainapp.conf index 4fa824f..379f3e3 100644 --- a/px4_confs/mainapp.conf +++ b/px4_confs/lpe/mainapp.conf @@ -3,14 +3,6 @@ muorb start logger start -t -b 200 param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 -## Use LPE for Vision fusion -param set SYS_MC_EST_GROUP 1 -param set LPE_FUSION 247 -param set ATT_EXT_HDG_M 1 -param set LPE_VIS_XY 0.01 -param set LPE_VIS_Z 0.05 -# -# dataman start navigator start mavlink start -u 14556 -r 1000000 @@ -26,4 +18,12 @@ mavlink stream -u 14559 -s HIGHRES_IMU -r 250 mavlink boot_complete # sleep 1 +attitude_estimator_q start +sleep 1 +## Use LPE for Vision fusion +param set LPE_FUSION 12 +param set ATT_EXT_HDG_M 1 +param set LPE_VIS_XY 0.001 +param set LPE_VIS_Z 0.001 +LPE_VIS_DELAY 0.00 local_position_estimator start diff --git a/px4_confs/px4.config b/px4_confs/lpe/px4.config similarity index 98% rename from px4_confs/px4.config rename to px4_confs/lpe/px4.config index b3197cc..2440b92 100644 --- a/px4_confs/px4.config +++ b/px4_confs/lpe/px4.config @@ -54,6 +54,7 @@ param set UART_ESC_MOTOR2 2 param set UART_ESC_MOTOR3 1 param set UART_ESC_MOTOR4 3 # param set SYS_MC_EST_GROUP 2 +param set SYS_MC_EST_GROUP 1 sleep 1 df_mpu9250_wrapper start df_bmp280_wrapper start From 438974044363834f831d5676de5de648cb2eeec4 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 17:05:25 +0200 Subject: [PATCH 34/62] Moved ekf2 px4 configuration files into the corresponding subdir --- px4_confs/ekf2/mainapp.conf | 24 +++++++++++++ px4_confs/ekf2/px4.config | 72 +++++++++++++++++++++++++++++++++++++ 2 files changed, 96 insertions(+) create mode 100644 px4_confs/ekf2/mainapp.conf create mode 100644 px4_confs/ekf2/px4.config diff --git a/px4_confs/ekf2/mainapp.conf b/px4_confs/ekf2/mainapp.conf new file mode 100644 index 0000000..0bf0217 --- /dev/null +++ b/px4_confs/ekf2/mainapp.conf @@ -0,0 +1,24 @@ +uorb start +muorb start +logger start -t -b 200 +param set MAV_BROADCAST 1 +param set SDLOG_PRIO_BOOST 3 +# EKF2: fuse external vision pose estimate +param set EKF2_AID_MASK 25 +# EKF2L Switch from barometer to vision as main source for height estimation +param set EKF2_HGT_MODE 3 +param set EKF2_EVP_NOISE 0.01 +param set EKF2_EVA_NOISE 0.01 +dataman start +navigator start +mavlink start -u 14556 -r 1000000 +sleep 1 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink stream -u 14556 -s RC_CHANNELS -r 20 +# +# mavlink instance for vislam +# +mavlink start -u 14559 -r 100000 +mavlink stream -u 14559 -s HIGHRES_IMU -r 250 +mavlink boot_complete diff --git a/px4_confs/ekf2/px4.config b/px4_confs/ekf2/px4.config new file mode 100644 index 0000000..9163fe3 --- /dev/null +++ b/px4_confs/ekf2/px4.config @@ -0,0 +1,72 @@ +uorb start +qshell start +param set SYS_AUTOSTART 4001 +param set MAV_TYPE 2 +param set MC_YAW_P 12 +param set MC_YAWRATE_P 0.08 +param set MC_YAWRATE_I 0.0 +param set MC_YAWRATE_D 0 +param set MC_PITCH_P 7.0 +param set MC_PITCHRATE_P 0.08 +param set MC_PITCHRATE_I 0.0 +param set MC_PITCHRATE_D 0.001 +param set MC_ROLL_P 7.0 +param set MC_ROLLRATE_P 0.08 +param set MC_ROLLRATE_I 0.0 +param set MC_ROLLRATE_D 0.001 +param set RC_MAP_THROTTLE 1 +param set RC_MAP_ROLL 2 +param set RC_MAP_PITCH 3 +param set RC_MAP_YAW 4 +param set RC_MAP_MODE_SW 5 +param set RC_MAP_POSCTL_SW 6 +param set RC1_MAX 1900 +param set RC1_MIN 1099 +param set RC1_TRIM 1099 +param set RC1_REV 1 +param set RC2_MAX 1900 +param set RC2_MIN 1099 +param set RC2_TRIM 1500 +param set RC2_REV -1 +param set RC3_MAX 1900 +param set RC3_MIN 1099 +param set RC3_TRIM 1500 +param set RC3_REV 1 +param set RC4_MAX 1900 +param set RC4_MIN 1099 +param set RC4_TRIM 1500 +param set RC4_REV -1 +param set RC5_MAX 1900 +param set RC5_MIN 1099 +param set RC5_TRIM 1500 +param set RC5_REV 1 +param set RC6_MAX 1900 +param set RC6_MIN 1099 +param set RC6_TRIM 1099 +param set RC6_REV 1 +param set ATT_W_MAG 0.00 +param set SENS_BOARD_ROT 0 +param set RC_RECEIVER_TYPE 1 +param set UART_ESC_MODEL 2 +param set UART_ESC_BAUDRTE 250000 +param set UART_ESC_MOTOR1 4 +param set UART_ESC_MOTOR2 2 +param set UART_ESC_MOTOR3 1 +param set UART_ESC_MOTOR4 3 +param set SYS_MC_EST_GROUP 2 +sleep 1 +df_mpu9250_wrapper start +df_bmp280_wrapper start +sensors start +commander start +ekf2 start +land_detector start multicopter +mc_pos_control start +mc_att_control start +uart_esc start -D /dev/tty-2 +rc_receiver start -D /dev/tty-1 +sleep 1 +list_devices +list_files +list_tasks +list_topics From 329d75719522718164e380bbd0a7186dd6f56435 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 17:06:59 +0200 Subject: [PATCH 35/62] Renamed px4_confs directory --- {px4_confs => px4_configs}/ekf2/mainapp.conf | 0 {px4_confs => px4_configs}/ekf2/px4.config | 0 {px4_confs => px4_configs}/lpe/mainapp.conf | 0 {px4_confs => px4_configs}/lpe/px4.config | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename {px4_confs => px4_configs}/ekf2/mainapp.conf (100%) rename {px4_confs => px4_configs}/ekf2/px4.config (100%) rename {px4_confs => px4_configs}/lpe/mainapp.conf (100%) rename {px4_confs => px4_configs}/lpe/px4.config (100%) diff --git a/px4_confs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf similarity index 100% rename from px4_confs/ekf2/mainapp.conf rename to px4_configs/ekf2/mainapp.conf diff --git a/px4_confs/ekf2/px4.config b/px4_configs/ekf2/px4.config similarity index 100% rename from px4_confs/ekf2/px4.config rename to px4_configs/ekf2/px4.config diff --git a/px4_confs/lpe/mainapp.conf b/px4_configs/lpe/mainapp.conf similarity index 100% rename from px4_confs/lpe/mainapp.conf rename to px4_configs/lpe/mainapp.conf diff --git a/px4_confs/lpe/px4.config b/px4_configs/lpe/px4.config similarity index 100% rename from px4_confs/lpe/px4.config rename to px4_configs/lpe/px4.config From 9213c41ed04bdb53c7ce302b3f3b79159cd55e70 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 17:08:45 +0200 Subject: [PATCH 36/62] Changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 025a725..121b40f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,8 +5,11 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] + +## 0.0.2 - 2017-06-1 ### Changed - EKF2 now using vision as main source for height estimation instead of the barometer. Height estimate is now as good as `X` and `Y` estimations. +- Added px4 configuration files to run LPE estimator instead of EKF2. Seems to work better than EKF2 as anticipated in the [official documentation](https://dev.px4.io/en/ros/external_position_estimation.html) ## 0.0.1 - 2017-06-1 ### Changed From 6b1954c12b2343d7472d59819d1fe962c52491e8 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 1 Jun 2017 17:23:36 +0200 Subject: [PATCH 37/62] Launch file edit --- launch/mavros_vislam.launch | 3 --- 1 file changed, 3 deletions(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index ad7de17..a3a7131 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -23,7 +23,4 @@ - - - From 589ded77b97637cceef9cad1ccc03e98a6643232 Mon Sep 17 00:00:00 2001 From: Potaito Date: Fri, 2 Jun 2017 08:38:26 +0200 Subject: [PATCH 38/62] Removed unused imu manager --- src/imu/SnapdragonImuManager.cpp | 182 ------------------------------- src/imu/SnapdragonImuManager.hpp | 151 ------------------------- 2 files changed, 333 deletions(-) delete mode 100644 src/imu/SnapdragonImuManager.cpp delete mode 100644 src/imu/SnapdragonImuManager.hpp diff --git a/src/imu/SnapdragonImuManager.cpp b/src/imu/SnapdragonImuManager.cpp deleted file mode 100644 index 96e807c..0000000 --- a/src/imu/SnapdragonImuManager.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/**************************************************************************** - * Copyright (c) 2016 Ramakrishna Kintada. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ATLFlight nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include "SnapdragonImuManager.hpp" -#include "SnapdragonDebugPrint.h" -#include - -Snapdragon::ImuManager::ImuManager() { - thread_stop_ = false; - initialized_ = false; - thread_started_ = false; - imu_api_handle_ptr_ = nullptr; -} - -int32_t Snapdragon::ImuManager::Initialize() { - initialized_ = true; - thread_started_ = false; - return 0; -} - -int32_t Snapdragon::ImuManager::SetupImuApi() { - int16_t api_rc = 0; - //get the imu_api handle first. - imu_api_handle_ptr_ = sensor_imu_attitude_api_get_instance(); - if( imu_api_handle_ptr_ == nullptr ) { - //error - ERROR_PRINT( "Error getting the IMU API handler." ); - return -1; - } - - // check the version is the correct one. - char* api_version = sensor_imu_attitude_api_get_version( imu_api_handle_ptr_ ); - std::string str_version( api_version ); - std::string str_expected_version( SENSOR_IMU_AND_ATTITUDE_API_VERSION ); - if( str_version != str_expected_version ) { - ERROR_PRINT( "Error: Imu API version Mismatch. Make sure to link against the correct version(%s)", - str_expected_version.c_str() ); - return -2; - } - - //the API' are good. Call the API's initialize method. - api_rc = sensor_imu_attitude_api_initialize( imu_api_handle_ptr_, SENSOR_CLOCK_SYNC_TYPE_MONOTONIC ); - if( api_rc != 0 ) { - ERROR_PRINT( "Error calling the IMU API's initialize method api_rc(%d)", api_rc ); - return api_rc; - } - - api_rc = sensor_imu_attitude_api_wait_on_driver_init( imu_api_handle_ptr_ ); - if( api_rc != 0 ) { - ERROR_PRINT( "Error calling the IMU API for driver init completion(%d)", api_rc ); - return api_rc; - } - return api_rc; -} - -void Snapdragon::ImuManager::ImuThreadMain() { - if( !initialized_ ) { - ERROR_PRINT( "Starting IMU thread without initialization. Exiting Thread." ); - thread_started_ = false; - return; - } - - if( SetupImuApi() != 0 ) { - ERROR_PRINT( "Error initializing the IMU API's; exiting thread." ); - thread_started_ = false; - return; - } - - int32_t max_imu_samples = 100; - sensor_imu imu_buffer[ max_imu_samples ]; - int32_t returned_sample_count = 0; - int16_t api_rc; - uint32_t prev_sequence_number = 0; - uint32_t current_seqeunce_number; - - while( !thread_stop_ ) { - returned_sample_count = 0; - api_rc = sensor_imu_attitude_api_get_imu_raw( imu_api_handle_ptr_, imu_buffer, 100, &returned_sample_count ); - if( api_rc != 0 ) { - WARN_PRINT( "WARN: Error getting imu samples from imu api(%d)", api_rc ); - } - else { - if( returned_sample_count > 0 ) { - current_seqeunce_number = imu_buffer[0].sequence_number; - if( prev_sequence_number != 0 && prev_sequence_number + 1 != current_seqeunce_number ) { - WARN_PRINT( "Missed IMU Samples: Expected:(%u) Got(%u) sample count: (%d)", - (prev_sequence_number+1), current_seqeunce_number, returned_sample_count ); - } - prev_sequence_number = imu_buffer[returned_sample_count-1].sequence_number; - - // call the handlers. - { - std::lock_guard lock( sync_mutex_ ); - for( auto h : imu_listeners_ ) { - h->Imu_IEventListener_ProcessSamples( imu_buffer, returned_sample_count ); - } - } - } - } - } - INFO_PRINT( "Exiting the IMU Manager Thread." ); - sensor_imu_attitude_api_terminate( imu_api_handle_ptr_ ); - thread_started_ = false; -} - -int32_t Snapdragon::ImuManager::Start() { - // start the IMU reader thread. - if( !thread_started_ ) { - thread_started_ = true; - imu_reader_thread_ = std::thread( &Snapdragon::ImuManager::ImuThreadMain, this ); - } - else { - WARN_PRINT( "WARN: Imu Reader Thread is already running" ); - } - return 0; -} - -int32_t Snapdragon::ImuManager::Stop() { - //this is to stop the thread. - thread_stop_ = true; - initialized_ = false; - if( imu_reader_thread_.joinable() ) { - imu_reader_thread_.join(); - } - imu_api_handle_ptr_ = nullptr; - return 0; -} - -int32_t Snapdragon::ImuManager::Terminate() { - Stop(); -} - - -int32_t Snapdragon::ImuManager::AddHandler( Snapdragon::Imu_IEventListener* handler ) { - int32_t rc = -1; - std::lock_guard lock( sync_mutex_ ); - if( std::find( imu_listeners_.begin(), imu_listeners_.end(), handler ) == imu_listeners_.end() ) { - imu_listeners_.push_back( handler ); - rc = 0; - } - return rc; -} - -int32_t Snapdragon::ImuManager::RemoveHandler( Snapdragon::Imu_IEventListener* handler ) { - int32_t rc = -1; - std::lock_guard lock( sync_mutex_ ); - auto element = std::find( imu_listeners_.begin(), imu_listeners_.end(), handler ); - if( element != imu_listeners_.end() ) { - imu_listeners_.erase( element ); - rc = 0; - } - return rc; -} - diff --git a/src/imu/SnapdragonImuManager.hpp b/src/imu/SnapdragonImuManager.hpp deleted file mode 100644 index c7723f1..0000000 --- a/src/imu/SnapdragonImuManager.hpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * Copyright (c) 2016 Ramakrishna Kintada. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ATLFlight nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#pragma once -#include -#include -#include -#include -#include - -#include -#include - -namespace Snapdragon { - class ImuManager; - class Imu_IEventListener; -} - -/** - * class to invoke call backs for received IMU samples. - **/ -class Snapdragon::Imu_IEventListener { -public: - /** - * Callback function with ImuSmaples received. - * @param samples - * The Imu Sample buffer; - * @param sample_count - * The number of samples returned. - * @return int32_t - * 0 = success - * otherwise = failure. - */ - virtual int32_t Imu_IEventListener_ProcessSamples( sensor_imu* samples, uint32_t sample_count ) = 0; - - /** - * Virtual Destructor - **/ - virtual ~Imu_IEventListener() {}; -}; - -/** - * Class to manage the IMU samples using the IMU API. - **/ -class Snapdragon::ImuManager { -public: - - /** - * Constructor - **/ - ImuManager(); - - /** - * Initializes the manager. This should be called first. - * @return int32_t - * 0 = success - * otherwise = failure. - **/ - int32_t Initialize(); - - /** - * Start a thread to read the IMU samples from the API. - * @return int32_t - * 0 = success - * otherwise = failure. - **/ - int32_t Start(); - - /** - * stops the thread to read IMU samples and waits till thread is exited. - * @return int32_t - * 0 = success - * otherwise = failure. - **/ - int32_t Stop(); - - /** - * Call this to perform any clean-up of resources created as part of Initilize() - * @return int32_t - * 0 = success - * otherwise = failure. - **/ - int32_t Terminate(); - - /** - * Add a Callback handler. - * @param* handler - * The handler to add. - * @return int32_t - * 0 = success - * otherwise = failure. - * If the handler is already present, an error code is returned. - **/ - int32_t AddHandler( Snapdragon::Imu_IEventListener* handler ); - - /** - * Remove a Callback handler. - * @param* handler - * The handler to remove. - * @return int32_t - * 0 = success - * otherwise = failure. - * If the handler is not present, an error code is returned. - **/ - int32_t RemoveHandler( Snapdragon::Imu_IEventListener* handler ); -private: - // thread function to read the imu samples. - void ImuThreadMain(); - - // utility function to handle initialize the imu apis. - int32_t SetupImuApi(); - - // class member variables. - std::atomic thread_stop_; - std::atomic initialized_; - std::atomic thread_started_; - std::thread imu_reader_thread_; - std::mutex sync_mutex_; - std::vector imu_listeners_; - - //internal sensor_handle for imu_api's - sensor_handle* imu_api_handle_ptr_; -}; \ No newline at end of file From 27823ab8b5360f4b8ce3d449ea4587d895e786f8 Mon Sep 17 00:00:00 2001 From: Potaito Date: Fri, 2 Jun 2017 08:47:12 +0200 Subject: [PATCH 39/62] Readme update --- CHANGELOG.md | 2 + CMakeLists.txt | 1 - README.md | 215 +++++++++++------------------------------------ README_MAVROS.md | 39 --------- 4 files changed, 49 insertions(+), 208 deletions(-) delete mode 100644 README_MAVROS.md diff --git a/CHANGELOG.md b/CHANGELOG.md index 121b40f..fe08cdc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,8 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] +### Changed +- Replaced original ATLFlight README with a link. ## 0.0.2 - 2017-06-1 ### Changed diff --git a/CMakeLists.txt b/CMakeLists.txt index f7d0f25..9bae9a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,7 +128,6 @@ add_library(snap_vislam src/camera/SnapdragonCameraManager.cpp src/common/SnapdragonDebugPrint.cpp src/camera/SnapdragonCameraUtil.cpp - src/imu/SnapdragonImuManager.cpp src/vislam/SnapdragonVislamManager.cpp ) diff --git a/README.md b/README.md index 0e480b8..9f3963b 100644 --- a/README.md +++ b/README.md @@ -1,31 +1,36 @@ # Snapdragon Flight VISLAM-ROS Sample Code +This repository is a fork of the VISLAM from ATLFlight/ros-examples, modified to work with the px4 flight stack. The original README can be found in the [ATLFlight/ros-examples](https://github.com/ATLFlight/ros-examples/blob/master/README.md) repository. -This repo provides the sample code and instructions to run Visual-Inertial Simultaneous Localization And Mapping (VISLAM) as a ROS node on the [Qualcomm Snapdragon Platform](https://developer.qualcomm.com/hardware/snapdragon-flight)TM. +## Preparations +### Mavros +Get the mavros sources into your catkin's source directory by following the instructions from the [mavros git page](https://github.com/mavlink/mavros/tree/master/mavros#source-installation). This example code is for MV SDK release [mv_1.0.2](https://developer.qualcomm.com/hardware/snapdragon-flight/tools). -**NOTE**: The older release(mv0.9.1) instructions are [here](https://github.com/ATLFlight/ros-examples/tree/master). - -This example assumes that you are familiar with ROS framework. If you are new to ROS, refer to [ROS Start Guide](http://wiki.ros.org/ROS/StartGuide) first to get started. - -1. [High-Level Block Diagram](#high-level-block-diagram) -1. [Setup and build process](#setup-and-build-process) - * [Summary of changes from previous release](#summary-of-changes-from-previous-release) - * [Pre-requisites](#pre-requisites) - * [Platform BSP](#platform-bsp) - * [Cross-Compile Build Environment](#cross-compile-build-environment) - * [Install ROS on Snapdragon Platform](#install-ros-on-snapdragon-platform) - * [Install Snapdragon Machine Vision SDK](#install-snapdragon-machine-vision-sdk) - * [Machine Vision SDK License Installation](#machine-vision-sdk-license-installation) - * [Clone and build sample code](#clone-and-build-sample-code) -1. [Run sample code](#run-sample-code) - * [Start roscore](#start-roscore) - * [Start imu_app application](#start-imu_app-application) - * [Start VISLAM ROS node](#start-vislam-ros-node) - * [Verification](#verification) -1. [Snapdragon Machine Vision VISLAM FAQ's](#snapdragon-machine-vision-vislam-faqs) - * [VISLAM IMU to Camera Transformation(tbc/ombc)](#imu-to-camera-transformation) +**NOTE**: The older release(mv0.9.1) instructions are [here](https://github.com/ATLFlight/ros-examples/tree/master). +**NOTE**: Step 3 should also include `--rosdistro kinetic` flag. + +### Other packages +On a freshly flashed snapdragon, I had to install the following packages +* python pip +* python future (`pip install future`) +* `sudo apt-get install ros-indigo-diagnostic-updater` +* `sudo apt-get install ros-indigo-angles` +* `sudo apt-get install ros-indigo-eigen-conversions` +* `sudo apt-get install ros-indigo-urdf` +* `sudo apt-get install ros-indigo-tf` +* `sudo apt-get install ros-indigo-control-toolbox` + If `control-toolbox` cannot be installed due to unmet dependencies, you might have to install the missing dependency yourself. In my case overwriting an existing file was necessary for the package `fontconfig-config`. That can be done with + `sudo apt-get -o Dpkg::Options::="--force-overwrite" install fontconfig-config` +>>>>>>> Readme update + +## Build +`catkin build` + +Compiling mavros can easily take 30 minutes on the snapdragon. + +<<<<<<< 2121ee3681d9575c9a3f9d06fea03a7e8cca1ec7 ## High-Level Block Diagram ![SnapVislamRosNodeBlockDiagram](images/SnapVislamRosNodeBlockDiagram.jpg) @@ -131,172 +136,46 @@ adb shell source /home/linaro/.bashrc roscd cd ../src -git clone -b mv-release-1.0.2 https://github.com/ATLFlight/ros-examples.git snap_ros_examples +git clone -b mv-release-1.0.2 https://github.com/ATLFlight/ros-examples.git snap_ros_examples ``` * Build the code +======= +>>>>>>> Readme update +## Configure +Copy the px4 configuration files from `./px4_configs` to their respective location on the snapdragon. For using the recommended LPE: ``` -adb shell -source /home/linaro/.bashrc -roscd -cd .. -catkin_make snap_vislam_node +cd snapdragon_mavros_vislam +adb push ./px4_confs/lpe/mainapp.conf /home/linaro/mainapp.conf +adb push ./px4_confs/lpe/px4.config /usr/share/data/adsp/px4.config ``` -**NOTE**: To clean the code, remove the "build" folder - +and if you want to give EKF2 a try: ``` -adb shell -source /home/linaro/.bashrc -roscd -cd ../ -rm -rf build +cd snapdragon_mavros_vislam +adb push ./px4_confs/ekf2/mainapp.conf /home/linaro/mainapp.conf +adb push ./px4_confs/ekf2/px4.config /usr/share/data/adsp/px4.config ``` -## Run sample code - -This example assumes that the user is familiar with ROS framework. If you are new to ROS, refer to [ROS Start Guide](http://wiki.ros.org/ROS/StartGuide) first to get started. - -This assumes that the ROS build command is successful. - -**NOTE**: This process is for running VISLAM in standalone mode only. Integration with a flight stack such as PX4 is not verified. - -**NOTE**: The steps below show each application to be run in a separate adb shell. This is for illustration purpose only. The applications, like **rocore**, **imu_app**, **snap_vislam_node** can be run in the background to meet your development/runtime workflow. - -### Start roscore -Start roscore in a shell as follows: +The configurations are intended to provide a working starting point. Adjust parameters to your likings. +## Run +Get a roscore up and running ``` adb shell -source /home/linaro/.bashrc roscore ``` -### Start imu_app application - -Starting the **imu_app** is **MANDATORY**. Without this, the sample code will not run and the behavior is unpredictable. **imu_app** is already pre-installed on the target. - -For VISLAM to work, it needs the IMU data. On Snapdragon FlightTM, the MPU9x50(IMU) is connected to the ADSP and the Snapdragon FlightTM supports an Apps processor API to get the IMU Data from ADSP. Start the imu_app before using the API to retrieve IMU data. - -**NOTE**: Staring the **imu_app** will initialize the MPU9x50 driver on ADSP. There can be only one app that initilizes the MPU9x50 driver on ADSP. If there are more than one app that intializes the MPU9x50 driver, the attempts after the first one, will fail. - -Run **imu_app** in a shell. - -``` -adb shell -imu_app -s 2 -``` - -To verify that it works, run the following command in a different shell instance (optional): - +In another terminal, launch mavros and vislam ``` adb shell -sensor_imu_tester 5 -``` - -The app runs for 5 seconds and collects the IMU data received from the ADSP. It would generate a few IMU_*.txt files. If these files get generated and they contain IMU samples, then the IMU data reception mechanism on apps processor works fine. - -### Start VISLAM ROS node -To start the "snap_vislam_node" ROS node, the 2 steps above: [start roscore](#start-roscore) and [start imu_app](#start_imu_app_application) should be completed to make this work. - +roslaunch snapdragon_mavros_vislam mavros_vislam.launch ``` -adb shell -source /home/linaro/.bashrc -rosrun snap_ros_examples snap_vislam_node -``` - -### Verification -If the above steps are complete and the applications starts successfully, the snap_vislam_node will publish /vislam/pose and /vislam/odometry topics. To view these topics, use the rostopic echo command. +Start the px4 flight stack ``` adb shell -source /home/linaro/.bashrc -rostopic echo /vislam/pose -``` - -## Snapdragon Machine Vision VISLAM FAQs - -### I do not see any ROS vislam/pose topic when I run the snap_ros_node - -VISLAM requires a minimum set of points to initialize correctly. If the points are not detected, the pose quality reported by the algorithm is "FAIL" or "INITIALIZING". In this case, there won't be any ROS vislam/pose topics published. One scenario is if board is laying on a surface and the downward camera is very close to the surface. - -To fix this, lift up the board a few inches from the surface and set it stationary for few seconds until the initalization is complete. Once completed, you should see the ROS vislam/pose topics published. - -The VISLAM algorithm has a parameter, "noInitWhenMoving", that allows to enable or disable initialization when moving. The current example sets this parameter to be **true**, and hence you need to keep the board stationary. To change the parameter edit the [SnapdragonRosNodeVislam.cpp](src/nodes/SnapdragonRosNodeVislam.cpp) file as follows: - -``` - vislamParams.noInitWhenMoving = false; -``` - -Once the change is done, recompile the code and re-test. - -**NOTE**: Allowing for initialization when board is moving will potentially give bad VISLAM pose information. The recommended setting is as used in the example. - -### I do see the ROS vislam/pose topics published how do I verify if the VISLAM is working? - -The example demonstrates console echo for vislam/pose information. To validate if the vislam/pose is correct, you can move the board exercising all 6DOF motion to see the x,y,z changes in both the linear and rotational axes. For example, if you move the board up and down, the "z" axes of the linear pose should change. Similarily if you rotate the board on the "z" axes, the "z" value for the quaternion form should change. See the [ROS pose message](http://docs.ros.org/api/geometry_msgs/html/msg/Pose.html) for the details on the pose message fields. - -Since the example is generating a ROS topic, ROS visulation tool like RVIZ can be used to view the pose information. The RVIZ integration is left as an exercise for the user. - -### How do I set my own values for the initial IMU-to-camera transformation? - -The transformation between the IMU and camera is required for initializing VISLAM. While VISLAM actively estimates this -transform, an accurate initial estimate is important. If you plan to change the location of the camera and/or IMU, you -will need to provide a new initial estimate for this transform. - -The transform from IMU to camera is defined by the parameters called "tbc" and "ombc" for the translational and -rotational components, respectively. "b" refers to the body, or IMU, frame and "c" refers to the camera frame. - -#### Coordinate frame conventions - -The IMU coordinate frame is right-handed and defined as follows: - -* X forward on the Snapdragon Flight board -* Y to the right of the board -* Z down with respect to the board - -The camera coordinate frame is right-handed and defined as follows: - -* X to the right in the image -* Y down in the image -* Z out of the camera (into the image) - -![VislamCoordinateFrames](images/VislamCoordinateFrames.png) - -#### Setting tbc and ombc - -Given these coordinate frames, tbc and ombc values are found as follows: - -* tbc is the position of the camera frame origin with respect to the IMU frame origin, written in the IMU coordinate frame, in meters. -* ombc is the associated rotation that transforms a vector in the camera frame to the IMU frame, converted to axis-angle - representation (three components), in radians. - -If the ombc is converted to a rotation matrix (R), it can be combined with the tbc (T) such that a vector in the camera -frame can be expressed in the IMU frame, X_IMU = R * X_camera + T. - -The ombc uses axis-angle parameterization that has only three parameters. (See [here](https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector) ). -The X, Y, and Z components describe the vector about which we rotate, and the magnitude of this vector describes the magnitude of rotation. - -The default values for tbc and ombc are provided in [SnapdragonRosNodeVislam.cpp](src/nodes/SnapdragonRosNodeVislam.cpp), which are - +cd $HOME +./px4 mainapp.config ``` - vislamParams.tbc[0] = 0.005; // X displacement [m] - vislamParams.tbc[1] = 0.0150; // Y displacement [m] - vislamParams.tbc[2] = 0.0; // Z displacement [m] - - vislamParams.ombc[0] = 0.0; // rotation of 90 deg about Z - vislamParams.ombc[1] = 0.0; // rotation of 90 deg about Z - vislamParams.ombc[2] = 1.57; // rotation of 90 deg about Z -``` - -and describe the downward-facing camera shifted in X and Y, and rotated 90 degrees about the Z axis, with respect to the -IMU. - -#### Verifying correct estimates - -The VISLAM module provides the estimated transformation within the mvVISLAMPose struct, through the fields called "tbc" -and "Rbc". tbc is exactly as defined above. Rbc is the 3x3 rotation matrix describing ombc above. If the initial -estimates for tbc and ombc are correct, the values of tbc and Rbc should converge to values close to these initial -guesses. Convergence can be verified by plotting the mvVISLAMPose output after sufficient excitation of all six degrees of -freedom of the board (usually after 30 seconds of dynamic motion). diff --git a/README_MAVROS.md b/README_MAVROS.md deleted file mode 100644 index 1cf02dc..0000000 --- a/README_MAVROS.md +++ /dev/null @@ -1,39 +0,0 @@ -# Snapdragon Flight VISLAM-ROS Sample Code -This repository is a copy of the VISLAM from ATLFlight/ros-examples, modified to work with the px4 flight stack. - -## Preparations -Get the mavros sources into your catkin's source directory by following the instructions from the [mavros git page](https://github.com/mavlink/mavros/tree/master/mavros#source-installation). - -Note: Step 3 should also include `--rosdistro kinetic` flag. - -Note2: On a freshly flashed snapdragon, I had to install the following things -* python pip -* python future (`pip install future`) -* `sudo apt-get install ros-indigo-diagnostic-updater` -* `sudo apt-get install ros-indigo-angles` -* `sudo apt-get install ros-indigo-eigen-conversions` -* `sudo apt-get install ros-indigo-urdf` -* `sudo apt-get install ros-indigo-tf` -* `sudo apt-get install ros-indigo-control-toolbox` - If `control-toolbox` cannot be installed due to unmet dependencies, you might have to install the missing dependency yourself. In my case overwriting an existing file was necessary for the package `fontconfig-config`. That can be done with - `sudo apt-get -o Dpkg::Options::="--force-overwrite" install fontconfig-config` - - -## Build -`catkin build` - -This can easily take 30 minutes on the snapdragon. - - -## Run -Get a roscore running -`roscore` - -Start the px4 flight stack -``` -cd $HOME -./px4 mainapp.config -``` - -Launch rpg_qualcomm_vislam -`roslaunch rpg_qualcomm_vislam mavros_vislam.launch` From e9084ff6f2b13714c18a1c3565baf632e89c065f Mon Sep 17 00:00:00 2001 From: Potaito Date: Tue, 6 Jun 2017 14:05:25 +0200 Subject: [PATCH 40/62] Minor changes in readme etc. --- CHANGELOG.md | 4 ++-- README.md | 4 ---- package.xml | 3 +-- src/vislam/SnapdragonVislamManager.cpp | 1 - 4 files changed, 3 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fe08cdc..d6fcb07 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,10 +6,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] ### Changed -- Replaced original ATLFlight README with a link. -## 0.0.2 - 2017-06-1 +## 0.0.2 - 2017-06-6 ### Changed +- Replaced original ATLFlight README with a link. - EKF2 now using vision as main source for height estimation instead of the barometer. Height estimate is now as good as `X` and `Y` estimations. - Added px4 configuration files to run LPE estimator instead of EKF2. Seems to work better than EKF2 as anticipated in the [official documentation](https://dev.px4.io/en/ros/external_position_estimation.html) diff --git a/README.md b/README.md index 9f3963b..5d1c528 100644 --- a/README.md +++ b/README.md @@ -23,14 +23,12 @@ On a freshly flashed snapdragon, I had to install the following packages * `sudo apt-get install ros-indigo-control-toolbox` If `control-toolbox` cannot be installed due to unmet dependencies, you might have to install the missing dependency yourself. In my case overwriting an existing file was necessary for the package `fontconfig-config`. That can be done with `sudo apt-get -o Dpkg::Options::="--force-overwrite" install fontconfig-config` ->>>>>>> Readme update ## Build `catkin build` Compiling mavros can easily take 30 minutes on the snapdragon. -<<<<<<< 2121ee3681d9575c9a3f9d06fea03a7e8cca1ec7 ## High-Level Block Diagram ![SnapVislamRosNodeBlockDiagram](images/SnapVislamRosNodeBlockDiagram.jpg) @@ -140,8 +138,6 @@ git clone -b mv-release-1.0.2 https://github.com/ATLFlight/ros-examples.git snap ``` * Build the code -======= ->>>>>>> Readme update ## Configure Copy the px4 configuration files from `./px4_configs` to their respective location on the snapdragon. For using the recommended LPE: diff --git a/package.xml b/package.xml index 1757d87..0b1be92 100644 --- a/package.xml +++ b/package.xml @@ -3,8 +3,7 @@ snapdragon_mavros_vislam 0.1.1 - Private fork of the Qualcomm Vislam example code from ATLFlight - (https://github.com/ATLFlight/ros-examples) for use with the Snapdragon Flight + Fork of the Qualcomm Vislam example code from ATLFlight (https://github.com/ATLFlight/ros-examples) modified for use with the px4 flight stack. diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index da37b39..cdedfbf 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -186,7 +186,6 @@ int32_t Snapdragon::VislamManager::Start() { int32_t rc = 0; if( initialized_ ) { //start the camera - // rc = imu_man_ptr_->Start(); rc |= cam_man_ptr_->Start(); //wait till we get the first frame. From d8482e4f6d25d08affaae8d532fe42837b359055 Mon Sep 17 00:00:00 2001 From: Potaito Date: Thu, 8 Jun 2017 08:34:53 +0200 Subject: [PATCH 41/62] Added mavros tested version info --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5d1c528..51c7706 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ This repository is a fork of the VISLAM from ATLFlight/ros-examples, modified to ## Preparations ### Mavros -Get the mavros sources into your catkin's source directory by following the instructions from the [mavros git page](https://github.com/mavlink/mavros/tree/master/mavros#source-installation). +Get the mavros sources into your catkin's source directory by following the instructions from the [mavros git page](https://github.com/mavlink/mavros/tree/master/mavros#source-installation). Last tested version of mavros is version [0.19](https://github.com/mavlink/mavros/releases/tag/0.19.0) and the respective commit [399a04ef57](https://github.com/mavlink/mavros/tree/399a04ef5739bd93fe1c596b63d6bdbfba7d84ef). This example code is for MV SDK release [mv_1.0.2](https://developer.qualcomm.com/hardware/snapdragon-flight/tools). From 1abcf70a9e426982f0c55ad32ce93702d95176f9 Mon Sep 17 00:00:00 2001 From: Potaito Date: Fri, 9 Jun 2017 16:23:27 +0200 Subject: [PATCH 42/62] fix in mainapp.conf --- px4_configs/lpe/mainapp.conf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/px4_configs/lpe/mainapp.conf b/px4_configs/lpe/mainapp.conf index 379f3e3..02e4a29 100644 --- a/px4_configs/lpe/mainapp.conf +++ b/px4_configs/lpe/mainapp.conf @@ -25,5 +25,5 @@ param set LPE_FUSION 12 param set ATT_EXT_HDG_M 1 param set LPE_VIS_XY 0.001 param set LPE_VIS_Z 0.001 -LPE_VIS_DELAY 0.00 +param set LPE_VIS_DELAY 0.00 local_position_estimator start From 4d7af953fbf2ac0009978c14dfb80916dff84d95 Mon Sep 17 00:00:00 2001 From: Potaito Date: Fri, 14 Jul 2017 12:36:02 +0200 Subject: [PATCH 43/62] Added offset computation. Now camera and imu timestamps carry the REALTIME timestamp, which conforms with ros standards --- launch/snapdragon_px4_config.yaml | 2 +- src/vislam/SnapdragonVislamManager.cpp | 18 ++++++++++++++++-- src/vislam/SnapdragonVislamManager.hpp | 3 ++- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/launch/snapdragon_px4_config.yaml b/launch/snapdragon_px4_config.yaml index 5362b58..2a66e27 100644 --- a/launch/snapdragon_px4_config.yaml +++ b/launch/snapdragon_px4_config.yaml @@ -20,7 +20,7 @@ sys: # sys_time time: time_ref_source: "fcu" # time_reference source - timesync_mode: PASSTHROUGH + timesync_mode: ONBOARD timesync_avg_alpha: 0.6 # timesync averaging factor # --- mavros plugins (alphabetical order) --- diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index cdedfbf..9c5bcb3 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -254,14 +254,28 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr // adjust the frame-timestamp for VISLAM at it needs the time at the center of the exposure and not the sof. // Correction from exposure time float correction = 1e3 * (cam_man_ptr_->GetExposureTimeUs()/2.f); - float modified_timestamp = frame_ts_ns - static_cast(correction); + + // Adjust timestamp with clock offset MONOTONIC <-> REALTIME: + // Camera images will correctly have REALTIME timestamps, IMU messages + // however carry the MONOTONIC timestamp. + timespec mono_time, wall_time; + if (clock_gettime(CLOCK_MONOTONIC, &mono_time) || + clock_gettime(CLOCK_REALTIME, &wall_time)) + { + ERROR_PRINT("Cannot access clock time"); + return -1; + } + // TODO: Perhaps filter clock_offset_ns. So far I only observed jumps of 1 milisecond though. + int64_t clock_offset_ns = (wall_time.tv_sec - mono_time.tv_sec) * 1e9 + + wall_time.tv_nsec - mono_time.tv_nsec; + uint64_t modified_timestamp = frame_ts_ns - static_cast(correction) + clock_offset_ns; { std::lock_guard lock( sync_mutex_ ); mvVISLAM_AddImage(vislam_ptr_, modified_timestamp, image_buffer_ ); pose = mvVISLAM_GetPose(vislam_ptr_); pose_frame_id = frame_id; timestamp_ns = static_cast(modified_timestamp); - ROS_INFO_STREAM_THROTTLE(1, "Image timestamp [ns]: \t" << timestamp_ns); + ROS_INFO_STREAM_THROTTLE(1, "Image timestamp [ns]: \t" << modified_timestamp); } } return rc; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index bd37804..a993b2e 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -30,8 +30,9 @@ * ****************************************************************************/ #pragma once -#include #include +#include // std::atomic +#include // clock_gettime #include #include #include From b0541a7e02ad5b7c235fc9a5e7620047f2a69656 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 15 Dec 2017 15:47:47 +0100 Subject: [PATCH 44/62] Use port 14555 for MavROS and mode magic (no other mavlink streams) --- launch/mavros_vislam.launch | 8 ++++---- px4_configs/ekf2/mainapp.conf | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index a3a7131..8b20a2d 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -1,7 +1,7 @@ - + @@ -10,17 +10,17 @@ - + - + - + diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index 0bf0217..8a9fa01 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -19,6 +19,6 @@ mavlink stream -u 14556 -s RC_CHANNELS -r 20 # # mavlink instance for vislam # -mavlink start -u 14559 -r 100000 -mavlink stream -u 14559 -s HIGHRES_IMU -r 250 +mavlink start -u 14555 -r 100000 -m magic +mavlink stream -u 14555 -s HIGHRES_IMU -r 250 mavlink boot_complete From 57de39553b5a91d48e6ea036a15503e331720ac3 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 15 Dec 2017 15:48:13 +0100 Subject: [PATCH 45/62] use mavlink port 14550 for QGC --- px4_configs/ekf2/mainapp.conf | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index 8a9fa01..3dbe80f 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -11,11 +11,13 @@ param set EKF2_EVP_NOISE 0.01 param set EKF2_EVA_NOISE 0.01 dataman start navigator start -mavlink start -u 14556 -r 1000000 +mavlink start -u 14550 -r 1000000 sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 +mavlink stream -u 14550 -s HIGHRES_IMU -r 50 +mavlink stream -u 14550 -s ATTITUDE -r 50 +mavlink stream -u 14550 -s RC_CHANNELS -r 20 +mavlink stream -u 14550 -s LOCAL_POSITION_NED -r 10 +mavlink stream -u 14550 -s VISION_POSITION_ESTIMATE -r 10 # # mavlink instance for vislam # From f1f90f5f7ef34372a6ae6071f3aa87918a628eb4 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 15 Dec 2017 15:54:05 +0100 Subject: [PATCH 46/62] Use imu-cam translation that the algorithm converges to --- src/nodes/SnapdragonRosNodeVislam.cpp | 44 +++++++++++++-------------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 9a8f135..837f64a 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -63,7 +63,7 @@ Snapdragon::RosNode::Vislam::~Vislam() } int32_t Snapdragon::RosNode::Vislam::Initialize() -{ +{ vislam_initialized_ = true; return 0; } @@ -116,17 +116,15 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { Snapdragon::VislamManager::InitParams vislamParams; - // Transformation between camera and ROS IMU frame. It seems that mavros - // does not only invert the IMU Y- and Z-Axis, but also transforms the sensor - // readings to the board coordinate frame. I did not confirm that in the mavros - // sources, but the online translation and rotation estimates converge nicely - // nicely to values supporting this hypothesis. - vislamParams.tbc[0] = 0.009; // default: 0.005 - vislamParams.tbc[1] = 0.000; // default 0.015 - vislamParams.tbc[2] = 0.0; // default 0.0 - - vislamParams.ombc[0] = 2.221; // pi / sqrt(2) - vislamParams.ombc[1] = -2.221; // -pi / sqrt(2) + // use the translation that the algorithm converges to + vislamParams.tbc[0] = -0.006; + vislamParams.tbc[1] = -0.020; + vislamParams.tbc[2] = 0.0; + + // Transformation between camera and ROS IMU frame (forward-left-up). + // Unit vector (1/sqrt(2), -1/sqrt(2), 0) times rotation magnitude PI + vislamParams.ombc[0] = 2.221; // PI / sqrt(2) + vislamParams.ombc[1] = -2.221; // -PI / sqrt(2) vislamParams.ombc[2] = 0.0; vislamParams.delta = -0.008; @@ -154,7 +152,7 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { vislamParams.useLogCameraHeight = false; vislamParams.logCameraHeightBootstrap = -3.22; vislamParams.noInitWhenMoving = true; - vislamParams.limitedIMUbWtrigger = 35.0; + vislamParams.limitedIMUbWtrigger = 35.0; Snapdragon::CameraParameters param; param.enable_cpa = 1; @@ -171,7 +169,7 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { cpaConfig.legacyCost.exposureCost = 1.0f; cpaConfig.legacyCost.gainCost = 0.3333f; - param.mv_cpa_config = cpaConfig; + param.mv_cpa_config = cpaConfig; Snapdragon::VislamManager vislam_man(nh_); if( vislam_man.Initialize( param, vislamParams ) != 0 ) { ROS_WARN_STREAM( "Snapdragon::RosNodeVislam::VislamThreadMain: Error initializing the VISLAM Manager " ); @@ -195,7 +193,7 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { vislam_ret = vislam_man.GetPose( vislamPose, vislamFrameId, timestamp_ns ); if( vislam_ret == 0 ) { //check if the pose quality is good. If not do not publish the data. - if( vislamPose.poseQuality != MV_TRACKING_STATE_FAILED && + if( vislamPose.poseQuality != MV_TRACKING_STATE_FAILED && vislamPose.poseQuality != MV_TRACKING_STATE_INITIALIZING ) { // Publish Pose Data PublishVislamData( vislamPose, vislamFrameId, timestamp_ns ); @@ -255,7 +253,7 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose vislamPose.bodyPose.matrix[1][2], vislamPose.bodyPose.matrix[2][0], vislamPose.bodyPose.matrix[2][1], - vislamPose.bodyPose.matrix[2][2]); + vislamPose.bodyPose.matrix[2][2]); tf2::Quaternion q; R.getRotation(q); pose_msg.pose.position.x = vislamPose.bodyPose.matrix[0][3]; @@ -266,32 +264,32 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose pose_msg.pose.orientation.z = q.getZ(); pose_msg.pose.orientation.w = q.getW(); pub_vislam_pose_.publish(pose_msg); - + // Publish translation and rotation estimates geometry_msgs::Vector3 tbc_msg; tbc_msg.x = vislamPose.tbc[0]; tbc_msg.y = vislamPose.tbc[1]; tbc_msg.z = vislamPose.tbc[2]; pub_vislam_tbc_estimate_.publish(tbc_msg); - + geometry_msgs::Vector3 rbc_x_msg; rbc_x_msg.x = vislamPose.Rbc[0][0]; rbc_x_msg.y = vislamPose.Rbc[0][1]; rbc_x_msg.z = vislamPose.Rbc[0][2]; pub_vislam_rbc_estimate_x_.publish(rbc_x_msg); - + geometry_msgs::Vector3 rbc_y_msg; rbc_y_msg.x = vislamPose.Rbc[1][0]; rbc_y_msg.y = vislamPose.Rbc[1][1]; rbc_y_msg.z = vislamPose.Rbc[1][2]; pub_vislam_rbc_estimate_y_.publish(rbc_y_msg); - + geometry_msgs::Vector3 rbc_z_msg; rbc_z_msg.x = vislamPose.Rbc[2][0]; rbc_z_msg.y = vislamPose.Rbc[2][1]; rbc_z_msg.z = vislamPose.Rbc[2][2]; pub_vislam_rbc_estimate_z_.publish(rbc_z_msg); - + //publish the odometry message. nav_msgs::Odometry odom_msg; @@ -312,7 +310,7 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose } } pub_vislam_odometry_.publish(odom_msg); - + // Publish pose with covariance (for mavros) geometry_msgs::PoseWithCovarianceStamped pose_cov_msg; pose_cov_msg.header = odom_msg.header; @@ -338,5 +336,5 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose // broadcast transforms static tf2_ros::TransformBroadcaster br; - br.sendTransform(transforms); + br.sendTransform(transforms); } From d5e6cba7bdf402623111d3874b823ef9a1402322 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 15 Dec 2017 15:54:57 +0100 Subject: [PATCH 47/62] beautify --- src/vislam/SnapdragonVislamManager.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 9c5bcb3..9944ef8 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -49,7 +49,7 @@ void Snapdragon::VislamManager::ImuCallback( { // Convert from ENU (mavros) to NED (vislam expectation) frame sensor_msgs::Imu msg_ned = *msg; - + int64_t current_timestamp_ns = msg->header.stamp.toNSec(); static int64_t last_timestamp = 0; @@ -125,18 +125,18 @@ int32_t Snapdragon::VislamManager::CleanUp() { if( vislam_ptr_ != nullptr ) { mvVISLAM_Deinitialize( vislam_ptr_ ); vislam_ptr_ = nullptr; - } + } if( image_buffer_ != nullptr ) { delete[] image_buffer_; image_buffer_ = nullptr; image_buffer_size_bytes_ = 0; } - return 0; + return 0; } int32_t Snapdragon::VislamManager::Initialize -( +( const Snapdragon::CameraParameters& cam_params, const Snapdragon::VislamManager::InitParams& vislam_params ) { @@ -196,7 +196,7 @@ int32_t Snapdragon::VislamManager::Start() { image_buffer_size_bytes_ = cam_man_ptr_->GetImageSize(); INFO_PRINT( "image Size: %d frameId: %lld", cam_man_ptr_->GetImageSize(), cam_man_ptr_->GetLatestFrameId() ); image_buffer_ = new uint8_t[ image_buffer_size_bytes_ ]; - + // Setup publishers/subscribers imu_sub_ = nh_.subscribe("mavros/imu/data_raw", 10, &Snapdragon::VislamManager::ImuCallback, this); @@ -210,7 +210,7 @@ int32_t Snapdragon::VislamManager::Start() { int32_t Snapdragon::VislamManager::Stop() { CleanUp(); - + // Unsubscribe from IMU topic imu_sub_.shutdown(); return 0; @@ -235,7 +235,7 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr } // first set the Image from the camera. - // Next add it to the mvVISLAM + // Next add it to the mvVISLAM // Then call the API to get the Pose. int64_t frame_id; uint32_t used = 0; @@ -254,9 +254,9 @@ int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_fr // adjust the frame-timestamp for VISLAM at it needs the time at the center of the exposure and not the sof. // Correction from exposure time float correction = 1e3 * (cam_man_ptr_->GetExposureTimeUs()/2.f); - + // Adjust timestamp with clock offset MONOTONIC <-> REALTIME: - // Camera images will correctly have REALTIME timestamps, IMU messages + // Camera images will correctly have REALTIME timestamps, IMU messages // however carry the MONOTONIC timestamp. timespec mono_time, wall_time; if (clock_gettime(CLOCK_MONOTONIC, &mono_time) || From ec0d6a9f1eacdea4ca2b2b9bec3aa2f73745b196 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 22 Dec 2017 14:13:39 +0100 Subject: [PATCH 48/62] mavros launch: use default px4 mavros config and remap to mavros/odometry/odom --- launch/mavros_vislam.launch | 4 +- launch/snapdragon_px4_config.yaml | 181 ------------------------------ 2 files changed, 2 insertions(+), 183 deletions(-) delete mode 100644 launch/snapdragon_px4_config.yaml diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 8b20a2d..56a9ef8 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -9,7 +9,7 @@ - + @@ -20,7 +20,7 @@ - + diff --git a/launch/snapdragon_px4_config.yaml b/launch/snapdragon_px4_config.yaml deleted file mode 100644 index 2a66e27..0000000 --- a/launch/snapdragon_px4_config.yaml +++ /dev/null @@ -1,181 +0,0 @@ -# Common configuration for PX4 autopilot -# -# node: -startup_px4_usb_quirk: true - -# --- system plugins --- - -# sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat timeout in seconds - timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) - system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) - -# sys_status -sys: - min_voltage: 10.0 # diagnostics min voltage - disable_diag: false # disable all sys_status diagnostics, except heartbeat - -# sys_time -time: - time_ref_source: "fcu" # time_reference source - timesync_mode: ONBOARD - timesync_avg_alpha: 0.6 # timesync averaging factor - -# --- mavros plugins (alphabetical order) --- - -# 3dr_radio -tdr_radio: - low_rssi: 40 # raw rssi lower level for diagnostics - -# actuator_control -# None - -# command -cmd: - use_comp_id_system_control: false # quirk for some old FCUs - -# dummy -# None - -# ftp -# None - -# global_position -global_position: - frame_id: "fcu" # pose and fix frame_id - rot_covariance: 99999.0 # covariance for attitude? - tf: - send: false # send TF? - frame_id: "local_origin" # TF frame_id - child_frame_id: "fcu_utm" # TF child_frame_id - -# imu_pub -imu: - frame_id: "fcu" - # need find actual values - linear_acceleration_stdev: 0.0003 - angular_velocity_stdev: !degrees 0.02 - orientation_stdev: 1.0 - magnetic_stdev: 0.0 - -# local_position -local_position: - frame_id: "fcu" - tf: - send: false - frame_id: "local_origin" - child_frame_id: "fcu" - send_fcu: false - -# param -# None, used for FCU params - -# rc_io -# None - -# safety_area -safety_area: - p1: {x: 1.0, y: 1.0, z: 1.0} - p2: {x: -1.0, y: -1.0, z: -1.0} - -# setpoint_accel -setpoint_accel: - send_force: false - -# setpoint_attitude -setpoint_attitude: - reverse_throttle: false # allow reversed throttle - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "attitude" - rate_limit: 10.0 - -# setpoint_position -setpoint_position: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "setpoint" - rate_limit: 50.0 - -# setpoint_velocity -# None - -# vfr_hud -# None - -# waypoint -mission: - pull_after_gcs: true # update mission if gcs updates - -# --- mavros extras plugins (same order) --- - -# distance_sensor (PX4 only) -distance_sensor: - hrlv_ez4_pub: - id: 0 - frame_id: "hrlv_ez4_sonar" - orientation: ROLL_180 # RPY:{180.0, 0.0, 0.0} - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - lidarlite_pub: - id: 1 - frame_id: "lidarlite_laser" - orientation: ROLL_180 - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - sonar_1_sub: - subscriber: true - id: 2 - orientation: ROLL_180 - laser_1_sub: - subscriber: true - id: 3 - orientation: ROLL_180 - -## Currently available orientations: -# Check http://wiki.ros.org/mavros/Enumerations -## - -# image_pub -image: - frame_id: "px4flow" - -# mocap_pose_estimate -mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose - -# px4flow -px4flow: - frame_id: "px4flow" - ranger_fov: !degrees 0.0 # XXX TODO - ranger_min_range: 0.3 # meters - ranger_max_range: 5.0 # meters - -# vision_pose_estimate -vision_pose: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "vision" - rate_limit: 10.0 - -# vision_speed_estimate -vision_speed: - listen_twist: false - -# vibration -vibration: - frame_id: "vibration" - -# adsb -# None - -# vim:set ts=2 sw=2 et: From ae138ec0162672dfd71821985181a714b2030a6b Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 22 Dec 2017 14:19:35 +0100 Subject: [PATCH 49/62] remove unused pos_cov msg --- src/nodes/SnapdragonRosNodeVislam.cpp | 7 ------- src/nodes/SnapdragonRosNodeVislam.hpp | 1 - 2 files changed, 8 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 837f64a..078eaa4 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -44,7 +44,6 @@ Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) { pub_vislam_pose_ = nh_.advertise("vislam/pose",1); - pub_vislam_pose_cov_ = nh_.advertise("vislam/pose_cov",1); pub_vislam_odometry_ = nh_.advertise("vislam/odometry",1); pub_vislam_tbc_estimate_ = nh_.advertise("vislam/tbc",1); pub_vislam_rbc_estimate_x_ = nh_.advertise("vislam/rbc_x", 1); @@ -311,12 +310,6 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose } pub_vislam_odometry_.publish(odom_msg); - // Publish pose with covariance (for mavros) - geometry_msgs::PoseWithCovarianceStamped pose_cov_msg; - pose_cov_msg.header = odom_msg.header; - pose_cov_msg.pose = odom_msg.pose; - pub_vislam_pose_cov_.publish(pose_cov_msg); - // compute transforms std::vector transforms; geometry_msgs::TransformStamped transform; diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index 4bd3332..0732faa 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -103,7 +103,6 @@ class Snapdragon::RosNode::Vislam std::atomic vislam_initialized_; ros::NodeHandle nh_; ros::Publisher pub_vislam_pose_; - ros::Publisher pub_vislam_pose_cov_; ros::Publisher pub_vislam_odometry_; ros::Publisher pub_vislam_tbc_estimate_; ros::Publisher pub_vislam_rbc_estimate_x_; From cbc375d6c562f8f7a53f962f8d4b3938ca9c134c Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 22 Dec 2017 17:08:48 +0100 Subject: [PATCH 50/62] rotate pose to ENU for mavros TODO find nicer solution --- src/nodes/SnapdragonRosNodeVislam.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 078eaa4..ab812ae 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -308,6 +308,28 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose odom_msg.pose.covariance[ i*6 + j ] = vislamPose.errCovPose[i][j]; } } + + //rotate to ENU mavros standard TODO find nicer solution + odom_msg.pose.pose.position.x = -pose_msg.pose.position.y; + odom_msg.pose.pose.position.y = pose_msg.pose.position.x; + odom_msg.twist.twist.linear.x = -vislamPose.velocity[1]; + odom_msg.twist.twist.linear.y = vislamPose.velocity[0]; + odom_msg.twist.twist.angular.x = -vislamPose.angularVelocity[1]; + odom_msg.twist.twist.angular.y = vislamPose.angularVelocity[0]; + //switch the first two rows of the covariance matrix + for (int16_t i = 0; i < 6; i++) + { + float64_t temp = odom_msg.pose.covariance[i]; //save value of upper row + odom_msg.pose.covariance[i] = odom_msg.pose.covariance[6 + i]; + odom_msg.pose.covariance[6 + i] = temp; + } + float64_t temp = odom_msg.pose.covariance[0]; + odom_msg.pose.covariance[0] = odom_msg.pose.covariance[1]; + odom_msg.pose.covariance[1] = temp; + temp = odom_msg.pose.covariance[7]; + odom_msg.pose.covariance[7] = odom_msg.pose.covariance[8]; + odom_msg.pose.covariance[8] = temp; + pub_vislam_odometry_.publish(odom_msg); // compute transforms From ffc616eeae677375d10b993e8e6ace6085813890 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 5 Jan 2018 11:59:24 +0100 Subject: [PATCH 51/62] update mainapp.conf for ekf2 --- px4_configs/ekf2/mainapp.conf | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index 3dbe80f..c4ecf20 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -7,20 +7,20 @@ param set SDLOG_PRIO_BOOST 3 param set EKF2_AID_MASK 25 # EKF2L Switch from barometer to vision as main source for height estimation param set EKF2_HGT_MODE 3 -param set EKF2_EVP_NOISE 0.01 -param set EKF2_EVA_NOISE 0.01 +param set EKF2_EV_GATE 15 dataman start navigator start -mavlink start -u 14550 -r 1000000 +mavlink start -u 14556 -r 1000000 sleep 1 -mavlink stream -u 14550 -s HIGHRES_IMU -r 50 -mavlink stream -u 14550 -s ATTITUDE -r 50 -mavlink stream -u 14550 -s RC_CHANNELS -r 20 -mavlink stream -u 14550 -s LOCAL_POSITION_NED -r 10 -mavlink stream -u 14550 -s VISION_POSITION_ESTIMATE -r 10 +mavlink stream -u 14556 -s HIGHRES_IMU -r 10 +mavlink stream -u 14556 -s ATTITUDE -r 10 +mavlink stream -u 14556 -s RC_CHANNELS -r 10 +mavlink stream -u 14556 -s LOCAL_POSITION_NED -r 10 +mavlink stream -u 14556 -s VISION_POSITION_ESTIMATE -r 10 # # mavlink instance for vislam # mavlink start -u 14555 -r 100000 -m magic -mavlink stream -u 14555 -s HIGHRES_IMU -r 250 +sleep 1 +mavlink stream -u 14555 -s SCALED_IMU -r 250 mavlink boot_complete From 6994ff71200585b4895f6617a94e651ba69ab60a Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 5 Jan 2018 12:00:09 +0100 Subject: [PATCH 52/62] remove ekf2 px4.config -> use default PX4/Firmware --- px4_configs/ekf2/px4.config | 72 ------------------------------------- 1 file changed, 72 deletions(-) delete mode 100644 px4_configs/ekf2/px4.config diff --git a/px4_configs/ekf2/px4.config b/px4_configs/ekf2/px4.config deleted file mode 100644 index 9163fe3..0000000 --- a/px4_configs/ekf2/px4.config +++ /dev/null @@ -1,72 +0,0 @@ -uorb start -qshell start -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -param set ATT_W_MAG 0.00 -param set SENS_BOARD_ROT 0 -param set RC_RECEIVER_TYPE 1 -param set UART_ESC_MODEL 2 -param set UART_ESC_BAUDRTE 250000 -param set UART_ESC_MOTOR1 4 -param set UART_ESC_MOTOR2 2 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 -param set SYS_MC_EST_GROUP 2 -sleep 1 -df_mpu9250_wrapper start -df_bmp280_wrapper start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_pos_control start -mc_att_control start -uart_esc start -D /dev/tty-2 -rc_receiver start -D /dev/tty-1 -sleep 1 -list_devices -list_files -list_tasks -list_topics From 1974893942896947f509d02e6150e9c8d9effac0 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 8 Jan 2018 11:14:52 +0100 Subject: [PATCH 53/62] use front-left-up output and add static transforms for mavros --- src/nodes/SnapdragonRosNodeVislam.cpp | 47 +++++++++++++++------------ 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index ab812ae..b53c5e3 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) @@ -52,6 +53,30 @@ Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) vislam_initialized_ = false; thread_started_ = false; thread_stop_ = false; + + // send static transform from "vislam" (front-left-up) to NED that Mavros needs + tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster; + geometry_msgs::TransformStamped static_transformStamped_1; + geometry_msgs::TransformStamped static_transformStamped_2; + + static_transformStamped_1.header.stamp = ros::Time::now(); + static_transformStamped_1.header.frame_id = "vislam"; + static_transformStamped_1.child_frame_id = "local_origin_ned"; + static_transformStamped_2.header.stamp = ros::Time::now(); + static_transformStamped_2.header.frame_id = "vislam"; + static_transformStamped_2.child_frame_id = "fcu_frd"; + + tf2::Quaternion quat; + quat.setRPY(M_PI, 0, 0); + static_transformStamped_1.transform.rotation.x = quat.x(); + static_transformStamped_1.transform.rotation.y = quat.y(); + static_transformStamped_1.transform.rotation.z = quat.z(); + static_transformStamped_1.transform.rotation.w = quat.w(); + static_transformStamped_2.transform = static_transformStamped_1.transform; + + tf2_static_broadcaster.sendTransform(static_transformStamped_1); + tf2_static_broadcaster.sendTransform(static_transformStamped_2); + // sleep here so tf buffer can get populated ros::Duration(1).sleep(); // sleep for 1 second } @@ -294,6 +319,7 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose nav_msgs::Odometry odom_msg; odom_msg.header.stamp = frame_time; odom_msg.header.frame_id = "vislam"; + odom_msg.child_frame_id = "vislam"; odom_msg.pose.pose = pose_msg.pose; odom_msg.twist.twist.linear.x = vislamPose.velocity[0]; odom_msg.twist.twist.linear.y = vislamPose.velocity[1]; @@ -309,27 +335,6 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose } } - //rotate to ENU mavros standard TODO find nicer solution - odom_msg.pose.pose.position.x = -pose_msg.pose.position.y; - odom_msg.pose.pose.position.y = pose_msg.pose.position.x; - odom_msg.twist.twist.linear.x = -vislamPose.velocity[1]; - odom_msg.twist.twist.linear.y = vislamPose.velocity[0]; - odom_msg.twist.twist.angular.x = -vislamPose.angularVelocity[1]; - odom_msg.twist.twist.angular.y = vislamPose.angularVelocity[0]; - //switch the first two rows of the covariance matrix - for (int16_t i = 0; i < 6; i++) - { - float64_t temp = odom_msg.pose.covariance[i]; //save value of upper row - odom_msg.pose.covariance[i] = odom_msg.pose.covariance[6 + i]; - odom_msg.pose.covariance[6 + i] = temp; - } - float64_t temp = odom_msg.pose.covariance[0]; - odom_msg.pose.covariance[0] = odom_msg.pose.covariance[1]; - odom_msg.pose.covariance[1] = temp; - temp = odom_msg.pose.covariance[7]; - odom_msg.pose.covariance[7] = odom_msg.pose.covariance[8]; - odom_msg.pose.covariance[8] = temp; - pub_vislam_odometry_.publish(odom_msg); // compute transforms From f0115fec70e037bfdb1081d6fb9f52873438abf1 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 8 Jan 2018 11:44:02 +0100 Subject: [PATCH 54/62] replace static transform in code with node in launch file --- launch/mavros_vislam.launch | 3 +++ src/nodes/SnapdragonRosNodeVislam.cpp | 25 ------------------------- 2 files changed, 3 insertions(+), 25 deletions(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 56a9ef8..27b717f 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -18,6 +18,9 @@ + + + diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index b53c5e3..9e2f132 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) @@ -53,30 +52,6 @@ Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) vislam_initialized_ = false; thread_started_ = false; thread_stop_ = false; - - // send static transform from "vislam" (front-left-up) to NED that Mavros needs - tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster; - geometry_msgs::TransformStamped static_transformStamped_1; - geometry_msgs::TransformStamped static_transformStamped_2; - - static_transformStamped_1.header.stamp = ros::Time::now(); - static_transformStamped_1.header.frame_id = "vislam"; - static_transformStamped_1.child_frame_id = "local_origin_ned"; - static_transformStamped_2.header.stamp = ros::Time::now(); - static_transformStamped_2.header.frame_id = "vislam"; - static_transformStamped_2.child_frame_id = "fcu_frd"; - - tf2::Quaternion quat; - quat.setRPY(M_PI, 0, 0); - static_transformStamped_1.transform.rotation.x = quat.x(); - static_transformStamped_1.transform.rotation.y = quat.y(); - static_transformStamped_1.transform.rotation.z = quat.z(); - static_transformStamped_1.transform.rotation.w = quat.w(); - static_transformStamped_2.transform = static_transformStamped_1.transform; - - tf2_static_broadcaster.sendTransform(static_transformStamped_1); - tf2_static_broadcaster.sendTransform(static_transformStamped_2); - // sleep here so tf buffer can get populated ros::Duration(1).sleep(); // sleep for 1 second } From 2e3c1ac3b2cc373b4df1a25a05e5956831d58d50 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 8 Jan 2018 17:03:25 +0100 Subject: [PATCH 55/62] update readme --- README.md | 41 +++++++---------------------------------- 1 file changed, 7 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index 51c7706..27e4b39 100644 --- a/README.md +++ b/README.md @@ -15,19 +15,14 @@ This example code is for MV SDK release [mv_1.0.2](https://developer.qualcomm.co On a freshly flashed snapdragon, I had to install the following packages * python pip * python future (`pip install future`) -* `sudo apt-get install ros-indigo-diagnostic-updater` -* `sudo apt-get install ros-indigo-angles` -* `sudo apt-get install ros-indigo-eigen-conversions` -* `sudo apt-get install ros-indigo-urdf` -* `sudo apt-get install ros-indigo-tf` -* `sudo apt-get install ros-indigo-control-toolbox` +* `sudo apt-get install ros-indigo-diagnostic-updater ros-indigo-angles ros-indigo-eigen-conversions ros-indigo-urdf ros-indigo-tf ros-indigo-control-toolbox` If `control-toolbox` cannot be installed due to unmet dependencies, you might have to install the missing dependency yourself. In my case overwriting an existing file was necessary for the package `fontconfig-config`. That can be done with `sudo apt-get -o Dpkg::Options::="--force-overwrite" install fontconfig-config` ## Build `catkin build` -Compiling mavros can easily take 30 minutes on the snapdragon. +Compiling mavros can easily take 30 minutes on the snapdragon. You might have to create a swap file when building catkin. Check [this website](https://www.digitalocean.com/community/tutorials/how-to-add-swap-space-on-ubuntu-16-04) on how to do that. ## High-Level Block Diagram ![SnapVislamRosNodeBlockDiagram](images/SnapVislamRosNodeBlockDiagram.jpg) @@ -127,43 +122,21 @@ echo "source /home/linaro/ros_ws/devel/setup.bash" >> /home/linaro/.bashrc This ensures that the ROS workspace is setup correctly. #### Clone the sample code -* The repo may be cloned from [here]( from https://github.com/ATLFlight/ros-examples.git) directly on the target, or cloned on the host computer and then pushed to the target using ADB. The recommended method is to clone directly on the target. - -``` -adb shell -source /home/linaro/.bashrc -roscd -cd ../src -git clone -b mv-release-1.0.2 https://github.com/ATLFlight/ros-examples.git snap_ros_examples -``` +* The repo may be cloned directly on the target (catkin workspace), or cloned on the host computer and then pushed to the target using ADB. The recommended method is to clone directly on the target. * Build the code ## Configure -Copy the px4 configuration files from `./px4_configs` to their respective location on the snapdragon. For using the recommended LPE: +Copy the px4 configuration files from `./px4_configs` to their respective location on the snapdragon. For using the recommended EKF2: ``` cd snapdragon_mavros_vislam -adb push ./px4_confs/lpe/mainapp.conf /home/linaro/mainapp.conf -adb push ./px4_confs/lpe/px4.config /usr/share/data/adsp/px4.config -``` - -and if you want to give EKF2 a try: -``` -cd snapdragon_mavros_vislam -adb push ./px4_confs/ekf2/mainapp.conf /home/linaro/mainapp.conf -adb push ./px4_confs/ekf2/px4.config /usr/share/data/adsp/px4.config +adb push ./px4_confs/ekf2/mainapp.conf /home/linaro/mainapp_vislam.conf ``` The configurations are intended to provide a working starting point. Adjust parameters to your likings. ## Run -Get a roscore up and running -``` -adb shell -roscore -``` - -In another terminal, launch mavros and vislam +In a terminal, launch mavros and vislam ``` adb shell roslaunch snapdragon_mavros_vislam mavros_vislam.launch @@ -173,5 +146,5 @@ Start the px4 flight stack ``` adb shell cd $HOME -./px4 mainapp.config +./px4 mainapp_vislam.config ``` From af9433713d3d72c0711747213ce960c4568122ca Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 15 Jan 2018 13:45:15 +0100 Subject: [PATCH 56/62] mainapp.config: wait for muorb to start before setting params --- px4_configs/ekf2/mainapp.conf | 2 ++ px4_configs/lpe/mainapp.conf | 2 ++ 2 files changed, 4 insertions(+) diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index c4ecf20..e2222a2 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -1,6 +1,8 @@ uorb start muorb start logger start -t -b 200 +# Wait 1s before setting parameters for muorb to initialize +sleep 1 param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 # EKF2: fuse external vision pose estimate diff --git a/px4_configs/lpe/mainapp.conf b/px4_configs/lpe/mainapp.conf index 02e4a29..daeba39 100644 --- a/px4_configs/lpe/mainapp.conf +++ b/px4_configs/lpe/mainapp.conf @@ -1,6 +1,8 @@ uorb start muorb start logger start -t -b 200 +# Wait 1s before setting parameters for muorb to initialize +sleep 1 param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 dataman start From f34ab3e067d2adcb8a3dea7126f998a70dab6492 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 15 Jan 2018 13:46:29 +0100 Subject: [PATCH 57/62] provide num tracked features instead of bool if updated --- src/vislam/SnapdragonVislamManager.cpp | 9 ++++----- src/vislam/SnapdragonVislamManager.hpp | 9 ++++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 9944ef8..8814756 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -75,7 +75,7 @@ void Snapdragon::VislamManager::ImuCallback( // Parse IMU message float lin_acc[3], ang_vel[3]; - + // Convert ENU to NED coordinates lin_acc[0] = msg->linear_acceleration.x; lin_acc[1] = msg->linear_acceleration.y; @@ -221,10 +221,9 @@ int32_t Snapdragon::VislamManager::GetPointCloud( mvVISLAMMapPoint* points, uint return mvVISLAM_GetPointCloud( vislam_ptr_, points, max_points ); } -bool Snapdragon::VislamManager::HasUpdatedPointCloud() { - std::lock_guard lock( sync_mutex_); - int32_t mv_ret = mvVISLAM_HasUpdatedPointCloud( vislam_ptr_ ); - return (mv_ret != 0 ); +int32_t Snapdragon::VislamManager::HasUpdatedPointCloud() { + std::lock_guard lock( sync_mutex_ ); + return mvVISLAM_HasUpdatedPointCloud(vislam_ptr_); } int32_t Snapdragon::VislamManager::GetPose( mvVISLAMPose& pose, int64_t& pose_frame_id, uint64_t& timestamp_ns ) { diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index a993b2e..db8d993 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -114,12 +114,11 @@ class Snapdragon::VislamManager{ /** * This is wrapper for the MVSDK's vislam API. Provides information - * if there is an updated PointCloud. - * @return bool - * true = if it has updated point cloud - * false = otherwise + * on how many features have been updated in the PointCloud. + * @return int32_t + * # features being tracked **/ - bool HasUpdatedPointCloud(); + int32_t HasUpdatedPointCloud(); /** * MV SDK's wrapper to get the Pose Information. From 76ccf03262359c6e411edda746e885db391e437f Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 15 Jan 2018 13:47:20 +0100 Subject: [PATCH 58/62] mark odom_msg invalid by providing large covariance --- src/nodes/SnapdragonRosNodeVislam.cpp | 16 +++++++++++++--- src/nodes/SnapdragonRosNodeVislam.hpp | 2 +- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index 9e2f132..7d0bb0c 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -41,6 +41,8 @@ #include #include +#define MIN_NUM_FEATURES 20 + Snapdragon::RosNode::Vislam::Vislam( ros::NodeHandle nh ) : nh_(nh) { pub_vislam_pose_ = nh_.advertise("vislam/pose",1); @@ -190,12 +192,16 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { int32_t vislam_ret; while( !thread_stop_ ) { vislam_ret = vislam_man.GetPose( vislamPose, vislamFrameId, timestamp_ns ); + + // get the total number of tracked points + int32_t num_tracked_points = vislam_man.HasUpdatedPointCloud(); + if( vislam_ret == 0 ) { //check if the pose quality is good. If not do not publish the data. if( vislamPose.poseQuality != MV_TRACKING_STATE_FAILED && vislamPose.poseQuality != MV_TRACKING_STATE_INITIALIZING ) { // Publish Pose Data - PublishVislamData( vislamPose, vislamFrameId, timestamp_ns ); + PublishVislamData( vislamPose, vislamFrameId, timestamp_ns, num_tracked_points ); } // Log changes in tracking state @@ -233,7 +239,8 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { return; } -int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose, int64_t vislamFrameId, uint64_t timestamp_ns ) { +int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose, int64_t vislamFrameId, + uint64_t timestamp_ns, int32_t num_tracked_points ) { geometry_msgs::PoseStamped pose_msg; ros::Time frame_time; frame_time.sec = (int32_t)(timestamp_ns/1000000000UL); @@ -306,7 +313,10 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose //set the error covariance for the pose. for( int16_t i = 0; i < 6; i++ ) { for( int16_t j = 0; j < 6; j++ ) { - odom_msg.pose.covariance[ i*6 + j ] = vislamPose.errCovPose[i][j]; + if (num_tracked_points > MIN_NUM_FEATURES) + odom_msg.pose.covariance[ i*6 + j ] = vislamPose.errCovPose[i][j]; + else + odom_msg.pose.covariance[ i*6 + j ] = 10001.0f; } } diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index 0732faa..cf8fcb6 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -93,7 +93,7 @@ class Snapdragon::RosNode::Vislam private: // class methods int32_t PublishVislamData(mvVISLAMPose& vislamPose, int64_t vislamFrameId, - uint64_t timestamp_ns); + uint64_t timestamp_ns, int32_t num_tracked_points); void ThreadMain(); // data members; From d0955f23d8185f12132ca3a100c1460bb5cae632 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 22 Jan 2018 10:53:27 +0100 Subject: [PATCH 59/62] update px4 configs add mavlink streams that mavros needs (also for avoidance) --- px4_configs/ekf2/mainapp.conf | 8 +++++++- px4_configs/lpe/mainapp.conf | 15 +++++++++------ px4_configs/lpe/px4.config | 2 +- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index e2222a2..c3b6903 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -9,7 +9,10 @@ param set SDLOG_PRIO_BOOST 3 param set EKF2_AID_MASK 25 # EKF2L Switch from barometer to vision as main source for height estimation param set EKF2_HGT_MODE 3 -param set EKF2_EV_GATE 15 +param set EKF2_EVP_NOISE 0.08 +param set EKF2_EVA_NOISE 0.08 +param set EKF2_EV_DELAY 40 +param set EKF2_EV_GATE 20 dataman start navigator start mavlink start -u 14556 -r 1000000 @@ -25,4 +28,7 @@ mavlink stream -u 14556 -s VISION_POSITION_ESTIMATE -r 10 mavlink start -u 14555 -r 100000 -m magic sleep 1 mavlink stream -u 14555 -s SCALED_IMU -r 250 +mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50 +mavlink stream -u 14555 -s ATTITUDE -r 50 +mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50 mavlink boot_complete diff --git a/px4_configs/lpe/mainapp.conf b/px4_configs/lpe/mainapp.conf index daeba39..319bc1b 100644 --- a/px4_configs/lpe/mainapp.conf +++ b/px4_configs/lpe/mainapp.conf @@ -15,8 +15,12 @@ mavlink stream -u 14556 -s RC_CHANNELS -r 20 # # mavlink instance for vislam # -mavlink start -u 14559 -r 100000 -mavlink stream -u 14559 -s HIGHRES_IMU -r 250 +mavlink start -u 14555 -r 100000 -m magic +sleep 1 +mavlink stream -u 14555 -s SCALED_IMU -r 250 +mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50 +mavlink stream -u 14555 -s ATTITUDE -r 50 +mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50 mavlink boot_complete # sleep 1 @@ -25,7 +29,6 @@ sleep 1 ## Use LPE for Vision fusion param set LPE_FUSION 12 param set ATT_EXT_HDG_M 1 -param set LPE_VIS_XY 0.001 -param set LPE_VIS_Z 0.001 -param set LPE_VIS_DELAY 0.00 -local_position_estimator start +param set LPE_VIS_XY 0.005 +param set LPE_VIS_Z 0.005 +param set LPE_VIS_DELAY 0.40 diff --git a/px4_configs/lpe/px4.config b/px4_configs/lpe/px4.config index 2440b92..bdc9b11 100644 --- a/px4_configs/lpe/px4.config +++ b/px4_configs/lpe/px4.config @@ -60,7 +60,7 @@ df_mpu9250_wrapper start df_bmp280_wrapper start sensors start commander start -ekf2 start +local_position_estimator start land_detector start multicopter mc_pos_control start mc_att_control start From 969ecac577113869b7537dc5c070be99ea212708 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Thu, 1 Jan 1970 00:29:25 +0000 Subject: [PATCH 60/62] Divide RAW_IMU by 1000 And some other corrections required for proper snapdragon configuration --- launch/mavros_vislam.launch | 1 + px4_configs/ekf2/mainapp.conf | 4 ++-- src/vislam/SnapdragonVislamManager.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/launch/mavros_vislam.launch b/launch/mavros_vislam.launch index 27b717f..6f4450c 100644 --- a/launch/mavros_vislam.launch +++ b/launch/mavros_vislam.launch @@ -24,6 +24,7 @@ + diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index c3b6903..13a7f8b 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -6,7 +6,7 @@ sleep 1 param set MAV_BROADCAST 1 param set SDLOG_PRIO_BOOST 3 # EKF2: fuse external vision pose estimate -param set EKF2_AID_MASK 25 +param set EKF2_AID_MASK 24 # EKF2L Switch from barometer to vision as main source for height estimation param set EKF2_HGT_MODE 3 param set EKF2_EVP_NOISE 0.08 @@ -27,7 +27,7 @@ mavlink stream -u 14556 -s VISION_POSITION_ESTIMATE -r 10 # mavlink start -u 14555 -r 100000 -m magic sleep 1 -mavlink stream -u 14555 -s SCALED_IMU -r 250 +mavlink stream -u 14555 -s RAW_IMU -r 250 mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50 mavlink stream -u 14555 -s ATTITUDE -r 50 mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50 diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 8814756..8c67059 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -76,10 +76,10 @@ void Snapdragon::VislamManager::ImuCallback( // Parse IMU message float lin_acc[3], ang_vel[3]; - // Convert ENU to NED coordinates - lin_acc[0] = msg->linear_acceleration.x; - lin_acc[1] = msg->linear_acceleration.y; - lin_acc[2] = msg->linear_acceleration.z; + // Convert RAW IMU to correct scale + lin_acc[0] = msg->linear_acceleration.x/1000.0; + lin_acc[1] = msg->linear_acceleration.y/1000.0; + lin_acc[2] = msg->linear_acceleration.z/1000.0; ang_vel[0] = msg->angular_velocity.x; ang_vel[1] = msg->angular_velocity.y; ang_vel[2] = msg->angular_velocity.z; From 9a82a56128f583799408e1d1dc48cd55b599350d Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 25 May 2018 10:47:19 +0200 Subject: [PATCH 61/62] removed scale on accel, new versions of mavros already take it into account Signed-off-by: Roman --- src/vislam/SnapdragonVislamManager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index 8c67059..ae4a45b 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -77,9 +77,9 @@ void Snapdragon::VislamManager::ImuCallback( float lin_acc[3], ang_vel[3]; // Convert RAW IMU to correct scale - lin_acc[0] = msg->linear_acceleration.x/1000.0; - lin_acc[1] = msg->linear_acceleration.y/1000.0; - lin_acc[2] = msg->linear_acceleration.z/1000.0; + lin_acc[0] = msg->linear_acceleration.x; + lin_acc[1] = msg->linear_acceleration.y; + lin_acc[2] = msg->linear_acceleration.z; ang_vel[0] = msg->angular_velocity.x; ang_vel[1] = msg->angular_velocity.y; ang_vel[2] = msg->angular_velocity.z; From 8f3ae0b08ccbc1814c87223566091a6087332f9d Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 28 May 2018 09:53:42 +0200 Subject: [PATCH 62/62] mainapp.config: replace RAW_IMU with SCALED_IMU which is used in PX4/master Signed-off-by: Roman --- px4_configs/ekf2/mainapp.conf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf index 13a7f8b..ee80acc 100644 --- a/px4_configs/ekf2/mainapp.conf +++ b/px4_configs/ekf2/mainapp.conf @@ -27,7 +27,7 @@ mavlink stream -u 14556 -s VISION_POSITION_ESTIMATE -r 10 # mavlink start -u 14555 -r 100000 -m magic sleep 1 -mavlink stream -u 14555 -s RAW_IMU -r 250 +mavlink stream -u 14555 -s SCALED_IMU -r 250 mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50 mavlink stream -u 14555 -s ATTITUDE -r 50 mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50