forked from patrickpoirier51/ROVIO
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ROVIO_MAVROS
215 lines (132 loc) · 7.84 KB
/
ROVIO_MAVROS
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
roscore
roslaunch mpu6050_serial_to_imu demo.launch
roslaunch ueye_cam cam0_ext_400.launch
roslaunch rovio rovio_ueye.launch
--OR--
roslaunch rovio rovio_full.launch
MAVROS LAUNCH
roslaunch mavros apm.launch fcu_url:=udp://0.0.0.0:14551@
EDIT /opt/ros/kinetic/share/mavros/launch/apm_config.yaml
timesync_rate: 0.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
MAVROS INCREASE RATE!
rosrun mavros mavsys rate --all 100
rosrun mavros mavsys rate --raw-sensors 100
Within launch
<node pkg="mavros" type="mavsys" name="mavsys" args="--wait rate --all 100" />
ARDUPILOT LAUNCH
sudo ./arducopter -C udp:192.168.8.34:14550 -D udp:10.0.0.2:14551
==================================================
----------CONFIG----------------
I have the following parameters on my quad:
AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
EK2_GPS_TYPE 3
COMPASS_USE 0
VISO_TYPE 0
------GENERATED MESSAGES----------
https://github.com/ethz-asl/rovio/blob/master/include/rovio/RovioNode.hpp
// Advertise topics
pubTransform_ = nh_.advertise<geometry_msgs::TransformStamped>("rovio/transform", 1);
pubOdometry_ = nh_.advertise<nav_msgs::Odometry>("rovio/odometry", 1);
pubPoseWithCovStamped_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("rovio/pose_with_covariance_stamped", 1);
pubPcl_ = nh_.advertise<sensor_msgs::PointCloud2>("rovio/pcl", 1);
pubPatch_ = nh_.advertise<sensor_msgs::PointCloud2>("rovio/patch", 1);
pubMarkers_ = nh_.advertise<visualization_msgs::Marker>("rovio/markers", 1 );
---MAVROS ----
https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/vision_pose_estimate.cpp#L155
void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req)
{
Eigen::Affine3d tr;
tf::poseMsgToEigen(req->pose.pose, tr);
send_vision_estimate(req->header.stamp, tr, req->pose.covariance);
}
-----VISION_POSITION_ESTIMATE ( #102 )---------
https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE
VISION_POSITION_ESTIMATE ( #102 )
Global position/attitude estimate from a vision source.
Field Name Type Units Description
usec uint64_t us Timestamp (UNIX time or time since system boot)
x float m Global X position
y float m Global Y position
z float m Global Z position
roll float rad Roll angle
pitch float rad Pitch angle
yaw float rad Yaw angle
covariance ** float[21] Pose covariance matrix upper right triangular (first six entries are the first ROW, next five entries are the second ROW, etc.)
.......................looks like a remap to me..................
Lets try
<remap from="rovio/pose_with_covariance_stamped" to="/mavros/vision_pose/pose_cov"/>
<!-- launch ROVIO -->
<node pkg="rovio" type="rovio_node" name="rovio" output="screen">
<param name="filter_config" value="$(find rovio)/cfg/rovio.info"/>
<param name="camera0_config" value="$(find rovio)/cfg/ueye_cam400_fisheye.yaml"/>
<remap from="rovio/pose_with_covariance_stamped" to="/mavros/vision_pose/pose_cov"/>
</node>
YEP !!!!!!!!! WORKS !!!!
======ArduPilot ==== A little History==========
Add ability to navigate using MAVLink vision messages & AP_NavEKF2: Enable fusion of external nav position data
https://github.com/ArduPilot/ardupilot/pull/7980/files
-------
https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/GCS_Common.cpp#L2766
void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t usec,
const float x,
const float y,
const float z,
const float roll,
const float pitch,
const float yaw,
const uint16_t payload_size)
AP_NavEKF2: add external navigation data lag
https://github.com/ArduPilot/ardupilot/pull/9658
================================
Rovio Parameters
You can set many parameters within rovio.info. A short description is provided for every parameter. Here are the mose important parameters:
Common - doVECalibration: should the IMU-Camera extrinsics be co-estimated online.
Common - vebose: enables the verbose
CameraX - CalibrationFile: path to the calibration file of camera X
CameraX - qCM: quaternion representing the orientation of camera X with respect to the IMU coordinate frame
CameraX - MrMC: position of camera X with respect to the IMU coordinate frame
Init - State - ***: initial values for the rovio filter state
Init - Covariance - ***: covariance of the initial values of the rovio filter state
ImgUpdate - doFrameVisualisation false; Should the frames be visualized
ImgUpdate - visualizePatches false; Should the patches be visualized
ImgUpdate - useDirectMethod: should the EKF use the photometric error (true) or the repojection error (false)
ImgUpdate - startLevel: highest pyramid level employed for the photometric error, must be smaller than ROVIO_NLEVELS
ImgUpdate - endLevel: lowest pyramid level employed for the photometric error
ImgUpdate - UpdateNoise - nor: covariance used for the repojection error (if not using the photometric error)
ImgUpdate - UpdateNoise - int: covariance used for the photometric error
ImgUpdate - initCovFeature_0: initial covariance of the distance parameter
ImgUpdate - initDepth: initial distance parameter guess of a feature
ImgUpdate - penaltyDistance: increase to 100 for outdoor environment to avoid clustering of features on horizon
Prediction - PredictionNoise - ***: prediction noise employed by EKF, should be adapted to the IMU specification
PoseUpdate - ***: parameters used if additional external pose measurements are provided, e.g. can be used for fusing GPS measurements
=============================================
https://github.com/PX4/Devguide/blob/master/en/ros/external_position_estimation.md#relaying_pose_data_to_px4
Visual Inertial Odometry (VIO) systems allow vehicles to navigate when a global position source is unavailable or unreliable (e.g. indoors, or when flying under a bridge. etc.). VIOdetermine a vehicle's pose (position and attitude) from "visual" information using onboard sensors to get pose data from the vehicle's perspective.
Pose data can be used to update a PX4-based autopilot's local position estimate (relative to the local origin) and also can optionally also be fused into the vehicle attitude estimation.
Getting Pose Data Into ROS
MAVROS has plugins to relay a visual estimation from a VIO or MoCap system using the following pipelines:
ROS MAVLink
/mavros/vision_pose/pose VISION_POSITION_ESTIMATE
If you're working with EKF2, only the "vision" pipelines are supported. To use MoCap data with EKF2 you will have to remap the pose topic that you get from MoCap:
MoCap ROS topics of type geometry_msgs/PoseStamped or geometry_msgs/PoseWithCovarianceStamped must be remapped to /mavros/vision_pose/pose. The geometry_msgs/PoseStamped topic is most common as MoCap doesn't usually have associated covariances to the data.
=======KABIR'S EPIPHANY=====
https://github.com/PX4/Firmware/pull/6074
=====ETHZ DIY DRONE ====
https://github.com/ethz-asl/mav_dji_ros_interface/wiki
This repository presents a complete visual-inertial (VI-) odometry-aided MAV platform that makes use of off-the-shelf elements such as the device itself, a computer, and VI-sensor.
====KABIR SERVICE CALLER FOR RESET==
https://github.com/ethz-asl/rovio/issues/41
RQT Service Caller PLUGIN
Call /ROVIO/RESET
Made a Python scrit : rovio_reset.py
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty
rospy.init_node('rovio_reset')
rospy.wait_for_service('/rovio/reset')
rovio_reset = rospy.ServiceProxy('/rovio/reset', Empty)
rovio_reset()
===============================