Compare commits
48 Commits
v0.16-alph
...
nti2019
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
c8bcede60a | ||
|
|
1d591965a3 | ||
|
|
c589235984 | ||
|
|
8bb3f751ca | ||
|
|
d4ab87ad9e | ||
|
|
7519958698 | ||
|
|
f90c1a6329 | ||
|
|
c63e4265d6 | ||
|
|
60c97d2318 | ||
|
|
be7624b309 | ||
|
|
e9e8c84ddf | ||
|
|
6535943cc8 | ||
|
|
375b19146c | ||
|
|
77602021ae | ||
|
|
8dec500702 | ||
|
|
0df66a8df7 | ||
|
|
ae9302bfc2 | ||
|
|
993cc50276 | ||
|
|
09a8f702a7 | ||
|
|
bcefb03f04 | ||
|
|
bc1ceb2fa0 | ||
|
|
06bf2d5b56 | ||
|
|
db03222a19 | ||
|
|
fea6992964 | ||
|
|
67ddfa6c5e | ||
|
|
9e77a11cf5 | ||
|
|
928d2e38d4 | ||
|
|
eb9b621662 | ||
|
|
dfd6736fb0 | ||
|
|
08ea466232 | ||
|
|
07b6dcde51 | ||
|
|
66aa4729ad | ||
|
|
bb825c3c30 | ||
|
|
32635def32 | ||
|
|
0342e7da39 | ||
|
|
995a1395de | ||
|
|
99f207d0f6 | ||
|
|
29c401e5fa | ||
|
|
b3c0e2d290 | ||
|
|
d053571053 | ||
|
|
e59a0221ca | ||
|
|
b53bf19c8d | ||
|
|
b6c493513c | ||
|
|
24bf9f8907 | ||
|
|
3338d42a77 | ||
|
|
27e890825d | ||
|
|
68f810cd1a | ||
|
|
0c872a101f |
@@ -1,5 +1,4 @@
|
|||||||
iOS-приложение для управления Клевером
|
# iOS-приложение для управления Клевером
|
||||||
--------------------------------------
|
|
||||||
|
|
||||||
Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
|
Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
|
||||||
|
|
||||||
@@ -8,3 +7,11 @@ pod install
|
|||||||
```
|
```
|
||||||
|
|
||||||
Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
|
Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
|
||||||
|
|
||||||
|
## Политика конфиденциальности
|
||||||
|
|
||||||
|
App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
|
||||||
|
|
||||||
|
## Privacy policy
|
||||||
|
|
||||||
|
The App Store app CLEVER RC does not collect and store any personal user data.
|
||||||
|
|||||||
6
aruco_pose/map/nti_novgorod.txt
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
34 0.33 0 0 0 0 0 0
|
||||||
|
37 0.33 0.74 0 0 0 0 0
|
||||||
|
25 0.33 0 0.54 0 0 0 0
|
||||||
|
28 0.33 0.74 0.54 0 0 0 0
|
||||||
|
32 0.33 2.79 -0.15 1.20 0 0 0
|
||||||
|
29 0.33 2.46 0.65 1.20 0 0 0
|
||||||
@@ -102,6 +102,42 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Draw a (potentially partially visible) line. */
|
||||||
|
static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
|
||||||
|
int thickness = 1, int lineType = LINE_8, int shift = 0)
|
||||||
|
{
|
||||||
|
// If both points are behind the screen, don't draw anything
|
||||||
|
if (p1.z <= 0 && p2.z <= 0)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
Point2f p1p{p1.x, p1.y};
|
||||||
|
Point2f p2p{p2.x, p2.y};
|
||||||
|
// If points are on the different sides of the plane, compute intersection point
|
||||||
|
if (p1.z * p2.z < 0)
|
||||||
|
{
|
||||||
|
// Compute intersection point with the screen
|
||||||
|
// We denote alpha as such:
|
||||||
|
// xi = (1 - alpha) * x1 + alpha * x2
|
||||||
|
// yi = (1 - alpha) * y1 + alpha * y2
|
||||||
|
// zi = (1 - alpha) * z1 + alpha * z2 = 0
|
||||||
|
// Thus, alpha can be expressed as
|
||||||
|
// alpha = z1 / (z1 - z2)
|
||||||
|
float alpha = p1.z / (p1.z - p2.z);
|
||||||
|
Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
|
||||||
|
// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
|
||||||
|
if (p1.z < 0)
|
||||||
|
{
|
||||||
|
p1p = pi;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
p2p = pi;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
line(image, p1p, p2p, color, thickness, lineType, shift);
|
||||||
|
}
|
||||||
|
|
||||||
void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
|
void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||||
InputArray _rvec, InputArray _tvec, float length) {
|
InputArray _rvec, InputArray _tvec, float length) {
|
||||||
|
|
||||||
@@ -118,26 +154,10 @@ void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _di
|
|||||||
std::vector< Point3f > imagePointsZ;
|
std::vector< Point3f > imagePointsZ;
|
||||||
_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
|
_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
|
||||||
|
|
||||||
if (imagePointsZ[0].z < 0 ||
|
|
||||||
imagePointsZ[1].z < 0 ||
|
|
||||||
imagePointsZ[2].z < 0 ||
|
|
||||||
imagePointsZ[3].z < 0)
|
|
||||||
{
|
|
||||||
// Any axis point is behind screen plane -> don't draw anything
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Intersect axis lines with screen plane (they may be outside)
|
|
||||||
std::vector<Point2f> imagePoints(4);
|
|
||||||
imagePoints[0] = Point2f{imagePointsZ[0].x, imagePointsZ[0].y};
|
|
||||||
imagePoints[1] = Point2f{imagePointsZ[1].x, imagePointsZ[1].y};
|
|
||||||
imagePoints[2] = Point2f{imagePointsZ[2].x, imagePointsZ[2].y};
|
|
||||||
imagePoints[3] = Point2f{imagePointsZ[3].x, imagePointsZ[3].y};
|
|
||||||
|
|
||||||
// draw axis lines
|
// draw axis lines
|
||||||
line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3);
|
linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
|
||||||
line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3);
|
linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
|
||||||
line(_image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), 3);
|
linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
static CvMat _cvMat(const cv::Mat& m)
|
static CvMat _cvMat(const cv::Mat& m)
|
||||||
|
|||||||
@@ -7,7 +7,8 @@ After=roscore.service
|
|||||||
User=pi
|
User=pi
|
||||||
EnvironmentFile=/lib/systemd/system/roscore.env
|
EnvironmentFile=/lib/systemd/system/roscore.env
|
||||||
ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
|
ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
|
||||||
Restart=on-abort
|
Restart=on-failure
|
||||||
|
RestartSec=3
|
||||||
|
|
||||||
[Install]
|
[Install]
|
||||||
WantedBy=multi-user.target
|
WantedBy=multi-user.target
|
||||||
|
|||||||
35
builder/assets/clever_rename
Executable file
@@ -0,0 +1,35 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
|
||||||
|
# Set Clever hostname to the specified value
|
||||||
|
|
||||||
|
set -e
|
||||||
|
|
||||||
|
NEW_NAME_OPT=$1
|
||||||
|
|
||||||
|
if [[ -z ${NEW_NAME_OPT} ]]; then
|
||||||
|
echo "Please specify new name for this Clever"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
NEW_NAME=$(echo ${NEW_NAME_OPT} | tr '[:upper:]' '[:lower:]')
|
||||||
|
|
||||||
|
echo "Setting name to ${NEW_NAME}"
|
||||||
|
|
||||||
|
echo "Backing up /etc/hostname"
|
||||||
|
cp /etc/hostname /etc/hostname.bak
|
||||||
|
echo "Writing new /etc/hostname"
|
||||||
|
echo ${NEW_NAME} > /etc/hostname
|
||||||
|
|
||||||
|
echo "Backing up /etc/hosts"
|
||||||
|
cp /etc/hosts /etc/hosts.bak
|
||||||
|
echo "Rewriting /etc/hosts with new values"
|
||||||
|
sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_NAME}'/g' /etc/hosts
|
||||||
|
|
||||||
|
echo "Changing hostname in /lib/systemd/system/roscore.env"
|
||||||
|
sed -i 's/ROS_HOSTNAME=.*/ROS_HOSTNAME='${NEW_NAME}'.local/g' /lib/systemd/system/roscore.env
|
||||||
|
|
||||||
|
echo "Changing hostname in .bashrc"
|
||||||
|
sed -i 's/export ROS_HOSTNAME=.*/export ROS_HOSTNAME='${NEW_NAME}'.local/g' /home/pi/.bashrc
|
||||||
|
|
||||||
|
echo "Done, reboot your Clever to see the results"
|
||||||
|
|
||||||
@@ -32,7 +32,9 @@ echo_stamp() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
echo_stamp "Rename SSID"
|
echo_stamp "Rename SSID"
|
||||||
sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
|
NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
|
||||||
|
sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
|
||||||
|
clever_rename ${NEW_SSID}
|
||||||
|
|
||||||
echo_stamp "Harware setup"
|
echo_stamp "Harware setup"
|
||||||
/root/hardware_setup.sh
|
/root/hardware_setup.sh
|
||||||
|
|||||||
8
builder/assets/ros_python_paths
Normal file
@@ -0,0 +1,8 @@
|
|||||||
|
Defaults env_keep += "PYTHONPATH"
|
||||||
|
Defaults env_keep += "PATH"
|
||||||
|
Defaults env_keep += "ROS_ROOT"
|
||||||
|
Defaults env_keep += "ROS_MASTER_URI"
|
||||||
|
Defaults env_keep += "ROS_PACKAGE_PATH"
|
||||||
|
Defaults env_keep += "ROS_LOCATIONS"
|
||||||
|
Defaults env_keep += "ROS_HOME"
|
||||||
|
Defaults env_keep += "ROS_LOG_DIR"
|
||||||
@@ -6,7 +6,8 @@ After=network.target
|
|||||||
User=pi
|
User=pi
|
||||||
EnvironmentFile=/lib/systemd/system/roscore.env
|
EnvironmentFile=/lib/systemd/system/roscore.env
|
||||||
ExecStart=/opt/ros/kinetic/bin/roscore
|
ExecStart=/opt/ros/kinetic/bin/roscore
|
||||||
Restart=on-abort
|
Restart=on-failure
|
||||||
|
RestartSec=3
|
||||||
|
|
||||||
[Install]
|
[Install]
|
||||||
WantedBy=multi-user.target
|
WantedBy=multi-user.target
|
||||||
|
|||||||
14
builder/assets/rosled.service
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
[Unit]
|
||||||
|
Description=ROS ws281x support
|
||||||
|
Requires=roscore.service
|
||||||
|
After=roscore.service
|
||||||
|
|
||||||
|
[Service]
|
||||||
|
EnvironmentFile=/lib/systemd/system/roscore.env
|
||||||
|
ExecStart=/opt/ros/kinetic/bin/roslaunch ros_ws281x clever4.launch --wait --screen
|
||||||
|
Restart=on-failure
|
||||||
|
RestartSec=3
|
||||||
|
TimeoutSec=infinity
|
||||||
|
|
||||||
|
[Install]
|
||||||
|
WantedBy=multi-user.target
|
||||||
@@ -11,7 +11,7 @@
|
|||||||
|
|
||||||
set -e # Exit immidiately on non-zero result
|
set -e # Exit immidiately on non-zero result
|
||||||
|
|
||||||
SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-06-29/2018-06-27-raspbian-stretch-lite.zip"
|
SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-11-15/2018-11-13-raspbian-stretch-lite.zip"
|
||||||
|
|
||||||
export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
|
export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
|
||||||
export LANG=${LANG:='C.UTF-8'}
|
export LANG=${LANG:='C.UTF-8'}
|
||||||
@@ -108,9 +108,13 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.
|
|||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
|
||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
|
||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
|
||||||
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
|
||||||
# ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
|
# ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
|
||||||
# Add PX4 udev rules
|
# Add PX4 udev rules
|
||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
|
||||||
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/rosled.service' '/lib/systemd/system/'
|
||||||
|
# Add rename script
|
||||||
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever_rename' '/usr/local/bin/clever_rename'
|
||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
|
||||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
|
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
|
||||||
|
|
||||||
|
|||||||
@@ -137,6 +137,10 @@ fi
|
|||||||
|
|
||||||
export ROS_IP='127.0.0.1' # needed for running tests
|
export ROS_IP='127.0.0.1' # needed for running tests
|
||||||
|
|
||||||
|
echo_stamp "Adding ros_ws281x ROS package"
|
||||||
|
cd /home/pi/catkin_ws/src
|
||||||
|
git clone https://github.com/sfalexrog/ros_ws281x
|
||||||
|
|
||||||
echo_stamp "Installing CLEVER" \
|
echo_stamp "Installing CLEVER" \
|
||||||
&& cd /home/pi/catkin_ws/src/clever \
|
&& cd /home/pi/catkin_ws/src/clever \
|
||||||
&& git status \
|
&& git status \
|
||||||
@@ -145,7 +149,7 @@ echo_stamp "Installing CLEVER" \
|
|||||||
&& my_travis_retry pip install wheel \
|
&& my_travis_retry pip install wheel \
|
||||||
&& my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
|
&& my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
|
||||||
&& source /opt/ros/kinetic/setup.bash \
|
&& source /opt/ros/kinetic/setup.bash \
|
||||||
&& catkin_make -j2 -DCMAKE_BUILD_TYPE=Release \
|
&& catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DROS_WS2811_REAL_LIB=ON \
|
||||||
&& catkin_make run_tests \
|
&& catkin_make run_tests \
|
||||||
&& catkin_test_results \
|
&& catkin_test_results \
|
||||||
&& systemctl enable roscore \
|
&& systemctl enable roscore \
|
||||||
@@ -153,6 +157,9 @@ echo_stamp "Installing CLEVER" \
|
|||||||
&& echo_stamp "All CLEVER was installed!" "SUCCESS" \
|
&& echo_stamp "All CLEVER was installed!" "SUCCESS" \
|
||||||
|| (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
|
|| (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
|
||||||
|
|
||||||
|
echo_stamp "Enabling ROS LED service"
|
||||||
|
systemctl enable rosled
|
||||||
|
|
||||||
echo_stamp "Build CLEVER documentation"
|
echo_stamp "Build CLEVER documentation"
|
||||||
cd /home/pi/catkin_ws/src/clever
|
cd /home/pi/catkin_ws/src/clever
|
||||||
NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
|
NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
|
||||||
|
|||||||
@@ -91,7 +91,7 @@ tcpdump \
|
|||||||
ltrace \
|
ltrace \
|
||||||
libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
|
libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
|
||||||
python-rosdep \
|
python-rosdep \
|
||||||
python-rosinstall-generator=0.1.14-1 \
|
python-rosinstall-generator \
|
||||||
python-wstool=0.1.17-1 \
|
python-wstool=0.1.17-1 \
|
||||||
python-rosinstall=0.7.8-1 \
|
python-rosinstall=0.7.8-1 \
|
||||||
build-essential=12.3 \
|
build-essential=12.3 \
|
||||||
@@ -105,6 +105,9 @@ python3-dev \
|
|||||||
&& echo_stamp "Everything was installed!" "SUCCESS" \
|
&& echo_stamp "Everything was installed!" "SUCCESS" \
|
||||||
|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
|
|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
|
||||||
|
|
||||||
|
echo_stamp "Updating kernel to fix camera bug"
|
||||||
|
apt-get install --no-install-recommends -y raspberrypi-kernel=1.20190215-1
|
||||||
|
|
||||||
# Deny byobu to check available updates
|
# Deny byobu to check available updates
|
||||||
sed -i "s/updates_available//" /usr/share/byobu/status/status
|
sed -i "s/updates_available//" /usr/share/byobu/status/status
|
||||||
# sed -i "s/updates_available//" /home/pi/.byobu/status
|
# sed -i "s/updates_available//" /home/pi/.byobu/status
|
||||||
@@ -113,6 +116,7 @@ echo_stamp "Installing pip"
|
|||||||
curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
|
curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
|
||||||
python3 get-pip.py
|
python3 get-pip.py
|
||||||
python get-pip.py
|
python get-pip.py
|
||||||
|
rm get-pip.py
|
||||||
#my_travis_retry pip install --upgrade pip
|
#my_travis_retry pip install --upgrade pip
|
||||||
#my_travis_retry pip3 install --upgrade pip
|
#my_travis_retry pip3 install --upgrade pip
|
||||||
|
|
||||||
@@ -135,6 +139,9 @@ mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
|
|||||||
mv /root/monkey /etc/monkey/sites/default
|
mv /root/monkey /etc/monkey/sites/default
|
||||||
systemctl enable monkey.service
|
systemctl enable monkey.service
|
||||||
|
|
||||||
|
echo_stamp "Install paho-mqtt"
|
||||||
|
my_travis_retry pip install paho-mqtt
|
||||||
|
|
||||||
echo_stamp "Install Node.js"
|
echo_stamp "Install Node.js"
|
||||||
cd /home/pi
|
cd /home/pi
|
||||||
wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
|
wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
|
||||||
|
|||||||
@@ -46,3 +46,4 @@ rosversion rosserial
|
|||||||
rosversion usb_cam
|
rosversion usb_cam
|
||||||
rosversion cv_camera
|
rosversion cv_camera
|
||||||
rosversion web_video_server
|
rosversion web_video_server
|
||||||
|
rosversion ros_ws281x
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
<launch>
|
<launch>
|
||||||
<arg name="aruco_detect" default="true"/>
|
<arg name="aruco_detect" default="true"/>
|
||||||
<arg name="aruco_map" default="false"/>
|
<arg name="aruco_map" default="true"/>
|
||||||
<arg name="aruco_vpe" default="false"/>
|
<arg name="aruco_vpe" default="true"/>
|
||||||
|
|
||||||
<!-- For additional help go to https://clever.copterexpress.com/aruco.html -->
|
<!-- For additional help go to https://clever.copterexpress.com/aruco.html -->
|
||||||
|
|
||||||
|
|||||||
@@ -5,10 +5,10 @@
|
|||||||
<arg name="web_video_server" default="true"/>
|
<arg name="web_video_server" default="true"/>
|
||||||
<arg name="rosbridge" default="true"/>
|
<arg name="rosbridge" default="true"/>
|
||||||
<arg name="main_camera" default="true"/>
|
<arg name="main_camera" default="true"/>
|
||||||
<arg name="optical_flow" default="false"/>
|
<arg name="optical_flow" default="true"/>
|
||||||
<arg name="aruco" default="false"/>
|
<arg name="aruco" default="true"/>
|
||||||
<arg name="rc" default="true"/>
|
<arg name="rc" default="false"/>
|
||||||
<arg name="rangefinder_vl53l1x" default="false"/>
|
<arg name="rangefinder_vl53l1x" default="true"/>
|
||||||
<arg name="arduino" default="false"/>
|
<arg name="arduino" default="false"/>
|
||||||
|
|
||||||
<!-- mavros -->
|
<!-- mavros -->
|
||||||
|
|||||||
4
clever/launch/main_camera.launch
Executable file → Normal file
@@ -5,10 +5,10 @@
|
|||||||
<!-- article about camera setup: https://clever.copterexpress.com/camera_frame.html -->
|
<!-- article about camera setup: https://clever.copterexpress.com/camera_frame.html -->
|
||||||
|
|
||||||
<!-- camera is oriented downward, camera cable goes backward [option 1] -->
|
<!-- camera is oriented downward, camera cable goes backward [option 1] -->
|
||||||
<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
|
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
|
||||||
|
|
||||||
<!-- camera is oriented downward, camera cable goes forward [option 2] -->
|
<!-- camera is oriented downward, camera cable goes forward [option 2] -->
|
||||||
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>-->
|
<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="-0.04 0 -0.08 1.5707963 0 3.1415926 base_link main_camera_optical"/>
|
||||||
|
|
||||||
<!-- camera is oriented upward, camera cable goes backward [option 3] -->
|
<!-- camera is oriented upward, camera cable goes backward [option 3] -->
|
||||||
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
|
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
|
||||||
|
|||||||
@@ -44,6 +44,8 @@ def check(name):
|
|||||||
for f in failures:
|
for f in failures:
|
||||||
rospy.logwarn('%s: %s', name, f)
|
rospy.logwarn('%s: %s', name, f)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
|
for f in failures:
|
||||||
|
rospy.logwarn('%s: %s', name, f)
|
||||||
traceback.print_exc()
|
traceback.print_exc()
|
||||||
rospy.logwarn('%s: exception occurred', name)
|
rospy.logwarn('%s: exception occurred', name)
|
||||||
return
|
return
|
||||||
@@ -59,7 +61,7 @@ param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
|
|||||||
def get_param(name):
|
def get_param(name):
|
||||||
res = param_get(param_id=name)
|
res = param_get(param_id=name)
|
||||||
if not res.success:
|
if not res.success:
|
||||||
failure('Unable to retrieve PX4 parameter%s', name)
|
failure('Unable to retrieve PX4 parameter %s', name)
|
||||||
else:
|
else:
|
||||||
if res.value.integer != 0:
|
if res.value.integer != 0:
|
||||||
return res.value.integer
|
return res.value.integer
|
||||||
|
|||||||
@@ -528,12 +528,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
|||||||
nav_speed = speed;
|
nav_speed = speed;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
||||||
if (std::isnan(yaw) && yaw_rate == 0) {
|
// if (std::isnan(yaw) && yaw_rate == 0) {
|
||||||
// keep yaw unchanged
|
// // keep yaw unchanged
|
||||||
yaw = tf2::getYaw(local_position.pose.orientation);
|
// // TODO: this is incorrect, because we need yaw in desired frame
|
||||||
}
|
// yaw = tf2::getYaw(local_position.pose.orientation);
|
||||||
}
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||||
// destination point and/or yaw
|
// destination point and/or yaw
|
||||||
|
|||||||
@@ -116,7 +116,7 @@ int main(int argc, char **argv) {
|
|||||||
nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
|
nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
|
||||||
nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
|
nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
|
||||||
nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
|
nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
|
||||||
offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 5.0));
|
offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
|
||||||
|
|
||||||
if (!frame_id.empty()) {
|
if (!frame_id.empty()) {
|
||||||
ROS_INFO("vpe_publisher: using data from TF");
|
ROS_INFO("vpe_publisher: using data from TF");
|
||||||
|
|||||||
BIN
docs/assets/px4flow_alignment.jpg
Normal file
|
After Width: | Height: | Size: 111 KiB |
BIN
docs/assets/px4flow_bottom.jpg
Normal file
|
After Width: | Height: | Size: 40 KiB |
BIN
docs/assets/px4flow_pixhawk_connection.jpg
Normal file
|
After Width: | Height: | Size: 62 KiB |
BIN
docs/assets/px4flow_pixracer_connection.jpg
Normal file
|
After Width: | Height: | Size: 69 KiB |
BIN
docs/assets/px4flow_qgc_calibration.png
Normal file
|
After Width: | Height: | Size: 205 KiB |
BIN
docs/assets/px4flow_qgc_camera_feed.png
Normal file
|
After Width: | Height: | Size: 62 KiB |
BIN
docs/assets/px4flow_qgc_connected.png
Normal file
|
After Width: | Height: | Size: 57 KiB |
BIN
docs/assets/px4flow_qgc_firmware.png
Normal file
|
After Width: | Height: | Size: 90 KiB |
BIN
docs/assets/px4flow_qgc_video_only_param.png
Normal file
|
After Width: | Height: | Size: 74 KiB |
BIN
docs/assets/px4flow_top.jpg
Normal file
|
After Width: | Height: | Size: 83 KiB |
@@ -5,7 +5,7 @@
|
|||||||
|
|
||||||
«Клевер» — это учебный конструктор программируемого квадрокоптера, состоящего из популярных открытых компонентов, а также набор необходимой документации и библиотек для работы с ним.
|
«Клевер» — это учебный конструктор программируемого квадрокоптера, состоящего из популярных открытых компонентов, а также набор необходимой документации и библиотек для работы с ним.
|
||||||
|
|
||||||
Набор включает в себя полетный контроллер Pixhawk/Pixracer с полетным стеком PX4, Raspberry Pi 3 в качестве управлящего бортового компьютера, модуль камеры для реализации полетов с использованием компьютерного зрения, а также набор различных датчиков и другой периферии.
|
Набор включает в себя полетный контроллер Pixhawk/Pixracer с полетным стеком PX4, Raspberry Pi 3 в качестве управляющего бортового компьютера, модуль камеры для реализации полетов с использованием компьютерного зрения, а также набор различных датчиков и другой периферии.
|
||||||
|
|
||||||
На базе точно такой же платформы были созданы многие «большие» проекты компании Copter Express, например, дроны для [пиар-акций по автономной доставке пиццы](https://www.youtube.com/watch?v=hmkAoZOtF58) (Самара, Казань); дрон-доставщик кофе в Сколково, мониторинговый дрон с зарядной станцией, дроны-победители на полевых испытаниях «[Робокросс-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU)», «[Робокросс-2017](https://youtu.be/AQnd2CRczbQ)» и многие другие.
|
На базе точно такой же платформы были созданы многие «большие» проекты компании Copter Express, например, дроны для [пиар-акций по автономной доставке пиццы](https://www.youtube.com/watch?v=hmkAoZOtF58) (Самара, Казань); дрон-доставщик кофе в Сколково, мониторинговый дрон с зарядной станцией, дроны-победители на полевых испытаниях «[Робокросс-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU)», «[Робокросс-2017](https://youtu.be/AQnd2CRczbQ)» и многие другие.
|
||||||
|
|
||||||
@@ -15,6 +15,8 @@
|
|||||||
|
|
||||||
Также у нас есть чат для программистов, которые разрабатывают под PX4, автономную навигацию в помещениях и рои дронов https://t.me/DroneCode.
|
Также у нас есть чат для программистов, которые разрабатывают под PX4, автономную навигацию в помещениях и рои дронов https://t.me/DroneCode.
|
||||||
|
|
||||||
|
The English version of this documentation [is available](../en/).
|
||||||
|
|
||||||
Образ для Raspberry Pi
|
Образ для Raspberry Pi
|
||||||
----------------------
|
----------------------
|
||||||
|
|
||||||
|
|||||||
@@ -36,7 +36,10 @@
|
|||||||
* [ROS](ros.md)
|
* [ROS](ros.md)
|
||||||
* [MAVROS](mavros.md)
|
* [MAVROS](mavros.md)
|
||||||
* [Автономный полет в OFFBOARD](simple_offboard.md)
|
* [Автономный полет в OFFBOARD](simple_offboard.md)
|
||||||
* [Навигация по ArUco-маркерам](aruco.md)
|
* Визуальные маркеры (ArUco)
|
||||||
|
* [Общая информация](aruco.md)
|
||||||
|
* [Распознавание маркеров](aruco_marker.md)
|
||||||
|
* [Распознавание карт маркеров](aruco_map.md)
|
||||||
* [Навигация по Optical Flow](optical_flow.md)
|
* [Навигация по Optical Flow](optical_flow.md)
|
||||||
* [Автоматическая проверка](selfcheck.md)
|
* [Автоматическая проверка](selfcheck.md)
|
||||||
* [Примеры кода](snippets.md)
|
* [Примеры кода](snippets.md)
|
||||||
@@ -59,12 +62,14 @@
|
|||||||
* [CopterHack-2018](copterhack2018.md)
|
* [CopterHack-2018](copterhack2018.md)
|
||||||
* [CopterHack-2017](copterhack2017.md)
|
* [CopterHack-2017](copterhack2017.md)
|
||||||
* Дополнительные материалы
|
* Дополнительные материалы
|
||||||
|
* [Олимпиада НТИ 2019](nti2019.md)
|
||||||
* [Вклад в Клевер](contributing.md)
|
* [Вклад в Клевер](contributing.md)
|
||||||
* [Прошивка ESC контроллеров](esc_firmware.md)
|
* [Прошивка ESC контроллеров](esc_firmware.md)
|
||||||
* [Протокол MAVLink](mavlink.md)
|
* [Протокол MAVLink](mavlink.md)
|
||||||
* [Работа с логами PX4](flight_logs.md)
|
* [Работа с логами PX4](flight_logs.md)
|
||||||
* [Калибровка камеры](calibration.md)
|
* [Калибровка камеры](calibration.md)
|
||||||
* [Работа с ИК датчиками](ir_sensors.md)
|
* [Работа с ИК датчиками](ir_sensors.md)
|
||||||
|
* [Подключение PX4FLOW](px4flow.md)
|
||||||
* Учебник
|
* Учебник
|
||||||
* [Теория и видеоуроки](lessons.md)
|
* [Теория и видеоуроки](lessons.md)
|
||||||
* [Учебно-методическое пособие](metod.md)
|
* [Учебно-методическое пособие](metod.md)
|
||||||
|
|||||||
165
docs/ru/aruco.md
@@ -1,172 +1,23 @@
|
|||||||
# Навигация с использованием ArUco-маркеров
|
# ArUco-маркеры
|
||||||
|
|
||||||
> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/aruco.md).
|
> **Note** Документация для версий [образа](microsd_images.md), начиная с версии **0.16**. Для более ранних версий см. [документацию для версии **0.15.1**](https://github.com/CopterExpress/clever/blob/v0.15.1/docs/ru/aruco.md).
|
||||||
|
|
||||||
[ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования
|
[ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования
|
||||||
робототехнических систем с использованием компьютерного зрения.
|
робототехнических систем с использованием компьютерного зрения.
|
||||||
|
|
||||||
Пример ArUco-маркеров:
|
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
> **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
|
> **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
|
||||||
|
|
||||||
Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.
|
Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.
|
||||||
|
|
||||||
## aruco\_pose
|
На [образе Клевера для RPi](microsd_images.md) предустановлен пакет `aruco_pose`, предназначенный для работы с ArUco-маркерами.
|
||||||
|
|
||||||
Модуль `aruco_pose` позволяет восстанавливать позицию коптера относительно карты ArUco-маркеров и сообщать ее полетному контролеру, используя механизм [Vision Position Estimation](https://dev.px4.io/en/ros/external_position_estimation.html).
|
## Режимы работы
|
||||||
|
|
||||||
При наличии источника положения коптера по маркерам появляется возможность производить точную автономную indoor-навигацию по позициям при помощи модуля [simple\_offboard](simple_offboard.md).
|
Клевер имеет несколько преднастроенных режимов работы с ArUco-маркерами:
|
||||||
|
|
||||||
### Включение
|
* [распознавание и навигация по отдельным маркерам](aruco_marker.md);
|
||||||
|
* [распознавание и навигация по картам маркеров](aruco_map.md).
|
||||||
|
|
||||||
Необходимо убедиться, что в launch-файле Клевера \(`~/catkin_ws/src/clever/clever/launch/clever.launch`\) включен запуск aruco\_pose и [камеры для компьютерного зрения](camera.md):
|
> **Info** Исчерпывающую документацию по пакету `aruco_pose` на английском языке можно посмотреть [на GitHub](https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md).
|
||||||
|
|
||||||
```xml
|
|
||||||
<arg name="main_camera" default="true"/>
|
|
||||||
```
|
|
||||||
|
|
||||||
```xml
|
|
||||||
<arg name="aruco" default="true"/>
|
|
||||||
```
|
|
||||||
|
|
||||||
При изменении launch-файла необходимо перезапустить пакет `clever`:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
sudo systemctl restart clever
|
|
||||||
```
|
|
||||||
|
|
||||||
### Настройка карты ArUco-меток
|
|
||||||
|
|
||||||
В качестве карты меток можно использовать автоматически сгенерированный [ArUco-board](https://docs.opencv.org/trunk/db/da9/tutorial_aruco_board_detection.html).
|
|
||||||
|
|
||||||
Настройка карты меток производится с помощью файла `~/catkin_ws/src/clever/clever/launch/aruco.launch`. Для использования ArUco-board введите его параметры:
|
|
||||||
|
|
||||||
```xml
|
|
||||||
<node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager">
|
|
||||||
<param name="frame_id" value="aruco_map_raw"/>
|
|
||||||
<!-- тип маркерного поля -->
|
|
||||||
<param name="type" value="gridboard"/>
|
|
||||||
|
|
||||||
<!-- количество маркеров по x -->
|
|
||||||
<param name="markers_x" value="1"/>
|
|
||||||
|
|
||||||
<!-- количество маркеров по y -->
|
|
||||||
<param name="markers_y" value="6"/>
|
|
||||||
|
|
||||||
<!-- ID маркера первого маркера (левого верхнего) -->
|
|
||||||
<param name="first_marker" value="240"/>
|
|
||||||
|
|
||||||
<!-- длина стороны маркера в метрах -->
|
|
||||||
<param name="markers_side" value="0.3362"/>
|
|
||||||
|
|
||||||
<!-- расстояние между маркерами -->
|
|
||||||
<param name="markers_sep" value="0.46"/>
|
|
||||||
</node>
|
|
||||||
```
|
|
||||||
|
|
||||||
Можно задать отдельно расстояние между маркерами по горизонтали и вертикали:
|
|
||||||
|
|
||||||
```xml
|
|
||||||
<!-- расстояние между маркерами по горизонтали -->
|
|
||||||
<param name="markers_sep_x" value="0.97"/>
|
|
||||||
|
|
||||||
<!-- расстояние между маркерами по вертикали -->
|
|
||||||
<param name="markers_sep_y" value="1.435"/>
|
|
||||||
```
|
|
||||||
|
|
||||||
Если используется карта с нестандартным порядком ID меток, то можно использовать параметр `marker_ids`:
|
|
||||||
|
|
||||||
```xml
|
|
||||||
<rosparam param="marker_ids">[5, 7, 9, 11, 13, 15]</rosparam>
|
|
||||||
```
|
|
||||||
|
|
||||||
Нумерация маркеров ведется с левого верхнего угла поля.
|
|
||||||
|
|
||||||
Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика `aruco_pose/map_image`. Через браузер его можно просмотреть при помощи [web\_video\_server](web_video_server.md) по ссылке [http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/map\_image](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/map_image):
|
|
||||||
|
|
||||||

|
|
||||||
|
|
||||||
При полетах необходимо убедиться, что наклеенные на пол метки соответствуют карте.
|
|
||||||
|
|
||||||
В топике `aruco_pose/debug` \([http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/debug](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/debug)\) доступен текущий результат распознавания меток:
|
|
||||||
|
|
||||||
TODO
|
|
||||||
|
|
||||||
### Система координат
|
|
||||||
|
|
||||||
По [соглашению](http://www.ros.org/reps/rep-0103.html), в маркерном поле используется стандартная система координат ENU:
|
|
||||||
|
|
||||||
* x — вправо \(условный "восток"\);
|
|
||||||
* y — вперед \(условный "север"\);
|
|
||||||
* z — вверх.
|
|
||||||
|
|
||||||
_Примечание_: указанное выше определение приведено для ситуации, когда поле маркеров лежит на полу.
|
|
||||||
|
|
||||||
Таким образом, нулевой является левая нижняя точка маркерного поля. Угол по рысканью считается равным 0, когда коптер смотрит направо \(по оси x\).
|
|
||||||
|
|
||||||

|
|
||||||
|
|
||||||
### Настройка полетного контролера
|
|
||||||
|
|
||||||
Для правильной работы Vision Position Estimation необходимо \(через [QGroundControl](gcs_bridge.md)\) убедиться, что:
|
|
||||||
|
|
||||||
* **Для Pixhawk**: Установлена прошивка с LPE \(local position estimator\). Для Pixhawk необходимо [скачать прошивку `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
|
|
||||||
|
|
||||||
**Для Pixracer**: параметр `SYS_MC_EST_GROUP` должен быть установлен в `local_position_estimator, attitude_estimator_q`.
|
|
||||||
|
|
||||||
> **Note** После изменения значения параметра `SYS_MC_EST_GROUP` необходимо перезагрузить полетный контроллер.
|
|
||||||
* В параметре `LPE_FUSION` включены **только** флажки `vision position`, `land detector`. Итоговое значение _20_.
|
|
||||||
* Выключен компас: `ATT_W_MAG` = 0
|
|
||||||
* Вес угла по рысканью по зрению: `ATT_W_EXT_HDG` = 0.5
|
|
||||||
* Включена ориентация по Yaw по зрению: `ATT_EXT_HDG_M` = 2 `MOCAP`.
|
|
||||||
* Настройки VPE: `LPE_VIS_DELAY` = 0 sec, `LPE_VIS_XY` = 0.1 m, `LPE_VIS_Z` = 0.15 m.
|
|
||||||
* Рекомендуемые настройки land detector'а:
|
|
||||||
* `COM_DISARM_LAND` = 1 s
|
|
||||||
* `LNDMC_ROT_MAX` = 45 deg
|
|
||||||
* `LNDMC_THR_RANGE` = 0.5
|
|
||||||
* `LNDMC_Z_VEL_MAX` = 1 m/s
|
|
||||||
|
|
||||||
<!--
|
|
||||||
Для простоты настройки можно воспользоваться готовым файлом настроек для [Clever 2](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever2LPE_160118.params) или для [Clever 3](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever3_LPE_020218.params) и вгрузить его в контроллер с помощью меню Tools - Load from file из раздела Parameters в QGroundControl.
|
|
||||||
|
|
||||||

|
|
||||||
-->
|
|
||||||
|
|
||||||
### Полет
|
|
||||||
|
|
||||||
При правильной настройке коптер начнет удерживать позицию по VPE \(в [режимах](modes.md) `POSCTL` или `OFFBOARD`\) автоматически.
|
|
||||||
|
|
||||||
Для [автономных полетов](simple_offboard.md) можно будет использовать функции `navigate`, `set_position`, `set_velocity`. Для полета в определенные координаты маркерного поля необходимо использовать фрейм `aruco_map`:
|
|
||||||
|
|
||||||
```python
|
|
||||||
# Вначале необходимо взлететь, чтобы коптер увидел карту меток
|
|
||||||
# и появился фрейм aruco_map:
|
|
||||||
navigate(0, 0, 2, frame_id='body', speed=0.5, auto_arm=True) # взлет на 2 метра
|
|
||||||
|
|
||||||
time.sleep(5)
|
|
||||||
|
|
||||||
# Полет в координату 2:2 маркерного поля, высота 2 метра
|
|
||||||
navigate(2, 2, 2, speed=1, frame_id='aruco_map') # полет в координату 2:2, высота 3 метра
|
|
||||||
```
|
|
||||||
|
|
||||||
См. [другие функции](simple_offboard.md) simple_offboard.
|
|
||||||
|
|
||||||
### Расположение маркеров на потолке
|
|
||||||
|
|
||||||

|
|
||||||
|
|
||||||
Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_frame.md).
|
|
||||||
|
|
||||||
Чтобы задавать карту маркеров в "перевернутой" системе координат, необходимо изменить параметр `aruco_orientation` в файле `~/catkin_ws/src/clever/clever/aruco.launch`:
|
|
||||||
|
|
||||||
```xml
|
|
||||||
<param name="aruco_orientation" value="map_upside_down"/>
|
|
||||||
```
|
|
||||||
|
|
||||||
При задании вышеуказанного параметра фрейм aruco\_map также окажется "перевернутым". Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2:
|
|
||||||
|
|
||||||
```python
|
|
||||||
navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')
|
|
||||||
```
|
|
||||||
|
|||||||
@@ -1,23 +0,0 @@
|
|||||||
# ArUco-маркеры
|
|
||||||
|
|
||||||
> **Note** Документация для версий [образа](microsd_images.md), начиная с версии **0.16**. Для более ранних версий см. [документацию для версии **0.15.1**](https://github.com/CopterExpress/clever/blob/v0.15.1/docs/ru/aruco.md).
|
|
||||||
|
|
||||||
[ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования
|
|
||||||
робототехнических систем с использованием компьютерного зрения.
|
|
||||||
|
|
||||||

|
|
||||||
|
|
||||||
> **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
|
|
||||||
|
|
||||||
Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.
|
|
||||||
|
|
||||||
На [образе Клевера для RPi](microsd_images.md) предустановлен пакет `aruco_pose`, предназначенный для работы с ArUco-маркерами.
|
|
||||||
|
|
||||||
## Режимы работы
|
|
||||||
|
|
||||||
Клевер имеет несколько преднастроенных режимов работы с ArUco-маркерами:
|
|
||||||
|
|
||||||
* [распознавание и навигация по отдельным маркерам](aruco_marker.md);
|
|
||||||
* [распознавание и навигация по картам маркеров](aruco_map.md).
|
|
||||||
|
|
||||||
> **Info** Исчерпывающую документацию по пакету `aruco_pose` на английском языке можно посмотреть [на GitHub](https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md).
|
|
||||||
157
docs/ru/nti2019.md
Normal file
@@ -0,0 +1,157 @@
|
|||||||
|
# Олимпиада НТИ 2019
|
||||||
|
|
||||||
|
## Работа с MQTT
|
||||||
|
|
||||||
|
[MQTT](https://ru.wikipedia.org/wiki/MQTT) – протокол для обмена сообщениями между различными устройствами. Этот протокол используется для отправки команд дрону на Олимпиаде НТИ 2019. Для отправки сообщения оно публикуется в определенный топик; все подписчики этого топика получают это сообщение.
|
||||||
|
|
||||||
|
### Подписка на топики
|
||||||
|
|
||||||
|
В образе Клевера для Олимпиады НТИ 2019 предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже:
|
||||||
|
|
||||||
|
```python
|
||||||
|
import paho.mqtt.client as mqtt # Импортирование библиотеки mqtt
|
||||||
|
|
||||||
|
# Callback, вызываемый при получении от сервера подтверждения о подключении
|
||||||
|
def on_connect(client, userdata, flags, rc):
|
||||||
|
print ("Connected with result code "+str(rc))
|
||||||
|
|
||||||
|
# Если подписываться на топик в on_connect, то при обрыве соединения
|
||||||
|
# и повторном подключении произойдёт автоматическое переподписание
|
||||||
|
client.subscribe("/copters/copter1")
|
||||||
|
|
||||||
|
# Callback, вызываемый при появлении сообщения в одном из топиков, на который
|
||||||
|
# подписан клиент
|
||||||
|
def on_message(client, userdata, msg):
|
||||||
|
# В объекте msg хранится топик, в который пришло сообщение (в поле topic)
|
||||||
|
# и само сообщение (в поле payload)
|
||||||
|
print(msg.topic, str(msg.payload))
|
||||||
|
|
||||||
|
# Инициализация клиента MQTT
|
||||||
|
client = mqtt.Client()
|
||||||
|
# Здесь указываются callback'и, вызываемые при подключении и получении сообщения
|
||||||
|
client.on_connect = on_connect
|
||||||
|
client.on_message = on_message
|
||||||
|
|
||||||
|
# Подключение к MQTT-брокеру. Первый параметр - имя или адрес брокера, второй - порт
|
||||||
|
# (по умолчанию 1883), третий - максимальное время между сообщениями в секундах
|
||||||
|
# (по умолчанию 60).
|
||||||
|
client.connect('192.168.11.162', 1883, 60)
|
||||||
|
|
||||||
|
# Метод loop_start создаёт поток, в котором будет производиться опрос сервера и
|
||||||
|
# вызов callback'ов.
|
||||||
|
client.loop_start()
|
||||||
|
# Далее продолжается ваша программа
|
||||||
|
```
|
||||||
|
|
||||||
|
Более подробная документация доступна на [странице библиотеки в PyPI](https://pypi.org/project/paho-mqtt/).
|
||||||
|
|
||||||
|
### Проверка
|
||||||
|
|
||||||
|
Для проверки вы можете опубликовать любое сообщение в топик с помощью команды `hbmqtt_pub`:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщение'
|
||||||
|
```
|
||||||
|
|
||||||
|
Где `192.168.0.1` – IP-адрес MQTT-брокера, `сообщение` – сообщение для публикации, `/copters/copter1` – необходимый топик для публикации.
|
||||||
|
|
||||||
|
### Работа с MQTT
|
||||||
|
|
||||||
|
В образе Клевера предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже:
|
||||||
|
|
||||||
|
```python
|
||||||
|
import paho.mqtt.client as mqtt # Импортирование библиотеки mqtt
|
||||||
|
|
||||||
|
# Callback, вызываемый при получении от сервера подтверждения о подключении
|
||||||
|
def on_connect(client, userdata, flags, rc):
|
||||||
|
print ("Connected with result code "+str(rc))
|
||||||
|
|
||||||
|
# Если подписываться на топик в on_connect, то при обрыве соединения
|
||||||
|
# и повторном подключении произойдёт автоматическое переподписание
|
||||||
|
client.subscribe("/copters/copter1")
|
||||||
|
|
||||||
|
# Callback, вызываемый при появлении сообщения в одном из топиков, на который
|
||||||
|
# подписан клиент
|
||||||
|
def on_message(client, userdata, msg):
|
||||||
|
# В объекте msg хранится топик, в который пришло сообщение (в поле topic)
|
||||||
|
# и само сообщение (в поле payload)
|
||||||
|
print (msg.topic+" "+str(msg.payload))
|
||||||
|
|
||||||
|
# Инициализация клиента MQTT
|
||||||
|
client = mqtt.Client()
|
||||||
|
# Здесь указываются callback'и, вызываемые при подключении и получении сообщения
|
||||||
|
client.on_connect = on_connect
|
||||||
|
client.on_message = on_message
|
||||||
|
|
||||||
|
# Подключение к MQTT-брокеру. Первый параметр - имя или адрес брокера, второй - порт
|
||||||
|
# (по умолчанию 1883), третий - максимальное время между сообщениями в секундах
|
||||||
|
# (по умолчанию 60).
|
||||||
|
client.connect("192.168.11.162", 1883, 60)
|
||||||
|
|
||||||
|
# Метод loop_start создаёт поток, в котором будет производиться опрос сервера и
|
||||||
|
# вызов callback'ов.
|
||||||
|
client.loop_start()
|
||||||
|
# Далее продолжается ваша программа
|
||||||
|
```
|
||||||
|
|
||||||
|
Более подробная документация доступна на [странице библиотеки в PyPI](https://pypi.org/project/paho-mqtt/).
|
||||||
|
|
||||||
|
## Работа с Клевером
|
||||||
|
|
||||||
|
Для выполнения команд на Клевере:
|
||||||
|
|
||||||
|
* подключитесь в Wi-Fi сети NTI;
|
||||||
|
* подключитесь к вашему Клеверу по SSH по его IP-адресу (подробнее см. [подключение по SSH](ssh.md));
|
||||||
|
|
||||||
|
Для редактирования файлов на Клевере вы можете использовать консольные редакторы `nano` или `vim`. Также вы можете загружать файлы используя PyCharm или WinSCP.
|
||||||
|
|
||||||
|
Для автономного полета используйте API модуля [simple_offboard](simple_offboard.md).
|
||||||
|
|
||||||
|
Пример программы, выполняющей взлет, полет в точку в системе координат площадки и посадку на Python:
|
||||||
|
|
||||||
|
```python
|
||||||
|
# coding: utf8
|
||||||
|
|
||||||
|
import rospy
|
||||||
|
from clever import srv
|
||||||
|
from std_srvs.srv import Trigger
|
||||||
|
|
||||||
|
rospy.init_node('flight')
|
||||||
|
|
||||||
|
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||||
|
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
|
||||||
|
land = rospy.ServiceProxy('land', Trigger)
|
||||||
|
|
||||||
|
# Взлет на 1 метр со скоростью 1 метр в секунду
|
||||||
|
navigate(x=0, y=0, z=1, speed=1, frame_id='body', auto_arm=True)
|
||||||
|
|
||||||
|
# Ждем 5 секунд
|
||||||
|
rospy.sleep(5)
|
||||||
|
|
||||||
|
# Полет на координаты x=3, y=2, z=1 площадки с углом по рысканью 3.14 радиан со скоростью 0.5 метров в секунду
|
||||||
|
navigate(x=3, y=2, z=1, yaw=3.14, speed=0.5, frame_id='aruco_map')
|
||||||
|
|
||||||
|
# Ждем 5 секунд
|
||||||
|
rospy.sleep(5)
|
||||||
|
|
||||||
|
# Посадка
|
||||||
|
land()
|
||||||
|
```
|
||||||
|
|
||||||
|
Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboard.md) и [примеры кода](snippets.md).
|
||||||
|
|
||||||
|
Пример взлета на высоту 1 метр из командной строки:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
|
||||||
|
```
|
||||||
|
|
||||||
|
Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboard.md) и [примеры кода](snippets.md).
|
||||||
|
|
||||||
|
### Работа со светодиодной лентой
|
||||||
|
|
||||||
|
В используемой версии Клевера LED-лента подключена напрямую к Raspberry Pi. При включении всех светодиодов ленты на полную мощность возможно повреждение цепей питания микрокомпьютера.
|
||||||
|
|
||||||
|
Сигнальный провод ленты подключен к GPIO-пину 18.
|
||||||
|
|
||||||
|
Подробнее про работу с LED-лентой можно прочитать [в соответствующей статье](leds.md)
|
||||||
@@ -100,6 +100,8 @@ navigate(x=1.5, frame_id='body')
|
|||||||
* изменить значение параметра `MPC_THR_HOVER`;
|
* изменить значение параметра `MPC_THR_HOVER`;
|
||||||
* выставить `MPC_ALT_MODE` = 2 (Terrain following).
|
* выставить `MPC_ALT_MODE` = 2 (Terrain following).
|
||||||
|
|
||||||
|
При использовании Optical Flow максимальная горизонтальная скорость дополнительно ограничивается. За это косвенно отвечает параметр `SENS_FLOW_MAXR` (максимальная достоверная "угловая скорость" оптического потока). При нормальном полёте горизонтальная скорость будет регулироваться так, чтобы показания Optical Flow не превышали 50% значения данного параметра.
|
||||||
|
|
||||||
## Неисправности
|
## Неисправности
|
||||||
|
|
||||||
При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
|
При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
|
||||||
|
|||||||
98
docs/ru/px4flow.md
Normal file
@@ -0,0 +1,98 @@
|
|||||||
|
# Смарт-камера PX4FLOW
|
||||||
|
|
||||||
|
 
|
||||||
|
|
||||||
|
[PX4FLOW](https://docs.px4.io/en/sensor/px4flow.html) – смарт-камера, реализующая алгоритм вычисления "оптического потока" ([optical flow](https://docs.px4.io/en/sensor/optical_flow.html)) и способная передать полученные данные в полётный контроллер. Optical flow часто применяется для полётов коптера в помещении.
|
||||||
|
|
||||||
|
## Спецификации
|
||||||
|
|
||||||
|
Модуль PX4FLOW содержит:
|
||||||
|
|
||||||
|
* микропроцессор Cortex M4F (168 MHz, 128 + 64 KB RAM);
|
||||||
|
* светочувствительную матрицу MT9V034 (разрешение: 752x480 точек);
|
||||||
|
* трёхосевой гироскоп L3GD20.
|
||||||
|
|
||||||
|
Для светочувствительной матрицы используется объектив 16мм М12 с установленным ИК-фильтром.
|
||||||
|
|
||||||
|
Габариты модуля составляют 45.5x35x25 мм (в зависимости от настройки объектива).
|
||||||
|
|
||||||
|
Модуль использует питание 5 В и потребляет до 115 мА.
|
||||||
|
|
||||||
|
Интерфейсы:
|
||||||
|
|
||||||
|
* USB (разъём microUSB) – используется для программирования и первоначальной настройки;
|
||||||
|
* I2C (разъём Hirose DF13 4 pos) – используется для передачи данных на полётный контроллер;
|
||||||
|
* USART2, USART3 (разъёмы Hirose DF13 6 pos) – могут использоваться для подключения к полётному контроллеру и последовательного соединения нескольких модулей.
|
||||||
|
|
||||||
|
> **Hint** Использование Optical Flow требует наличия дальномера!
|
||||||
|
|
||||||
|
На некоторых моделях PX4FLOW может быть установлен сонар Maxbotix LZ-EZ4. В этом случае его показания будут пересылаться вместе с показаниями камеры.
|
||||||
|
|
||||||
|
## Первоначальная настройка
|
||||||
|
|
||||||
|
Настройка камеры производится с помощью программы [QGroundControl](http://qgroundcontrol.com/). Запустите её и подключите модуль PX4FLOW к компьютеру по USB. Откройте режим `Vehicle setup` (иконка с шестерёнками); окно QGroundControl при этом будет выглядеть примерно так:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
Если это первое включение камеры, то вам надо будет произвести [обновление её прошивки](firmware.md) по аналогии с перепрошивкой полётного контроллера.
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
> **Info** При перепрошивке PX4FLOW следует выбрать прошивку `PX4 Pro` при использовании полётного контроллера на базе `PX4` и `ArduPilot` при использовании полётного контроллера с прошивкой `ArduPilot`.
|
||||||
|
|
||||||
|
## Фокусировка камеры
|
||||||
|
|
||||||
|
Для оптимальной работы модуля PX4FLOW следует настроить фокус его камеры на предполагаемое расстояние от пола, при котором будут осуществляться полёты. Это можно сделать в QGroundControl в режиме `Vehicle setup` во вкладке `PX4Flow`. Там вы сможете увидеть изображение с камеры:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
> **Hint** Для калибровки камеры лучше использовать специальный режим `Video only`. Для его включения зайдите во вкладку `Parameters`, найдите параметр `VIDEO_ONLY` и установите его в значение `1`
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
Фокусировку следует проводить в хорошо освещённом месте, модуль камеры при этом следует закрепить. Для фокусировки рекомендуется использовать книгу или коробку с текстом. Расположите объект, на котором вы будете фокусироваться, на таком расстоянии, на котором вы предполагаете летать над полом.
|
||||||
|
|
||||||
|
> **Info** В режиме калибровки камера будет выдавать изображение большего разрешения, чем при штатной работе, но с меньшей частотой кадров.
|
||||||
|
|
||||||
|
При работе в режиме калибровки рекомендуется развернуть окно QGroundControl на весь экран, чтобы лучше видеть изображение:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
> **Hint** Изображение с камеры будет отражено по вертикальной оси. Это является нормальным поведением и не должно быть поводом для беспокойства.
|
||||||
|
|
||||||
|
Отпустите контргайку на объективе и медленно вращайте его, пока изображение не станет резким. После этого аккуратно закрепите объектив с помощью контргайки.
|
||||||
|
|
||||||
|
> **Info** Остальные параметры в данный момент не сохраняются, поэтому настройка камеры ограничивается её фокусировкой.
|
||||||
|
|
||||||
|
## Установка и подключение
|
||||||
|
|
||||||
|
По умолчанию предполагается, что модуль PX4FLOW будет расположен так, что направление оси `y` будет совпадать со стрелкой на полётном контроллере:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
> **Hint** Поворот модуля PX4FLOW относительно полётного контроллера указывается параметром [`SENS_FLOW_ROT`](https://docs.px4.io/en/advanced_config/parameter_reference.html#SENS_FLOW_ROT) в PX4. Его значение по умолчанию (`Yaw 270°`) соответствует размещению на иллюстрации.
|
||||||
|
|
||||||
|
### Подключение к Pixhawk
|
||||||
|
|
||||||
|
Подключите PX4FLOW с помощью поставляемых кабелей к разъёму I2C на Pixhawk:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
### Подключение к Pixracer
|
||||||
|
|
||||||
|
В полётном контроллере Pixracer используется совмещённый порт [UART/I2C](https://docs.px4.io/en/flight_controller/pixracer.html#pinouts) (помечен как GPS).
|
||||||
|
|
||||||
|
> **Warning** На более ранних версиях PX4FLOW разъём I2C может быть перевёрнут; ориентируйтесь на кабель, идущий в комплекте – на нём красным отмечен провод питания.
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
> **Hint** Для подключения PX4FLOW к Pixracer рекомендуется использовать [GPS/I2C сплиттер](https://store.mrobotics.io/mRo-JST-GH-GPS-Port-to-I-C-Bus-Splitter-p/mro-jstgh-gps-i2c-split-mr.htm)
|
||||||
|
|
||||||
|
## Настройка полётного контроллера
|
||||||
|
|
||||||
|
Для того, чтобы полётный контроллер использовал данные с PX4FLOW, следует установить соответствующие флаги в настройках estimator'а:
|
||||||
|
|
||||||
|
* Для **LPE** (`SYS_MC_EST_GROUP` = `local_position_estimator`): в `LPE_FUSION` включены флажки `fuse optical flow` и `flow gyro compensation`.
|
||||||
|
* Для **EKF2** (`SYS_MC_EST_GROUP` = `ekf2`): в `EKF_AID_MASK` включен флажок `use optical flow`.
|
||||||
|
|
||||||
|
> **Hint** Остальные настройки PX4FLOW описаны в [статье по Optical Flow](optical_flow.md). Значения по умолчанию должны обеспечить оптимальную работу PX4FLOW.
|
||||||
@@ -118,7 +118,7 @@ docker run \
|
|||||||
```
|
```
|
||||||
touch /tmp/.docker.xauth
|
touch /tmp/.docker.xauth
|
||||||
|
|
||||||
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f /tmp/.docker/xauth nmerge -
|
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f /tmp/.docker.xauth nmerge -
|
||||||
|
|
||||||
docker run \
|
docker run \
|
||||||
-it \
|
-it \
|
||||||
|
|||||||
@@ -784,21 +784,21 @@
|
|||||||
|
|
||||||
### Теория FPV полетов
|
### Теория FPV полетов
|
||||||
|
|
||||||
**1. Какой стик является основным для позиционирования при FPV полетах? **
|
**1. Какой стик является основным для позиционирования при FPV полетах?**
|
||||||
|
|
||||||
1. Roll
|
1. Roll
|
||||||
2. Pitch
|
2. Pitch
|
||||||
3. Yaw
|
3. Yaw
|
||||||
4. Throttle
|
4. Throttle
|
||||||
|
|
||||||
**2. Каким стиком удерживается высота? **
|
**2. Каким стиком удерживается высота?**
|
||||||
|
|
||||||
1. Roll
|
1. Roll
|
||||||
2. Pitch
|
2. Pitch
|
||||||
3. Yaw
|
3. Yaw
|
||||||
4. Throttle
|
4. Throttle
|
||||||
|
|
||||||
**3. Что такое FPV пилотирование? **
|
**3. Что такое FPV пилотирование?**
|
||||||
|
|
||||||
1. Полеты с ориентацией “от первого лица”
|
1. Полеты с ориентацией “от первого лица”
|
||||||
2. Полеты с грузом
|
2. Полеты с грузом
|
||||||
|
|||||||