diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..d6fcb07 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,22 @@ +# 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] +### Changed + +## 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) + +## 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`. diff --git a/CMakeLists.txt b/CMakeLists.txt index a02481e..9bae9a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(snap_ros_examples) +project(snapdragon_mavros_vislam) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -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 ) @@ -149,14 +148,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/README.md b/README.md index 591e484..27e4b39 100644 --- a/README.md +++ b/README.md @@ -1,30 +1,28 @@ # 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). 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_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. +**NOTE**: Step 3 should also include `--rosdistro kinetic` flag. -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) +### 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 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. 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) @@ -36,11 +34,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 | -|----|----|----| -|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 | +| 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 | 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 +50,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: @@ -70,26 +68,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) - -**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 +77,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 +93,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 @@ -142,179 +122,29 @@ 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 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 EKF2: ``` -adb shell -source /home/linaro/.bashrc -roscd -cd .. -catkin_make snap_vislam_node -``` - -**NOTE**: To clean the code, remove the "build" folder - -``` -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_vislam.conf ``` -## 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 +In a terminal, launch mavros and vislam ``` adb shell -source /home/linaro/.bashrc -roscore +roslaunch snapdragon_mavros_vislam mavros_vislam.launch ``` -### 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. - +Start the px4 flight stack ``` adb shell -imu_app -s 2 -``` - -To verify that it works, run the following command in a different shell instance (optional): - +cd $HOME +./px4 mainapp_vislam.config ``` -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. - -``` -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. - -``` -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 - -``` - 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/launch/mavros_vislam.launch b/launch/mavros_vislam.launch new file mode 100644 index 0000000..6f4450c --- /dev/null +++ b/launch/mavros_vislam.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 2f420e0..0b1be92 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,15 @@ - snap_ros_examples + snapdragon_mavros_vislam 0.1.1 - This package provides example code as ros nodes for select features of snapdragon flight platfrom(TM) + + Fork of the Qualcomm Vislam example code from ATLFlight (https://github.com/ATLFlight/ros-examples) modified for use with the px4 flight stack. + - rkintada + potaito @@ -47,6 +49,7 @@ roscpp rospy std_msgs + mavros diff --git a/px4_configs/ekf2/mainapp.conf b/px4_configs/ekf2/mainapp.conf new file mode 100644 index 0000000..ee80acc --- /dev/null +++ b/px4_configs/ekf2/mainapp.conf @@ -0,0 +1,34 @@ +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 +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 +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 +sleep 1 +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 +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 new file mode 100644 index 0000000..319bc1b --- /dev/null +++ b/px4_configs/lpe/mainapp.conf @@ -0,0 +1,34 @@ +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 +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 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 +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.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 new file mode 100644 index 0000000..bdc9b11 --- /dev/null +++ b/px4_configs/lpe/px4.config @@ -0,0 +1,73 @@ +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 +param set SYS_MC_EST_GROUP 1 +sleep 1 +df_mpu9250_wrapper start +df_bmp280_wrapper start +sensors start +commander start +local_position_estimator 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 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/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 diff --git a/src/nodes/SnapdragonRosNodeVislam.cpp b/src/nodes/SnapdragonRosNodeVislam.cpp index bfc9f8b..7d0bb0c 100644 --- a/src/nodes/SnapdragonRosNodeVislam.cpp +++ b/src/nodes/SnapdragonRosNodeVislam.cpp @@ -35,15 +35,22 @@ #include #include #include +#include #include #include #include #include +#define MIN_NUM_FEATURES 20 + 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; @@ -57,7 +64,7 @@ Snapdragon::RosNode::Vislam::~Vislam() } int32_t Snapdragon::RosNode::Vislam::Initialize() -{ +{ vislam_initialized_ = true; return 0; } @@ -110,13 +117,16 @@ void Snapdragon::RosNode::Vislam::ThreadMain() { Snapdragon::VislamManager::InitParams vislamParams; - vislamParams.tbc[0] = 0.005; - vislamParams.tbc[1] = 0.0150; + // use the translation that the algorithm converges to + vislamParams.tbc[0] = -0.006; + vislamParams.tbc[1] = -0.020; 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 (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; @@ -143,7 +153,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; @@ -153,15 +163,15 @@ 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; - - param.mv_cpa_config = cpaConfig; - Snapdragon::VislamManager vislam_man; + 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(nh_); if( vislam_man.Initialize( param, vislamParams ) != 0 ) { ROS_WARN_STREAM( "Snapdragon::RosNodeVislam::VislamThreadMain: Error initializing the VISLAM Manager " ); thread_started_ = false; @@ -182,13 +192,41 @@ 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 && + 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 + if (previous_mv_tracking_state_ != vislamPose.poseQuality) + { + 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; + + 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" ); @@ -201,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); @@ -220,7 +259,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]; @@ -232,10 +271,37 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose 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; 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]; @@ -247,10 +313,14 @@ 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; } } - pub_vislam_odometry_.publish(odom_msg); + + pub_vislam_odometry_.publish(odom_msg); // compute transforms std::vector transforms; @@ -271,6 +341,5 @@ int32_t Snapdragon::RosNode::Vislam::PublishVislamData( mvVISLAMPose& vislamPose // broadcast transforms static tf2_ros::TransformBroadcaster br; - br.sendTransform(transforms); + br.sendTransform(transforms); } - diff --git a/src/nodes/SnapdragonRosNodeVislam.hpp b/src/nodes/SnapdragonRosNodeVislam.hpp index 88f3523..cf8fcb6 100644 --- a/src/nodes/SnapdragonRosNodeVislam.hpp +++ b/src/nodes/SnapdragonRosNodeVislam.hpp @@ -89,18 +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, int32_t num_tracked_points); 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; }; diff --git a/src/vislam/SnapdragonVislamManager.cpp b/src/vislam/SnapdragonVislamManager.cpp index e840a67..ae4a45b 100644 --- a/src/vislam/SnapdragonVislamManager.cpp +++ b/src/vislam/SnapdragonVislamManager.cpp @@ -32,9 +32,8 @@ #include "SnapdragonVislamManager.hpp" #include "SnapdragonDebugPrint.h" -Snapdragon::VislamManager::VislamManager() { +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; @@ -45,15 +44,74 @@ Snapdragon::VislamManager::~VislamManager() { CleanUp(); } -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; +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; + 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; + + ROS_INFO_STREAM_THROTTLE(1, "IMU timestamp [ns]: \t" << last_timestamp); + + // Parse IMU message + float lin_acc[3], ang_vel[3]; + + // Convert RAW IMU to correct scale + 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; + + // 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() { //stop the camera. if( cam_man_ptr_ != nullptr ) { WARN_PRINT( "Stopping Camera...." ); @@ -67,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 ) { @@ -95,22 +153,11 @@ 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 ( - &(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, @@ -139,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. @@ -150,6 +196,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" ); @@ -160,6 +210,9 @@ int32_t Snapdragon::VislamManager::Start() { int32_t Snapdragon::VislamManager::Stop() { CleanUp(); + + // Unsubscribe from IMU topic + imu_sub_.shutdown(); return 0; } @@ -168,72 +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::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::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 ) { @@ -244,7 +234,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; @@ -263,13 +253,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" << modified_timestamp); } } return rc; diff --git a/src/vislam/SnapdragonVislamManager.hpp b/src/vislam/SnapdragonVislamManager.hpp index 4bc9765..db8d993 100644 --- a/src/vislam/SnapdragonVislamManager.hpp +++ b/src/vislam/SnapdragonVislamManager.hpp @@ -30,12 +30,16 @@ * ****************************************************************************/ #pragma once -#include +#include +#include // std::atomic +#include // clock_gettime +#include +#include +#include +#include #include "SnapdragonCameraTypes.hpp" #include "mvVISLAM.h" #include "SnapdragonCameraManager.hpp" -#include "SnapdragonImuManager.hpp" -#include namespace Snapdragon { class VislamManager; @@ -44,12 +48,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 { @@ -76,25 +80,25 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { /** * Constructor **/ - VislamManager(); + VislamManager(ros::NodeHandle nh); /** * 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 +106,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. **/ @@ -110,12 +114,11 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { /** * 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. @@ -131,7 +134,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 @@ -147,37 +150,31 @@ class Snapdragon::VislamManager : public Snapdragon::Imu_IEventListener { * @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 ); - /** * Destructor */ virtual ~VislamManager(); private: - // utility methods + void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg); + + // 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_; mvVISLAM* vislam_ptr_; std::mutex sync_mutex_; uint8_t* image_buffer_; size_t image_buffer_size_bytes_; + + ros::NodeHandle nh_; + ros::Subscriber imu_sub_; }; 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;