mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-26 11:43:25 +00:00
Merge pull request #488 from CopterExpress/v0.24-release
V0.24 release changes
This commit is contained in:
Oleg Kalachev
3
.github/workflows/docs.yml
vendored
3
.github/workflows/docs.yml
vendored
@@ -81,5 +81,8 @@ jobs:
|
||||
needs: docs
|
||||
steps:
|
||||
- name: Deploy to GitHub Pages
|
||||
env:
|
||||
FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
|
||||
if: ${{ !env.FREEZE_DOCS }}
|
||||
id: deployment
|
||||
uses: actions/deploy-pages@v1
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>aruco_pose</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>Positioning with ArUco markers</description>
|
||||
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
|
||||
@@ -16,3 +16,723 @@ web_video_server:
|
||||
ws281x:
|
||||
debian:
|
||||
buster: [ros-noetic-ws281x]
|
||||
catkin:
|
||||
debian:
|
||||
buster: [ros-noetic-catkin]
|
||||
genmsg:
|
||||
debian:
|
||||
buster: [ros-noetic-genmsg]
|
||||
gencpp:
|
||||
debian:
|
||||
buster: [ros-noetic-gencpp]
|
||||
geneus:
|
||||
debian:
|
||||
buster: [ros-noetic-geneus]
|
||||
genlisp:
|
||||
debian:
|
||||
buster: [ros-noetic-genlisp]
|
||||
gennodejs:
|
||||
debian:
|
||||
buster: [ros-noetic-gennodejs]
|
||||
genpy:
|
||||
debian:
|
||||
buster: [ros-noetic-genpy]
|
||||
bond_core:
|
||||
debian:
|
||||
buster: [ros-noetic-bond-core]
|
||||
cmake_modules:
|
||||
debian:
|
||||
buster: [ros-noetic-cmake-modules]
|
||||
class_loader:
|
||||
debian:
|
||||
buster: [ros-noetic-class-loader]
|
||||
common_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-common-msgs]
|
||||
common_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-common-tutorials]
|
||||
cpp_common:
|
||||
debian:
|
||||
buster: [ros-noetic-cpp-common]
|
||||
desktop:
|
||||
debian:
|
||||
buster: [ros-noetic-desktop]
|
||||
diagnostics:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostics]
|
||||
executive_smach:
|
||||
debian:
|
||||
buster: [ros-noetic-executive-smach]
|
||||
geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry]
|
||||
geometry_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry-tutorials]
|
||||
gl_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-gl-dependency]
|
||||
image_common:
|
||||
debian:
|
||||
buster: [ros-noetic-image-common]
|
||||
image_pipeline:
|
||||
debian:
|
||||
buster: [ros-noetic-image-pipeline]
|
||||
image_transport_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-image-transport-plugins]
|
||||
laser_pipeline:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-pipeline]
|
||||
mavlink:
|
||||
debian:
|
||||
buster: [ros-noetic-mavlink]
|
||||
media_export:
|
||||
debian:
|
||||
buster: [ros-noetic-media-export]
|
||||
message_generation:
|
||||
debian:
|
||||
buster: [ros-noetic-message-generation]
|
||||
message_runtime:
|
||||
debian:
|
||||
buster: [ros-noetic-message-runtime]
|
||||
mk:
|
||||
debian:
|
||||
buster: [ros-noetic-mk]
|
||||
nodelet_core:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-core]
|
||||
orocos_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-orocos-kdl]
|
||||
perception:
|
||||
debian:
|
||||
buster: [ros-noetic-perception]
|
||||
perception_pcl:
|
||||
debian:
|
||||
buster: [ros-noetic-perception-pcl]
|
||||
python_orocos_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-python-orocos-kdl]
|
||||
qt_dotgraph:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-dotgraph]
|
||||
qt_gui:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui]
|
||||
qt_gui_py_common:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui-py-common]
|
||||
qwt_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-qwt-dependency]
|
||||
robot:
|
||||
debian:
|
||||
buster: [ros-noetic-robot]
|
||||
ros:
|
||||
debian:
|
||||
buster: [ros-noetic-ros]
|
||||
ros_base:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-base]
|
||||
ros_comm:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-comm]
|
||||
ros_core:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-core]
|
||||
ros_environment:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-environment]
|
||||
ros_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-tutorials]
|
||||
rosapi:
|
||||
debian:
|
||||
buster: [ros-noetic-rosapi]
|
||||
rosbag_migration_rule:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag-migration-rule]
|
||||
rosbash:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbash]
|
||||
rosboost_cfg:
|
||||
debian:
|
||||
buster: [ros-noetic-rosboost-cfg]
|
||||
rosbridge_server:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-server]
|
||||
rosbridge_suite:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-suite]
|
||||
rosbuild:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbuild]
|
||||
rosclean:
|
||||
debian:
|
||||
buster: [ros-noetic-rosclean]
|
||||
roscpp_core:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-core]
|
||||
roscpp_traits:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-traits]
|
||||
roscreate:
|
||||
debian:
|
||||
buster: [ros-noetic-roscreate]
|
||||
rosgraph:
|
||||
debian:
|
||||
buster: [ros-noetic-rosgraph]
|
||||
roslang:
|
||||
debian:
|
||||
buster: [ros-noetic-roslang]
|
||||
roslint:
|
||||
debian:
|
||||
buster: [ros-noetic-roslint]
|
||||
roslisp:
|
||||
debian:
|
||||
buster: [ros-noetic-roslisp]
|
||||
rosmake:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmake]
|
||||
rosmaster:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmaster]
|
||||
rospack:
|
||||
debian:
|
||||
buster: [ros-noetic-rospack]
|
||||
roslib:
|
||||
debian:
|
||||
buster: [ros-noetic-roslib]
|
||||
rosparam:
|
||||
debian:
|
||||
buster: [ros-noetic-rosparam]
|
||||
rospy:
|
||||
debian:
|
||||
buster: [ros-noetic-rospy]
|
||||
rosserial:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial]
|
||||
rosserial_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-msgs]
|
||||
rosserial_python:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-python]
|
||||
rosservice:
|
||||
debian:
|
||||
buster: [ros-noetic-rosservice]
|
||||
rostime:
|
||||
debian:
|
||||
buster: [ros-noetic-rostime]
|
||||
roscpp_serialization:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-serialization]
|
||||
python_qt_binding:
|
||||
debian:
|
||||
buster: [ros-noetic-python-qt-binding]
|
||||
roslaunch:
|
||||
debian:
|
||||
buster: [ros-noetic-roslaunch]
|
||||
rosunit:
|
||||
debian:
|
||||
buster: [ros-noetic-rosunit]
|
||||
angles:
|
||||
debian:
|
||||
buster: [ros-noetic-angles]
|
||||
libmavconn:
|
||||
debian:
|
||||
buster: [ros-noetic-libmavconn]
|
||||
rosconsole:
|
||||
debian:
|
||||
buster: [ros-noetic-rosconsole]
|
||||
pluginlib:
|
||||
debian:
|
||||
buster: [ros-noetic-pluginlib]
|
||||
qt_gui_cpp:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui-cpp]
|
||||
resource_retriever:
|
||||
debian:
|
||||
buster: [ros-noetic-resource-retriever]
|
||||
rosconsole_bridge:
|
||||
debian:
|
||||
buster: [ros-noetic-rosconsole-bridge]
|
||||
roslz4:
|
||||
debian:
|
||||
buster: [ros-noetic-roslz4]
|
||||
rosserial_client:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-client]
|
||||
rostest:
|
||||
debian:
|
||||
buster: [ros-noetic-rostest]
|
||||
rqt_action:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-action]
|
||||
rqt_bag:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-bag]
|
||||
rqt_bag_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-bag-plugins]
|
||||
rqt_common_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-common-plugins]
|
||||
rqt_console:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-console]
|
||||
rqt_dep:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-dep]
|
||||
rqt_graph:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-graph]
|
||||
rqt_gui:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui]
|
||||
rqt_logger_level:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-logger-level]
|
||||
rqt_moveit:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-moveit]
|
||||
rqt_msg:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-msg]
|
||||
rqt_nav_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-nav-view]
|
||||
rqt_plot:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-plot]
|
||||
rqt_pose_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-pose-view]
|
||||
rqt_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-publisher]
|
||||
rqt_py_console:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-py-console]
|
||||
rqt_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-reconfigure]
|
||||
rqt_robot_dashboard:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-dashboard]
|
||||
rqt_robot_monitor:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-monitor]
|
||||
rqt_robot_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-plugins]
|
||||
rqt_robot_steering:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-steering]
|
||||
rqt_runtime_monitor:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-runtime-monitor]
|
||||
rqt_service_caller:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-service-caller]
|
||||
rqt_shell:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-shell]
|
||||
rqt_srv:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-srv]
|
||||
rqt_tf_tree:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-tf-tree]
|
||||
rqt_top:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-top]
|
||||
rqt_topic:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-topic]
|
||||
rqt_web:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-web]
|
||||
smach:
|
||||
debian:
|
||||
buster: [ros-noetic-smach]
|
||||
smclib:
|
||||
debian:
|
||||
buster: [ros-noetic-smclib]
|
||||
std_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-std-msgs]
|
||||
actionlib_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib-msgs]
|
||||
bond:
|
||||
debian:
|
||||
buster: [ros-noetic-bond]
|
||||
diagnostic_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-msgs]
|
||||
geometry_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry-msgs]
|
||||
eigen_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-eigen-conversions]
|
||||
kdl_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-conversions]
|
||||
nav_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-nav-msgs]
|
||||
rosbridge_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-msgs]
|
||||
rosgraph_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosgraph-msgs]
|
||||
rosmsg:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmsg]
|
||||
rqt_py_common:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-py-common]
|
||||
shape_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-shape-msgs]
|
||||
smach_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-smach-msgs]
|
||||
std_srvs:
|
||||
debian:
|
||||
buster: [ros-noetic-std-srvs]
|
||||
tf2_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-msgs]
|
||||
tf2:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2]
|
||||
tf2_eigen:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-eigen]
|
||||
trajectory_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-trajectory-msgs]
|
||||
control_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-control-msgs]
|
||||
urdf_parser_plugin:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf-parser-plugin]
|
||||
urdfdom_py:
|
||||
debian:
|
||||
buster: [ros-noetic-urdfdom-py]
|
||||
uuid_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-uuid-msgs]
|
||||
geographic_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-geographic-msgs]
|
||||
vision_opencv:
|
||||
debian:
|
||||
buster: [ros-noetic-vision-opencv]
|
||||
visualization_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-msgs]
|
||||
visualization_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-tutorials]
|
||||
viz:
|
||||
debian:
|
||||
buster: [ros-noetic-viz]
|
||||
webkit_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-webkit-dependency]
|
||||
xmlrpcpp:
|
||||
debian:
|
||||
buster: [ros-noetic-xmlrpcpp]
|
||||
roscpp:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp]
|
||||
bondcpp:
|
||||
debian:
|
||||
buster: [ros-noetic-bondcpp]
|
||||
bondpy:
|
||||
debian:
|
||||
buster: [ros-noetic-bondpy]
|
||||
nodelet:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet]
|
||||
nodelet_tutorial_math:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-tutorial-math]
|
||||
pluginlib_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-pluginlib-tutorials]
|
||||
roscpp_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-tutorials]
|
||||
rosout:
|
||||
debian:
|
||||
buster: [ros-noetic-rosout]
|
||||
camera_calibration:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-calibration]
|
||||
diagnostic_aggregator:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-aggregator]
|
||||
diagnostic_updater:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-updater]
|
||||
diagnostic_common_diagnostics:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-common-diagnostics]
|
||||
dynamic_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-dynamic-reconfigure]
|
||||
filters:
|
||||
debian:
|
||||
buster: [ros-noetic-filters]
|
||||
joint_state_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-joint-state-publisher]
|
||||
message_filters:
|
||||
debian:
|
||||
buster: [ros-noetic-message-filters]
|
||||
rosauth:
|
||||
debian:
|
||||
buster: [ros-noetic-rosauth]
|
||||
rosbag_storage:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag-storage]
|
||||
rosnode:
|
||||
debian:
|
||||
buster: [ros-noetic-rosnode]
|
||||
rospy_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-rospy-tutorials]
|
||||
rosshow:
|
||||
debian:
|
||||
buster: [ros-noetic-rosshow]
|
||||
rostopic:
|
||||
debian:
|
||||
buster: [ros-noetic-rostopic]
|
||||
rqt_gui_cpp:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui-cpp]
|
||||
rqt_gui_py:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui-py]
|
||||
self_test:
|
||||
debian:
|
||||
buster: [ros-noetic-self-test]
|
||||
smach_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-smach-ros]
|
||||
tf2_py:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-py]
|
||||
topic_tools:
|
||||
debian:
|
||||
buster: [ros-noetic-topic-tools]
|
||||
rosbag:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag]
|
||||
actionlib:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib]
|
||||
actionlib_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib-tutorials]
|
||||
diagnostic_analysis:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-analysis]
|
||||
nodelet_topic_tools:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-topic-tools]
|
||||
roswtf:
|
||||
debian:
|
||||
buster: [ros-noetic-roswtf]
|
||||
rqt_launch:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-launch]
|
||||
sensor_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-sensor-msgs]
|
||||
camera_calibration_parsers:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-calibration-parsers]
|
||||
cv_bridge:
|
||||
debian:
|
||||
buster: [ros-noetic-cv-bridge]
|
||||
image_geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-image-geometry]
|
||||
image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-image-transport]
|
||||
camera_info_manager:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-info-manager]
|
||||
compressed_depth_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-compressed-depth-image-transport]
|
||||
compressed_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-compressed-image-transport]
|
||||
cv_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-cv-camera]
|
||||
image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-image-proc]
|
||||
image_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-image-publisher]
|
||||
map_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-map-msgs]
|
||||
mavros_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros-msgs]
|
||||
pcl_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-msgs]
|
||||
pcl_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-conversions]
|
||||
polled_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-polled-camera]
|
||||
rqt_image_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-image-view]
|
||||
stereo_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-stereo-msgs]
|
||||
image_view:
|
||||
debian:
|
||||
buster: [ros-noetic-image-view]
|
||||
rosbridge_library:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-library]
|
||||
stereo_image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-stereo-image-proc]
|
||||
tf2_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-ros]
|
||||
depth_image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-depth-image-proc]
|
||||
mavros:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros]
|
||||
tf:
|
||||
debian:
|
||||
buster: [ros-noetic-tf]
|
||||
interactive_markers:
|
||||
debian:
|
||||
buster: [ros-noetic-interactive-markers]
|
||||
interactive_marker_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-interactive-marker-tutorials]
|
||||
laser_geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-geometry]
|
||||
laser_assembler:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-assembler]
|
||||
laser_filters:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-filters]
|
||||
pcl_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-ros]
|
||||
tf2_geometry_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-geometry-msgs]
|
||||
image_rotate:
|
||||
debian:
|
||||
buster: [ros-noetic-image-rotate]
|
||||
tf2_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-kdl]
|
||||
tf2_web_republisher:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-web-republisher]
|
||||
tf_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-tf-conversions]
|
||||
theora_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-theora-image-transport]
|
||||
turtlesim:
|
||||
debian:
|
||||
buster: [ros-noetic-turtlesim]
|
||||
turtle_actionlib:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-actionlib]
|
||||
turtle_tf:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-tf]
|
||||
turtle_tf2:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-tf2]
|
||||
urdf:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf]
|
||||
kdl_parser:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-parser]
|
||||
kdl_parser_py:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-parser-py]
|
||||
mavros_extras:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros-extras]
|
||||
robot_state_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-robot-state-publisher]
|
||||
rviz:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz]
|
||||
librviz_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-librviz-tutorial]
|
||||
rqt_rviz:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-rviz]
|
||||
rviz_plugin_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz-plugin-tutorials]
|
||||
rviz_python_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz-python-tutorial]
|
||||
urdf_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf-tutorial]
|
||||
usb_cam:
|
||||
debian:
|
||||
buster: [ros-noetic-usb-cam]
|
||||
visualization_marker_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-marker-tutorials]
|
||||
vl53l1x:
|
||||
debian:
|
||||
buster: [ros-noetic-vl53l1x]
|
||||
xacro:
|
||||
debian:
|
||||
buster: [ros-noetic-xacro]
|
||||
ddynamic_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-ddynamic-reconfigure]
|
||||
librealsense2:
|
||||
debian:
|
||||
buster: [ros-noetic-librealsense2]
|
||||
realsense2_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-realsense2-camera]
|
||||
realsense2_description:
|
||||
debian:
|
||||
buster: [ros-noetic-realsense2-description]
|
||||
|
||||
@@ -49,7 +49,7 @@ echo_stamp() {
|
||||
my_travis_retry() {
|
||||
local result=0
|
||||
local count=1
|
||||
local max_count=50
|
||||
local max_count=5
|
||||
while [ $count -le $max_count ]; do
|
||||
[ $result -ne 0 ] && {
|
||||
echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
|
||||
|
||||
@@ -37,3 +37,7 @@ apt-cache show openvpn
|
||||
|
||||
echo "Move /etc/ld.so.preload back to its original position"
|
||||
mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload
|
||||
|
||||
echo "Largest packages installed"
|
||||
sudo -E sh -c 'apt-get install -y debian-goodies'
|
||||
dpigs -H -n 100
|
||||
|
||||
@@ -80,11 +80,10 @@ catkin_python_setup()
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
add_message_files(
|
||||
FILES
|
||||
State.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
@@ -92,6 +91,9 @@ add_service_files(
|
||||
GetTelemetry.srv
|
||||
Navigate.srv
|
||||
NavigateGlobal.srv
|
||||
SetAltitude.srv
|
||||
SetYaw.srv
|
||||
SetYawRate.srv
|
||||
SetPosition.srv
|
||||
SetVelocity.srv
|
||||
SetAttitude.srv
|
||||
@@ -306,4 +308,5 @@ endif()
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
add_rostest(test/basic.test)
|
||||
add_rostest(test/offboard.test)
|
||||
endif()
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
# - cuts out a central square from the camera image;
|
||||
# - publishes this cropped image to the topic `/cv/center`;
|
||||
# - computes the average color of it;
|
||||
# - prints its name to the console.
|
||||
# - prints its name to the console.
|
||||
|
||||
import rospy
|
||||
import cv2
|
||||
|
||||
@@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
|
||||
land = rospy.ServiceProxy('land', Trigger)
|
||||
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
|
||||
frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
|
||||
frame_id=frame_id, auto_arm=auto_arm)
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
|
||||
|
||||
if not res.success:
|
||||
return res
|
||||
|
||||
97
clover/examples/red_circle.py
Normal file
97
clover/examples/red_circle.py
Normal file
@@ -0,0 +1,97 @@
|
||||
# This example makes the drone find and follow the red circle.
|
||||
# To test in the simulator, place 'Red Circle' model on the floor.
|
||||
# More information: https://clover.coex.tech/red_circle
|
||||
|
||||
# Input topic: main_camera/image_raw (camera image)
|
||||
# Output topics:
|
||||
# cv/mask (red color mask)
|
||||
# cv/red_circle (position of the center of the red circle in 3D space)
|
||||
|
||||
import rospy
|
||||
import cv2
|
||||
import numpy as np
|
||||
from math import nan
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from geometry_msgs.msg import PointStamped, Point
|
||||
from cv_bridge import CvBridge
|
||||
from clover import long_callback, srv
|
||||
import tf2_ros
|
||||
import tf2_geometry_msgs
|
||||
|
||||
rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c
|
||||
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
|
||||
bridge = CvBridge()
|
||||
|
||||
tf_buffer = tf2_ros.Buffer()
|
||||
tf_listener = tf2_ros.TransformListener(tf_buffer)
|
||||
|
||||
mask_pub = rospy.Publisher('~mask', Image, queue_size=1)
|
||||
point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1)
|
||||
|
||||
# read camera info
|
||||
camera_info = rospy.wait_for_message('main_camera/camera_info', CameraInfo)
|
||||
camera_matrix = np.float64(camera_info.K).reshape(3, 3)
|
||||
distortion = np.float64(camera_info.D).flatten()
|
||||
|
||||
|
||||
def img_xy_to_point(xy, dist):
|
||||
xy = cv2.undistortPoints(xy, camera_matrix, distortion, P=camera_matrix)[0][0]
|
||||
|
||||
# Shift points to center
|
||||
xy -= camera_info.width // 2, camera_info.height // 2
|
||||
|
||||
fx = camera_matrix[0, 0]
|
||||
fy = camera_matrix[1, 1]
|
||||
|
||||
return Point(x=xy[0] * dist / fx, y=xy[1] * dist / fy, z=dist)
|
||||
|
||||
def get_center_of_mass(mask):
|
||||
M = cv2.moments(mask)
|
||||
if M['m00'] == 0:
|
||||
return None
|
||||
return M['m10'] // M['m00'], M['m01'] // M['m00']
|
||||
|
||||
follow_red_circle = False
|
||||
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
img = bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# we need to use two ranges for red color
|
||||
mask1 = cv2.inRange(img_hsv, (0, 150, 150), (15, 255, 255))
|
||||
mask2 = cv2.inRange(img_hsv, (160, 150, 150), (180, 255, 255))
|
||||
|
||||
# combine two masks using bitwise OR
|
||||
mask = cv2.bitwise_or(mask1, mask2)
|
||||
|
||||
# publish the mask
|
||||
if mask_pub.get_num_connections() > 0:
|
||||
mask_pub.publish(bridge.cv2_to_imgmsg(mask, 'mono8'))
|
||||
|
||||
# calculate x and y of the circle
|
||||
xy = get_center_of_mass(mask)
|
||||
if xy is None:
|
||||
return
|
||||
|
||||
# calculate and publish the position of the circle in 3D space
|
||||
altitude = get_telemetry('terrain').z
|
||||
xy3d = img_xy_to_point(xy, altitude)
|
||||
target = PointStamped(header=msg.header, point=xy3d)
|
||||
point_pub.publish(target)
|
||||
|
||||
if follow_red_circle:
|
||||
# follow the target
|
||||
setpoint = tf_buffer.transform(target, 'map', timeout=rospy.Duration(0.2))
|
||||
set_position(x=setpoint.point.x, y=setpoint.point.y, z=nan, yaw=nan, frame_id=setpoint.header.frame_id)
|
||||
|
||||
# process each camera frame:
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
rospy.loginfo('Hit enter to follow the red circle')
|
||||
input()
|
||||
follow_red_circle = True
|
||||
rospy.spin()
|
||||
@@ -45,12 +45,13 @@
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<param name="calc_flow_gyro" value="true"/>
|
||||
<param name="roi_rad" value="0.8"/>
|
||||
<param name="disable_on_vpe" value="false"/>
|
||||
<param name="disable_on_vpe" value="true"/>
|
||||
</node>
|
||||
|
||||
<!-- simplified offboard control -->
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
|
||||
<param name="reference_frames/main_camera_optical" value="map"/>
|
||||
<param name="terrain_frame_mode" value="range"/>
|
||||
</node>
|
||||
|
||||
<!-- main camera -->
|
||||
|
||||
38
clover/msg/State.msg
Normal file
38
clover/msg/State.msg
Normal file
@@ -0,0 +1,38 @@
|
||||
uint8 MODE_NONE = 0
|
||||
uint8 MODE_NAVIGATE = 1
|
||||
uint8 MODE_NAVIGATE_GLOBAL = 2
|
||||
uint8 MODE_POSITION = 3
|
||||
uint8 MODE_VELOCITY = 4
|
||||
uint8 MODE_ATTITUDE = 5
|
||||
uint8 MODE_RATES = 6
|
||||
|
||||
uint8 YAW_MODE_YAW = 0
|
||||
uint8 YAW_MODE_YAW_RATE = 1
|
||||
uint8 YAW_MODE_YAW_TOWARDS = 2
|
||||
|
||||
# type of offboard control
|
||||
uint8 mode
|
||||
uint8 yaw_mode
|
||||
|
||||
# targets
|
||||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 speed
|
||||
float32 lat
|
||||
float32 lon
|
||||
float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 yaw_rate
|
||||
float32 thrust
|
||||
|
||||
# frames of reference
|
||||
string xy_frame_id
|
||||
string z_frame_id
|
||||
string yaw_frame_id
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>clover</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>The Clover package</description>
|
||||
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
|
||||
@@ -35,11 +35,8 @@ def print_current_map_position():
|
||||
dist = rospy.wait_for_message('rangefinder/range', Range).range
|
||||
print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
|
||||
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
|
||||
frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
|
||||
frame_id=frame_id, auto_arm=auto_arm)
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
|
||||
|
||||
if not res.success:
|
||||
return res
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
import rospy
|
||||
import math
|
||||
from math import nan
|
||||
from math import nan, inf
|
||||
import signal
|
||||
import sys
|
||||
from clover import srv
|
||||
@@ -15,6 +15,8 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
|
||||
navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
|
||||
set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
|
||||
set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
|
||||
set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
|
||||
set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
|
||||
set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
|
||||
@@ -28,11 +30,8 @@ def interrupt(sig, frame):
|
||||
|
||||
signal.signal(signal.SIGINT, interrupt)
|
||||
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
|
||||
frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
|
||||
frame_id=frame_id, auto_arm=auto_arm)
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
|
||||
|
||||
if not res.success:
|
||||
return res
|
||||
@@ -69,17 +68,17 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
|
||||
rospy.sleep(2)
|
||||
set_position(frame_id='body')
|
||||
|
||||
input('Rotate right 90° [enter] ')
|
||||
navigate(yaw=-math.pi / 2, frame_id='navigate_target')
|
||||
input('Rotate right 90° using set_yaw [enter] ')
|
||||
set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
|
||||
rospy.sleep(3)
|
||||
|
||||
input('Use set_attitude to fly backwards [enter]')
|
||||
set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
|
||||
set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
|
||||
rospy.sleep(0.3)
|
||||
set_position(frame_id='body')
|
||||
|
||||
input('Use set_attitude to fly right [enter]')
|
||||
set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
|
||||
set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
|
||||
rospy.sleep(0.5)
|
||||
set_position(frame_id='body')
|
||||
|
||||
@@ -88,13 +87,13 @@ set_rates(roll_rate=1.2, thrust=0.5)
|
||||
rospy.sleep(0.4)
|
||||
set_position(frame_id='body')
|
||||
|
||||
input('Rotate 360° to the right using yaw_rate [enter]')
|
||||
set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
|
||||
input('Rotate 360° to the right using set_yaw_rate [enter]')
|
||||
set_yaw_rate(yaw_rate=-1)
|
||||
rospy.sleep(2 * math.pi)
|
||||
set_position(frame_id='body')
|
||||
|
||||
input('Return to start point [enter]')
|
||||
navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
|
||||
input('Return to start point heading forward [enter]')
|
||||
navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
|
||||
|
||||
input('Land [enter]')
|
||||
land()
|
||||
|
||||
@@ -23,12 +23,14 @@
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <std_srvs/Trigger.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/QuaternionStamped.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <sensor_msgs/BatteryState.h>
|
||||
#include <sensor_msgs/Range.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/PositionTarget.h>
|
||||
@@ -37,14 +39,19 @@
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/StatusText.h>
|
||||
#include <mavros_msgs/ManualControl.h>
|
||||
#include <mavros_msgs/Altitude.h>
|
||||
|
||||
#include <clover/GetTelemetry.h>
|
||||
#include <clover/Navigate.h>
|
||||
#include <clover/NavigateGlobal.h>
|
||||
#include <clover/SetAltitude.h>
|
||||
#include <clover/SetYaw.h>
|
||||
#include <clover/SetYawRate.h>
|
||||
#include <clover/SetPosition.h>
|
||||
#include <clover/SetVelocity.h>
|
||||
#include <clover/SetAttitude.h>
|
||||
#include <clover/SetRates.h>
|
||||
#include <clover/State.h>
|
||||
|
||||
using std::string;
|
||||
using std::isnan;
|
||||
@@ -54,6 +61,7 @@ using namespace clover;
|
||||
using mavros_msgs::PositionTarget;
|
||||
using mavros_msgs::AttitudeTarget;
|
||||
using mavros_msgs::Thrust;
|
||||
using mavros_msgs::Altitude;
|
||||
|
||||
// tf2
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
@@ -79,35 +87,43 @@ float default_speed;
|
||||
bool auto_release;
|
||||
bool land_only_in_offboard, nav_from_sp, check_kill_switch;
|
||||
std::map<string, string> reference_frames;
|
||||
string terrain_frame_mode;
|
||||
|
||||
// Publishers
|
||||
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
|
||||
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
|
||||
|
||||
// Service clients
|
||||
ros::ServiceClient arming, set_mode;
|
||||
|
||||
// Containers
|
||||
ros::Timer setpoint_timer;
|
||||
tf::Quaternion tq;
|
||||
PoseStamped position_msg;
|
||||
PositionTarget position_raw_msg;
|
||||
AttitudeTarget att_raw_msg;
|
||||
Thrust thrust_msg;
|
||||
TwistStamped rates_msg;
|
||||
//TwistStamped rates_msg;
|
||||
TransformStamped target, setpoint;
|
||||
geometry_msgs::TransformStamped body;
|
||||
geometry_msgs::TransformStamped terrain;
|
||||
|
||||
// State
|
||||
PoseStamped nav_start;
|
||||
PoseStamped setpoint_position, setpoint_position_transformed;
|
||||
Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
|
||||
QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
|
||||
float setpoint_yaw_rate;
|
||||
PointStamped setpoint_position;
|
||||
PointStamped setpoint_altitude;
|
||||
Vector3Stamped setpoint_velocity;
|
||||
float setpoint_yaw, setpoint_roll, setpoint_pitch;
|
||||
Vector3 setpoint_rates;
|
||||
string yaw_frame_id;
|
||||
float setpoint_thrust;
|
||||
float nav_speed;
|
||||
float setpoint_lat = NAN, setpoint_lon = NAN;
|
||||
bool busy = false;
|
||||
bool wait_armed = false;
|
||||
bool nav_from_sp_flag = false;
|
||||
|
||||
// Last published
|
||||
PoseStamped setpoint_pose_local;
|
||||
Vector3Stamped setpoint_velocity_local;
|
||||
float yaw_local;
|
||||
|
||||
enum setpoint_type_t {
|
||||
NONE,
|
||||
NAVIGATE,
|
||||
@@ -115,7 +131,10 @@ enum setpoint_type_t {
|
||||
POSITION,
|
||||
VELOCITY,
|
||||
ATTITUDE,
|
||||
RATES
|
||||
RATES,
|
||||
_ALTITUDE,
|
||||
_YAW,
|
||||
_YAW_RATE,
|
||||
};
|
||||
|
||||
enum setpoint_type_t setpoint_type = NONE;
|
||||
@@ -170,7 +189,7 @@ void handleLocalPosition(const PoseStamped& pose)
|
||||
{
|
||||
local_position = pose;
|
||||
publishBodyFrame();
|
||||
// TODO: terrain?, home?
|
||||
// TODO: home?
|
||||
}
|
||||
|
||||
// wait for transform without interrupting publishing setpoints
|
||||
@@ -188,6 +207,29 @@ inline bool waitTransform(const string& target, const string& source,
|
||||
return false;
|
||||
}
|
||||
|
||||
void publishTerrain(const double distance, const ros::Time& stamp)
|
||||
{
|
||||
if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return;
|
||||
|
||||
auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp);
|
||||
t.child_frame_id = terrain.child_frame_id;
|
||||
t.transform.translation.z -= distance;
|
||||
static_transform_broadcaster->sendTransform(t);
|
||||
}
|
||||
|
||||
void handleAltitude(const Altitude& alt)
|
||||
{
|
||||
if (!std::isfinite(alt.bottom_clearance)) return;
|
||||
publishTerrain(alt.bottom_clearance, alt.header.stamp);
|
||||
}
|
||||
|
||||
void handleRange(const Range& range)
|
||||
{
|
||||
if (!std::isfinite(range.range)) return;
|
||||
// TODO: check it's facing down
|
||||
publishTerrain(range.range, range.header.stamp);
|
||||
}
|
||||
|
||||
#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
|
||||
|
||||
bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
|
||||
@@ -207,11 +249,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
|
||||
res.vx = NAN;
|
||||
res.vy = NAN;
|
||||
res.vz = NAN;
|
||||
res.pitch = NAN;
|
||||
res.roll = NAN;
|
||||
res.pitch = NAN;
|
||||
res.yaw = NAN;
|
||||
res.pitch_rate = NAN;
|
||||
res.roll_rate = NAN;
|
||||
res.pitch_rate = NAN;
|
||||
res.yaw_rate = NAN;
|
||||
res.voltage = NAN;
|
||||
res.cell_voltage = NAN;
|
||||
@@ -341,20 +383,20 @@ inline float getDistance(const Point& from, const Point& to)
|
||||
return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
|
||||
}
|
||||
|
||||
void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
|
||||
void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint)
|
||||
{
|
||||
if (wait_armed) {
|
||||
// don't start navigating if we're waiting arming
|
||||
nav_start.header.stamp = stamp;
|
||||
}
|
||||
|
||||
float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
|
||||
float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position);
|
||||
float time = distance / speed;
|
||||
float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
|
||||
|
||||
nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
|
||||
nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
|
||||
nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
|
||||
nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed;
|
||||
nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed;
|
||||
nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
|
||||
}
|
||||
|
||||
PoseStamped globalToLocal(double lat, double lon)
|
||||
@@ -385,44 +427,101 @@ PoseStamped globalToLocal(double lat, double lon)
|
||||
return pose;
|
||||
}
|
||||
|
||||
// publish navigate_target frame
|
||||
void publishTarget(ros::Time stamp, bool _static = false)
|
||||
{
|
||||
bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id);
|
||||
|
||||
// handle yaw for target frame
|
||||
if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate
|
||||
if (setpoint_altitude.header.frame_id == yaw_frame_id) {
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw);
|
||||
} else {
|
||||
single_frame = false;
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
} else if (setpoint_yaw_type == TOWARDS) {
|
||||
single_frame = false;
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
|
||||
if (_static && single_frame) {
|
||||
// publish at user's command, if all frames are the same
|
||||
target.header.frame_id = setpoint_position.header.frame_id;
|
||||
target.header.stamp = stamp;
|
||||
target.transform.translation.x = setpoint_position.point.x;
|
||||
target.transform.translation.y = setpoint_position.point.y;
|
||||
target.transform.translation.z = setpoint_position.point.z;
|
||||
|
||||
} else if (!_static) {
|
||||
// publish at each iteration, if frames are different
|
||||
target.header = setpoint_pose_local.header;
|
||||
target.transform.translation.x = setpoint_pose_local.pose.position.x;
|
||||
target.transform.translation.y = setpoint_pose_local.pose.position.y;
|
||||
target.transform.translation.z = setpoint_pose_local.pose.position.z;
|
||||
}
|
||||
|
||||
static_transform_broadcaster->sendTransform(target);
|
||||
}
|
||||
|
||||
void publish(const ros::Time stamp)
|
||||
{
|
||||
if (setpoint_type == NONE) return;
|
||||
|
||||
position_raw_msg.header.stamp = stamp;
|
||||
thrust_msg.header.stamp = stamp;
|
||||
rates_msg.header.stamp = stamp;
|
||||
|
||||
try {
|
||||
// transform position and/or yaw
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
|
||||
setpoint_position.header.stamp = stamp;
|
||||
tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
|
||||
// transform position
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
setpoint_position.header.stamp = stamp;
|
||||
setpoint_altitude.header.stamp = stamp;
|
||||
// transform xy
|
||||
try {
|
||||
auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05));
|
||||
setpoint_pose_local.header = xy.header;
|
||||
setpoint_pose_local.pose.position.x = xy.point.x;
|
||||
setpoint_pose_local.pose.position.y = xy.point.y;
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform xy, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
|
||||
// transform velocity
|
||||
if (setpoint_type == VELOCITY) {
|
||||
setpoint_velocity.header.stamp = stamp;
|
||||
tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
|
||||
// transform altitude
|
||||
try {
|
||||
setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform altitude, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
|
||||
} catch (const tf2::TransformException& e) {
|
||||
ROS_WARN_THROTTLE(10, "can't transform");
|
||||
}
|
||||
|
||||
// transform yaw
|
||||
if (setpoint_yaw_type == YAW) {
|
||||
try {
|
||||
QuaternionStamped q;
|
||||
q.header.stamp = stamp;
|
||||
q.header.frame_id = yaw_frame_id;
|
||||
q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw);
|
||||
yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion);
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform yaw, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
// compute navigate setpoint
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
|
||||
position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
|
||||
getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
|
||||
|
||||
if (setpoint_yaw_type == TOWARDS) {
|
||||
double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
|
||||
position_msg.pose.position.x - nav_start.pose.position.x);
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
|
||||
yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
|
||||
position_msg.pose.position.x - nav_start.pose.position.x);
|
||||
}
|
||||
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION) {
|
||||
position_msg = setpoint_position_transformed;
|
||||
position_msg = setpoint_pose_local;
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
|
||||
@@ -439,14 +538,14 @@ void publish(const ros::Time stamp)
|
||||
PositionTarget::IGNORE_AFY +
|
||||
PositionTarget::IGNORE_AFZ +
|
||||
PositionTarget::IGNORE_YAW;
|
||||
position_raw_msg.yaw_rate = setpoint_yaw_rate;
|
||||
position_raw_msg.yaw_rate = setpoint_rates.z;
|
||||
position_raw_msg.position = position_msg.pose.position;
|
||||
position_raw_pub.publish(position_raw_msg);
|
||||
}
|
||||
|
||||
// publish setpoint frame
|
||||
if (!setpoint.child_frame_id.empty()) {
|
||||
if (setpoint.header.stamp == position_msg.header.stamp) {
|
||||
if (setpoint.header.stamp >= position_msg.header.stamp) {
|
||||
return; // avoid TF_REPEATED_DATA warnings
|
||||
}
|
||||
|
||||
@@ -458,9 +557,22 @@ void publish(const ros::Time stamp)
|
||||
setpoint.header.stamp = position_msg.header.stamp;
|
||||
transform_broadcaster->sendTransform(setpoint);
|
||||
}
|
||||
|
||||
// publish dynamic target frame
|
||||
publishTarget(stamp);
|
||||
}
|
||||
|
||||
if (setpoint_type == VELOCITY) {
|
||||
// transform velocity to local frame
|
||||
setpoint_velocity.header.stamp = stamp;
|
||||
try {
|
||||
setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05));
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform velocity, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
|
||||
// publish velocity
|
||||
position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
|
||||
PositionTarget::IGNORE_PY +
|
||||
PositionTarget::IGNORE_PZ +
|
||||
@@ -468,14 +580,22 @@ void publish(const ros::Time stamp)
|
||||
PositionTarget::IGNORE_AFY +
|
||||
PositionTarget::IGNORE_AFZ;
|
||||
position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
|
||||
position_raw_msg.velocity = setpoint_velocity_transformed.vector;
|
||||
position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
|
||||
position_raw_msg.yaw_rate = setpoint_yaw_rate;
|
||||
position_raw_msg.velocity = setpoint_velocity_local.vector;
|
||||
position_raw_msg.yaw = yaw_local;
|
||||
position_raw_msg.yaw_rate = setpoint_rates.z;
|
||||
position_raw_pub.publish(position_raw_msg);
|
||||
}
|
||||
|
||||
if (setpoint_type == ATTITUDE) {
|
||||
attitude_pub.publish(setpoint_position_transformed);
|
||||
PoseStamped msg;
|
||||
msg.header.stamp = stamp;
|
||||
msg.header.frame_id = local_frame;
|
||||
msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local);
|
||||
attitude_pub.publish(msg);
|
||||
|
||||
Thrust thrust_msg;
|
||||
thrust_msg.header.stamp = stamp;
|
||||
thrust_msg.thrust = setpoint_thrust;
|
||||
thrust_pub.publish(thrust_msg);
|
||||
}
|
||||
|
||||
@@ -484,11 +604,12 @@ void publish(const ros::Time stamp)
|
||||
// thrust_pub.publish(thrust_msg);
|
||||
// mavros rates topics waits for rates in local frame
|
||||
// use rates in body frame for simplicity
|
||||
AttitudeTarget att_raw_msg;
|
||||
att_raw_msg.header.stamp = stamp;
|
||||
att_raw_msg.header.frame_id = fcu_frame;
|
||||
att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
|
||||
att_raw_msg.body_rate = rates_msg.twist.angular;
|
||||
att_raw_msg.thrust = thrust_msg.thrust;
|
||||
att_raw_msg.body_rate = setpoint_rates;
|
||||
att_raw_msg.thrust = setpoint_thrust;
|
||||
attitude_raw_pub.publish(att_raw_msg);
|
||||
}
|
||||
}
|
||||
@@ -528,10 +649,59 @@ inline void checkState()
|
||||
throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
|
||||
}
|
||||
|
||||
void publishState()
|
||||
{
|
||||
clover::State msg;
|
||||
msg.mode = setpoint_type;
|
||||
msg.yaw_mode = setpoint_yaw_type;
|
||||
|
||||
if (setpoint_position.header.frame_id.empty()) {
|
||||
msg.x = NAN;
|
||||
msg.y = NAN;
|
||||
msg.z = NAN;
|
||||
} else {
|
||||
msg.x = setpoint_position.point.x;
|
||||
msg.y = setpoint_position.point.y;
|
||||
msg.z = setpoint_altitude.point.z;
|
||||
}
|
||||
|
||||
msg.speed = nav_speed;
|
||||
msg.lat = setpoint_lat;
|
||||
msg.lon = setpoint_lon;
|
||||
msg.vx = setpoint_velocity.vector.x;
|
||||
msg.vy = setpoint_velocity.vector.y;
|
||||
msg.vz = setpoint_velocity.vector.z;
|
||||
msg.roll = setpoint_roll;
|
||||
msg.pitch = setpoint_pitch;
|
||||
msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
|
||||
|
||||
msg.roll_rate = setpoint_rates.x;
|
||||
msg.pitch_rate = setpoint_rates.y;
|
||||
msg.yaw_rate = setpoint_rates.z;
|
||||
msg.thrust = setpoint_thrust;
|
||||
|
||||
if (setpoint_type == VELOCITY) {
|
||||
msg.xy_frame_id = setpoint_velocity.header.frame_id;
|
||||
msg.z_frame_id = setpoint_velocity.header.frame_id;
|
||||
} else {
|
||||
msg.xy_frame_id = setpoint_position.header.frame_id;
|
||||
msg.z_frame_id = setpoint_altitude.header.frame_id;
|
||||
}
|
||||
msg.yaw_frame_id = yaw_frame_id;
|
||||
|
||||
state_pub.publish(msg);
|
||||
}
|
||||
|
||||
inline float safe(float value) {
|
||||
return std::isfinite(value) ? value : 0;
|
||||
}
|
||||
|
||||
#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
|
||||
|
||||
#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
|
||||
|
||||
bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
|
||||
float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
|
||||
float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
|
||||
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
|
||||
uint8_t& success, string& message) // editorconfig-checker-disable-line
|
||||
{
|
||||
@@ -558,69 +728,40 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
auto search = reference_frames.find(frame_id);
|
||||
const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
|
||||
|
||||
// Serve "partial" commands
|
||||
ENSURE_NON_INF(x);
|
||||
ENSURE_NON_INF(y);
|
||||
ENSURE_NON_INF(z);
|
||||
ENSURE_NON_INF(speed); // TODO: allow inf
|
||||
ENSURE_NON_INF(vx);
|
||||
ENSURE_NON_INF(vy);
|
||||
ENSURE_NON_INF(vz);
|
||||
ENSURE_NON_INF(roll);
|
||||
ENSURE_NON_INF(pitch);
|
||||
ENSURE_NON_INF(roll_rate);
|
||||
ENSURE_NON_INF(pitch_rate);
|
||||
ENSURE_NON_INF(yaw_rate);
|
||||
ENSURE_NON_INF(thrust);
|
||||
|
||||
if (!auto_arm && std::isfinite(yaw) &&
|
||||
isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
|
||||
isnan(pitch) && isnan(roll) && isnan(thrust) &&
|
||||
isnan(lat) && isnan(lon)) {
|
||||
// change only the yaw
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
|
||||
if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
|
||||
throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
|
||||
|
||||
message = "Changing yaw only";
|
||||
|
||||
QuaternionStamped q;
|
||||
q.header.frame_id = frame_id;
|
||||
q.header.stamp = stamp;
|
||||
q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
|
||||
setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
|
||||
setpoint_yaw_type = YAW;
|
||||
goto publish_setpoint;
|
||||
} else {
|
||||
throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
|
||||
}
|
||||
}
|
||||
|
||||
if (!auto_arm && std::isfinite(yaw_rate) &&
|
||||
isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
|
||||
isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
|
||||
isnan(lat) && isnan(lon)) {
|
||||
// change only the yaw rate
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
|
||||
message = "Changing yaw rate only";
|
||||
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_yaw_rate = yaw_rate;
|
||||
goto publish_setpoint;
|
||||
} else {
|
||||
throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
|
||||
}
|
||||
}
|
||||
|
||||
// Serve normal commands
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == POSITION) {
|
||||
ENSURE_FINITE(x);
|
||||
ENSURE_FINITE(y);
|
||||
ENSURE_FINITE(z);
|
||||
} else if (sp_type == NAVIGATE_GLOBAL) {
|
||||
if (sp_type == NAVIGATE_GLOBAL) {
|
||||
ENSURE_FINITE(lat);
|
||||
ENSURE_FINITE(lon);
|
||||
ENSURE_FINITE(z);
|
||||
} else if (sp_type == VELOCITY) {
|
||||
ENSURE_FINITE(vx);
|
||||
ENSURE_FINITE(vy);
|
||||
ENSURE_FINITE(vz);
|
||||
} else if (sp_type == ATTITUDE) {
|
||||
ENSURE_FINITE(pitch);
|
||||
ENSURE_FINITE(roll);
|
||||
ENSURE_FINITE(thrust);
|
||||
} else if (sp_type == RATES) {
|
||||
ENSURE_FINITE(pitch_rate);
|
||||
ENSURE_FINITE(roll_rate);
|
||||
ENSURE_FINITE(thrust);
|
||||
}
|
||||
|
||||
if (isfinite(x) != isfinite(y)) {
|
||||
throw std::runtime_error("x and y can be set only together");
|
||||
}
|
||||
|
||||
if (isfinite(yaw_rate)) {
|
||||
if (sp_type > RATES && setpoint_type == ATTITUDE) {
|
||||
throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
|
||||
}
|
||||
}
|
||||
|
||||
// set_altitude
|
||||
if (sp_type == _ALTITUDE) {
|
||||
if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
|
||||
throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode.");
|
||||
}
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
@@ -634,20 +775,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
speed = default_speed;
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
||||
if (yaw_rate != 0 && !std::isnan(yaw))
|
||||
throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
|
||||
|
||||
if (std::isnan(yaw_rate) && std::isnan(yaw))
|
||||
throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE_GLOBAL) {
|
||||
if (TIMEOUT(global_position, global_position_timeout))
|
||||
throw std::runtime_error("No global position");
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// if any value need to be transformed to reference frame
|
||||
if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) {
|
||||
// make sure transform from frame_id to reference frame available
|
||||
if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
|
||||
throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
|
||||
@@ -664,15 +798,27 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
|
||||
x = xy_in_req_frame.pose.position.x;
|
||||
y = xy_in_req_frame.pose.position.y;
|
||||
setpoint_lat = lat;
|
||||
setpoint_lon = lon;
|
||||
}
|
||||
|
||||
// Everything fine - switch setpoint type
|
||||
setpoint_type = sp_type;
|
||||
if (sp_type <= RATES) {
|
||||
setpoint_type = sp_type;
|
||||
}
|
||||
|
||||
if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
|
||||
if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
|
||||
nav_from_sp_flag = false;
|
||||
}
|
||||
|
||||
bool to_auto_arm = auto_arm && (state.mode != "OFFBOARD" || !state.armed);
|
||||
if (to_auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
|
||||
// invalidate position setpoint
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
// starting point
|
||||
if (nav_from_sp && nav_from_sp_flag) {
|
||||
@@ -681,89 +827,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
} else {
|
||||
nav_start = local_position;
|
||||
}
|
||||
nav_speed = speed;
|
||||
|
||||
if (!isnan(speed)) {
|
||||
nav_speed = speed;
|
||||
}
|
||||
|
||||
nav_from_sp_flag = true;
|
||||
}
|
||||
|
||||
// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
||||
// if (std::isnan(yaw) && yaw_rate == 0) {
|
||||
// // keep yaw unchanged
|
||||
// // TODO: this is incorrect, because we need yaw in desired frame
|
||||
// yaw = tf2::getYaw(local_position.pose.orientation);
|
||||
// }
|
||||
// }
|
||||
// handle position
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
|
||||
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// destination point and/or attitude
|
||||
PoseStamped ps;
|
||||
ps.header.frame_id = frame_id;
|
||||
ps.header.stamp = stamp;
|
||||
ps.pose.position.x = x;
|
||||
ps.pose.position.y = y;
|
||||
ps.pose.position.z = z;
|
||||
ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
|
||||
PointStamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.point.x = safe(x);
|
||||
desired.point.y = safe(y);
|
||||
desired.point.z = safe(z);
|
||||
|
||||
if (sp_type == ATTITUDE) {
|
||||
ps.pose.position.x = 0;
|
||||
ps.pose.position.y = 0;
|
||||
ps.pose.position.z = 0;
|
||||
ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
|
||||
} else if (std::isnan(yaw)) {
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_yaw_rate = yaw_rate;
|
||||
} else if (std::isinf(yaw) && yaw > 0) {
|
||||
// yaw towards
|
||||
setpoint_yaw_type = TOWARDS;
|
||||
yaw = 0;
|
||||
setpoint_yaw_rate = 0;
|
||||
} else {
|
||||
setpoint_yaw_type = YAW;
|
||||
setpoint_yaw_rate = 0;
|
||||
ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
|
||||
// set horizontal position
|
||||
if (isfinite(x) && isfinite(y)) {
|
||||
setpoint_position = desired;
|
||||
} else if (setpoint_position.header.frame_id.empty()) {
|
||||
// TODO: use transform for current stamp
|
||||
setpoint_position.header = local_position.header;
|
||||
setpoint_position.point = local_position.pose.position;
|
||||
}
|
||||
|
||||
tf_buffer.transform(ps, setpoint_position, reference_frame);
|
||||
// set altitude
|
||||
if (isfinite(z)) {
|
||||
setpoint_altitude = desired;
|
||||
} else if (setpoint_altitude.header.frame_id.empty()) {
|
||||
setpoint_altitude.header = local_position.header;
|
||||
setpoint_altitude.point = local_position.pose.position;
|
||||
}
|
||||
}
|
||||
|
||||
// handle velocity
|
||||
if (sp_type == VELOCITY) {
|
||||
Vector3Stamped vel;
|
||||
vel.header.frame_id = frame_id;
|
||||
vel.header.stamp = stamp;
|
||||
vel.vector.x = vx;
|
||||
vel.vector.y = vy;
|
||||
vel.vector.z = vz;
|
||||
tf_buffer.transform(vel, setpoint_velocity, reference_frame);
|
||||
// TODO: allow setting different modes by altitude and xy
|
||||
Vector3Stamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.vector.x = safe(vx);
|
||||
desired.vector.y = safe(vy);
|
||||
desired.vector.z = safe(vz);
|
||||
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
setpoint_velocity.header = desired.header;
|
||||
|
||||
// set horizontal velocity
|
||||
if (isfinite(vx) && isfinite(vy)) {
|
||||
setpoint_velocity.vector.x = desired.vector.x;
|
||||
setpoint_velocity.vector.y = desired.vector.y;
|
||||
}
|
||||
|
||||
// set vertical velocity
|
||||
if (isfinite(vz)) {
|
||||
setpoint_velocity.vector.z = desired.vector.z;
|
||||
}
|
||||
}
|
||||
|
||||
if (sp_type == ATTITUDE || sp_type == RATES) {
|
||||
thrust_msg.thrust = thrust;
|
||||
// handle yaw
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) {
|
||||
if (isfinite(yaw)) {
|
||||
setpoint_yaw_type = YAW;
|
||||
QuaternionStamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.quaternion = tf::createQuaternionMsgFromYaw(yaw);
|
||||
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
setpoint_yaw = tf2::getYaw(desired.quaternion);
|
||||
yaw_frame_id = reference_frame;
|
||||
|
||||
} else if (isinf(yaw) && yaw > 0) {
|
||||
// yaw towards
|
||||
setpoint_yaw_type = TOWARDS;
|
||||
|
||||
} else if (yaw_frame_id.empty() || sp_type == _YAW) {
|
||||
// yaw is nan and not set previously OR set_yaw(yaw=nan) was called
|
||||
setpoint_yaw_type = YAW;
|
||||
setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw
|
||||
yaw_frame_id = local_position.header.frame_id;
|
||||
}
|
||||
}
|
||||
|
||||
if (sp_type == RATES) {
|
||||
rates_msg.twist.angular.x = roll_rate;
|
||||
rates_msg.twist.angular.y = pitch_rate;
|
||||
rates_msg.twist.angular.z = yaw_rate;
|
||||
// handle roll
|
||||
if (isfinite(roll)) {
|
||||
setpoint_roll = roll;
|
||||
}
|
||||
|
||||
// handle pitch
|
||||
if (isfinite(pitch)) {
|
||||
setpoint_pitch = pitch;
|
||||
}
|
||||
|
||||
// handle yaw rate
|
||||
if (isfinite(yaw_rate)) {
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_rates.z = yaw_rate;
|
||||
}
|
||||
|
||||
// handle pitch rate
|
||||
if (isfinite(roll_rate)) {
|
||||
setpoint_rates.x = roll_rate;
|
||||
}
|
||||
|
||||
// handle roll rate
|
||||
if (isfinite(pitch_rate)) {
|
||||
setpoint_rates.y = pitch_rate;
|
||||
}
|
||||
|
||||
// handle thrust
|
||||
if (isfinite(thrust)) {
|
||||
setpoint_thrust = thrust;
|
||||
}
|
||||
|
||||
wait_armed = auto_arm;
|
||||
|
||||
publish_setpoint:
|
||||
publish(stamp); // calculate initial transformed messages first
|
||||
setpoint_timer.start();
|
||||
|
||||
// publish target frame
|
||||
if (!target.child_frame_id.empty()) {
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
target.header.frame_id = setpoint_position.header.frame_id;
|
||||
target.header.stamp = stamp;
|
||||
target.transform.translation.x = setpoint_position.pose.position.x;
|
||||
target.transform.translation.y = setpoint_position.pose.position.y;
|
||||
target.transform.translation.z = setpoint_position.pose.position.z;
|
||||
target.transform.rotation = setpoint_position.pose.orientation;
|
||||
static_transform_broadcaster->sendTransform(target);
|
||||
}
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
publishTarget(stamp, true);
|
||||
}
|
||||
|
||||
publishState();
|
||||
|
||||
if (auto_arm) {
|
||||
offboardAndArm();
|
||||
wait_armed = false;
|
||||
@@ -788,27 +984,39 @@ publish_setpoint:
|
||||
}
|
||||
|
||||
bool navigate(Navigate::Request& req, Navigate::Response& res) {
|
||||
return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
|
||||
return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) {
|
||||
return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setYaw(SetYaw::Request& req, SetYaw::Response& res) {
|
||||
return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) {
|
||||
return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
|
||||
return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
|
||||
return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
|
||||
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setRates(SetRates::Request& req, SetRates::Response& res) {
|
||||
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
|
||||
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
@@ -840,9 +1048,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
auto start = ros::Time::now();
|
||||
while (ros::ok()) {
|
||||
if (state.mode == "AUTO.LAND") {
|
||||
res.success = true;
|
||||
busy = false;
|
||||
return true;
|
||||
break;
|
||||
}
|
||||
if (ros::Time::now() - start > land_timeout)
|
||||
throw std::runtime_error("Land request timed out");
|
||||
@@ -851,6 +1057,18 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
// stop setpoints and invalidate position setpoint
|
||||
setpoint_timer.stop();
|
||||
setpoint_type = NONE;
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
publishState();
|
||||
|
||||
res.success = true;
|
||||
busy = false;
|
||||
return true;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
res.message = e.what();
|
||||
ROS_INFO("%s", e.what());
|
||||
@@ -863,6 +1081,11 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
{
|
||||
setpoint_timer.stop();
|
||||
setpoint_type = NONE;
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
publishState();
|
||||
res.success = true;
|
||||
return true;
|
||||
}
|
||||
@@ -888,6 +1111,8 @@ int main(int argc, char **argv)
|
||||
nh_priv.param("check_kill_switch", check_kill_switch, true);
|
||||
nh_priv.param("default_speed", default_speed, 0.5f);
|
||||
nh_priv.param<string>("body_frame", body.child_frame_id, "body");
|
||||
nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
|
||||
nh_priv.param<string>("terrain_frame_mode", terrain_frame_mode, "altitude");
|
||||
nh_priv.getParam("reference_frames", reference_frames);
|
||||
|
||||
// Default reference frames
|
||||
@@ -923,6 +1148,20 @@ int main(int argc, char **argv)
|
||||
auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
|
||||
auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
|
||||
|
||||
ros::Subscriber altitude_sub;
|
||||
if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
|
||||
terrain.header.frame_id = local_frame;
|
||||
if (terrain_frame_mode == "altitude") {
|
||||
altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
|
||||
} else if (terrain_frame_mode == "range") {
|
||||
string range_topic = nh_priv.param("range_topic", string("rangefinder/range"));
|
||||
altitude_sub = nh.subscribe(range_topic, 1, &handleRange);
|
||||
} else {
|
||||
ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str());
|
||||
ros::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
// Setpoint publishers
|
||||
position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
|
||||
position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
|
||||
@@ -931,10 +1170,16 @@ int main(int argc, char **argv)
|
||||
rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
|
||||
thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
|
||||
|
||||
// State publisher
|
||||
state_pub = nh_priv.advertise<clover::State>("state", 1, true);
|
||||
|
||||
// Service servers
|
||||
auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
|
||||
auto na_serv = nh.advertiseService("navigate", &navigate);
|
||||
auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
|
||||
auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
|
||||
auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
|
||||
auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
|
||||
auto sp_serv = nh.advertiseService("set_position", &setPosition);
|
||||
auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
|
||||
auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
|
||||
@@ -948,7 +1193,7 @@ int main(int argc, char **argv)
|
||||
position_msg.header.frame_id = local_frame;
|
||||
position_raw_msg.header.frame_id = local_frame;
|
||||
position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
|
||||
rates_msg.header.frame_id = fcu_frame;
|
||||
//rates_msg.header.frame_id = fcu_frame;
|
||||
|
||||
ROS_INFO("ready");
|
||||
ros::spin();
|
||||
|
||||
@@ -13,11 +13,11 @@ float32 alt
|
||||
float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 pitch
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32 pitch_rate
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 yaw_rate
|
||||
float32 voltage
|
||||
float32 cell_voltage
|
||||
|
||||
@@ -2,7 +2,6 @@ float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
|
||||
@@ -2,7 +2,6 @@ float64 lat
|
||||
float64 lon
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
|
||||
5
clover/srv/SetAltitude.srv
Normal file
5
clover/srv/SetAltitude.srv
Normal file
@@ -0,0 +1,5 @@
|
||||
float32 z
|
||||
string frame_id
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -1,5 +1,5 @@
|
||||
float32 pitch
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32 thrust
|
||||
string frame_id
|
||||
|
||||
@@ -2,7 +2,6 @@ float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
---
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
float32 pitch_rate
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 yaw_rate
|
||||
float32 thrust
|
||||
bool auto_arm
|
||||
|
||||
@@ -2,7 +2,6 @@ float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
---
|
||||
|
||||
5
clover/srv/SetYaw.srv
Normal file
5
clover/srv/SetYaw.srv
Normal file
@@ -0,0 +1,5 @@
|
||||
float32 yaw
|
||||
string frame_id
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
4
clover/srv/SetYawRate.srv
Normal file
4
clover/srv/SetYawRate.srv
Normal file
@@ -0,0 +1,4 @@
|
||||
float32 yaw_rate
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
437
clover/test/offboard.py
Executable file
437
clover/test/offboard.py
Executable file
@@ -0,0 +1,437 @@
|
||||
import rospy
|
||||
import pytest
|
||||
from pytest import approx
|
||||
import threading
|
||||
import mavros_msgs.msg
|
||||
from mavros_msgs.srv import SetMode
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from clover import srv
|
||||
from clover.msg import State
|
||||
from std_srvs.srv import Trigger
|
||||
from math import nan, inf
|
||||
import tf2_ros
|
||||
import tf2_geometry_msgs
|
||||
|
||||
@pytest.fixture()
|
||||
def node():
|
||||
return rospy.init_node('offboard_test', anonymous=True)
|
||||
|
||||
@pytest.fixture
|
||||
def tf_buffer():
|
||||
buf = tf2_ros.Buffer()
|
||||
tf2_ros.TransformListener(buf)
|
||||
return buf
|
||||
|
||||
def get_state():
|
||||
return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
|
||||
|
||||
def get_navigate_target(tf_buffer):
|
||||
target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1))
|
||||
assert target.child_frame_id == 'navigate_target'
|
||||
return target
|
||||
|
||||
def test_offboard(node, tf_buffer):
|
||||
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
|
||||
set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
|
||||
set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
|
||||
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
|
||||
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
land = rospy.ServiceProxy('land', Trigger)
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('State timeout')
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == False
|
||||
|
||||
# mocked state publisher
|
||||
state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
|
||||
state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
|
||||
|
||||
def publish_state():
|
||||
r = rospy.Rate(2)
|
||||
while not rospy.is_shutdown():
|
||||
state_msg.header.stamp = rospy.Time.now()
|
||||
state_pub.publish(state_msg)
|
||||
r.sleep()
|
||||
|
||||
# start publishing state
|
||||
threading.Thread(target=publish_state, daemon=True).start()
|
||||
rospy.sleep(0.5)
|
||||
|
||||
# set_mode service mock
|
||||
def set_mode(req):
|
||||
state_msg.mode = req.custom_mode # set mocked mode to requested
|
||||
return True,
|
||||
|
||||
rospy.Service('/mavros/set_mode', SetMode, set_mode)
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == False
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('No connection to FCU')
|
||||
|
||||
state_msg.connected = True
|
||||
rospy.sleep(1)
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == True
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('No local position')
|
||||
|
||||
local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1)
|
||||
local_position_msg = PoseStamped()
|
||||
local_position_msg.header.frame_id = 'map'
|
||||
local_position_msg.pose.position.x = 1
|
||||
local_position_msg.pose.position.y = 2
|
||||
local_position_msg.pose.position.z = 3
|
||||
local_position_msg.pose.orientation.w = 1
|
||||
|
||||
def publish_local_position():
|
||||
r = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
local_position_msg.header.stamp = rospy.Time.now()
|
||||
local_position_pub.publish(local_position_msg)
|
||||
r.sleep()
|
||||
|
||||
# start publishing local position
|
||||
threading.Thread(target=publish_local_position, daemon=True).start()
|
||||
rospy.sleep(0.5)
|
||||
|
||||
# check body frame
|
||||
body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1))
|
||||
assert body.child_frame_id == 'body'
|
||||
assert body.transform.translation.x == approx(1)
|
||||
assert body.transform.translation.y == approx(2)
|
||||
assert body.transform.translation.z == approx(3)
|
||||
|
||||
res = navigate(x=3, y=2, z=1, frame_id='map')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 3
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
target = get_navigate_target(tf_buffer)
|
||||
assert target.header.frame_id == 'map'
|
||||
assert target.transform.translation.x == approx(3)
|
||||
assert target.transform.translation.y == approx(2)
|
||||
assert target.transform.translation.z == approx(1)
|
||||
assert target.transform.rotation.x == 0
|
||||
assert target.transform.rotation.y == 0
|
||||
assert target.transform.rotation.z == 0
|
||||
assert target.transform.rotation.w == 1
|
||||
|
||||
# try to set only the y
|
||||
res = navigate(x=nan, y=1, z=nan)
|
||||
assert res.success == False
|
||||
assert res.message.startswith('x and y can be set only together')
|
||||
|
||||
# set z in body frame
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='body')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 3
|
||||
assert state.y == 2
|
||||
assert state.z == 4
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set xy in test frame
|
||||
res = navigate(x=1, y=2, z=nan, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 4
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'test'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'test'
|
||||
|
||||
# auto_arm should not invalidate the setpoint if not effective
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'test'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# auto_arm should invalidate the setpoint if effective
|
||||
state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode
|
||||
rospy.sleep(1)
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
state_msg.mode = 'OFFBOARD'
|
||||
rospy.sleep(1)
|
||||
|
||||
# set_attitude should invalidate the setpoint
|
||||
res = set_attitude()
|
||||
assert res.success == True
|
||||
|
||||
res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_altitude
|
||||
res = set_altitude(z=7, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_yaw
|
||||
res = set_yaw(yaw=0.5, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw == 0.5
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# test set_yaw_rate
|
||||
res = set_yaw_rate(yaw_rate=2)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw_rate == 2
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
|
||||
# navigate(yaw=nan) should keep yaw rate mode
|
||||
res = navigate(x=nan, y=nan, z=nan, yaw=nan)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw_rate == 2
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
|
||||
# set_yaw(nan) should change back to yaw mode
|
||||
res = set_yaw(yaw=nan)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.yaw == 0
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_position
|
||||
res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 13
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test2'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_altitude should not change the mode
|
||||
res = set_altitude(z=3, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_yaw should not change the main mode
|
||||
res = set_yaw(yaw=1, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 1
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# test set_velocity
|
||||
res = set_velocity(vx=1, frame_id='body')
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_VELOCITY
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.vx == 1
|
||||
assert state.vy == 0
|
||||
assert state.vz == 0
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_altitude should not work in velocity mode
|
||||
res = set_altitude(z=3, frame_id='test')
|
||||
assert res.success == False
|
||||
assert res.message.startswith('Altitude cannot be set in')
|
||||
|
||||
# test set_attitude
|
||||
res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_ATTITUDE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.roll == approx(0.1)
|
||||
assert state.pitch == approx(0.2)
|
||||
assert state.yaw == approx(0.3)
|
||||
assert state.thrust == approx(0.5)
|
||||
assert state.yaw_frame_id == 'map'
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3)
|
||||
# Tait-Bryan ZYX angle (rzyx) converted to quaternion
|
||||
assert msg.pose.orientation.x == approx(0.0342708)
|
||||
assert msg.pose.orientation.y == approx(0.10602051)
|
||||
assert msg.pose.orientation.z == approx(0.14357218)
|
||||
assert msg.pose.orientation.w == approx(0.98334744)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3)
|
||||
assert msg.thrust == approx(0.5)
|
||||
|
||||
# set_yaw should work in attitude mode
|
||||
res = set_yaw(yaw=0.7, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_ATTITUDE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.roll == approx(0.1)
|
||||
assert state.pitch == approx(0.2)
|
||||
assert state.yaw == approx(0.7)
|
||||
assert state.thrust == approx(0.5)
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# set_yaw_rate should not work in attitude mode
|
||||
res = set_yaw_rate(yaw_rate=0.3)
|
||||
assert res.success == False
|
||||
assert res.message.startswith('Yaw rate cannot be set in')
|
||||
|
||||
# test set_rates
|
||||
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0)
|
||||
assert state.pitch_rate == approx(0)
|
||||
assert state.yaw_rate == approx(0.3)
|
||||
assert state.thrust == approx(0.6)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
|
||||
assert msg.thrust == approx(0.6)
|
||||
|
||||
res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.1)
|
||||
assert state.thrust == approx(0.4)
|
||||
|
||||
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.1)
|
||||
assert state.thrust == approx(0.3)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
|
||||
assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE
|
||||
assert msg.body_rate.x == approx(0.3)
|
||||
assert msg.body_rate.y == approx(0.2)
|
||||
assert msg.body_rate.z == approx(0.1)
|
||||
|
||||
# set_yaw_rate should work in rates mode
|
||||
res = set_yaw_rate(yaw_rate=0.4)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.4)
|
||||
assert state.thrust == approx(0.3)
|
||||
|
||||
res = set_rates(roll_rate=inf)
|
||||
assert res.success == False
|
||||
assert res.message == 'roll_rate argument cannot be Inf'
|
||||
|
||||
# test land service
|
||||
res = land()
|
||||
assert res.success == True
|
||||
assert state_msg.mode == 'AUTO.LAND' # check that the mode was set correctly
|
||||
10
clover/test/offboard.test
Normal file
10
clover/test/offboard.test
Normal file
@@ -0,0 +1,10 @@
|
||||
<launch>
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
|
||||
|
||||
<param name="test_module" value="$(find clover)/test/offboard.py"/>
|
||||
<test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
|
||||
</launch>
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>clover_blocks</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>Blockly programming support for Clover</description>
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
@@ -15,6 +15,7 @@ const COLOR_GPIO = 200;
|
||||
const DOCS_URL = 'https://clover.coex.tech/en/blocks.html';
|
||||
|
||||
var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]];
|
||||
var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]);
|
||||
|
||||
function considerFrameId(e) {
|
||||
if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return;
|
||||
@@ -22,7 +23,7 @@ function considerFrameId(e) {
|
||||
var frameId = this.getFieldValue('FRAME_ID');
|
||||
// set appropriate coordinates labels
|
||||
if (this.getInput('X')) { // block has x-y-z fields
|
||||
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') {
|
||||
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
|
||||
this.getInput('X').fieldRow[0].setValue('forward');
|
||||
this.getInput('Y').fieldRow[0].setValue('left');
|
||||
this.getInput('Z').fieldRow[0].setValue('up');
|
||||
@@ -59,8 +60,8 @@ function updateSetpointBlock(e) {
|
||||
this.getInput('VY').setVisible(velocity);
|
||||
this.getInput('VZ').setVisible(velocity);
|
||||
this.getInput('YAW').setVisible(attitude);
|
||||
this.getInput('PITCH').setVisible(attitude);
|
||||
this.getInput('ROLL').setVisible(attitude);
|
||||
this.getInput('PITCH').setVisible(attitude);
|
||||
this.getInput('THRUST').setVisible(attitude);
|
||||
this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
|
||||
|
||||
@@ -73,7 +74,7 @@ function updateSetpointBlock(e) {
|
||||
|
||||
Blockly.Blocks['navigate'] = {
|
||||
init: function () {
|
||||
let navFrameId = frameIds.slice();
|
||||
let navFrameId = frameIdsWithTerrain.slice();
|
||||
navFrameId.push(['global', 'GLOBAL_LOCAL'])
|
||||
navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
|
||||
this.appendDummyInput()
|
||||
@@ -163,14 +164,14 @@ Blockly.Blocks['setpoint'] = {
|
||||
this.appendValueInput("VZ")
|
||||
.setCheck("Number")
|
||||
.appendField("vz");
|
||||
this.appendValueInput("PITCH")
|
||||
.setCheck("Number")
|
||||
.appendField("pitch")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("ROLL")
|
||||
.setCheck("Number")
|
||||
.appendField("roll")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("PITCH")
|
||||
.setCheck("Number")
|
||||
.appendField("pitch")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("YAW")
|
||||
.setCheck("Number")
|
||||
.appendField("yaw")
|
||||
@@ -213,7 +214,7 @@ Blockly.Blocks['get_position'] = {
|
||||
.appendField("current")
|
||||
.appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD")
|
||||
.appendField("relative to")
|
||||
.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
|
||||
.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
|
||||
this.appendValueInput("ID")
|
||||
.setCheck("Number")
|
||||
.appendField("with ID")
|
||||
@@ -247,7 +248,7 @@ Blockly.Blocks['get_attitude'] = {
|
||||
init: function () {
|
||||
this.appendDummyInput()
|
||||
.appendField("current")
|
||||
.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
|
||||
.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
|
||||
this.setOutput(true, "Number");
|
||||
this.setColour(COLOR_STATE);
|
||||
this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian).");
|
||||
@@ -268,6 +269,19 @@ Blockly.Blocks['voltage'] = {
|
||||
}
|
||||
};
|
||||
|
||||
Blockly.Blocks['get_rc'] = {
|
||||
init: function () {
|
||||
this.appendDummyInput()
|
||||
.appendField("RC channel")
|
||||
this.appendValueInput("CHANNEL")
|
||||
.setCheck("Number");
|
||||
this.setInputsInline(true);
|
||||
this.setOutput(true, "Number");
|
||||
this.setColour(COLOR_STATE);
|
||||
this.setTooltip("Returns current RC channel value.");
|
||||
this.setHelpUrl(DOCS_URL + '#' + this.type);
|
||||
}
|
||||
}
|
||||
|
||||
Blockly.Blocks['armed'] = {
|
||||
init: function () {
|
||||
@@ -509,7 +523,7 @@ Blockly.Blocks['distance'] = {
|
||||
.appendField("z");
|
||||
this.appendDummyInput()
|
||||
.appendField("relative to")
|
||||
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID");
|
||||
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID");
|
||||
this.appendValueInput("ID")
|
||||
.setCheck("Number")
|
||||
.appendField("with ID")
|
||||
|
||||
@@ -69,8 +69,8 @@
|
||||
<value name="VX"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="VY"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="VZ"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="YAW"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="THRUST"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
|
||||
<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
@@ -100,6 +100,9 @@
|
||||
<block type="mode"></block>
|
||||
<block type="armed"></block>
|
||||
<block type="voltage"></block>
|
||||
<block type="get_rc">
|
||||
<value name="CHANNEL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
</block>
|
||||
</category>
|
||||
<category name="LED" colour="#02d754">
|
||||
<block type="set_effect">
|
||||
|
||||
@@ -81,7 +81,10 @@ function generateROSDefinitions() {
|
||||
code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`;
|
||||
code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`;
|
||||
if (rosDefinitions.navigateGlobal) {
|
||||
code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
|
||||
code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
|
||||
}
|
||||
if (rosDefinitions.setYaw) {
|
||||
code += `set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)\n`;
|
||||
}
|
||||
if (rosDefinitions.setVelocity) {
|
||||
code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`;
|
||||
@@ -276,10 +279,11 @@ Blockly.Python.angle = function(block) {
|
||||
}
|
||||
|
||||
Blockly.Python.set_yaw = function(block) {
|
||||
rosDefinitions.setYaw = true;
|
||||
simpleOffboard();
|
||||
let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE);
|
||||
let frameId = buildFrameId(block);
|
||||
let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
|
||||
let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
|
||||
if (block.getFieldValue('WAIT') == 'TRUE') {
|
||||
rosDefinitions.waitYaw = true;
|
||||
simpleOffboard();
|
||||
@@ -328,11 +332,11 @@ Blockly.Python.setpoint = function(block) {
|
||||
} else if (type == 'ATTITUDE') {
|
||||
rosDefinitions.setAttitude = true;
|
||||
simpleOffboard();
|
||||
return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
|
||||
return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
|
||||
} else if (type == 'RATES') {
|
||||
rosDefinitions.setRates = true;
|
||||
simpleOffboard();
|
||||
return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
|
||||
return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -398,6 +402,12 @@ Blockly.Python.voltage = function(block) {
|
||||
return [code, Blockly.Python.ORDER_FUNCTION_CALL];
|
||||
}
|
||||
|
||||
Blockly.Python.get_rc = function(block) {
|
||||
Blockly.Python.definitions_['import_rcin'] = 'from mavros_msgs.msg import RCIn';
|
||||
var channel = Blockly.Python.valueToCode(block, 'CHANNEL', Blockly.Python.ORDER_NONE);
|
||||
return [`rospy.wait_for_message('mavros/rc/in', RCIn).channels[${channel}]`, Blockly.Python.ORDER_FUNCTION_CALL]
|
||||
}
|
||||
|
||||
function parseColor(color) {
|
||||
return {
|
||||
r: parseInt(color.substr(2, 2), 16),
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>clover_description</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
|
||||
|
||||
<maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>
|
||||
|
||||
@@ -31,7 +31,7 @@ param set-default EKF2_OF_DELAY 0
|
||||
param set-default EKF2_OF_QMIN 10
|
||||
param set-default EKF2_OF_N_MIN 0.05
|
||||
param set-default EKF2_OF_N_MAX 0.2
|
||||
param set-default EKF2_HGT_MODE 2 # 0 = baro, 1 = gps, 2 = range, 3 = vision
|
||||
param set-default EKF2_HGT_MODE 3 # 0 = baro, 1 = gps, 2 = range, 3 = vision
|
||||
param set-default EKF2_EVA_NOISE 0.1
|
||||
param set-default EKF2_EVP_NOISE 0.1
|
||||
param set-default EKF2_EV_DELAY 0
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
material red_circle
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
scene_blend alpha_blend
|
||||
texture_unit
|
||||
{
|
||||
texture red_circle.png
|
||||
filtering none
|
||||
scale 1.0 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 7.9 KiB |
13
clover_simulation/models/red_circle/model.config
Normal file
13
clover_simulation/models/red_circle/model.config
Normal file
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Red Circle</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.5">red_circle.sdf</sdf>
|
||||
<author>
|
||||
<name>Oleg Kalachev</name>
|
||||
<email>okalachev@gmail.com</email>
|
||||
</author>
|
||||
<description>
|
||||
Red circle of size 40 cm on the floor for recognizing by a drone
|
||||
</description>
|
||||
</model>
|
||||
24
clover_simulation/models/red_circle/red_circle.sdf
Normal file
24
clover_simulation/models/red_circle/red_circle.sdf
Normal file
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.5">
|
||||
<model name="red_circle">
|
||||
<static>true</static>
|
||||
<link name="red_circle_link">
|
||||
<pose>0 0 1e-3 0 0 0</pose>
|
||||
<visual name="red_circle_texture">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.4 0.4 1e-3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://red_circle/materials/scripts</uri>
|
||||
<uri>model://red_circle/materials/textures</uri>
|
||||
<name>red_circle</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
7
clover_simulation/models/red_circle/red_circle.svg
Normal file
7
clover_simulation/models/red_circle/red_circle.svg
Normal file
@@ -0,0 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>
|
||||
red_circle
|
||||
</title><g fill="red">
|
||||
<circle cx="10.05" cy="10.05" r="9.9"/>
|
||||
</g></svg>
|
||||
|
After Width: | Height: | Size: 221 B |
@@ -1,6 +1,6 @@
|
||||
<package format="3">
|
||||
<name>clover_simulation</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
|
||||
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
|
||||
@@ -72,12 +72,6 @@ Sample code to fly to a point 1 metre to the left and 2 metres above marker with
|
||||
navigate(frame_id='aruco_7', x=-1, y=0, z=2)
|
||||
```
|
||||
|
||||
Sample code to rotate counterclockwise while hovering 1.5 metres above marker id 10:
|
||||
|
||||
```python
|
||||
navigate(frame_id='aruco_10', x=0, y=0, z=1.5, yaw_rate=0.5)
|
||||
```
|
||||
|
||||
Note that if the required marker isn't detected for 0.5 seconds after the `navigate` command, the command will be ignored.
|
||||
|
||||
These frames may also be used in other services that accept TF frames (like `get_telemetry`). The following code will get the drone's position relative to the marker with id 3:
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# Working with the camera
|
||||
|
||||
> **Note** The following applies to [image versions](image.md) **0.24** and up. Older documentation is still available for [for version **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/en/camera.md).
|
||||
|
||||
Make sure the camera is enabled in the `~/catkin_ws/src/clover/clover/launch/clover.launch` file:
|
||||
|
||||
```xml
|
||||
@@ -14,7 +16,7 @@ The `clover` service must be restarted after the launch-file has been edited:
|
||||
sudo systemctl restart clover
|
||||
```
|
||||
|
||||
You may use rqt or [web_video_server](web_video_server.md) to view the camera stream.
|
||||
You may use [rqt](rviz.md) or [web_video_server](web_video_server.md) to view the camera stream.
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
@@ -52,8 +54,6 @@ The [SD card image](image.md) comes with a preinstalled [OpenCV](https://opencv.
|
||||
|
||||
### Python
|
||||
|
||||
Main article: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython.
|
||||
|
||||
An example of creating a subscriber for a topic with an image from the main camera for processing with OpenCV:
|
||||
|
||||
```python
|
||||
@@ -61,12 +61,14 @@ import rospy
|
||||
import cv2
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
from clover import long_callback
|
||||
|
||||
rospy.init_node('computer_vision_sample')
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
@long_callback
|
||||
def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
# Do any image processing with cv2...
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
|
||||
@@ -74,19 +76,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
|
||||
rospy.spin()
|
||||
```
|
||||
|
||||
> **Note** Image processing may take significant time to finish. This can cause an [issue](https://github.com/ros/ros_comm/issues/1901) in rospy library, which would lead to processing stale camera frames. To solve this problem you need to use `long_callback` decorator from `clover` library, as in the example above.
|
||||
|
||||
#### Limiting CPU usage
|
||||
|
||||
When using the `main_camera/image_raw` topic, the script will process the maximum number of frames from the camera, actively utilizing the CPU (up to 100%). In tasks, where processing each camera frame is not critical, you can use the topic, where the frames are published at rate 5 Hz: `main_camera/image_raw_throttled`:
|
||||
|
||||
```python
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
|
||||
```
|
||||
|
||||
#### Publishing images
|
||||
|
||||
To debug image processing, you can publish a separate topic with the processed image:
|
||||
|
||||
```python
|
||||
image_pub = rospy.Publisher('~debug', Image)
|
||||
```
|
||||
|
||||
Publishing the processed image (at the end of the image_callback function):
|
||||
Publishing the processed image:
|
||||
|
||||
```python
|
||||
image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
|
||||
image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
|
||||
```
|
||||
|
||||
The obtained images can be viewed using [web_video_server](web_video_server.md).
|
||||
The published images can be viewed using [web_video_server](web_video_server.md) or [rqt](rviz.md).
|
||||
|
||||
#### Retrieving one frame
|
||||
|
||||
@@ -97,7 +111,7 @@ import rospy
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
rospy.init_node('computer_vision_sample')
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
# ...
|
||||
@@ -119,40 +133,32 @@ QR codes recognition in Python:
|
||||
```python
|
||||
import rospy
|
||||
from pyzbar import pyzbar
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import Image
|
||||
from clover import long_callback
|
||||
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
rospy.init_node('barcode_test')
|
||||
|
||||
# Image subscriber callback function
|
||||
def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
barcodes = pyzbar.decode(cv_image)
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
img = bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
barcodes = pyzbar.decode(img)
|
||||
for barcode in barcodes:
|
||||
b_data = barcode.data.decode("utf-8")
|
||||
b_data = barcode.data.decode('utf-8')
|
||||
b_type = barcode.type
|
||||
(x, y, w, h) = barcode.rect
|
||||
xc = x + w/2
|
||||
yc = y + h/2
|
||||
print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc))
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
|
||||
|
||||
rospy.spin()
|
||||
```
|
||||
|
||||
The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`):
|
||||
|
||||
> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration.
|
||||
|
||||
```xml
|
||||
<node pkg="topic_tools" name="cam_throttle" type="throttle"
|
||||
args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>
|
||||
```
|
||||
|
||||
The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`.
|
||||
> **Hint** See other computer vision examples in the `~/examples` directory of the [RPi image](image.md).
|
||||
|
||||
## Video recording
|
||||
|
||||
|
||||
@@ -44,7 +44,7 @@ In case of using EKF2 (official firmware):
|
||||
|`EKF2_OF_QMIN`|10||
|
||||
|`EKF2_OF_N_MIN`|0.05||
|
||||
|`EKF2_OF_N_MAX`|0.2||
|
||||
|`EKF2_HGT_MODE`|2 (*Range sensor*)|If the [rangefinder](laser.md) is present and flying over horizontal floor|
|
||||
|`EKF2_HGT_MODE`|3 (*Vision*)|If the [rangefinder](laser.md) is present and flying over horizontal floor – 2 (*Range sensor*)|
|
||||
|`EKF2_EVA_NOISE`|0.1||
|
||||
|`EKF2_EVP_NOISE`|0.1||
|
||||
|`EKF2_EV_DELAY`|0||
|
||||
@@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use.
|
||||
|
||||
Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes:
|
||||
|
||||
* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate;
|
||||
* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations);
|
||||
* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate;
|
||||
* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations);
|
||||
* Copter position (in the local coordinate system) – x, y, z;
|
||||
* Copter speed (in the local coordinate system) – vx, vy, vz;
|
||||
* Global coordinates of the copter – latitude, longitude, altitude;
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# Autonomous flight
|
||||
|
||||
> **Note** The following applies to [image versions](image.md) **0.24** and up. Older documentation is still available for [for version **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/en/simple_offboard.md).
|
||||
|
||||
The `simple_offboard` module of the `clover` package is intended for simplified programming of the autonomous drone flight (`OFFBOARD` [flight mode](modes.md)). It allows setting the desired flight tasks, and automatically transforms [coordinates between frames](frames.md).
|
||||
|
||||
`simple_offboard` is a high level system for interacting with the flight controller. For a more low level system, see [mavros](mavros.md).
|
||||
@@ -20,6 +22,9 @@ rospy.init_node('flight') # 'flight' is name of your ROS node
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
|
||||
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
|
||||
set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
|
||||
set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
|
||||
set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
|
||||
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
@@ -51,11 +56,11 @@ Response format:
|
||||
* `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module;
|
||||
* `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not <abbr title="Above Mean Sea Level">AMSL</abbr>!), requires [GPS](gps.md) module;
|
||||
* `vx, vy, vz` – drone velocity *(m/s)*;
|
||||
* `pitch` – pitch angle *(radians)*;
|
||||
* `roll` – roll angle *(radians)*;
|
||||
* `pitch` – pitch angle *(radians)*;
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `pitch_rate` — angular pitch velocity *(rad/s)*;
|
||||
* `roll_rate` – angular roll velocity *(rad/s)*;
|
||||
* `pitch_rate` — angular pitch velocity *(rad/s)*;
|
||||
* `yaw_rate` – angular yaw velocity *(rad/s)*;
|
||||
* `voltage` – total battery voltage *(V)*;
|
||||
* `cell_voltage` – battery cell voltage *(V)*.
|
||||
@@ -100,7 +105,6 @@ Parameters:
|
||||
|
||||
* `x`, `y`, `z` — coordinates *(m)*;
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `yaw_rate` – angular yaw velocity (will be used if yaw is set to `NaN`) *(rad/s)*;
|
||||
* `speed` – flight speed (setpoint speed) *(m/s)*;
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
|
||||
* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
|
||||
@@ -119,7 +123,7 @@ Flying in a straight line to point 5:0 (altitude 2) in the local system of coord
|
||||
navigate(x=5, y=0, z=3, speed=0.8)
|
||||
```
|
||||
|
||||
Flying to point 5:0 without changing the yaw angle (`yaw` = `NaN`, `yaw_rate` = 0):
|
||||
Flying to point 5:0 without changing the yaw angle:
|
||||
|
||||
```python
|
||||
navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan'))
|
||||
@@ -149,22 +153,10 @@ Flying to point 3:2 (with the altitude of 2 m) in the [ArUco map](aruco.md) coor
|
||||
navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
Rotating on the spot at the speed of 0.5 rad/s (counterclockwise):
|
||||
|
||||
```python
|
||||
navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body')
|
||||
```
|
||||
|
||||
Flying 3 meters forwards at the speed of 0.5 m/s, yaw-rotating at the speed of 0.2 rad/s:
|
||||
|
||||
```python
|
||||
navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body')
|
||||
```
|
||||
|
||||
Ascending to the altitude of 2 m (command line):
|
||||
|
||||
```(bash)
|
||||
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
|
||||
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
|
||||
```
|
||||
|
||||
> **Note** Consider using the `navigate_target` frame instead of `body` for missions that primarily use relative movements forward/back/left/right. This negates inaccuracies in relative point calculations.
|
||||
@@ -178,7 +170,6 @@ Parameters:
|
||||
* `lat`, `lon` — latitude and longitude *(degrees)*;
|
||||
* `z` — altitude *(m)*;
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `yaw_rate` – angular yaw velocity (used for setting the yaw to `NaN`) *(rad/s)*;
|
||||
* `speed` – flight speed (setpoint speed) *(m/s)*;
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
|
||||
* `frame_id` – [coordinate system](frames.md) for `z` and `yaw` (Default value: `map`).
|
||||
@@ -191,7 +182,7 @@ Flying to a global point at the speed of 5 m/s, while maintaining current altitu
|
||||
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body')
|
||||
```
|
||||
|
||||
Flying to a global point without changing the yaw angle (`yaw` = `NaN`, `yaw_rate` = 0):
|
||||
Flying to a global point without changing the yaw angle:
|
||||
|
||||
```python
|
||||
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body')
|
||||
@@ -200,7 +191,71 @@ navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), fr
|
||||
Flying to a global point (command line):
|
||||
|
||||
```bash
|
||||
rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"
|
||||
rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"
|
||||
```
|
||||
|
||||
### set_altitude
|
||||
|
||||
Change the desired flight altitude. The service is used to set the altitude and its coordinate system independently, after calling [`navigate`](#navigate) or [`set_position`](#setposition).
|
||||
|
||||
Parameters:
|
||||
|
||||
* `z` – flight altitude *(m)*;
|
||||
* `frame_id` – [coordinate system](frames.md) for computing the altitude.
|
||||
|
||||
Set the desired altitude to 2 m relative to the floor:
|
||||
|
||||
```python
|
||||
set_altitude(z=2, frame_id='terrain')
|
||||
```
|
||||
|
||||
Set the desired altitude to 1 m relative to [the ArUco map](aruco.md):
|
||||
|
||||
```python
|
||||
set_altitude(z=1, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
### set_yaw
|
||||
|
||||
Change the desired yaw angle (and its coordinate system), keeping the previous command in effect.
|
||||
|
||||
Parameters:
|
||||
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `frame_id` – [coordinate system](frames.md) for computing the yaw.
|
||||
|
||||
Rotate by 90 degrees clockwise (the previous command continues):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=math.radians(-90), frame_id='body')
|
||||
```
|
||||
|
||||
Set the desired yaw angle to zero relative to [the ArUco map](aruco.md):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=0, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
Stop yaw rotation (caused by [`set_yaw_rate`](#setyawrate) call):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=float('nan'))
|
||||
```
|
||||
|
||||
### set_yaw_rate
|
||||
|
||||
The the desired angular yaw velocity, keeping the previous command in effect.
|
||||
|
||||
Parameters:
|
||||
|
||||
* `yaw_rate` – angular yaw velocity *(rad/s)*;
|
||||
|
||||
The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise.
|
||||
|
||||
Start yaw rotation at 0.5 rad/s (the previous command continues):
|
||||
|
||||
```python
|
||||
set_yaw_rate(yaw_rate=0.5)
|
||||
```
|
||||
|
||||
### set_position
|
||||
@@ -213,7 +268,6 @@ Parameters:
|
||||
|
||||
* `x`, `y`, `z` — point coordinates *(m)*;
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `yaw_rate` – angular yaw velocity (used for setting the yaw to NaN) *(rad/s)*;
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
|
||||
* `frame_id` – [coordinate system](frames.md) for `x`, `y`, `z` and `yaw` parameters (Default value: `map`).
|
||||
|
||||
@@ -235,19 +289,12 @@ Assigning the target point 1 m ahead of the current position:
|
||||
set_position(x=1, y=0, z=0, frame_id='body')
|
||||
```
|
||||
|
||||
Rotating on the spot at the speed of 0.5 rad/s:
|
||||
|
||||
```python
|
||||
set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5)
|
||||
```
|
||||
|
||||
### set_velocity
|
||||
|
||||
Set speed and yaw setpoints.
|
||||
|
||||
* `vx`, `vy`, `vz` – flight speed *(m/s)*;
|
||||
* `yaw` — yaw angle *(radians)*;
|
||||
* `yaw_rate` – angular yaw velocity (used for setting the yaw to NaN) *(rad/s)*;
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
|
||||
* `frame_id` – [coordinate system](frames.md) for `vx`, `vy`, `vz` and `yaw` (Default value: `map`).
|
||||
|
||||
@@ -261,26 +308,26 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
|
||||
|
||||
### set_attitude
|
||||
|
||||
Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
|
||||
Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
|
||||
|
||||
Parameters:
|
||||
|
||||
* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*;
|
||||
* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*;
|
||||
* `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
|
||||
* `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`).
|
||||
|
||||
### set_rates
|
||||
|
||||
Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
|
||||
Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
|
||||
|
||||
Parameters:
|
||||
|
||||
* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
|
||||
* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
|
||||
* `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
|
||||
* `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
|
||||
|
||||
The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise,`pitch_rate` rotation is forward, `roll_rate` rotation is to the left.
|
||||
The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise, `pitch_rate` rotation is forward, `roll_rate` rotation is to the left.
|
||||
|
||||
### land
|
||||
|
||||
|
||||
@@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down:
|
||||
PI_2 = math.pi / 2
|
||||
telem = get_telemetry()
|
||||
|
||||
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
|
||||
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
|
||||
```
|
||||
|
||||
### # {#angle-hor}
|
||||
@@ -155,8 +155,8 @@ Calculate the copter horizontal angle:
|
||||
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)))
|
||||
flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
|
||||
angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch)))
|
||||
if flipped:
|
||||
angle_to_horizon = math.pi - angle_to_horizon
|
||||
```
|
||||
@@ -324,7 +324,7 @@ def flip():
|
||||
|
||||
while True:
|
||||
telem = get_telemetry()
|
||||
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
|
||||
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
|
||||
if flipped:
|
||||
break
|
||||
|
||||
|
||||
@@ -84,12 +84,6 @@ navigate(frame_id='aruco_5', x=0, y=0, z=1)
|
||||
navigate(frame_id='aruco_7', x=-1, y=0, z=2)
|
||||
```
|
||||
|
||||
Вращаться против часовой стрелки на высоте 1.5 метра над маркером 10:
|
||||
|
||||
```python
|
||||
navigate(frame_id='aruco_10', x=0, y=0, z=1.5, yaw_rate=0.5)
|
||||
```
|
||||
|
||||
Если необходимый маркер не появится в поле зрения в течение полусекунды, дрон продолжит выполнять предыдущую команду.
|
||||
|
||||
Подобные значения `frame_id` можно использовать и в других сервисах, например `get_telemetry`. Получение расположения дрона относительно маркера 3:
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# Работа с камерой
|
||||
|
||||
> **Note** Документация для версий [образа](image.md), начиная с версии **0.24**. Для более ранних версий см. [документацию для версии **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/ru/camera.md).
|
||||
|
||||
<!-- TODO: физическое подключение -->
|
||||
|
||||
Для работы с основной камерой необходимо убедиться что она включена в файле `~/catkin_ws/src/clover/clover/launch/clover.launch`:
|
||||
@@ -54,8 +56,6 @@ raspistill -o test.jpg
|
||||
|
||||
### Python
|
||||
|
||||
Основная статья: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython.
|
||||
|
||||
Пример создания подписчика на топик с изображением с основной камеры для обработки с использованием OpenCV:
|
||||
|
||||
```python
|
||||
@@ -63,12 +63,14 @@ import rospy
|
||||
import cv2
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
from clover import long_callback
|
||||
|
||||
rospy.init_node('computer_vision_sample')
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
@long_callback
|
||||
def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
# Do any image processing with cv2...
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
|
||||
@@ -76,19 +78,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
|
||||
rospy.spin()
|
||||
```
|
||||
|
||||
> **Note** Обработка изображения может занимать значительное время. Это может вызвать [проблему](https://github.com/ros/ros_comm/issues/1901) в библиотеке rospy, которая приведет к обработке устаревших кадров с камеры. Для решения этой проблемы необходимо использовать декоратор `long_callback` из библиотеки `clover`, как в примере выше.
|
||||
|
||||
#### Ограничение использования CPU
|
||||
|
||||
При использовании топика `main_camera/image_raw` скрипт будет обрабатывать максимальное количество кадров с камеры, активно используя CPU (вплоть до 100%). В задачах, где обработка каждого кадра не критична, можно использовать топик, где кадры публикуются с частотой 5 Гц: `main_camera/image_raw_throttled`:
|
||||
|
||||
```python
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
|
||||
```
|
||||
|
||||
#### Публикация изображений
|
||||
|
||||
Для отладки обработки изображения можно публиковать отдельный топик с обработанным изображением:
|
||||
|
||||
```python
|
||||
image_pub = rospy.Publisher('~debug', Image)
|
||||
```
|
||||
|
||||
Публикация обработанного изображения (в конце функции image_callback):
|
||||
Публикация обработанного изображения:
|
||||
|
||||
```python
|
||||
image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
|
||||
image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
|
||||
```
|
||||
|
||||
Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md).
|
||||
Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md) или [rqt](rviz.md).
|
||||
|
||||
#### Получение одного кадра
|
||||
|
||||
@@ -99,12 +113,12 @@ import rospy
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
rospy.init_node('computer_vision_sample')
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
# ...
|
||||
|
||||
# Получение кадра:
|
||||
# Retrieve a frame:
|
||||
img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8')
|
||||
```
|
||||
|
||||
@@ -121,40 +135,32 @@ img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image
|
||||
```python
|
||||
import rospy
|
||||
from pyzbar import pyzbar
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import Image
|
||||
from clover import long_callback
|
||||
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
rospy.init_node('barcode_test')
|
||||
|
||||
# Image subscriber callback function
|
||||
def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
barcodes = pyzbar.decode(cv_image)
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
img = bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
barcodes = pyzbar.decode(img)
|
||||
for barcode in barcodes:
|
||||
b_data = barcode.data.decode("utf-8")
|
||||
b_data = barcode.data.decode('utf-8')
|
||||
b_type = barcode.type
|
||||
(x, y, w, h) = barcode.rect
|
||||
xc = x + w/2
|
||||
yc = y + h/2
|
||||
print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc))
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
|
||||
|
||||
rospy.spin()
|
||||
```
|
||||
|
||||
Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`):
|
||||
|
||||
> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации.
|
||||
|
||||
```xml
|
||||
<node pkg="topic_tools" name="cam_throttle" type="throttle"
|
||||
args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>
|
||||
```
|
||||
|
||||
Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`.
|
||||
> **Hint** Смотрите другие примеры по работе с компьютерным зрением в каталоге `~/examples` [образа для RPi](image.md).
|
||||
|
||||
## Запись видео
|
||||
|
||||
|
||||
@@ -44,7 +44,7 @@
|
||||
|`EKF2_OF_QMIN`|10||
|
||||
|`EKF2_OF_N_MIN`|0.05||
|
||||
|`EKF2_OF_N_MAX`|0.2||
|
||||
|`EKF2_HGT_MODE`|2 (*Range sensor*)|При наличии [дальномера](laser.md) и полете над ровным полом|
|
||||
|`EKF2_HGT_MODE`|3 (*Vision*)|При наличии [дальномера](laser.md) и полете над ровным полом — 2 (*Range sensor*)|
|
||||
|`EKF2_EVA_NOISE`|0.1||
|
||||
|`EKF2_EVP_NOISE`|0.1||
|
||||
|`EKF2_EV_DELAY`|0||
|
||||
@@ -60,8 +60,8 @@
|
||||
|
||||
Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит:
|
||||
|
||||
* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate;
|
||||
* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений);
|
||||
* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate;
|
||||
* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений);
|
||||
* позиция коптера (в локальной системе координат) – x, y, z;
|
||||
* скорость коптера (в локальной системе координат) – vx, vy, vz;
|
||||
* глобальные координаты коптера – latitude, longitude, altitude;
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# Автономный полет
|
||||
|
||||
> **Note** Документация для версий [образа](image.md), начиная с версии **0.24**. Для более ранних версий см. [документацию для версии **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/ru/simple_offboard.md).
|
||||
|
||||
Модуль `simple_offboard` пакета `clover` предназначен для упрощенного программирования автономного полета дрона ([режим](modes.md) `OFFBOARD`). Он позволяет устанавливать желаемые полетные задачи и автоматически трансформирует [систему координат](frames.md).
|
||||
|
||||
`simple_offboard` является высокоуровневым способом взаимодействия с полетным контроллером. Для более низкоуровневой работы см. [mavros](mavros.md).
|
||||
@@ -20,6 +22,9 @@ 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_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
|
||||
set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
|
||||
set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
|
||||
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
@@ -51,11 +56,11 @@ land = rospy.ServiceProxy('land', Trigger)
|
||||
* `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md);
|
||||
* `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не <abbr title="Above Mean Sea Level, выше среднего уровня моря">AMSL</abbr>!), необходимо наличие [GPS](gps.md);
|
||||
* `vx, vy, vz` – скорость коптера *(м/с)*;
|
||||
* `pitch` – угол по тангажу *(радианы)*;
|
||||
* `roll` – угол по крену *(радианы)*;
|
||||
* `pitch` – угол по тангажу *(радианы)*;
|
||||
* `yaw` – угол по рысканью *(радианы)*;
|
||||
* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
|
||||
* `roll_rate` – угловая скорость по крену *(рад/с)*;
|
||||
* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
|
||||
* `yaw_rate` – угловая скорость по рысканью *(рад/с)*;
|
||||
* `voltage` – общее напряжение аккумулятора *(В)*;
|
||||
* `cell_voltage` – напряжение аккумулятора на ячейку *(В)*.
|
||||
@@ -100,7 +105,6 @@ rosservice call /get_telemetry "{frame_id: ''}"
|
||||
|
||||
* `x`, `y`, `z` – координаты *(м)*;
|
||||
* `yaw` – угол по рысканью *(радианы)*;
|
||||
* `yaw_rate` – угловая скорость по рысканью (применяется при установке yaw в `NaN`) *(рад/с)*;
|
||||
* `speed` – скорость полета (скорость движения setpoint) *(м/с)*;
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
* `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`).
|
||||
@@ -119,7 +123,7 @@ navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True)
|
||||
navigate(x=5, y=0, z=3, speed=0.8)
|
||||
```
|
||||
|
||||
Полет в точку 5:0 без изменения угла по рысканью (`yaw` = `NaN`, `yaw_rate` = 0):
|
||||
Полет в точку 5:0 без изменения угла по рысканью:
|
||||
|
||||
```python
|
||||
navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan'))
|
||||
@@ -149,22 +153,10 @@ navigate(yaw=math.radians(-90), frame_id='body')
|
||||
navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
Вращение на месте со скоростью 0.5 рад/c (против часовой):
|
||||
|
||||
```python
|
||||
navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body')
|
||||
```
|
||||
|
||||
Полет вперед 3 метра со скоростью 0.5 м/с, вращаясь по рысканью со скоростью 0.2 рад/с:
|
||||
|
||||
```python
|
||||
navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body')
|
||||
```
|
||||
|
||||
Взлет на высоту 2 м (командная строка):
|
||||
|
||||
```bash
|
||||
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
|
||||
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
|
||||
```
|
||||
|
||||
> **Note** При программировании миссии дрона в терминах "вперед-назад-влево-вправо" рекомендуется использовать систему координат `navigate_target` вместо `body`, чтобы не учитывать неточность прилета дрона в предыдущую целевую точку при вычислении следующей.
|
||||
@@ -178,12 +170,11 @@ rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed
|
||||
* `lat`, `lon` – широта и долгота *(градусы)*;
|
||||
* `z` – высота *(м)*;
|
||||
* `yaw` – угол по рысканью *(радианы)*;
|
||||
* `yaw_rate` – угловая скорость по рысканью (при установке yaw в `NaN`) *(рад/с)*;
|
||||
* `speed` – скорость полета (скорость движения setpoint) *(м/с)*;
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
* `frame_id` – [система координат](frames.md), в которой заданы `z` и `yaw` (по умолчанию: `map`).
|
||||
|
||||
> **Note** Для полета без изменения угла по рысканью достаточно установить `yaw` в `NaN` (значение угловой скорости по умолчанию – 0).
|
||||
> **Note** Для полета без изменения угла по рысканью достаточно установить `yaw` в `NaN`.
|
||||
|
||||
Полет в глобальную точку со скоростью 5 м/с, оставаясь на текущей высоте (`yaw` установится в 0, коптер сориентируется передом на восток):
|
||||
|
||||
@@ -191,7 +182,7 @@ rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed
|
||||
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body')
|
||||
```
|
||||
|
||||
Полет в глобальную точку без изменения угла по рысканью (`yaw` = `NaN`, `yaw_rate` = 0):
|
||||
Полет в глобальную точку без изменения угла по рысканью:
|
||||
|
||||
```python
|
||||
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body')
|
||||
@@ -200,7 +191,71 @@ navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), fr
|
||||
Полет в глобальную точку (командная строка):
|
||||
|
||||
```bash
|
||||
rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"
|
||||
rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"
|
||||
```
|
||||
|
||||
### set_altitude
|
||||
|
||||
Изменить целевую высоту полета. Сервис используется для независимой установки высоты (и системы координат для расчета высота) в режимах полета [`navigate`](#navigate) и [`set_position`](#setposition).
|
||||
|
||||
Параметры:
|
||||
|
||||
* `z` – высота полета *(м)*;
|
||||
* `frame_id` – [система координат](frames.md) для расчета высоты полета.
|
||||
|
||||
Установить высоту полета в 2 м относительно пола:
|
||||
|
||||
```python
|
||||
set_altitude(z=2, frame_id='terrain')
|
||||
```
|
||||
|
||||
Установить высоту полета в 1 м относительно [маркерного поля](aruco.md):
|
||||
|
||||
```python
|
||||
set_altitude(z=1, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
### set_yaw
|
||||
|
||||
Изменить целевой угол по рысканью (и систему координат для его расчета), оставив предыдущую команду в силе.
|
||||
|
||||
Параметры:
|
||||
|
||||
* yaw – угол по рысканью *(радианы)*;
|
||||
* frame_id – [система координат](frames.md) для расчета угла по рысканью.
|
||||
|
||||
Повернуться на 90 градусов по часовой (продолжая выполнять предыдущую команду):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=math.radians(-90), frame_id='body')
|
||||
```
|
||||
|
||||
Установить угол по рысканью в ноль в системе координат [маркерного поля](aruco.md):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=0, frame_id='aruco_map')
|
||||
```
|
||||
|
||||
Остановить вращение по рысканью (при использовании [`set_yaw_rate`](#setyawrate)):
|
||||
|
||||
```python
|
||||
set_yaw(yaw=float('nan'))
|
||||
```
|
||||
|
||||
### set_yaw_rate
|
||||
|
||||
Изменить целевую угловую скорость по рысканью, оставив предыдущую команду в силе.
|
||||
|
||||
Параметры:
|
||||
|
||||
* yaw_rate – угловая скорость по рысканью *(рад/с)*.
|
||||
|
||||
Положительное направление вращения (при виде сверху) – против часовой.
|
||||
|
||||
Начать вращение на месте со скоростью 0.5 рад/c против часовой (продолжая выполнять предыдущую команду):
|
||||
|
||||
```python
|
||||
set_yaw_rate(yaw_rate=0.5)
|
||||
```
|
||||
|
||||
### set_position
|
||||
@@ -213,7 +268,6 @@ rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw:
|
||||
|
||||
* `x`, `y`, `z` – координаты точки *(м)*;
|
||||
* `yaw` – угол по рысканью *(радианы)*;
|
||||
* `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*;
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
* `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`).
|
||||
|
||||
@@ -235,19 +289,12 @@ set_position(x=0, y=0, z=3, frame_id='body')
|
||||
set_position(x=1, y=0, z=0, frame_id='body')
|
||||
```
|
||||
|
||||
Вращение на месте со скоростью 0.5 рад/c:
|
||||
|
||||
```python
|
||||
set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5)
|
||||
```
|
||||
|
||||
### set_velocity
|
||||
|
||||
Установить скорости и рысканье.
|
||||
|
||||
* `vx`, `vy`, `vz` – требуемая скорость полета *(м/с)*;
|
||||
* `yaw` – угол по рысканью *(радианы)*;
|
||||
* `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*;
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
* `frame_id` – [система координат](frames.md), в которой заданы `vx`, `vy`, `vz` и `yaw` (по умолчанию: `map`).
|
||||
|
||||
@@ -265,7 +312,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
|
||||
|
||||
Параметры:
|
||||
|
||||
* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
|
||||
* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
|
||||
* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ);
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
* `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`).
|
||||
@@ -276,7 +323,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
|
||||
|
||||
Параметры:
|
||||
|
||||
* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
|
||||
* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
|
||||
* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ).
|
||||
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
|
||||
|
||||
|
||||
@@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout)
|
||||
PI_2 = math.pi / 2
|
||||
telem = get_telemetry()
|
||||
|
||||
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
|
||||
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
|
||||
```
|
||||
|
||||
### # {#angle-hor}
|
||||
@@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
|
||||
PI_2 = math.pi / 2
|
||||
telem = get_telemetry()
|
||||
|
||||
flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
|
||||
flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= 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
|
||||
@@ -335,7 +335,7 @@ def flip():
|
||||
|
||||
while True:
|
||||
telem = get_telemetry()
|
||||
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
|
||||
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
|
||||
if flipped:
|
||||
break
|
||||
|
||||
|
||||
@@ -46,6 +46,13 @@
|
||||
* **Показывает возникающие ошибки и опечатки, допущенные в .launch файлах**
|
||||
* **Проверка на throttling**
|
||||
|
||||
### Автоматизированные тесты
|
||||
|
||||
* **Корректная работы автоматизированных тестов**:
|
||||
* Тест автономного полета: `rosrun clover autotest_flight.py`
|
||||
* Тест автономного полета по маркерам: `rosrun clover autotest_aruco.py`
|
||||
* Тест LED-ленты: `rosrun clover autotest_led.py`
|
||||
|
||||
### Тесты simple_offboard
|
||||
|
||||
* **Корректная работа simple_offboard – взлет, полет в точку в любом фрейме, отсутствие проблем с `yaw` и `yaw_rate`**
|
||||
@@ -53,6 +60,7 @@
|
||||
* **В фрейме `aruco_map`**
|
||||
* **В фрейме `map`**
|
||||
* **В фрейме `navigate_target`**
|
||||
* **В фрейме `terrain`**.
|
||||
* Корректное выполнения флипа
|
||||
* **Возможность лететь к отдельным маркерам в карте, которые вне кадра и в кадре**
|
||||
* **Корректное детектирование статуса kill switch при выполнение команды с флагом `auto_arm`**
|
||||
@@ -70,11 +78,6 @@
|
||||
* Полет по Optical Flow над 1 маркером
|
||||
* `aruco_map` не падает в случае маленьких размеров карты и маркеров
|
||||
|
||||
### Тесты [pigpiod](gpio.md)
|
||||
|
||||
* Корректная работа pigpiod, возможность работы с сонаром, сервой и электромагнитом по мануалу
|
||||
* Одновременная работа pigpiod и rpi_ws281x (правильная работа светодиодной ленты и сервы)
|
||||
|
||||
### Тесты [LED-ленты](leds.md)
|
||||
|
||||
* **Работает нода LED ленты на RPi 4**
|
||||
@@ -83,6 +86,11 @@
|
||||
* **Низкоуровневое управление отдельными диодами**
|
||||
* **Высокоуровневое управление эффектами**
|
||||
|
||||
### Тесты [pigpiod](gpio.md)
|
||||
|
||||
* Корректная работа pigpiod, возможность работы с сонаром, сервой и электромагнитом по мануалу
|
||||
* Одновременная работа pigpiod и rpi_ws281x (правильная работа светодиодной ленты и сервы)
|
||||
|
||||
### [Блочное программирование](blocks.md)
|
||||
|
||||
* Корректная работа функционала блочного программирования
|
||||
|
||||
@@ -65,6 +65,7 @@
|
||||
{ "from": "clover_vm/", "to": "en/simulation_vm.html" },
|
||||
{ "from": "gpio/", "to": "en/gpio.html" },
|
||||
{ "from": "blocks/", "to": "en/blocks.html" },
|
||||
{ "from": "red_circle/", "to": "en/camera.html" },
|
||||
|
||||
{ "from": "ru/microsd_images.html", "to": "image.html" },
|
||||
{ "from": "en/microsd_images.html", "to": "image.html" },
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>roswww_static</name>
|
||||
<version>0.23.0</version>
|
||||
<version>0.24.0</version>
|
||||
<description>Static web pages for ROS packages</description>
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
Reference in New Issue
Block a user