CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-27 05:29:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master' into buster-python3

Browse Source
This commit is contained in:
Alexey Rogachevskiy
2020-03-18 16:03:02 +03:00
parent 1da2f76758 461f4f5904
commit f7eda0be97
33 changed files with 574 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
aruco_pose/map/map.txt
View File

@@ -1,3 +1,4 @@
# id length x y z rot_z rot_y rot_x
1 0.33 0 0 0 0 0 0
2 0.33 1 0 0 0 0 0
3 0.33 0 1 0 0 0 0

27
aruco_pose/src/genmap.py
View File

@@ -13,17 +13,21 @@
Generate map file for aruco_map nodelet.
Usage:
genmap.py <length> <x> <y> <dist_x> <dist_y> <first> [--top-left]
genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [--top-left | --bottom-left]
genmap.py (-h | --help)
Options:
<length> Marker side length
<x> Marker count along X axis
<y> Marker count along Y axis
<dist_x> Distance between markers along X axis
<dist_y> Distance between markers along Y axis
<first> First marker ID
--top-left First marker is on top-left (not bottom-left)
<length> Marker side length
<x> Marker count along X axis
<y> Marker count along Y axis
<dist_x> Distance between markers along X axis
<dist_y> Distance between markers along Y axis
<first> First marker ID [default: 0]
--top-left First marker is on top-left (default)
--bottom-left First marker is on bottom-left
Example:
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
"""
from __future__ import print_function
@@ -34,20 +38,21 @@ from docopt import docopt
arguments = docopt(__doc__)
length = float(arguments['<length>'])
first = int(arguments['<first>'])
first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
markers_x = int(arguments['<x>'])
markers_y = int(arguments['<y>'])
dist_x = float(arguments['<dist_x>'])
dist_y = float(arguments['<dist_y>'])
top_left = arguments['--top-left']
bottom_left = arguments['--bottom-left']
max_y = (markers_y - 1) * dist_y
print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
for y in range(markers_y):
for x in range(markers_x):
pos_x = x * dist_x
pos_y = y * dist_y
if top_left:
if not bottom_left:
pos_y = max_y - pos_y
print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
first += 1

31
builder/assets/examples/flight.py Normal file
View File

@@ -0,0 +1,31 @@
# Information: https://clever.coex.tech/en/programming.html
import rospy
from clover 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)
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
land = rospy.ServiceProxy('land', Trigger)
# Takeoff and hover 1 m above the ground
navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
# Wait for 3 seconds
rospy.sleep(3)
# Fly forward 1 m
navigate(x=1, y=0, z=0, frame_id='body')
# Wait for 3 seconds
rospy.sleep(3)
# Perform landing
land()

25
builder/assets/examples/leds.py Normal file
View File

@@ -0,0 +1,25 @@
# Information: https://clever.coex.tech/en/leds.html
import rospy
from clover.srv import SetLEDEffect
rospy.init_node('leds')
set_effect = rospy.ServiceProxy('led/set_effect', SetLEDEffect) # define proxy to ROS-service
set_effect(r=255, g=0, b=0) # fill strip with red color
rospy.sleep(2)
set_effect(r=0, g=100, b=0) # fill strip with green color
rospy.sleep(2)
set_effect(effect='fade', r=0, g=0, b=255) # fade to blue color
rospy.sleep(2)
set_effect(effect='flash', r=255, g=0, b=0) # flash twice with red color
rospy.sleep(5)
set_effect(effect='blink', r=255, g=255, b=255) # blink with white color
rospy.sleep(5)
set_effect(effect='rainbow') # show rainbow

10
builder/assets/init_rpi.sh
View File

@@ -38,7 +38,12 @@ echo_stamp() {
NEW_SSID='clover-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
echo_stamp "Setting SSID to ${NEW_SSID}"
# TODO: Use wpa_cli insted direct file edit
cat << EOF >> /etc/wpa_supplicant/wpa_supplicant.conf
# FIXME: We rely on raspberrypi-net-mods to copy our file to /etc/wpa_supplicant.
# This is not very reliable, but seems to fix our rfkill problem.
cat << EOF >> /boot/wpa_supplicant.conf
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
network={
ssid="${NEW_SSID}"
psk="cloverwifi"
@@ -51,9 +56,6 @@ network={
}
EOF
echo_stamp "Unblocking wireless interface"
rfkill unblock wifi
NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
echo_stamp "Setting hostname to $NEW_HOSTNAME"
hostnamectl set-hostname $NEW_HOSTNAME

2
builder/image-build.sh
View File

@@ -104,6 +104,8 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterf
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
# software install
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
# examples
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/'
# network setup
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'

3
builder/test/tests.sh
View File

@@ -52,3 +52,6 @@ rosversion usb_cam
rosversion cv_camera
rosversion web_video_server
rosversion rosshow
# validate examples are present
[[ $(ls /home/pi/examples/*) ]]

7
clover/CMakeLists.txt
View File

@@ -81,6 +81,7 @@ add_service_files(
SetAttitude.srv
SetRates.srv
SetLEDEffect.srv
Execute.srv
)
## Generate actions in the 'action' folder
@@ -167,6 +168,8 @@ add_executable(vpe_publisher src/vpe_publisher.cpp)
add_executable(led src/led.cpp)
add_executable(shell src/shell.cpp)
target_link_libraries(simple_offboard
${catkin_LIBRARIES}
${GeographicLib_LIBRARIES}
@@ -180,10 +183,14 @@ target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
target_link_libraries(led ${catkin_LIBRARIES})
target_link_libraries(shell ${catkin_LIBRARIES})
add_dependencies(simple_offboard ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(led ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use

4
clover/launch/clover.launch
View File

@@ -10,6 +10,7 @@
<arg name="rangefinder_vl53l1x" default="true"/>
<arg name="led" default="true"/>
<arg name="rc" default="true"/>
<arg name="shell" default="true"/>
<!-- log formatting -->
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
@@ -76,4 +77,7 @@
<!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
<param name="use_fake_gcs" value="false"/>
</node>
<!-- Shell access through ROS service -->
<node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
</launch>

5
clover/src/led.cpp
View File

@@ -263,7 +263,10 @@ void handleMavrosState(const mavros_msgs::State& msg)
// remove the part before "."
mode = mode.substr(mode.find(".") + 1);
}
notify(mode);
std::string err;
if (ros::names::validate(mode, err)) {
notify(mode);
}
}
mavros_state = msg;
}

9
clover/src/selfcheck.py
View File

@@ -339,8 +339,11 @@ def is_process_running(binary, exact=False, full=False):
@check('ArUco markers')
def check_aruco():
if is_process_running('aruco_detect', full=True):
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
known_tilt = rospy.get_param('aruco_detect/known_tilt')
try:
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
except KeyError:
failure('aruco_detect/length parameter is not set')
known_tilt = rospy.get_param('aruco_detect/known_tilt', '')
if known_tilt == 'map':
known_tilt += ' (ALL markers are on the floor)'
elif known_tilt == 'map_flipped':
@@ -356,7 +359,7 @@ def check_aruco():
return
if is_process_running('aruco_map', full=True):
known_tilt = rospy.get_param('aruco_map/known_tilt')
known_tilt = rospy.get_param('aruco_map/known_tilt', '')
if known_tilt == 'map':
known_tilt += ' (marker\'s map is on the floor)'
elif known_tilt == 'map_flipped':

50
clover/src/shell.cpp Normal file
View File

@@ -0,0 +1,50 @@
#include <ros/ros.h>
#include <cstdio>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <array>
#include <std_msgs/String.h>
#include <clover/Execute.h>
ros::Duration timeout;
// TODO: handle timeout
bool handle(clover::Execute::Request& req, clover::Execute::Response& res)
{
ROS_INFO("Execute: %s", req.cmd.c_str());
std::array<char, 128> buffer;
std::string result;
FILE *fp = popen(req.cmd.c_str(), "r");
if (fp == NULL) {
res.code = clover::Execute::Request::CODE_FAIL;
res.output = "popen() failed";
return true;
}
while (fgets(buffer.data(), buffer.size(), fp) != nullptr) {
res.output += buffer.data();
}
res.code = pclose(fp);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "shell");
ros::NodeHandle nh, nh_priv("~");
timeout = ros::Duration(nh_priv.param("timeout", 3.0));
auto gt_serv = nh.advertiseService("exec", &handle);
ROS_INFO("shell: ready");
ros::spin();
}

14
clover/src/simple_offboard.cpp
View File

@@ -90,7 +90,7 @@ PositionTarget position_raw_msg;
AttitudeTarget att_raw_msg;
Thrust thrust_msg;
TwistStamped rates_msg;
TransformStamped target;
TransformStamped target, setpoint;
geometry_msgs::TransformStamped body;
// State
@@ -433,6 +433,17 @@ void publish(const ros::Time stamp)
position_raw_msg.position = position_msg.pose.position;
position_raw_pub.publish(position_raw_msg);
}
// publish setpoint frame
if (!setpoint.child_frame_id.empty()) {
setpoint.transform.translation.x = position_msg.pose.position.x;
setpoint.transform.translation.y = position_msg.pose.position.y;
setpoint.transform.translation.z = position_msg.pose.position.z;
setpoint.transform.rotation = position_msg.pose.orientation;
setpoint.header.frame_id = position_msg.header.frame_id;
setpoint.header.stamp = position_msg.header.stamp;
transform_broadcaster->sendTransform(setpoint);
}
}
if (setpoint_type == VELOCITY) {
@@ -764,6 +775,7 @@ int main(int argc, char **argv)
nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
nh_priv.param("auto_release", auto_release, true);
nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
nh_priv.param("nav_from_sp", nav_from_sp, true);

7
clover/srv/Execute.srv Normal file
View File

@@ -0,0 +1,7 @@
int32 CODE_FAIL = -1
int32 CODE_TIMEOUT = -2
string cmd
---
string output
int32 code

17
clover/test/basic.py
View File

@@ -2,6 +2,7 @@
import rospy
import pytest
from mavros_msgs.msg import State
from clover import srv
@pytest.fixture()
def node():
@@ -27,3 +28,19 @@ def test_simple_offboard_services_available():
def test_web_video_server(node):
import urllib2
urllib2.urlopen("http://localhost:8080").read()
def test_shell(node):
execute = rospy.ServiceProxy('exec', srv.Execute)
execute.wait_for_service(5)
res = execute(cmd='echo foo')
assert res.code == 0
assert res.output == 'foo\n'
res = execute(cmd='foo')
assert res.code == 32512
assert res.output == ''
res = execute(cmd='ls foo')
assert res.code == 512
assert res.output == ''

2
clover/test/basic.test
View File

@@ -32,6 +32,8 @@
<node name="rc" pkg="clover" type="rc" required="true" output="screen"/>
<node name="shell" pkg="clover" type="shell" required="true" output="screen"/>
<node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" required="true">
<rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
</node>

10
clover/www/index.html
View File

@@ -8,7 +8,17 @@
<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
</ul>
<div class="version"></div>
<script src="js/roslib.js"></script>
<script type="text/javascript">
document.querySelector("#wvs").href = location.origin + ':8080';
document.querySelector("#butterfly").href = location.origin + ':57575';
// Determine image version
var ros = new ROSLIB.Ros({ url: 'ws://' + location.hostname + ':9090' });
var exec = new ROSLIB.Service({ ros: ros, name : '/exec', serviceType : 'clover/Execute' });
exec.callService(new ROSLIB.ServiceRequest({ cmd: 'cat /etc/clover_version' }), function(result) {
document.querySelector('.version').innerHTML = 'Version: ' + result.output;
});
</script>

BIN
docs/assets/camera_calibration.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 221 KiB

BIN
docs/assets/chessboard.pdf Normal file
View File

Binary file not shown.

1
docs/en/SUMMARY.md
View File

@@ -13,6 +13,7 @@
* [RC setup](radio.md)
* [Flight modes](modes.md)
* [Power setup](power.md)
* [Failsafe configuration](failsafe.md)
* Working with Raspberry Pi
* [Raspberry Pi](raspberry.md)
* [RPi Image](image.md)

4
docs/en/aruco_map.md
View File

@@ -53,7 +53,7 @@ Grid maps may be generated using the `genmap.py` script:
rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
```
`length` is the size of each marker, `x` is the marker count along the *x* axis, `y` is the marker count along the *y* axis, `dist_x` is the distance between the centers of adjacent markers along the *x* axis, `dist_y` is the distance between the centers of the *y* axis, `first` is the ID of the first marker (bottom left marker, unless `--top-left` is specified), `test_map.txt` is the name of the generated map file. The optional `--top-left` parameter changes the numbering of markers, making the top left marker the first one.
`length` is the size of each marker, `x` is the marker count along the *x* axis, `y` is the marker count along the *y* axis, `dist_x` is the distance between the centers of adjacent markers along the *x* axis, `dist_y` is the distance between the centers of the *y* axis, `first` is the ID of the first marker (top left marker, unless `--bottom-left` is specified), `test_map.txt` is the name of the generated map file. The optional `--bottom-left` parameter changes the numbering of markers, making the bottom left marker the first one.
Usage example:
@@ -61,6 +61,8 @@ Usage example:
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
```
Additional information on the utility can be obtained using `-h` key: `rosrun aruco_pose genmap.py -h`.
<!-- You can also use the [online map editor](arucogenmap.md) to create ArUco maps. -->
### Checking the map

71
docs/en/contributing.md
View File

@@ -6,19 +6,21 @@ Clever is mostly an [open source](https://en.wikipedia.org/wiki/Open-source_soft
## Markdown
All Clever documentation is written in the widespread [Markdown](https://ru.wikipedia.org/wiki/Markdown) format. There are many guides on it on the Internet.
All Clever documentation is written in the widespread [Markdown](https://en.wikipedia.org/wiki/Markdown) format. There are many Markdown guides on the Internet.
In Russian: https://guides.hexlet.io/markdown/.
In English: https://www.markdownguide.org/getting-started, https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet.
For the ease of editing texts, you may use text editors with markdown support [Typora](https://typora.io), [Dillinger](https://dillinger.io/) (web), [VSCode](https://code.visualstudio.com) with the [Markdown Editor plugin](https://marketplace.visualstudio.com/items?itemName=MadsKristensen.MarkdownEditor).
For the ease of editing texts, you may use text editors with Markdown support: [Typora](https://typora.io), [Dillinger](https://dillinger.io/) (web), [VSCode](https://code.visualstudio.com) with the [Markdown Editor plugin](https://marketplace.visualstudio.com/items?itemName=MadsKristensen.MarkdownEditor).
We also recommend using the [Code Spell Checker](https://marketplace.visualstudio.com/items?itemName=streetsidesoftware.code-spell-checker) VScode plugin.
For a local build of a static documentation website, use the [`gitbook-cli`](https://github.com/GitbookIO/gitbook-cli) utility.
## Correcting errors in the documents
## Fixing documentation errors
If you have found an error in the documents, or if you want to improve it, use the Pull Request mechanism.
If you have found an error in the documentation or if you want to improve it, use the **Pull Request** mechanism.
1. Find a file with the article you want in the repository – https://github.com/CopterExpress/clever/tree/master/docs.
2. Click "Edit".
@@ -32,14 +34,61 @@ If you have found an error in the documents, or if you want to improve it, use t
More information about Pull Requests is available [at GitHub](https://help.github.com/articles/about-pull-requests/) (English) or in [GIT documentation](https://git-scm.com/book/ru/v2/GitHub-contributing-to_projects) (Russian).
<!--
## Adding a new article
## Contributing a new article
TODO
-->
> **Note** If you've made your own project based on Clever, you can add an article about it to the "Clever-based projects" section.
## Your project with Clever
Prepare your article and send it as a pull request to the [Clever repository](https://github.com/CopterExpress/clever).
If you have implemented your own interesting project using clever, you can add an article about it in section "Clever-based projects".
1. Fork the Clever repository:
<!-- TODO -->
<img src="../assets/github-fork.png" alt="GitHub Fork">
2. Check out the freshly-forked repository on your computer:
```bash
git clone https://github.com/<USERNAME>/clever.git
```
3. Open the directory with the source code checkout and create a new branch for your article (for example, `new-article`):
```bash
git checkout -b new-article
```
4. Write a new article in the [Markdown](https://en.wikipedia.org/wiki/Markdown) format and save it in the `docs/ru` or `docs/en` folder (for example, `docs/en/new_article.md`).
5. Place additional visual assets in the `docs/assets` folder and add them to your article.
6. Add a link to your article to the appropriate section in the `SUMMARY.md` file (in the same folder as in the fourth step):
```markdown
...
* Supplementary materials
* [COEX Pix](coex_pix.md)
* [Contribution guidelines](contributing.md)
* [New article](new_article.md)
* [RC troubleshooting](radioerrors.md)
* [Flashing ESCs](esc_firmware.md)
...
```
7. Commit your changes locally:
```bash
git add docs/
git commit -m "Add new article for Clever"
```
8. Upload your branch to your forked repository on GitHub:
```bash
git push -u origin new-article
```
9. Open your repository on GitHub and send a `pull request` from your branch to Clever:
<img src="../assets/github-pull-request.png" alt="GitHub Pull Request">
<img src="../assets/github-pull-request-create.png" alt="GitHub Create Pull">
10. Wait for the review, be ready to make changes if needed.
11. Look at your new and useful article at https://clever.coex.tech !

13
docs/en/failsafe.md Normal file
View File

@@ -0,0 +1,13 @@
# Failsafe configuration
Main article is available at https://docs.px4.io/master/en/config/safety.html.
The *Safety* panel allows you to configure actions that should be performed when a failsafe is triggered. You should at the very least configure the RC Loss failsafe, which is triggered when the RC transmitter link is lost:
1. Open the *Safety* panel.
2. Select one of the following actions in the *RC Loss Failsafe Trigger* option:
* *Land mode* – transition to automatic land mode;
* *Terminate* – set all outputs to their failsafe values.
3. Set the timeout value before RC Loss triggers in the *RC Loss Timeout* field. We recommend setting it to 0.5 s.
<img src="../assets/qgc-failsafe.png" alt="QGroundControl failsafe" class="zoom">

3
docs/en/frames.md
View File

@@ -10,7 +10,8 @@ Main frames in the `clever` package:
* `map` has its origin at the flight controller initialization point and may be considered stationary. It is shown as a white grid on the image above;
* `base_link` is rigidly bound to the drone. It is shown by the simplified drone model on the image above;
* `body` is bound to the drone, but its Z axis points up regardless of the drone's pitch and roll. It is shown by the red, blue and green lines in the illustration;
* `navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service).
* `navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service);
* `setpoint` is current position setpoint.
Additional frames become available when [ArUco positioning system](aruco.md) is active:

2
docs/en/power.md
View File

@@ -31,3 +31,5 @@ Further reading: https://docs.qgroundcontrol.com/en/SetupView/Power.html.
<img src="../assets/qgc-esc.png" class="zoom">
Further reading: https://docs.px4.io/v1.9.0/en/advanced_config/esc_calibration.html.
**Next**: [Failsafe configuration](failsafe.md)

2
docs/en/simple_offboard.md
View File

@@ -146,7 +146,7 @@ Flying 2 m to the left from the last navigation target:
navigate(x=0, y=2, z=0, speed=1, frame_id='navigate_target')
```
Turn 90 degrees counterclockwise:
Turn 90 degrees clockwise:
```python
navigate(yaw=math.radians(-90), frame_id='body')

3
docs/ru/SUMMARY.md
View File

@@ -53,6 +53,7 @@
* [CAD-модели Клевера](models.md)
* [Docker-контейнер с симулятором](sitl_docker.md)
* [Установка ROS Melodic](ros-install.md)
* [Калибровка камеры](camera_calibration.md)
* [Управление мультикоптером при помощи 4G связи](4g.md)
* [Пакеты Клевера на Jetson Nano](jetson_nano.md)
* [Пилотирование со смартфона](rc.md)
@@ -66,7 +67,6 @@
* [Неисправности радиоаппаратуры](radioerrors.md)
* [Прошивка ESC контроллеров](esc_firmware.md)
* [Настройка режима тренера](trainer_mode.md)
* [Калибровка камеры](camera_calibration.md)
* [Взаимодействие с Arduino](arduino.md)
* [Подключение GPS](gps.md)
* [Работа с ИК датчиками](ir_sensors.md)
@@ -95,6 +95,7 @@
* [Подсчет количества объектов c камеры](object_counting.md)
* [Пульт на Андроид](android.md)
* [Блочный конструктор полета](clever_blocks.md)
* [Калибровка камеры (legacy)](camera_calib.md)
* [Управление дроном для оценки позы человека](human_pose_estimation_drone_control.md)
## Учебник

6
docs/ru/aruco_map.md
View File

@@ -53,7 +53,7 @@ id_маркера размер_маркера x y z угол_z угол_y уго
rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
```
Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--top-left` позволяет нумеровать маркеры с левого верхнего угла.
Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--bottom-left` позволяет нумеровать маркеры с левого нижнего угла.
Пример:
@@ -61,7 +61,9 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/cle
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
```
Также, можно создать карту в специальном [конструкторе](arucogenmap.md).
Дополнительную информацию по утилите можно получить по ключу `-h`: `rosrun aruco_pose genmap.py -h`.
Также можно создать карту в специальном [конструкторе](arucogenmap.md).
### Проверка

245
docs/ru/camera_calib.md Normal file
View File

@@ -0,0 +1,245 @@
# Калибровка камеры
Для точной работы систем компьютерного зрения (например, для навигации по ArUco-маркерам) используемая камера должна быть откалибрована.
![distorted](../assets/img1.jpg)
> Изображение "скруглено" ближе к краям.
Какой-либо алгоритм компьютерного зрения будет воспринимать информацию с этой картинки неправильно.
## Установка приложения
Для начала, необходимо установить необходимые библиотеки:
```bash
pip install numpy
pip install opencv-python
pip install pyyaml
pip install urllib2
pip install flask, flask-wtf
```
Затем скачиваем исходный код из репозитория и проводим установку:
```bash
git clone https://github.com/tinderad/calibration_web_2.7.git
cd calibration_web_2.7.git
sudo python setup.py build
sudo python setup.py install
```
## Подготовка к калибровке
Вам необходимо подготовить калибровочную мишень. Она представляет собой «шахматную доску». Файл можно взять [отсюда](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png).
Наклейте распечатанную мишень на любую твердую поверхность. Посчитайте количество пересечений в длину и в ширину доски, измерьте размер клетки (в мм), как указано на изображении.
![asd](../assets/chessboard.jpg)
Включите Клевер и подключитесь к его Wifi.
> Перейдите на _192.168.11.1:8080_ и проверьте, получает ли компьютер изображения из топика _image_raw_.
## Калибровка
Подключитесь к Клеверу по протоколу SSH (например, при помощи PuTTY).
Запустите приложение:
```bash
>cd calibration_web_2.7/ccc_server
>python app.py
```
Далее вам необходимо на компьютере открыть в браузере страницу по адресу _192.168.11.1:8081_
> Порт можно настроить в файле _ccc_server/config.py_.
На открытой странице необходимо ввести параметры калибровочной мишени: количество перекрестий в длину и ширину, длину ребра квадрата. Для начала калибровки нажмите кнопку **_Start Calibration_**.
![asd](../assets/cam_calib1.png)
На следующей странице при помощи кнопки **_Catch photo_** можно делать фотографии калибровочной мишени.
![asd](../assets/cam_calib2.png)
Если программа нашла на изображении указанную мишень, откроется страница, на которой вам необходимо подтвердить корректность найденных перекрестий.
![asd](../assets/cam_calib3.png)
Если перекрестия были распознаныы правильно, нажмите на клавишу **_Add_**, и перейдите к получению новых фотографий. В противном же случае, ели перекристия были распознаны некорректно, пропустите данную фотографию при помощи клавиши **_Skip_**.
>В большинстве случаев найденные углы будут подсвечиваться разными цветами, но иногда подсветка будет становиться красной. это происходит в том случае, если углы распознаны, но неточно.
Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов. После преодоления данного порога появится кнопка **_Finish_**, по нажатию на которую начнется генерация калибровочного файла.
>Это может занять некоторое время.
На открывшейся странице выведется информация о результате калибровки: имя файла и re-projection error.
>re-projection error - отклонение от стандартной математической модели. Чем эта величина меньше, тем точнее проведена калибровка.
![asd](../assets/cam_calib4.png)
Программа обработает все полученные фотографии, и создаст **_.yaml_** файл в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.
> Если вы поменяете разрешение получаемого изображения, вам нужно будет снова калибровать камеру.
## Предыдущая версия
Также вы можете воспользоваться предыдущей версией программы, не имеющей web-интерфейса.
Запустите скрипт **_calibrate_cam_**:
```bash
>calibrate_cam
```
Задайте параметры мишени:
```bash
>calibrate_cam
Chessboard width: # Перекрестий в ширину
Chessboard height: # Перекрестий в длину
Square size: # Длина ребра клетки (в мм)
Saving mode (YES - on): # Режим сохранения
```
> Режим сохранения: если включен, то все полученные фотографии будут сохраняться в нынешней директории.
Скрипт начнет свою работу:
```
Calibration started!
Commands:
help, catch (key: Enter), delete, restart, stop, finish
```
Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов.
Чтобы сделать фото, введите команду **_catch_**.
```bash
>catch
```
Программа будет информировать вас о состоянии калибровки.
```bash
...
Chessboard not found, now 0 (25 required)
> # Enter
---
Image added, now 1 (25 required)
```
> Вместо того, чтобы каждый раз вводить команду **_catch_**, Вы можете просто нажимать клавишу **_Enter_** (вводить пустую строку).
После того, как будет набрано достаточное количество изображений, введите команду **_finish_**.
```bash
...
>finish
Calibration successful!
```
**Калибровка по существующим изображениям:**
Если же у вас уже есть изображения, то вы можете откалибровать камеру по ним при помощи скрипта **_calibrate_from_dir_**.
```bash
>calibrate_from_dir
```
Указываем характеристики мишени, а так же путь до папки с изображениями:
```bash
>calibrate_cam_ex
Chessboard width: # Перекрестий в ширину
Chessboard height: # Перекрестий в длину
Square size: # Длина ребра клетки (в мм)
Path: # Путь до папки с изображениями
```
В остальном этот скрипт работает аналогично **_calibrate_cam_**.
Программа обработает все полученные фотографии, и создаст файл **_camera_info_****_._****_yaml_** в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.
## Исправление искажений
За получение исправленного изображения отвечает функция
**clever_cam_calibration._get_undistorted_image(cv2_image, camera_info)_**:
* **_cv2_image_**: Закодированное в массив cv2 изображение.
* **_camera_****_­__****_info_**: Путь до файла калибровки.
Функция возвращает массив cv2, в котором закодировано исправленное изображение.
> Если вы используете fisheye-камеру, поставляемую вместе с Клевером, то для обработки изображений разрешением 320x240 или 640x480 вы можете использовать уже существующие параметры калибровки. Для этого в качестве аргумента **_camera_info_** передайте параметры **_clever_cam_calibration.CLEVER_FISHEYE_CAM_320_** или **_clever_cam_calibration.CLEVER_FISHEYE_CAM_640_** соответственно.
## Примеры работы
Изначальные изображения:
![asd](../assets/img1.jpg)
![asd](../assets/img2.jpg)
Иcправленные изображения:
![asd](../assets/calibresult.jpg)
![asd](../assets/calibresult1.jpg)
## Пример использования
**Обработка потока изображений с камеры**.
Данная программа получает изображения с камеры Клевера и выводит их на экран в исправленном виде, используя существующий калибровочный файл.
```python
import clever_cam_calibration as ccc
import cv2
import urllib.request
import numpy as np
while True:
req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
image = cv2.imdecode(arr, -1)
undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640)
cv2.imshow("undistort", undistorted_img)
cv2.waitKey(33)
cv2.destroyAllWindows()
```
## Использование для ArUco
Чтобы применить параметры калибровки к системе ArUco-навигации, требуется перенести калибровочный .yaml файл на Raspberry Pi Клевера и инициализировать его.
> Не забудьте подключиться к WiFI Клевера.
Для передачи файла используется протокол SFTP. В данном примере используется программа WinSCP.
Подключимся к Raspberry Pi по SFTP:
> Пароль: _**raspberry**_
![img](../assets/wcp1.png)
Нажимаем “Войти”. Переходим в _**/home/pi/catkin_ws/src/clever/clever/camera_info/**_ и копируем туда калибровочный .yaml файл:
![img](../assets/wcp2.jpg)
Теперь мы должны выбрать этот файл в конфигурации ArUco. Для этого используется связь по протоколу SSH. В данном примере используется программа PuTTY.
Подключимся к Raspberry Pi по SSH:
![img](../assets/pty1.jpg)
Войдем под логином _**pi**_ и паролем _**raspberry**_, перейдем в директорию _**/home/pi/catkin_ws/src/clever/clever/launch**_ и начнем редактировать конфигурацию _**main_camera.launch**_:
![img](../assets/pty2.jpg)
В строке _**camera node**_ заменим параметр _**camera_info**_ на _**camera_info.yaml**_:
![img](../assets/pty3.jpg)
> Не забудьте изменить разрешение камеры в *main_camera.launch*.

253
docs/ru/camera_calibration.md
View File

@@ -1,245 +1,52 @@
# Калибровка камеры
Для точной работы систем компьютерного зрения (например, для навигации по ArUco-маркерам) используемая камера должна быть откалибрована.
Калибровка камеры может значительно повысить качество работы модулей, связанным с компьютерным зрением: [распознавание ArUco-маркеров](aruco.md) и [Optical Flow](optical_flow.md).
![distorted](../assets/img1.jpg)
При калибровке камеры подбираются параметры, наиболее хорошо описывающие конкретный установленный объектив. Данные параметры включают в себя фокусные расстояния, расположение точки principal point (которое зависит от того, насколько ровно по центру установлен объектив), коэффициенты дисторсии *D*. Подробнее про использующуюся модель искажений камеры можно прочитать в [документации OpenCV](https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html).
> Изображение "скруглено" ближе к краям.
Какой-либо алгоритм компьютерного зрения будет воспринимать информацию с этой картинки неправильно.
Существует несколько инструментов, которые позволяют откалибровать камеру и прописать вычисленные параметры в систему. Обычно, они используют калибровочные изображения: "шахматные доски" (*Chessboard*), а так же комбинации шахматной доски и сетки ArUco-маркеров ([*ChArUco*](https://docs.opencv.org/3.4/df/d4a/tutorial_charuco_detection.html)).
## Установка приложения
## ROS-пакет camera_calibration
Для начала, необходимо установить необходимые библиотеки:
Основной туториал: http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration.
```bash
pip install numpy
pip install opencv-python
pip install pyyaml
pip install urllib2
pip install flask, flask-wtf
```
Для калибровки камеры с использованием ROS-пакета camera_calibration необходим компьютер с установленным ОС GNU/Linux и [ROS Melodic](ros-install.md).
Затем скачиваем исходный код из репозитория и проводим установку:
<img src="../assets/camera_calibration.png" alt="ROS Camera Calibrator" class="zoom center" width=600>
```bash
git clone https://github.com/tinderad/calibration_web_2.7.git
cd calibration_web_2.7.git
sudo python setup.py build
sudo python setup.py install
```
1. Используя Терминал, установите на компьютер пакет `camera_calibration`:
## Подготовка к калибровке
```bash
sudo apt-get install ros-melodic-camera-calibration
```
Вам необходимо подготовить калибровочную мишень. Она представляет собой «шахматную доску». Файл можно взять [отсюда](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png).
Наклейте распечатанную мишень на любую твердую поверхность. Посчитайте количество пересечений в длину и в ширину доски, измерьте размер клетки (в мм), как указано на изображении.
2. Скачайте калибровочную доску – [`chessboard.pdf`](../assets/chessboard.pdf). Распечатайте доску на принтере либо выведите ее на экран компьютера.
![asd](../assets/chessboard.jpg)
3. Подключитесь к [Wi-Fi Клевера](wifi.md).
Включите Клевер и подключитесь к его Wifi.
4. Запустите калибровку (на компьютере):
> Перейдите на _192.168.11.1:8080_ и проверьте, получает ли компьютер изображения из топика _image_raw_.
```bash
ROS_MASTER_URI=http://192.168.11.1:11311 rosrun camera_calibration cameracalibrator.py --size 6x8 --square 0.108 image:=/main_camera/image_raw camera:=/main_camera
```
## Калибровка
> **Note** Вместо значения *0.108* укажите реальный размер квадрата на распечатанной доске или на экране (в метрах). Например, значение *0.03* будет соответствовать 3 см.
Подключитесь к Клеверу по протоколу SSH (например, при помощи PuTTY).
5. Когда программа для калибровки запустится, начните перемещать дрон таким образом, чтобы калибровочная доска попадала в кадр под разными углами.
Запустите приложение:
* Перемещайте калибровочную доску в левый, правый, верхний и нижний торец кадра.
* Вращайте калибровочную доску вокруг всех 3-х осей.
* Отдаляйте и приближайте камеру к калибровочной доске.
```bash
>cd calibration_web_2.7/ccc_server
>python app.py
```
6. Нажмите кнопку *CALIBRATE*, когда она станет активной. Процесс вычисления параметров калибровки займет несколько минут.
Далее вам необходимо на компьютере открыть в браузере страницу по адресу _192.168.11.1:8081_
Когда калибровка завершится, в терминале вы увидите полученные параметры. В окне отобразится изображение с выправленными искажениями. При успешной калибровке все реальные прямые линии должны остаться прямыми на полученном изображении.
> Порт можно настроить в файле _ccc_server/config.py_.
7. Нажмите *COMMIT*, чтобы сохранить полученные параметры калибровки. Результат будет записан в файл калибровки основной камеры Клевера:
`/home/pi/catkin_ws/src/clever/clever/camera_info/fisheye_cam_320.yaml`.
На открытой странице необходимо ввести параметры калибровочной мишени: количество перекрестий в длину и ширину, длину ребра квадрата. Для начала калибровки нажмите кнопку **_Start Calibration_**.
8. Перезапустите сервисы Клевера:
![asd](../assets/cam_calib1.png)
На следующей странице при помощи кнопки **_Catch photo_** можно делать фотографии калибровочной мишени.
![asd](../assets/cam_calib2.png)
Если программа нашла на изображении указанную мишень, откроется страница, на которой вам необходимо подтвердить корректность найденных перекрестий.
![asd](../assets/cam_calib3.png)
Если перекрестия были распознаныы правильно, нажмите на клавишу **_Add_**, и перейдите к получению новых фотографий. В противном же случае, ели перекристия были распознаны некорректно, пропустите данную фотографию при помощи клавиши **_Skip_**.
>В большинстве случаев найденные углы будут подсвечиваться разными цветами, но иногда подсветка будет становиться красной. это происходит в том случае, если углы распознаны, но неточно.
Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов. После преодоления данного порога появится кнопка **_Finish_**, по нажатию на которую начнется генерация калибровочного файла.
>Это может занять некоторое время.
На открывшейся странице выведется информация о результате калибровки: имя файла и re-projection error.
>re-projection error - отклонение от стандартной математической модели. Чем эта величина меньше, тем точнее проведена калибровка.
![asd](../assets/cam_calib4.png)
Программа обработает все полученные фотографии, и создаст **_.yaml_** файл в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.
> Если вы поменяете разрешение получаемого изображения, вам нужно будет снова калибровать камеру.
## Предыдущая версия
Также вы можете воспользоваться предыдущей версией программы, не имеющей web-интерфейса.
Запустите скрипт **_calibrate_cam_**:
```bash
>calibrate_cam
```
Задайте параметры мишени:
```bash
>calibrate_cam
Chessboard width: # Перекрестий в ширину
Chessboard height: # Перекрестий в длину
Square size: # Длина ребра клетки (в мм)
Saving mode (YES - on): # Режим сохранения
```
> Режим сохранения: если включен, то все полученные фотографии будут сохраняться в нынешней директории.
Скрипт начнет свою работу:
```
Calibration started!
Commands:
help, catch (key: Enter), delete, restart, stop, finish
```
Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов.
Чтобы сделать фото, введите команду **_catch_**.
```bash
>catch
```
Программа будет информировать вас о состоянии калибровки.
```bash
...
Chessboard not found, now 0 (25 required)
> # Enter
---
Image added, now 1 (25 required)
```
> Вместо того, чтобы каждый раз вводить команду **_catch_**, Вы можете просто нажимать клавишу **_Enter_** (вводить пустую строку).
После того, как будет набрано достаточное количество изображений, введите команду **_finish_**.
```bash
...
>finish
Calibration successful!
```
**Калибровка по существующим изображениям:**
Если же у вас уже есть изображения, то вы можете откалибровать камеру по ним при помощи скрипта **_calibrate_from_dir_**.
```bash
>calibrate_from_dir
```
Указываем характеристики мишени, а так же путь до папки с изображениями:
```bash
>calibrate_cam_ex
Chessboard width: # Перекрестий в ширину
Chessboard height: # Перекрестий в длину
Square size: # Длина ребра клетки (в мм)
Path: # Путь до папки с изображениями
```
В остальном этот скрипт работает аналогично **_calibrate_cam_**.
Программа обработает все полученные фотографии, и создаст файл **_camera_info_****_._****_yaml_** в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.
## Исправление искажений
За получение исправленного изображения отвечает функция
**clever_cam_calibration._get_undistorted_image(cv2_image, camera_info)_**:
* **_cv2_image_**: Закодированное в массив cv2 изображение.
* **_camera_****_­__****_info_**: Путь до файла калибровки.
Функция возвращает массив cv2, в котором закодировано исправленное изображение.
> Если вы используете fisheye-камеру, поставляемую вместе с Клевером, то для обработки изображений разрешением 320x240 или 640x480 вы можете использовать уже существующие параметры калибровки. Для этого в качестве аргумента **_camera_info_** передайте параметры **_clever_cam_calibration.CLEVER_FISHEYE_CAM_320_** или **_clever_cam_calibration.CLEVER_FISHEYE_CAM_640_** соответственно.
## Примеры работы
Изначальные изображения:
![asd](../assets/img1.jpg)
![asd](../assets/img2.jpg)
Иcправленные изображения:
![asd](../assets/calibresult.jpg)
![asd](../assets/calibresult1.jpg)
## Пример использования
**Обработка потока изображений с камеры**.
Данная программа получает изображения с камеры Клевера и выводит их на экран в исправленном виде, используя существующий калибровочный файл.
```python
import clever_cam_calibration as ccc
import cv2
import urllib.request
import numpy as np
while True:
req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
image = cv2.imdecode(arr, -1)
undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640)
cv2.imshow("undistort", undistorted_img)
cv2.waitKey(33)
cv2.destroyAllWindows()
```
## Использование для ArUco
Чтобы применить параметры калибровки к системе ArUco-навигации, требуется перенести калибровочный .yaml файл на Raspberry Pi Клевера и инициализировать его.
> Не забудьте подключиться к WiFI Клевера.
Для передачи файла используется протокол SFTP. В данном примере используется программа WinSCP.
Подключимся к Raspberry Pi по SFTP:
> Пароль: _**raspberry**_
![img](../assets/wcp1.png)
Нажимаем “Войти”. Переходим в _**/home/pi/catkin_ws/src/clever/clever/camera_info/**_ и копируем туда калибровочный .yaml файл:
![img](../assets/wcp2.jpg)
Теперь мы должны выбрать этот файл в конфигурации ArUco. Для этого используется связь по протоколу SSH. В данном примере используется программа PuTTY.
Подключимся к Raspberry Pi по SSH:
![img](../assets/pty1.jpg)
Войдем под логином _**pi**_ и паролем _**raspberry**_, перейдем в директорию _**/home/pi/catkin_ws/src/clever/clever/launch**_ и начнем редактировать конфигурацию _**main_camera.launch**_:
![img](../assets/pty2.jpg)
В строке _**camera node**_ заменим параметр _**camera_info**_ на _**camera_info.yaml**_:
![img](../assets/pty3.jpg)
> Не забудьте изменить разрешение камеры в *main_camera.launch*.
```bash
sudo systemctl restart clever
```

4
docs/ru/camera_setup.md
View File

@@ -80,3 +80,7 @@
<img src="../assets/camera_option_4_clever.jpg" width=400>
> **Hint** [Утилита `selfcheck.py`](selfcheck.md) выдает словесное описание установленной в данной момент ориентации основной камеры.
## Калибровка
Для улучшение качества работы алгоритмов также рекомендуется произвести калибровку камеры, процесс которой описан [в отдельной статье](camera_calibration.md).

3
docs/ru/frames.md
View File

@@ -10,7 +10,8 @@
* `map` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации;
* `base_link` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации;
* `body` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синяя и зеленая линии на иллюстрации;
* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate)).
* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate));
* `setpoint` – текущий setpoint по позиции.
При использовании [системы позиционирования по ArUco-маркерам](aruco.md) появляются дополнительные фреймы:

2
docs/ru/simple_offboard.md
View File

@@ -146,7 +146,7 @@ navigate(x=0, y=-3, z=0, speed=1, frame_id='body')
navigate(x=0, y=2, z=0, speed=1, frame_id='navigate_target')
```
Повернуться на 90 градусов против часовой:
Повернуться на 90 градусов по часовой:
```python
navigate(yaw=math.radians(-90), frame_id='body')