mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-01 15:39:32 +00:00
Compare commits
54 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
f532372535 | ||
|
|
a2a34a4e2f | ||
|
|
6f96c9e3ff | ||
|
|
9aca12e0a5 | ||
|
|
ead55fe0e3 | ||
|
|
f2820471bc | ||
|
|
fb7885ada2 | ||
|
|
f9f4dc5a92 | ||
|
|
ffe4f562cc | ||
|
|
a906428734 | ||
|
|
abc7e6fec1 | ||
|
|
3b01cf3782 | ||
|
|
53d616fb16 | ||
|
|
8168c1f201 | ||
|
|
a0e1e032d6 | ||
|
|
77eca7578f | ||
|
|
d13badca50 | ||
|
|
acf4f84cae | ||
|
|
c24d135815 | ||
|
|
0d64476b04 | ||
|
|
091e110afd | ||
|
|
de8ba52643 | ||
|
|
8a2bb6eb32 | ||
|
|
ed649fd1b1 | ||
|
|
4cd4b99ae0 | ||
|
|
c903afa09d | ||
|
|
8aeb11f771 | ||
|
|
ed51b826a0 | ||
|
|
bb84eeb35e | ||
|
|
8a9fd2a97c | ||
|
|
3b49f9a67f | ||
|
|
2a4faedf67 | ||
|
|
82f7f82f54 | ||
|
|
276922104c | ||
|
|
3bd4a6673f | ||
|
|
1909feceba | ||
|
|
e585341933 | ||
|
|
e87054f0d3 | ||
|
|
6b0bd77d49 | ||
|
|
77f4cbcdd3 | ||
|
|
928c5938e9 | ||
|
|
03500d70af | ||
|
|
2c30a5361f | ||
|
|
c59d31fc21 | ||
|
|
5115ba6d8a | ||
|
|
4daab3d286 | ||
|
|
34512e5e49 | ||
|
|
0fddd90e1f | ||
|
|
29b6a58769 | ||
|
|
549b2e3815 | ||
|
|
88ef7d7eca | ||
|
|
d70c3f92ad | ||
|
|
4a25fed9d5 | ||
|
|
6fb4d43500 |
@@ -5,7 +5,7 @@ end_of_line = lf
|
||||
insert_final_newline = true
|
||||
charset = utf-8
|
||||
|
||||
[*.{py,cpp,h,swift,launch}]
|
||||
[*.{py,swift,launch}]
|
||||
indent_style = space
|
||||
indent_size = 4
|
||||
|
||||
|
||||
@@ -13,8 +13,8 @@
|
||||
"WireShark",
|
||||
"Wi-Fi",
|
||||
"Raspberry Pi",
|
||||
"PixHawk",
|
||||
"PixRacer",
|
||||
"Pixhawk",
|
||||
"Pixracer",
|
||||
"ArUco"
|
||||
],
|
||||
"code_blocks": false
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2018 Copter Express
|
||||
Copyright (c) 2018 Copter Express Technologies
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
// cleverrc
|
||||
//
|
||||
// Created by Oleg Kalachev on 20.01.2018.
|
||||
// Copyright © 2018 Copter Express. All rights reserved.
|
||||
// Copyright © 2018 Copter Express Technologies. All rights reserved.
|
||||
//
|
||||
|
||||
import UIKit
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
// cleverrc
|
||||
//
|
||||
// Created by Oleg Kalachev on 20.01.2018.
|
||||
// Copyright © 2018 Copter Express. All rights reserved.
|
||||
// Copyright © 2018 Copter Express Technologies. All rights reserved.
|
||||
//
|
||||
|
||||
import UIKit
|
||||
|
||||
@@ -21,6 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
tf
|
||||
tf2
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
image_transport
|
||||
cv_bridge
|
||||
)
|
||||
|
||||
|
||||
@@ -121,7 +124,7 @@ generate_messages(
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES clever
|
||||
LIBRARIES clever
|
||||
# CATKIN_DEPENDS other_catkin_pkg
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
@@ -137,7 +140,11 @@ include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# Declare a C++ library
|
||||
add_library(clever
|
||||
src/optical_flow.cpp
|
||||
)
|
||||
|
||||
add_library(fcu_horiz
|
||||
src/fcu_horiz.cpp
|
||||
)
|
||||
@@ -156,8 +163,12 @@ add_library(aruco_vpe
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
add_executable(rc src/rc.cpp)
|
||||
|
||||
add_executable(camera_markers src/camera_markers.cpp)
|
||||
|
||||
target_link_libraries(rc ${catkin_LIBRARIES})
|
||||
|
||||
target_link_libraries(camera_markers ${catkin_LIBRARIES})
|
||||
|
||||
## 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
|
||||
@@ -169,6 +180,10 @@ target_link_libraries(rc ${catkin_LIBRARIES})
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(clever
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
target_link_libraries(fcu_horiz
|
||||
${catkin_LIBRARIES}
|
||||
"/opt/ros/kinetic/lib/libtf2_ros.so"
|
||||
|
||||
@@ -2,15 +2,13 @@
|
||||
<arg name="fcu_conn" default="usb"/>
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
<arg name="viz" default="true"/>
|
||||
<arg name="web_server" default="true"/>
|
||||
<arg name="web_video_server" default="true"/>
|
||||
<arg name="rosbridge" default="true"/>
|
||||
<arg name="main_camera" default="true"/>
|
||||
<arg name="optical_flow" default="false"/>
|
||||
<arg name="aruco" default="false"/>
|
||||
<arg name="rc" value="true"/>
|
||||
<arg name="fpv_camera" default="false"/>
|
||||
<arg name="fpv_camera_device" default="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0"/>
|
||||
<arg name="arduino" default="false"/>
|
||||
|
||||
<!-- mavros -->
|
||||
@@ -18,12 +16,12 @@
|
||||
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
|
||||
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
|
||||
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
|
||||
<arg name="viz" value="$(arg viz)"/>
|
||||
<arg name="viz" value="true"/>
|
||||
</include>
|
||||
|
||||
|
||||
<!-- web server, serving /home/pi/catkin_ws/src/clever/clever/static -->
|
||||
<node name="web_server" pkg="clever" type="monkey" output="screen" if="$(arg web_server)"/>
|
||||
<node name="web_server" pkg="clever" type="monkey" output="screen" if="$(arg web_server)" respawn="true" respawn_delay="5"/>
|
||||
|
||||
<!-- web video server -->
|
||||
<node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5"/>
|
||||
@@ -31,6 +29,12 @@
|
||||
<!-- aruco vpe -->
|
||||
<include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
|
||||
|
||||
<!-- optical flow -->
|
||||
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true">
|
||||
<remap from="image" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
</node>
|
||||
|
||||
<!-- main nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
@@ -53,11 +57,6 @@
|
||||
<!-- rc backend -->
|
||||
<node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
|
||||
|
||||
<!-- FPV video streaming -->
|
||||
<include file="$(find clever)/launch/fpv_camera.launch" if="$(arg fpv_camera)">
|
||||
<arg name="device" value="$(arg fpv_camera_device)"/>
|
||||
</include>
|
||||
|
||||
<!-- Arduino bridge -->
|
||||
<include file="$(find clever)/launch/arduino.launch" if="$(arg arduino)"/>
|
||||
</launch>
|
||||
|
||||
@@ -1,12 +0,0 @@
|
||||
<launch>
|
||||
<remap to="mavros/local_position/pose" from="local_position"/>
|
||||
<remap to="mavros/setpoint_position/local" from="local_setpoint"/>
|
||||
|
||||
<node name="copter_visualization" pkg="mavros_extras" type="copter_visualization"/>
|
||||
|
||||
<param name="copter_visualization/fixed_frame_id" value="local_origin"/>
|
||||
<param name="copter_visualization/child_frame_id" value="fcu"/>
|
||||
<param name="copter_visualization/marker_scale" value="1"/>
|
||||
<param name="copter_visualization/max_track_size" value="500"/>
|
||||
<param name="copter_visualization/num_rotors" value="4"/>
|
||||
</launch>
|
||||
@@ -1,4 +1,7 @@
|
||||
<launch>
|
||||
<!-- Camera position and orientation are represented by fcu -> main_camera_optical transform -->
|
||||
<!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
|
||||
|
||||
<!-- clever 2 -->
|
||||
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
|
||||
|
||||
@@ -20,4 +23,9 @@
|
||||
<param name="image_width" value="320"/>
|
||||
<param name="image_height" value="240"/>
|
||||
</node>
|
||||
|
||||
<!-- camera visualization markers -->
|
||||
<node pkg="clever" type="camera_markers" ns="main_camera" name="main_camera_markers">
|
||||
<param name="scale" value="3.0"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -1,58 +1,95 @@
|
||||
<launch>
|
||||
<arg name="fcu_conn" default="uart"/>
|
||||
<arg name="fcu_conn" default="usb"/>
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
<arg name="viz" default="true"/>
|
||||
<arg name="respawn" default="true"/>
|
||||
|
||||
<node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="5" output="screen">
|
||||
<!-- UART connection -->
|
||||
<param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
|
||||
<!-- UART connection -->
|
||||
<param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
|
||||
|
||||
<!-- USB connection -->
|
||||
<param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
|
||||
<!-- USB connection -->
|
||||
<param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
|
||||
|
||||
<!-- sitl -->
|
||||
<param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
|
||||
<!-- sitl -->
|
||||
<param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
|
||||
|
||||
<!-- gcs bridge -->
|
||||
<param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
|
||||
<param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
|
||||
<param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
|
||||
<param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
|
||||
<param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
|
||||
<param name="gcs_quiet_mode" value="true"/>
|
||||
<param name="conn/timeout" value="8"/>
|
||||
<!-- gcs bridge -->
|
||||
<param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
|
||||
<param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
|
||||
<param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
|
||||
<param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
|
||||
<param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
|
||||
<param name="gcs_quiet_mode" value="true"/>
|
||||
<param name="conn/timeout" value="8"/>
|
||||
|
||||
<!-- default px4 params -->
|
||||
<rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
|
||||
<!-- default px4 params -->
|
||||
<rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
|
||||
|
||||
<!-- additional params -->
|
||||
<param name="local_position/frame_id" value="local_origin"/>
|
||||
<param name="local_position/tf/send" value="true"/>
|
||||
<param name="local_position/tf/frame_id" value="local_origin"/>
|
||||
<param name="local_position/tf/child_frame_id" value="fcu"/>
|
||||
<param name="global_position/tf/send" value="false"/>
|
||||
<param name="imu/frame_id" value="fcu"/>
|
||||
<rosparam param="plugin_blacklist">
|
||||
- safety_area
|
||||
- image_pub
|
||||
- vibration
|
||||
- distance_sensor
|
||||
- rangefinder
|
||||
- 3dr_radio
|
||||
- actuator_control
|
||||
- hil_controls
|
||||
- vfr_hud
|
||||
- px4flow
|
||||
- vision_speed_estimate
|
||||
- fake_gps
|
||||
- cam_imu_sync
|
||||
- hil
|
||||
- adsb
|
||||
</rosparam>
|
||||
<!-- rangefinders -->
|
||||
<rosparam>
|
||||
distance_sensor:
|
||||
rangefinder_0:
|
||||
id: 0
|
||||
frame_id: "rangefinder"
|
||||
orientation: PITCH_270
|
||||
field_of_view: 0.5
|
||||
rangefinder_1:
|
||||
id: 1
|
||||
frame_id: "rangefinder"
|
||||
orientation: PITCH_270
|
||||
field_of_view: 0.5
|
||||
rangefinder_2_sub:
|
||||
subscriber: true
|
||||
id: 2
|
||||
orientation: PITCH_270
|
||||
rangefinder_3_sub:
|
||||
subscriber: true
|
||||
id: 3
|
||||
orientation: PITCH_270
|
||||
</rosparam>
|
||||
|
||||
<!-- additional params -->
|
||||
<param name="local_position/frame_id" value="local_origin"/>
|
||||
<param name="local_position/tf/send" value="true"/>
|
||||
<param name="local_position/tf/frame_id" value="local_origin"/>
|
||||
<param name="local_position/tf/child_frame_id" value="fcu"/>
|
||||
<param name="global_position/tf/send" value="false"/>
|
||||
<param name="imu/frame_id" value="fcu"/>
|
||||
<rosparam param="plugin_blacklist">
|
||||
- safety_area
|
||||
- image_pub
|
||||
- vibration
|
||||
- rangefinder
|
||||
- 3dr_radio
|
||||
- actuator_control
|
||||
- hil_controls
|
||||
- vfr_hud
|
||||
- vision_speed_estimate
|
||||
- fake_gps
|
||||
- cam_imu_sync
|
||||
- hil
|
||||
- adsb
|
||||
- waypoint
|
||||
- obstacle_distance
|
||||
- setpoint_accel
|
||||
- trajectory
|
||||
- wind_estimation
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Rangefinders frame -->
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 fcu rangefinder"/>
|
||||
|
||||
<!-- Copter visualization -->
|
||||
<include file="$(find clever)/launch/copter_visualization.launch" if="$(arg viz)"/>
|
||||
<node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
|
||||
<remap to="mavros/local_position/pose" from="local_position"/>
|
||||
<remap to="mavros/setpoint_position/local" from="local_setpoint"/>
|
||||
<param name="fixed_frame_id" value="local_origin"/>
|
||||
<param name="child_frame_id" value="fcu"/>
|
||||
<param name="marker_scale" value="1"/>
|
||||
<param name="max_track_size" value="20"/>
|
||||
<param name="num_rotors" value="4"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -7,10 +7,10 @@
|
||||
<arg name="fcu_conn" value="udp"/>
|
||||
<arg name="fcu_ip" value="$(arg ip)"/>
|
||||
<arg name="gcs_bridge" value="false"/>
|
||||
<arg name="optical_flow" value="false"/>
|
||||
<arg name="web_server" default="false"/>
|
||||
<arg name="web_video_server" default="false"/>
|
||||
<arg name="main_camera" default="false"/>
|
||||
<arg name="fpv_camera" default="false"/>
|
||||
<arg name="rosbridge" value="$(arg rosbridge)"/>
|
||||
<arg name="aruco" default="false"/>
|
||||
</include>
|
||||
|
||||
@@ -1,3 +1,8 @@
|
||||
<library path="lib/libclever">
|
||||
<class name="clever/optical_flow" type="OpticalFlow" base_class_type="nodelet::Nodelet">
|
||||
<description/>
|
||||
</class>
|
||||
</library>
|
||||
<library path="lib/libfcu_horiz">
|
||||
<class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
|
||||
<description/>
|
||||
|
||||
101
clever/src/camera_markers.cpp
Normal file
101
clever/src/camera_markers.cpp
Normal file
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Visualization marker for camera alignment
|
||||
* Copyright (C) 2018 Copter Express Technologies
|
||||
*
|
||||
* Author: Oleg Kalachev <okalachev@gmail.com>
|
||||
*
|
||||
* Distributed under MIT License (available at https://opensource.org/licenses/MIT).
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
|
||||
using namespace visualization_msgs;
|
||||
|
||||
double markers_scale;
|
||||
std::string camera_frame;
|
||||
|
||||
MarkerArray createMarkers() {
|
||||
MarkerArray markers;
|
||||
|
||||
Marker lens;
|
||||
lens.header.frame_id = camera_frame;
|
||||
lens.ns = "camera_markers";
|
||||
lens.id = 0;
|
||||
lens.action = Marker::ADD;
|
||||
lens.type = visualization_msgs::Marker::CYLINDER;
|
||||
lens.frame_locked = true;
|
||||
lens.scale.x = 0.013 * markers_scale;
|
||||
lens.scale.y = 0.013 * markers_scale;
|
||||
lens.scale.z = 0.015 * markers_scale;
|
||||
lens.color.r = 0.3;
|
||||
lens.color.g = 0.3;
|
||||
lens.color.b = 0.3;
|
||||
lens.color.a = 0.9;
|
||||
lens.pose.position.z = 0.0075 * markers_scale;
|
||||
lens.pose.orientation.w = 1;
|
||||
|
||||
Marker board;
|
||||
board.header.frame_id = camera_frame;
|
||||
board.ns = "camera_markers";
|
||||
board.id = 1;
|
||||
board.action = Marker::ADD;
|
||||
board.type = Marker::CUBE;
|
||||
board.frame_locked = true;
|
||||
board.scale.x = 0.024 * markers_scale;
|
||||
board.scale.y = 0.024 * markers_scale;
|
||||
board.scale.z = 0.001 * markers_scale;
|
||||
board.color.r = 0.0;
|
||||
board.color.g = 0.8;
|
||||
board.color.b = 0.0;
|
||||
board.color.a = 0.9;
|
||||
board.pose.orientation.w = 1;
|
||||
|
||||
Marker wire;
|
||||
wire.header.frame_id = camera_frame;
|
||||
wire.ns = "camera_markers";
|
||||
wire.id = 2;
|
||||
wire.action = Marker::ADD;
|
||||
wire.type = Marker::CUBE;
|
||||
wire.frame_locked = true;
|
||||
wire.scale.x = 0.014 * markers_scale;
|
||||
wire.scale.y = 0.04 * markers_scale;
|
||||
wire.scale.z = 0.001 * markers_scale;
|
||||
wire.color.r = 0.9;
|
||||
wire.color.g = 0.9;
|
||||
wire.color.b = 1.0;
|
||||
wire.color.a = 0.8;
|
||||
wire.pose.position.x = 0;
|
||||
wire.pose.position.y = (0.01 + 0.02) * markers_scale;
|
||||
wire.pose.position.z = 0.002 * markers_scale;
|
||||
wire.pose.orientation.w = 1;
|
||||
|
||||
markers.markers.push_back(lens);
|
||||
markers.markers.push_back(board);
|
||||
markers.markers.push_back(wire);
|
||||
|
||||
return markers;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "camera_markers", ros::init_options::AnonymousName);
|
||||
ros::NodeHandle nh, nh_priv("~");
|
||||
|
||||
nh_priv.param("scale", markers_scale, 1.0);
|
||||
|
||||
// wait for camera info
|
||||
auto camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh);
|
||||
camera_frame = camera_info->header.frame_id;
|
||||
|
||||
ros::Publisher markers_pub = nh.advertise<visualization_msgs::MarkerArray>("camera_markers", 1, true);
|
||||
markers_pub.publish(createMarkers());
|
||||
|
||||
ROS_INFO("Camera markers initialized");
|
||||
ros::spin();
|
||||
}
|
||||
85
clever/src/interactive.py
Executable file
85
clever/src/interactive.py
Executable file
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python
|
||||
import copy
|
||||
|
||||
import rospy
|
||||
import tf.transformations as t
|
||||
from interactive_markers.interactive_marker_server import InteractiveMarkerServer
|
||||
from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl, InteractiveMarkerFeedback
|
||||
from clever import srv
|
||||
|
||||
|
||||
def make_box(msg):
|
||||
marker = Marker()
|
||||
|
||||
marker.type = Marker.CUBE
|
||||
marker.scale.x = msg.scale * 0.3
|
||||
marker.scale.y = msg.scale * 0.3
|
||||
marker.scale.z = msg.scale * 0.3
|
||||
marker.color.r = 0.5
|
||||
marker.color.g = 0.5
|
||||
marker.color.b = 0.5
|
||||
marker.color.a = 1.0
|
||||
marker.pose.orientation.w = 1
|
||||
|
||||
return marker
|
||||
|
||||
|
||||
def make_box_control(msg):
|
||||
control = InteractiveMarkerControl()
|
||||
control.always_visible = True
|
||||
control.orientation.w = 1
|
||||
control.markers.append(make_box(msg))
|
||||
msg.controls.append(control)
|
||||
return control
|
||||
|
||||
|
||||
def make_quadcopter_marker():
|
||||
marker = InteractiveMarker()
|
||||
marker.header.frame_id = 'fcu'
|
||||
marker.header.stamp = rospy.get_rostime()
|
||||
marker.scale = 1
|
||||
marker.pose.orientation.w = 1
|
||||
|
||||
marker.name = 'quadcopter'
|
||||
marker.description = 'Quadcopter'
|
||||
|
||||
make_box_control(marker)
|
||||
|
||||
control = InteractiveMarkerControl()
|
||||
control.orientation.w = 1
|
||||
control.orientation.x = 0
|
||||
control.orientation.y = 1
|
||||
control.orientation.z = 0
|
||||
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
|
||||
marker.controls.append(copy.deepcopy(control))
|
||||
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
|
||||
marker.controls.append(control)
|
||||
|
||||
return marker
|
||||
|
||||
|
||||
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
|
||||
|
||||
|
||||
def process_feedback(feedback):
|
||||
if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
|
||||
return
|
||||
|
||||
p = feedback.pose.position
|
||||
o = feedback.pose.orientation
|
||||
yaw = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')[0]
|
||||
rospy.loginfo('Navigate to %s', p)
|
||||
rospy.loginfo(navigate(x=p.x, y=p.y, z=p.z, yaw=yaw, speed=2,
|
||||
frame_id=feedback.header.frame_id, auto_arm=True))
|
||||
|
||||
|
||||
rospy.init_node('quadcopter_im')
|
||||
|
||||
server = InteractiveMarkerServer('quadcopter_im')
|
||||
|
||||
int_marker = make_quadcopter_marker()
|
||||
server.insert(int_marker, process_feedback)
|
||||
server.applyChanges()
|
||||
|
||||
rospy.loginfo('Interactive quadcopter marker initialized')
|
||||
rospy.spin()
|
||||
@@ -1,3 +1,3 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
exec /home/pi/monkey/build/monkey --port 80
|
||||
exec /home/pi/monkey/build/monkey --port 80 --workers 1
|
||||
|
||||
200
clever/src/optical_flow.cpp
Normal file
200
clever/src/optical_flow.cpp
Normal file
@@ -0,0 +1,200 @@
|
||||
/*
|
||||
* Optical Flow node for PX4
|
||||
* Copyright (C) 2018 Copter Express Technologies
|
||||
*
|
||||
* Author: Oleg Kalachev <okalachev@gmail.com>
|
||||
*
|
||||
* Distributed under MIT License (available at https://opensource.org/licenses/MIT).
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <tf2/exceptions.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <mavros_msgs/OpticalFlowRad.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
|
||||
using cv::Mat;
|
||||
|
||||
class OpticalFlow : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
OpticalFlow():
|
||||
camera_matrix_(3, 3, CV_64F),
|
||||
dist_coeffs_(8, 1, CV_64F),
|
||||
tf_listener_(tf_buffer_)
|
||||
{}
|
||||
|
||||
private:
|
||||
ros::Publisher flow_pub_, velo_pub_, shift_pub_;
|
||||
ros::Time prev_stamp_;
|
||||
std::string fcu_frame_id_;
|
||||
image_transport::CameraSubscriber img_sub_;
|
||||
image_transport::Publisher img_pub_;
|
||||
mavros_msgs::OpticalFlowRad flow_;
|
||||
int roi_, roi_2_;
|
||||
Mat hann_;
|
||||
Mat prev_, curr_;
|
||||
Mat camera_matrix_, dist_coeffs_;
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
|
||||
void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
ros::NodeHandle& nh_priv = getPrivateNodeHandle();
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::ImageTransport it_priv(nh_priv);
|
||||
|
||||
nh_priv.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "fcu");
|
||||
nh_priv.param("roi", roi_, 128);
|
||||
roi_2_ = roi_ / 2;
|
||||
|
||||
img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this);
|
||||
img_pub_ = it_priv.advertise("debug", 1);
|
||||
flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
|
||||
velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
|
||||
shift_pub_ = nh_priv.advertise<geometry_msgs::Vector3Stamped>("shift", 1);
|
||||
|
||||
flow_.integrated_xgyro = NAN; // no IMU available
|
||||
flow_.integrated_ygyro = NAN;
|
||||
flow_.integrated_zgyro = NAN;
|
||||
flow_.time_delta_distance_us = 0;
|
||||
flow_.distance = -1; // no distance sensor available
|
||||
flow_.temperature = 0;
|
||||
|
||||
ROS_INFO("Optical Flow initialized");
|
||||
}
|
||||
|
||||
void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
camera_matrix_.at<double>(i, j) = cinfo->K[3 * i + j];
|
||||
}
|
||||
}
|
||||
for (int k = 0; k < cinfo->D.size(); k++) {
|
||||
dist_coeffs_.at<double>(k) = cinfo->D[k];
|
||||
}
|
||||
}
|
||||
|
||||
void drawFlow(Mat& frame, double x, double y, double quality) const
|
||||
{
|
||||
double brightness = (1 - quality) * 25;;
|
||||
cv::Scalar color(brightness, brightness, brightness);
|
||||
double radius = std::sqrt(x * x + y * y);
|
||||
|
||||
// draw a circle and line indicating the shift direction...
|
||||
cv::Point center(frame.cols >> 1, frame.rows >> 1);
|
||||
cv::circle(frame, center, (int)(radius*5), color, 3, cv::LINE_AA);
|
||||
cv::line(frame, center, cv::Point(center.x + (int)(x*5), center.y + (int)(y*5)), color, 3, cv::LINE_AA);
|
||||
}
|
||||
|
||||
void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
|
||||
{
|
||||
parseCameraInfo(cinfo);
|
||||
|
||||
auto img = cv_bridge::toCvShare(msg, "mono8")->image;
|
||||
|
||||
// Apply ROI
|
||||
if (roi_ != 0) {
|
||||
img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_));
|
||||
}
|
||||
|
||||
img.convertTo(curr_, CV_64F);
|
||||
|
||||
if (prev_.empty()) {
|
||||
prev_ = curr_.clone();
|
||||
prev_stamp_ = msg->header.stamp;
|
||||
cv::createHanningWindow(hann_, curr_.size(), CV_64F);
|
||||
|
||||
} else {
|
||||
double response;
|
||||
cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
|
||||
|
||||
// Publish raw shift in pixels
|
||||
static geometry_msgs::Vector3Stamped shift_vec;
|
||||
shift_vec.header.stamp = msg->header.stamp;
|
||||
shift_vec.header.frame_id = msg->header.frame_id;
|
||||
shift_vec.vector.x = shift.x;
|
||||
shift_vec.vector.y = shift.y;
|
||||
shift_pub_.publish(shift_vec);
|
||||
|
||||
// Undistort flow in pixels
|
||||
uint32_t flow_center_x = msg->width / 2;
|
||||
uint32_t flow_center_y = msg->height / 2;
|
||||
shift.x += flow_center_x;
|
||||
shift.y += flow_center_y;
|
||||
|
||||
std::vector<cv::Point2d> points_dist = { shift };
|
||||
std::vector<cv::Point2d> points_undist(1);
|
||||
|
||||
cv::undistortPoints(points_dist, points_undist, camera_matrix_, dist_coeffs_, cv::noArray(), camera_matrix_);
|
||||
points_undist[0].x -= flow_center_x;
|
||||
points_undist[0].y -= flow_center_y;
|
||||
|
||||
// Calculate flow in radians
|
||||
double focal_length_x = camera_matrix_.at<double>(0, 0);
|
||||
double focal_length_y = camera_matrix_.at<double>(1, 1);
|
||||
double flow_x = atan2(points_undist[0].x, focal_length_x);
|
||||
double flow_y = atan2(points_undist[0].y, focal_length_y);
|
||||
|
||||
// // Convert to FCU frame
|
||||
static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
|
||||
flow_camera.header.frame_id = msg->header.frame_id;
|
||||
flow_camera.header.stamp = msg->header.stamp;
|
||||
flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
|
||||
flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
|
||||
tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
|
||||
|
||||
// Calculate integration time
|
||||
ros::Duration integration_time = msg->header.stamp - prev_stamp_;
|
||||
uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
|
||||
|
||||
// Publish flow in fcu frame
|
||||
flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp;
|
||||
flow_.integration_time_us = integration_time_us;
|
||||
flow_.integrated_x = flow_fcu.vector.x;
|
||||
flow_.integrated_y = flow_fcu.vector.y;
|
||||
flow_.quality = (uint8_t)(response * 255);
|
||||
flow_pub_.publish(flow_);
|
||||
|
||||
// Publish debug image
|
||||
if (img_pub_.getNumSubscribers() > 0) {
|
||||
// publish debug image
|
||||
drawFlow(img, shift_vec.vector.x, shift_vec.vector.y, response);
|
||||
cv_bridge::CvImage out_msg;
|
||||
out_msg.header.frame_id = msg->header.frame_id;
|
||||
out_msg.header.stamp = msg->header.stamp;
|
||||
out_msg.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
out_msg.image = img;
|
||||
img_pub_.publish(out_msg.toImageMsg());
|
||||
}
|
||||
|
||||
// Publish estimated angular velocity
|
||||
static geometry_msgs::TwistStamped velo;
|
||||
velo.header.stamp = msg->header.stamp;
|
||||
velo.header.frame_id = fcu_frame_id_;
|
||||
velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
|
||||
velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
|
||||
velo_pub_.publish(velo);
|
||||
|
||||
prev_ = curr_.clone();
|
||||
prev_stamp_ = msg->header.stamp;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)
|
||||
@@ -1,5 +1,9 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import math
|
||||
from subprocess import Popen, PIPE
|
||||
import re
|
||||
import traceback
|
||||
import rospy
|
||||
from std_srvs.srv import Trigger
|
||||
from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu
|
||||
@@ -8,100 +12,160 @@ from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
|
||||
|
||||
# TODO: roscore is running
|
||||
# TODO: disk free space
|
||||
# TODO: local_origin, fcu, fcu_horiz
|
||||
# TODO: rc service
|
||||
# TODO: perform commander check in PX4
|
||||
# TODO: check if FCU params setter succeed
|
||||
# TODO: selfcheck ROS service (with blacklists for checks)
|
||||
|
||||
|
||||
rospy.init_node('selfcheck')
|
||||
|
||||
|
||||
failures = []
|
||||
|
||||
|
||||
def failure(text, *args):
|
||||
failures.append(text % args)
|
||||
|
||||
|
||||
def check(name):
|
||||
def inner(fn):
|
||||
def wrapper(*args, **kwargs):
|
||||
failures[:] = []
|
||||
try:
|
||||
fn(*args, **kwargs)
|
||||
for f in failures:
|
||||
rospy.logwarn('%s: %s', name, f)
|
||||
except Exception as e:
|
||||
traceback.print_exc()
|
||||
rospy.logwarn('%s: exception occured', name)
|
||||
return
|
||||
if not failures:
|
||||
rospy.loginfo('%s: OK', name)
|
||||
return wrapper
|
||||
return inner
|
||||
|
||||
|
||||
@check('FCU')
|
||||
def check_fcu():
|
||||
try:
|
||||
state = rospy.wait_for_message('mavros/state', State, timeout=3)
|
||||
if not state.connected:
|
||||
raise Exception('No connection to the FCU')
|
||||
failure('No connection to the FCU')
|
||||
except rospy.ROSException:
|
||||
raise Exception('No MAVROS state')
|
||||
failure('No MAVROS state')
|
||||
|
||||
|
||||
@check('Camera')
|
||||
def check_camera(name):
|
||||
try:
|
||||
rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No %s camera images' % name)
|
||||
failure('No %s camera images' % name)
|
||||
try:
|
||||
rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No %s camera camera info' % name)
|
||||
failure('No %s camera camera info' % name)
|
||||
|
||||
|
||||
@check('Aruco detector')
|
||||
def check_aruco():
|
||||
try:
|
||||
rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No aruco_pose/debug topic')
|
||||
failure('No aruco_pose/debug messages')
|
||||
|
||||
|
||||
@check('Simple offboard node')
|
||||
def check_simpleoffboard():
|
||||
try:
|
||||
rospy.wait_for_service('navigate', timeout=3)
|
||||
rospy.wait_for_service('get_telemetry', timeout=3)
|
||||
rospy.wait_for_service('land', timeout=3)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No simple_offboard services')
|
||||
failure('No simple_offboard services')
|
||||
|
||||
|
||||
@check('IMU')
|
||||
def check_imu():
|
||||
try:
|
||||
rospy.wait_for_message('mavros/imu/data', Imu, timeout=1)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No IMU data')
|
||||
failure('No IMU data')
|
||||
|
||||
|
||||
@check('Local position')
|
||||
def check_local_position():
|
||||
try:
|
||||
rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No local position')
|
||||
failure('No local position')
|
||||
|
||||
|
||||
@check('Velocity estimation')
|
||||
def check_velocity():
|
||||
try:
|
||||
velocity = rospy.wait_for_message('mavros/local_position/velocity', TwistStamped, timeout=1)
|
||||
horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
|
||||
vert = velocity.twist.linear.z
|
||||
if abs(horiz) > 0.1:
|
||||
raise Exception('Horizontal velocity estimation is %s m/s; is the copter staying still?' % horiz)
|
||||
failure('Horizontal velocity estimation is %s m/s; is the copter staying still?' % horiz)
|
||||
if abs(vert) > 0.1:
|
||||
raise Exception('Vertical velocity estimation is %s m/s; is the copter staying still?' % vert)
|
||||
failure('Vertical velocity estimation is %s m/s; is the copter staying still?' % vert)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No velocity estimation')
|
||||
failure('No velocity estimation')
|
||||
|
||||
|
||||
@check('Global position (GPS)')
|
||||
def check_global_position():
|
||||
try:
|
||||
rospy.wait_for_message('mavros/global_position/global', PoseStamped, timeout=3)
|
||||
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=2)
|
||||
except rospy.ROSException:
|
||||
raise Exception('No global position')
|
||||
failure('No global position')
|
||||
|
||||
|
||||
def check(name, fn):
|
||||
try:
|
||||
fn()
|
||||
rospy.loginfo('%s: OK', name)
|
||||
except Exception as e:
|
||||
rospy.logwarn('%s: %s', name, str(e))
|
||||
@check('Boot duration')
|
||||
def check_boot_duration():
|
||||
proc = Popen('systemd-analyze', stdout=PIPE)
|
||||
proc.wait()
|
||||
output = proc.communicate()[0]
|
||||
r = re.compile(r'([\d\.]+)s$')
|
||||
duration = float(r.search(output).groups()[0])
|
||||
if duration > 15:
|
||||
failure('long Raspbian boot duration: %ss', duration)
|
||||
|
||||
|
||||
@check('CPU usage')
|
||||
def check_cpu_usage():
|
||||
WHITELIST = 'nodelet',
|
||||
CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
|
||||
proc = Popen(CMD, stdout=PIPE, shell=True)
|
||||
proc.wait()
|
||||
output = proc.communicate()[0]
|
||||
processes = output.split('\n')
|
||||
for process in processes:
|
||||
if not process:
|
||||
continue
|
||||
pid, cpu, cmd = process.split('\t')
|
||||
|
||||
if cmd.strip() not in WHITELIST and float(cpu) > 30:
|
||||
failure('High CPU usage (%s%%) detected: %s (PID %s)',
|
||||
cpu.strip(), cmd.strip(), pid.strip())
|
||||
|
||||
|
||||
def selfcheck():
|
||||
check('FCU', check_fcu)
|
||||
check('Simple offboard node', check_simpleoffboard)
|
||||
check('Main camera node', lambda: check_camera('main_camera'))
|
||||
check('aruco_pose/debug topic', check_aruco)
|
||||
check('IMU data', check_imu)
|
||||
check('Local position', check_local_position)
|
||||
check('Velocity estimation', check_velocity)
|
||||
check('Global position (GPS)', check_global_position)
|
||||
check_fcu()
|
||||
check_imu()
|
||||
check_local_position()
|
||||
check_velocity()
|
||||
check_global_position()
|
||||
check_camera('main_camera')
|
||||
check_aruco()
|
||||
check_simpleoffboard()
|
||||
check_cpu_usage()
|
||||
check_boot_duration()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
@@ -121,6 +121,8 @@ def offboard_and_arm():
|
||||
|
||||
ps = PoseStamped()
|
||||
vs = Vector3Stamped()
|
||||
pt = PositionTarget()
|
||||
at = AttitudeTarget()
|
||||
|
||||
|
||||
BRAKE_TIME = rospy.Duration(0)
|
||||
@@ -184,13 +186,14 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
|
||||
current_nav_start_stamp, req.speed)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
|
||||
type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
|
||||
position=setpoint,
|
||||
yaw=euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2],
|
||||
yaw_rate=req.yaw_rate)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.position = setpoint
|
||||
msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2]
|
||||
msg.yaw_rate = req.yaw_rate
|
||||
return position_pub, msg
|
||||
|
||||
elif isinstance(req, (srv.SetPositionRequest, srv.SetPositionGlobalRequest)):
|
||||
@@ -203,13 +206,14 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
|
||||
pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
|
||||
type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
|
||||
position=pose_local.pose.position,
|
||||
yaw=euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2],
|
||||
yaw_rate=req.yaw_rate)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.position = pose_local.pose.position
|
||||
msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
|
||||
msg.yaw_rate = req.yaw_rate
|
||||
return position_pub, msg
|
||||
|
||||
elif isinstance(req, srv.SetVelocityRequest):
|
||||
@@ -221,28 +225,33 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
|
||||
vector_local = tf_buffer.transform(vs, LOCAL_FRAME, TRANSFORM_TIMEOUT)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
|
||||
type_mask=PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ +
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
|
||||
velocity=vector_local.vector,
|
||||
yaw=euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2],
|
||||
yaw_rate=req.yaw_rate)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.velocity = vector_local.vector
|
||||
msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
|
||||
msg.yaw_rate = req.yaw_rate
|
||||
return position_pub, msg
|
||||
|
||||
elif isinstance(req, srv.SetAttitudeRequest):
|
||||
ps.header.frame_id = req.frame_id or LOCAL_FRAME
|
||||
ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
|
||||
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
|
||||
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
|
||||
thrust=req.thrust,
|
||||
type_mask=AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE)
|
||||
msg = at
|
||||
msg.orientation = pose_local.pose.orientation
|
||||
msg.thrust = req.thrust
|
||||
msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE
|
||||
return attitude_pub, msg
|
||||
|
||||
elif isinstance(req, srv.SetRatesRequest):
|
||||
msg = AttitudeTarget(thrust=req.thrust,
|
||||
type_mask=AttitudeTarget.IGNORE_ATTITUDE,
|
||||
body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate))
|
||||
msg = at
|
||||
msg.thrust = req.thrust
|
||||
msg.type_mask = AT.IGNORE_ATTITUDE
|
||||
msg.body_rate.x = req.roll_rate
|
||||
msg.body_rate.y = req.pitch_rate
|
||||
msg.body_rate.z = req.yaw_rate
|
||||
return attitude_pub, msg
|
||||
|
||||
|
||||
@@ -284,13 +293,13 @@ def handle(req):
|
||||
|
||||
try:
|
||||
with handle_lock:
|
||||
stamp = rospy.get_rostime()
|
||||
current_req = req
|
||||
current_pub, current_msg = get_publisher_and_message(req, stamp, False)
|
||||
rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
|
||||
stamp = rospy.get_rostime()
|
||||
current_req = req
|
||||
current_pub, current_msg = get_publisher_and_message(req, stamp, False)
|
||||
rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
|
||||
|
||||
current_msg.header.stamp = stamp
|
||||
current_pub.publish(current_msg)
|
||||
current_msg.header.stamp = stamp
|
||||
current_pub.publish(current_msg)
|
||||
|
||||
if req.auto_arm:
|
||||
offboard_and_arm()
|
||||
|
||||
@@ -2,11 +2,12 @@
|
||||
|
||||
<ul>
|
||||
<!-- <li><a href="">View user reference</a> (<a href="http://clever.copterexpress.com">http://clever.copterexpress.com</a> snapshot)</li> -->
|
||||
<!-- <li><a href="">Go to web terminal</a> (<code>Butterfly</code>)</li> -->
|
||||
<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
|
||||
<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
|
||||
<!-- <li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
|
||||
</ul>
|
||||
|
||||
<script type="text/javascript">
|
||||
document.querySelector("#wvs").href = location.origin + ':8080';
|
||||
document.querySelector("#butterfly").href = location.origin + ':57575';
|
||||
</script>
|
||||
|
||||
6
deploy/butterfly.service
Normal file
6
deploy/butterfly.service
Normal file
@@ -0,0 +1,6 @@
|
||||
[Unit]
|
||||
Description=Butterfly Terminal Server
|
||||
|
||||
[Service]
|
||||
ExecStart=/usr/local/bin/butterfly.server.py --host="0.0.0.0" --unsecure
|
||||
User=pi
|
||||
5
deploy/butterfly.socket
Normal file
5
deploy/butterfly.socket
Normal file
@@ -0,0 +1,5 @@
|
||||
[Socket]
|
||||
ListenStream=57575
|
||||
|
||||
[Install]
|
||||
WantedBy=sockets.target
|
||||
@@ -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)» и многие другие.
|
||||
|
||||
|
||||
@@ -7,12 +7,13 @@
|
||||
* [Полетные режимы](modes.md)
|
||||
* [Raspberry Pi](raspberry.md)
|
||||
* [Образ операционной системы на RPi](microsd_images.md)
|
||||
* [Подключение Raspberry Pi к PixHawk](connection.md)
|
||||
* [Подключение Raspberry Pi к Pixhawk](connection.md)
|
||||
* [Подключение по Wi-Fi](wifi.md)
|
||||
* [Работа с QGroundControl через Wi-Fi](gcs_bridge.md)
|
||||
* [Прошивка PixHawk/PixRacer](firmware.md)
|
||||
* [Прошивка Pixhawk/Pixracer](firmware.md)
|
||||
* [Пилотирование со смартфона](rc.md)
|
||||
* [SSH-доступ](ssh.md)
|
||||
* [Устройство UART](uart.md)
|
||||
* [Неисправности радиоаппаратуры](radioerrors.md)
|
||||
* [Безопасность](safety.md)
|
||||
* [Техника безопасности по пайке](tb.md)
|
||||
|
||||
@@ -110,7 +110,7 @@ _Примечание_: указанное выше определение пр
|
||||
|
||||
Для правильной работы 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`.
|
||||
* Для 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`.
|
||||
* В параметре `LPE_FUSION` включены **только** флажки `vision position`, `vision yaw`, `land detector`. Итоговое значение _28_.
|
||||
* Выключен компас: `ATT_W_MAG` = 0
|
||||
* Вес угла по рысканью по зрению: `ATT_W_EXT_HDG` = 0.5
|
||||
|
||||
BIN
docs/assets/hciuart_error.jpg
Normal file
BIN
docs/assets/hciuart_error.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
@@ -26,6 +26,6 @@
|
||||
|
||||
* ОС [Raspbian Jessie](https://www.raspberrypi.org/downloads/raspbian/)
|
||||
* Фреймворк [ROS](ros.md)
|
||||
* Пакет [MAVROS](mavros.md) для связи с PixHawk по [MAVLink](mavlink.md)
|
||||
* Пакет [MAVROS](mavros.md) для связи с Pixhawk по [MAVLink](mavlink.md)
|
||||
* Дополнительные пакеты ROS: web_video_server, usb_cam, rosbridge_suite и другие
|
||||
* Пакет программ clever_bundle
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
Подключение PixHawk/PixRacer к Raspberry Pi
|
||||
Подключение Pixhawk/Pixracer к Raspberry Pi
|
||||
===
|
||||
|
||||
Для программирования [автономных полетов](simple_offboard.md), [работы с PixHawk по Wi-Fi](gcs_bridge.md), использования [веб-пульта](web_rc.md) и других функций необходимо подсоединить Raspberry Pi к PixHawk.
|
||||
Для программирования [автономных полетов](simple_offboard.md), [работы с Pixhawk по Wi-Fi](gcs_bridge.md), использования [веб-пульта](web_rc.md) и других функций необходимо подсоединить Raspberry Pi к Pixhawk.
|
||||
|
||||
Убедиться в работоспособности подключения, выполнив на Raspberry Pi:
|
||||
|
||||
@@ -14,7 +14,7 @@ rostopic echo /mavros/state
|
||||
Подключение по USB
|
||||
---
|
||||
|
||||
Соедините PixHawk/PixRacer и Raspberry Pi micro-USB to USB кабелем.
|
||||
Соедините Pixhawk/Pixracer и Raspberry Pi micro-USB to USB кабелем.
|
||||
|
||||
Необходимо убедиться, что в launch-файле Клевера (`~/catkin_ws/src/clever/clever/launch/clever.launch`) тип подключения установлен на USB:
|
||||
|
||||
@@ -28,7 +28,7 @@ rostopic echo /mavros/state
|
||||
sudo systemctl restart clever
|
||||
```
|
||||
|
||||
> **Hint** Для корректной работы подключения Raspberry Pi и PixHawk по USB необходимо установить значение параметра `CBRK_USB_CHK` на 197848.
|
||||
> **Hint** Для корректной работы подключения Raspberry Pi и Pixhawk по USB необходимо установить значение параметра `CBRK_USB_CHK` на 197848.
|
||||
|
||||
Подключение по UART
|
||||
---
|
||||
@@ -47,7 +47,7 @@ TODO схема подключения
|
||||
sudo systemctl restart clever
|
||||
```
|
||||
|
||||
> **Hint** Для корректной работы подключения Raspberry Pi и PixHawk по UART необходимо установить значение параметра `SYS_COMPANION` на 921600.
|
||||
> **Hint** Для корректной работы подключения Raspberry Pi и Pixhawk по UART необходимо установить значение параметра `SYS_COMPANION` на 921600.
|
||||
|
||||
Подключение к SITL
|
||||
---
|
||||
|
||||
@@ -1,16 +1,16 @@
|
||||
Прошивка PixHawk / PixRacer
|
||||
Прошивка Pixhawk / Pixracer
|
||||
===
|
||||
|
||||
PixHawk или PixRacer можно прошить, используя QGroundControl или утилиты командной строки.
|
||||
Pixhawk или Pixracer можно прошить, используя QGroundControl или утилиты командной строки.
|
||||
|
||||
Различные варианты сборок стабильных прошивок PX4 можно скачать в разделе [Releases на GitHub](https://github.com/PX4/Firmware/releases).
|
||||
|
||||
В названии файла прошивки кодируется информации о целевой плате и варианте сборки. Примеры:
|
||||
|
||||
* `px4fmu-v2_default.px4` — прошивка для PixHawk с EKF2.
|
||||
* `px4fmu-v2_lpe.px4` — прошивка для PixHawk с LPE.
|
||||
* `px4fmu-v4_default.px4` — прошивка для PixRacer с EKF2 и LPE (*Клевер 3*).
|
||||
* `px4fmu-v3_default.px4` — прошивка для более новых версий PixHawk (чип ревизии 3, см. илл. + Bootloader v5) с EKF2 и LPE.
|
||||
* `px4fmu-v2_default.px4` — прошивка для Pixhawk с EKF2.
|
||||
* `px4fmu-v2_lpe.px4` — прошивка для Pixhawk с LPE.
|
||||
* `px4fmu-v4_default.px4` — прошивка для Pixracer с EKF2 и LPE (*Клевер 3*).
|
||||
* `px4fmu-v3_default.px4` — прошивка для более новых версий Pixhawk (чип ревизии 3, см. илл. + Bootloader v5) с EKF2 и LPE.
|
||||
|
||||

|
||||
|
||||
@@ -19,9 +19,9 @@ PixHawk или PixRacer можно прошить, используя QGroundCon
|
||||
QGroundControl
|
||||
---
|
||||
|
||||
В QGroundControl откройте раздел Firmware. **После** этого подключите PixHawk / PixRacer по USB.
|
||||
В QGroundControl откройте раздел Firmware. **После** этого подключите Pixhawk / Pixracer по USB.
|
||||
|
||||
Выберите PX4 Flight Stack. Для скачивания и загрузки стандартной прошивки (вариант с EKF2 для PixHawk) выберите пункт меню "Standard Version", для загрузки собственного файла прошивки выберите пункт "Custom firmware file...", затем нажмите OK.
|
||||
Выберите PX4 Flight Stack. Для скачивания и загрузки стандартной прошивки (вариант с EKF2 для Pixhawk) выберите пункт меню "Standard Version", для загрузки собственного файла прошивки выберите пункт "Custom firmware file...", затем нажмите OK.
|
||||
|
||||
> **Warning** Не отключайте USB-кабель до окончания процесса прошивки.
|
||||
|
||||
@@ -46,7 +46,7 @@ make px4fmu-v4_default upload
|
||||
|
||||
Где `px4fmu-v4_default` – требуемый вариант прошивки.
|
||||
|
||||
Для загрузки прошивки `v3` в PixHawk может понадобиться команда `force_upload`:
|
||||
Для загрузки прошивки `v3` в Pixhawk может понадобиться команда `force_upload`:
|
||||
|
||||
```
|
||||
make px4fmu-v3_default force-upload
|
||||
|
||||
@@ -38,7 +38,7 @@
|
||||
Мониторинг топиков в режиме реального времени
|
||||
---
|
||||
|
||||
Для более новых версий платы PixHawk (`px4fmu-v3`), а также для плат PixRacer, в прошивку включен модуль `topic_listener`, который позволяет просматривать значения топиков в режиме реального времени (в том числе в полете).
|
||||
Для более новых версий платы Pixhawk (`px4fmu-v3`), а также для плат Pixracer, в прошивку включен модуль `topic_listener`, который позволяет просматривать значения топиков в режиме реального времени (в том числе в полете).
|
||||
|
||||
Для ее использования нужно выбрать вкладку Mavlink Console в QGroundControl:
|
||||
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
## Полетный контроллер / автопилот
|
||||
|
||||
**1\.** Специализированная плата, спроектированная для управления мультикоптером, самолетом или другим аппаратом. Примеры:
|
||||
PixHawk, Ardupilot, Naze32, CC3D.
|
||||
Pixhawk, Ardupilot, Naze32, CC3D.
|
||||
|
||||
**2\.** Программное обеспечение для платы управления мультикоптером. Примеры: PX4, APM, CleanFlight.
|
||||
|
||||
@@ -47,11 +47,11 @@ Armed – состояние коптера готовности к полету
|
||||
|
||||
## PX4
|
||||
|
||||
Популярный полетный контроллер с открытым исходным кодом, работащий на платах PixHawk, PixRacer и других. PX4 рекомендуется для использования на Клевере.
|
||||
Популярный полетный контроллер с открытым исходным кодом, работащий на платах Pixhawk, Pixracer и других. PX4 рекомендуется для использования на Клевере.
|
||||
|
||||
## APM / ArduPilot
|
||||
|
||||
Полетный контроллер с открытым исходным кодом, изначально созданный для платы Arduino. Впоследствии был портирован на PixHawk, PixRacer и другие платы.
|
||||
Полетный контроллер с открытым исходным кодом, изначально созданный для платы Arduino. Впоследствии был портирован на Pixhawk, Pixracer и другие платы.
|
||||
|
||||
## MAVLink
|
||||
|
||||
@@ -64,3 +64,7 @@ Armed – состояние коптера готовности к полету
|
||||
## MAVROS
|
||||
|
||||
Библиотека-связующее звено между аппаратом, работающем по протоколу MAVLink, и ROS.
|
||||
|
||||
## UART
|
||||
|
||||
Последовательный асинхронный интерфейс передачи данных, применяемый во многих устройствах. Например GPS антенны, Wi-Fi роутеры или Pixhawk.
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
Основная документация: https://mavlink.io/en/.
|
||||
|
||||
MAVLink – это протокол для организации связи между автономными летательными и транспортными системами (дронами, самолетами, автомобилями). Проктол MAVLink лежит в основе взаимодействия между PixHawk и Raspberry Pi.
|
||||
MAVLink – это протокол для организации связи между автономными летательными и транспортными системами (дронами, самолетами, автомобилями). Проктол MAVLink лежит в основе взаимодействия между Pixhawk и Raspberry Pi.
|
||||
|
||||
В Клевер включено 2 обертки над этим протоколом: [MAVROS](mavros.md) и [simple_offboard](simple_offboard.md).
|
||||
|
||||
|
||||
@@ -8,7 +8,9 @@ MAVROS подписывается определенные ROS-топики в
|
||||
|
||||
Нода MAVROS автоматически запускается в launch-файле Клевера. Для [настройки типа подключения](connection.md) см. аргумент `fcu_conn`.
|
||||
|
||||
> **Note** Упрощенное взаимодействие с коптером возможно с использованием пакета [`simple_offboard`](simple_offboard.md).
|
||||
> **Hint** Упрощенное взаимодействие с коптером возможно с использованием пакета [`simple_offboard`](simple_offboard.md).
|
||||
|
||||
> **Note** В пакете `clever` некоторые плагины MAVROS отключены (в целях сохранения ресурсов). Подробнее см. параметр `plugin_blacklist` в файле `/home/pi/catkin_ws/src/clever/clever/launch/mavros.launch`.
|
||||
|
||||
## Основные сервисы
|
||||
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
|
||||
**Подведение итогов**
|
||||
|
||||
Ниже предложен один из вариантов подведения итогов курса.
|
||||
Ниже предложен один из вариантов подведения итогов курса.
|
||||
|
||||
Финальное мероприятие включает 3 раздела:
|
||||
|
||||
@@ -155,7 +155,7 @@
|
||||
| 5 | Заключение | Подвести итоги занятия, спросить, есть ли у класса вопросы. Спросить, что из изученного на занятии было для них интереснее всего. Попросить учеников ответить на контрольные вопросы. Предложить ученикам по желанию провести в интернете дополнительное исследование на пройденную тему. Сообщить ученикам, какую тему они будут проходить на следующем занятии. |
|
||||
| 6 | Резервное время | Показать видео и рассказать классу интересные факты по пройденной теме, не вошедшие в программу. |
|
||||
|
||||
## Урок №6. Тема: «Бесколлекторные двигатели и регуляторы их хода»
|
||||
## Урок №6. Тема: «Бесколлекторные двигатели и регуляторы их хода»
|
||||
|
||||
**Цель урока:** закрепить теоретические знания о строении и работе бесколлекторных
|
||||
электродвигателей. Сформировать знания о работе и настройке регуляторов хода,
|
||||
@@ -381,7 +381,7 @@
|
||||
| 5 | Меняем SSID | Рассказать, что такое SSID. Научить изменять имя wi-fi сети. Объяснить что такое демоны и в какой момент они запускаются. Проработать с конфигурацией одного из них. |
|
||||
| 6 | Используем права суперпользователя | Рассказать о типах и правах пользователей системы. Показать примеры использования sudo. |
|
||||
| 7 | Подготовка коптера к автономным полетам | Проверить подключенное оборудование для автономных полетов. Убедиться в работоспособности подключения можно выполнив на Raspberry Pi: rostopic echo /mavros/state |
|
||||
| 8 | Использование QGroundControl через Wi-Fi | Настроить беспроводное соединение для работы с PixHawk в QGroundControl. Предложить учащимся установить новую прошивку, которая подходит для автономных полетов и откалибровать коптер при беспроводном подключении. |
|
||||
| 8 | Использование QGroundControl через Wi-Fi | Настроить беспроводное соединение для работы с Pixhawk в QGroundControl. Предложить учащимся установить новую прошивку, которая подходит для автономных полетов и откалибровать коптер при беспроводном подключении. |
|
||||
| 9 | Заключение | Подвести итоги занятия, спросить, есть ли у класса вопросы, их должно быть много, нужно заранее продумать ответы на них. Попросить учеников ответить на контрольные вопросы. Предложить ученикам по желанию провести в интернете дополнительное исследование на пройденную тему. Сообщить ученикам, какую тему они будут проходить на следующем занятии.|
|
||||
| 10 | | |
|
||||
|
||||
|
||||
@@ -19,4 +19,4 @@ Raspberry Pi означает «малиновый пирог». Этот сво
|
||||
* домашний медиа-сервер
|
||||
* «мозговой центр» для автоматизированных станков или роботов
|
||||
|
||||
Собственно, в последнем качестве мы и будем его использовать, благодаря возможности подключения его к автопилоту PixHawk.
|
||||
Собственно, в последнем качестве мы и будем его использовать, благодаря возможности подключения его к автопилоту Pixhawk.
|
||||
|
||||
@@ -53,4 +53,4 @@ sudo systemctl restart clever
|
||||
|
||||
* Если интерфейс пульта отображает явно неправильное напряжение (напр. > 5 V), проверьте, что значение PX4-параметра `BAT_N_CELLS` соответствует реальному количество элементов батареи. Если отображаемое напряжение все равно неверно, откалибруйте батарею (TODO: ссылка).
|
||||
|
||||
* Если вместо режима PX4 отображается текст "DISCONNECTED FROM FCU", проверьте [подключение Raspberry Pi к PixHawk](connection.md).
|
||||
* Если вместо режима PX4 отображается текст "DISCONNECTED FROM FCU", проверьте [подключение Raspberry Pi к Pixhawk](connection.md).
|
||||
|
||||
@@ -30,3 +30,9 @@ export ROS_IP=192.168.11.1
|
||||
В качестве reference frame рекомендуется установить фрейм `local_origin`. Для визуализации коптера можно добавить визуализационные маркеры из топика `/vehicle_markers`. Можно просмотреть картинку с дополненной реальностью из топика основной камеры `/main_camera/image_raw`.
|
||||
|
||||
Axis или Grid настроенный на фрейм `aruco_map` будут визуализировать расположение [карты ArUco-меток](aruco.md).
|
||||
|
||||
Рекомендуется также установка набора дополнительных полезных плагинов [jsk_rviz_plugins](https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html). Это набор позволяет визуализировать топики типа `TwistStamped` (скорость), `CameraInfo`, `PolygonArray` и многое другое. Для установки используйте команду:
|
||||
|
||||
```bash
|
||||
sudo apt-get install ros-kinetic-jsk-visualization
|
||||
```
|
||||
|
||||
@@ -282,4 +282,4 @@
|
||||
|
||||
Далее: [Настройка-полетного-контролера](aruco.md#Настройка-полетного-контролера)
|
||||
|
||||
Далее: [Подключение Raspberry Pi к PixHawk](connection.md).
|
||||
Далее: [Подключение Raspberry Pi к Pixhawk](connection.md).
|
||||
|
||||
@@ -71,16 +71,29 @@ while True:
|
||||
rospy.sleep(0.2)
|
||||
```
|
||||
|
||||
--
|
||||
|
||||
Определение, перевернут ли коптер:
|
||||
|
||||
```python
|
||||
PI_2 = math.pi / 2
|
||||
telem = get_telemetry()
|
||||
|
||||
flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
Рассчет общего угла коптера к горизонту:
|
||||
|
||||
TODO: fix
|
||||
|
||||
```python
|
||||
PI_2 = math.pi / 2
|
||||
telem = get_telemetry()
|
||||
|
||||
flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
|
||||
angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
|
||||
if flipped:
|
||||
angle_to_horizon = math.pi - angle_to_horizon
|
||||
```
|
||||
|
||||
---
|
||||
@@ -107,6 +120,17 @@ while not rospy.is_shutdown():
|
||||
|
||||
---
|
||||
|
||||
Повторять действие с частотой 10 Гц:
|
||||
|
||||
```python
|
||||
r = rospy.Rate(10)
|
||||
while not rospy.is_shutdown():
|
||||
# Do anything
|
||||
r.sleep()
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
Пример подписки на топики из MAVROS:
|
||||
|
||||
|
||||
|
||||
85
docs/uart.md
Normal file
85
docs/uart.md
Normal file
@@ -0,0 +1,85 @@
|
||||
# Интерфейс UART
|
||||
|
||||
UART – последовательный асинхронный интерфейс для передачи данных, применяемый во многих устройствах. Например GPS-антенны, Wi-Fi роутеры или Pixhawk.
|
||||
|
||||
Интерфейс обычно содержит две линии: TX – линия для передачи данных, RX – линия для приёма данных. А также обычно использует 5-ти вольтовую логику.
|
||||
|
||||
Для соединения двух устройств необходимо линию TX первого устройства подать на RX второго. Аналогичную операцию нужно совершить с другой стороны, чтобы обеспечить двустороннюю передачу данных.
|
||||
|
||||
> Также необходимо синхронизировать уровни напряжений – соединить землю на двух устройствах.
|
||||
|
||||
Почитать больше про интерфейс и протокол можно в [этой статье](https://habr.com/post/109395/).
|
||||
|
||||
## Linux TTY
|
||||
|
||||
В Linux есть понятие Posix Terminal Interface (подробнее [здесь](https://ru.wikipedia.org/wiki/TTY-абстракция)). Это некоторая абстракция над последовательным или виртуальным интерфейсом, позволяющая работать с устройством нескольким агентам одновременно.
|
||||
|
||||
В качестве примера такой абстрации в Raspbian можно привести `/dev/tty1` – устройство вывода текста на экран подключенный по HDMI.
|
||||
|
||||
## UART на Raspberry Pi 3
|
||||
|
||||
В Raspberry Pi 3 есть два аппаратных UART интерфейса:
|
||||
|
||||
1. `Mini UART` (/dev/ttyAMA0) – для своей работы использует тактирование видеоядра RPi, в связи с чем ограничивает его частоту.
|
||||
2. `PL011` (/dev/ttyS0) – полноценный UART интерфейс выполненный на отдельном блоке кристалла микроконтроллера.
|
||||
|
||||
Подробнее про UART на Raspberry Pi в [официальной статье](https://www.raspberrypi.org/documentation/configuration/uart.md).
|
||||
|
||||
Данные интерфейсы с помощью вентелей микроконтроллера можно переключать между двуями физическими выходами:
|
||||
|
||||
1. разъём UART на GPIO;
|
||||
2. Bluetooth модуль RPi.
|
||||
|
||||
По умолчанию в Raspberry Pi 3 `PL011` подключен к Bluetooth модулю. А `Mini UART` отключен с помощью значения директивы `enable_uart`, по дефолту равной `0`.
|
||||
|
||||
> Надо понимать, что директива `enable_uart` меняет свое дефолтное значение исходя из того, какой UART подключен к Bluetooth модулю RPi с помощью директивы `dtoverlay=pi3-miniuart-bt`.
|
||||
|
||||
Для удобства работы с этими выходами в Raspbian существуют алиасы:
|
||||
|
||||
* `/dev/serial0` – всегда указывает на то TTY устройство, что подключено к GPIO портам.
|
||||
* `/dev/serial1` – всегда указывает на то TTY устройство, что подключено к Bluetooh модулю.
|
||||
|
||||
### Настройка UART на Raspberry Pi
|
||||
|
||||
Для настроек UART существуют директивы, которые находятся в `/boot/config.txt`.
|
||||
|
||||
Для включения UART интерфейса на GPIO:
|
||||
|
||||
```
|
||||
enable_uart=1
|
||||
```
|
||||
|
||||
Для отключения UART интерфейса от Bluetooth модуля:
|
||||
|
||||
```
|
||||
dtoverlay=pi3-disable-bt
|
||||
```
|
||||
|
||||
Для подключения `Mini UART` к Bluetooth модулю:
|
||||
|
||||
```
|
||||
dtoverlay=pi3-miniuart-bt
|
||||
```
|
||||
|
||||
В случае отключения Bluetooth модуля следует отключить `hciuart` сервис:
|
||||
|
||||
```bash
|
||||
sudo systemctl disable hciuart.service
|
||||
```
|
||||
|
||||
## Дефолтная настройка образа
|
||||
|
||||
На образе CLEVER мы изначально выключили `Mini UART` и Bluetooth модуль.
|
||||
|
||||
## Bugs
|
||||
|
||||
Если использовать подключение `Mini UART` к Bluetooth, `hciuart` падает с ошибкой:
|
||||
|
||||

|
||||
|
||||
В случае отключения Bluetooth
|
||||
|
||||
```
|
||||
/dev/serial0 -> ttyAMA0
|
||||
/dev/serial1 -> ttyS0
|
||||
```
|
||||
@@ -7,7 +7,7 @@ pipeline {
|
||||
|
||||
string(name: 'BUILD_DIR', defaultValue: '/mnt/hdd_builder/workspace', description: 'Build workspace')
|
||||
|
||||
string(name: 'RPI_DONWLOAD_URL', defaultValue: 'https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2017-12-01/2017-11-29-raspbian-stretch-lite.zip')
|
||||
string(name: 'RPI_DONWLOAD_URL', defaultValue: 'http://director.downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-06-29/2018-06-27-raspbian-stretch-lite.zip')
|
||||
// TODO: Add mirrorparameters
|
||||
|
||||
string(name: 'GWBT_URL', defaultValue: 'https://github.com/CopterExpress/clever.git')
|
||||
|
||||
@@ -676,5 +676,5 @@
|
||||
version: vision_opencv-release-release-kinetic-cv_bridge-1.12.8-0
|
||||
- tar:
|
||||
local-name: web_video_server
|
||||
uri: https://github.com/RobotWebTools-release/web_video_server-release/archive/release/kinetic/web_video_server/0.0.7-0.tar.gz
|
||||
version: web_video_server-release-release-kinetic-web_video_server-0.0.7-0
|
||||
uri: https://github.com/RobotWebTools-release/web_video_server-release/archive/release/kinetic/web_video_server/0.1.0-0.tar.gz
|
||||
version: web_video_server-release-release-kinetic-web_video_server-0.1.0-0
|
||||
|
||||
@@ -38,7 +38,8 @@ echo -e "\033[0;31m\033[1m$(date) | #6 Turn on UART\033[0m\033[0m"
|
||||
# https://github.com/RPi-Distro/raspi-config/pull/75
|
||||
/usr/bin/raspi-config nonint do_serial 1
|
||||
/usr/bin/raspi-config nonint set_config_var enable_uart 1 /boot/config.txt
|
||||
/usr/bin/raspi-config nonint set_config_var dtoverlay pi3-miniuart-bt /boot/config.txt
|
||||
/usr/bin/raspi-config nonint set_config_var dtoverlay pi3-disable-bt /boot/config.txt
|
||||
systemctl disable hciuart.service
|
||||
|
||||
# After adding to Raspbian OS
|
||||
# https://github.com/RPi-Distro/raspi-config/commit/d6d9ecc0d9cbe4aaa9744ae733b9cb239e79c116
|
||||
|
||||
@@ -55,7 +55,7 @@ then
|
||||
|
||||
cd /home/pi/ros_catkin_ws \
|
||||
&& rosinstall_generator \
|
||||
actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras \
|
||||
actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy \
|
||||
--rosdistro kinetic --deps --wet-only --tar > kinetic-custom_ros.rosinstall \
|
||||
&& wstool merge -t src kinetic-custom_ros.rosinstall \
|
||||
&& wstool update -t src
|
||||
@@ -165,4 +165,14 @@ apt-get clean
|
||||
# Remove local mirror repository key
|
||||
#apt-key del COEX-MIRROR
|
||||
|
||||
echo -e "\033[0;31m\033[1m$(date) | #14 Install Butterfly\033[0m\033[0m"
|
||||
# install and enable Butterfly (web terminal)
|
||||
# TODO: move it back to software_install
|
||||
# Had to move it here, as we don't have `clever` directory in software_install
|
||||
pip3 install butterfly
|
||||
pip3 install butterfly[systemd]
|
||||
ln -s /home/pi/catkin_ws/src/clever/deploy/butterfly.service /lib/systemd/system/
|
||||
ln -s /home/pi/catkin_ws/src/clever/deploy/butterfly.socket /lib/systemd/system/
|
||||
systemctl enable butterfly.socket
|
||||
|
||||
echo -e "\033[0;31m\033[1m$(date) | END of ROS INSTALLATION\033[0m\033[0m"
|
||||
|
||||
@@ -27,7 +27,13 @@ apt-get install --no-install-recommends -y \
|
||||
python3-pip=9.0.1-2+rpt2 \
|
||||
libjpeg8-dev=8d1-2 \
|
||||
tcpdump \
|
||||
libpoco-dev=1.7.6+dfsg1-5+deb9u1
|
||||
ltrace \
|
||||
libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
|
||||
libffi-dev # for butterfly
|
||||
|
||||
# Deny byobu to check available updates
|
||||
sed -i "s/updates_available//" /usr/share/byobu/status/status
|
||||
# sed -i "s/updates_available//" /home/pi/.byobu/status
|
||||
|
||||
# install Monkey web-server
|
||||
cd /home/pi
|
||||
@@ -36,7 +42,7 @@ cd monkey
|
||||
git checkout v1.6.9
|
||||
./configure --malloc-libc --local
|
||||
make
|
||||
sudo setcap 'cap_net_bind_service=+ep' ./build/monkey # allow using 80 port
|
||||
setcap 'cap_net_bind_service=+ep' ./build/monkey # allow using 80 port
|
||||
rm build/conf/sites/default
|
||||
ln -s /home/pi/catkin_ws/src/clever/deploy/monkey ./build/conf/sites/default
|
||||
cd /home/pi
|
||||
@@ -55,12 +61,7 @@ echo -e "\033[0;31m\033[1m$(date) | Miscellaneous\033[0m\033[0m"
|
||||
# vim settings
|
||||
echo "set mouse-=a
|
||||
syntax on
|
||||
autocmd BufNewFile,BufRead *.launch set syntax=xml
|
||||
" > /home/pi/.vimrc
|
||||
|
||||
# disable some auto-updating
|
||||
sudo systemctl disable apt-daily.service
|
||||
sudo systemctl disable apt-daily.timer
|
||||
sudo systemctl disable apt-daily-upgrade.timer
|
||||
sudo systemctl disable apt-daily-upgrade.service
|
||||
|
||||
echo -e "\033[0;31m\033[1m$(date) | End of network installation\033[0m\033[0m"
|
||||
|
||||
Reference in New Issue
Block a user