mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-04 00:39:32 +00:00
Compare commits
27 Commits
roswww-sta
...
known_vert
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
a72cb67d03 | ||
|
|
8ac757598d | ||
|
|
25c3f25642 | ||
|
|
4877d0101b | ||
|
|
d4a83bdf58 | ||
|
|
8fd69e4ea5 | ||
|
|
c3625490b2 | ||
|
|
cb1773b708 | ||
|
|
5afbcff949 | ||
|
|
cf62364ac7 | ||
|
|
c7bf964ea5 | ||
|
|
f719406c8b | ||
|
|
72f8d901d5 | ||
|
|
5ec04bbefa | ||
|
|
393801b023 | ||
|
|
275023b455 | ||
|
|
a0322c55f2 | ||
|
|
b3b530c1c7 | ||
|
|
3662f512a7 | ||
|
|
277eb7297f | ||
|
|
e719b0f1e2 | ||
|
|
e65d380b4b | ||
|
|
8fe34e90e6 | ||
|
|
54ab5ab4b5 | ||
|
|
2cda68ae4a | ||
|
|
d24b6617a4 | ||
|
|
640ec1ee1a |
7
.github/workflows/docs.yml
vendored
7
.github/workflows/docs.yml
vendored
@@ -11,10 +11,6 @@ permissions:
|
||||
pages: write
|
||||
id-token: write
|
||||
|
||||
concurrency:
|
||||
group: "pages"
|
||||
cancel-in-progress: true
|
||||
|
||||
defaults:
|
||||
run:
|
||||
shell: bash
|
||||
@@ -75,6 +71,9 @@ jobs:
|
||||
|
||||
deploy-docs:
|
||||
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
|
||||
concurrency:
|
||||
group: "pages"
|
||||
cancel-in-progress: true
|
||||
environment:
|
||||
name: github-pages
|
||||
url: ${{ steps.deployment.outputs.page_url }}
|
||||
|
||||
@@ -43,7 +43,8 @@ It's recommended to run it within the same nodelet manager with the camera nodel
|
||||
* `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
|
||||
* `~length` (*double*) – markers' sides length
|
||||
* `~length_override` (*map*) – lengths of markers with specified ids
|
||||
* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
|
||||
* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame
|
||||
* `~flip_vertical` – flip vertical vector
|
||||
|
||||
### Topics
|
||||
|
||||
@@ -71,7 +72,8 @@ It's recommended to run it within the same nodelet manager with the camera nodel
|
||||
|
||||
* `~map` – path to text file with markers list
|
||||
* `~frame_id` – published frame id (default: `aruco_map`)
|
||||
* `~known_tilt` – known tilt (pitch and roll) of markers map as a frame
|
||||
* `~known_vertical` – known vertical (Z axis) of markers map as a frame
|
||||
* `~flip_vertical` – flip vertical vector
|
||||
* `~image_width` – debug image width (default: 2000)
|
||||
* `~image_height` – debug image height (default: 2000)
|
||||
* `~image_margin` – debug image margin (default: 200)
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<package format="3">
|
||||
<name>aruco_pose</name>
|
||||
<version>0.23.0</version>
|
||||
<description>Positioning with ArUco markers</description>
|
||||
@@ -28,6 +28,8 @@
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>rostest</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
|
||||
|
||||
<test_depend>image_publisher</test_depend>
|
||||
<test_depend>ros_pytest</test_depend>
|
||||
|
||||
@@ -71,12 +71,12 @@ private:
|
||||
ros::Publisher markers_pub_, vis_markers_pub_;
|
||||
ros::Subscriber map_markers_sub_;
|
||||
ros::ServiceServer set_markers_srv_;
|
||||
bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_;
|
||||
bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_;
|
||||
bool waiting_for_map_;
|
||||
double length_;
|
||||
ros::Duration transform_timeout_;
|
||||
std::unordered_map<int, double> length_override_;
|
||||
std::string frame_id_prefix_, known_tilt_;
|
||||
std::string frame_id_prefix_, known_vertical_;
|
||||
Mat camera_matrix_, dist_coeffs_;
|
||||
aruco_pose::MarkerArray array_;
|
||||
std::unordered_set<int> map_markers_ids_;
|
||||
@@ -105,7 +105,8 @@ public:
|
||||
readLengthOverride(nh_priv_);
|
||||
transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
|
||||
|
||||
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
|
||||
known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
|
||||
flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
|
||||
auto_flip_ = nh_priv_.param("auto_flip", false);
|
||||
|
||||
frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
|
||||
@@ -144,7 +145,7 @@ private:
|
||||
vector<vector<cv::Point2f>> corners, rejected;
|
||||
vector<cv::Vec3d> rvecs, tvecs;
|
||||
vector<cv::Point3f> obj_points;
|
||||
geometry_msgs::TransformStamped snap_to;
|
||||
geometry_msgs::TransformStamped vertical;
|
||||
|
||||
// Detect markers
|
||||
cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
|
||||
@@ -179,12 +180,12 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
if (!known_tilt_.empty()) {
|
||||
if (!known_vertical_.empty()) {
|
||||
try {
|
||||
snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
|
||||
msg->header.stamp, transform_timeout_);
|
||||
vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_,
|
||||
msg->header.stamp, transform_timeout_);
|
||||
} catch (const tf2::TransformException& e) {
|
||||
NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
|
||||
NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -205,9 +206,9 @@ private:
|
||||
if (estimate_poses_) {
|
||||
fillPose(marker.pose, rvecs[i], tvecs[i]);
|
||||
|
||||
// snap orientation (if enabled and snap frame available)
|
||||
if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
|
||||
snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
|
||||
// apply known vertical (if enabled and vertical frame available)
|
||||
if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) {
|
||||
applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
|
||||
}
|
||||
|
||||
if (send_tf_) {
|
||||
|
||||
@@ -81,9 +81,9 @@ private:
|
||||
bool enabled_ = true;
|
||||
std::string type_;
|
||||
visualization_msgs::MarkerArray vis_array_;
|
||||
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
|
||||
std::string known_vertical_, map_, markers_frame_, markers_parent_frame_;
|
||||
int image_width_, image_height_, image_margin_;
|
||||
bool auto_flip_, image_axis_;
|
||||
bool flip_vertical_, auto_flip_, image_axis_;
|
||||
|
||||
public:
|
||||
virtual void onInit()
|
||||
@@ -104,7 +104,8 @@ public:
|
||||
|
||||
type_ = nh_priv_.param<std::string>("type", "map");
|
||||
transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
|
||||
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
|
||||
known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
|
||||
flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
|
||||
auto_flip_ = nh_priv_.param("auto_flip", false);
|
||||
image_width_ = nh_priv_.param("image_width" , 2000);
|
||||
image_height_ = nh_priv_.param("image_height", 2000);
|
||||
@@ -177,7 +178,7 @@ public:
|
||||
corners.push_back(marker_corners);
|
||||
}
|
||||
|
||||
if (known_tilt_.empty()) {
|
||||
if (known_vertical_.empty()) {
|
||||
// simple estimation
|
||||
valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
|
||||
rvec, tvec, false);
|
||||
@@ -191,7 +192,7 @@ public:
|
||||
|
||||
} else {
|
||||
Mat obj_points, img_points;
|
||||
// estimation with "snapping"
|
||||
// estimation with known vertical
|
||||
cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
|
||||
if (obj_points.empty()) goto publish_debug;
|
||||
|
||||
@@ -203,11 +204,11 @@ public:
|
||||
|
||||
fillTransform(transform_.transform, rvec, tvec);
|
||||
try {
|
||||
geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
|
||||
known_tilt_, markers->header.stamp, ros::Duration(0.02));
|
||||
snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
|
||||
geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
|
||||
known_vertical_, markers->header.stamp, ros::Duration(0.02));
|
||||
applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
|
||||
} catch (const tf2::TransformException& e) {
|
||||
NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
|
||||
NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
|
||||
}
|
||||
|
||||
geometry_msgs::TransformStamped shift;
|
||||
|
||||
@@ -106,26 +106,25 @@ inline bool isFlipped(tf::Quaternion& q)
|
||||
return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
|
||||
}
|
||||
|
||||
/* Set roll and pitch from "from" to "to", keeping yaw */
|
||||
inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
|
||||
/* Apply a vertical to an orientation */
|
||||
inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
|
||||
bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line
|
||||
{
|
||||
tf::Quaternion _from, _to;
|
||||
tf::quaternionMsgToTF(from, _from);
|
||||
tf::quaternionMsgToTF(to, _to);
|
||||
tf::Quaternion _vertical, _orientation;
|
||||
tf::quaternionMsgToTF(vertical, _vertical);
|
||||
tf::quaternionMsgToTF(orientation, _orientation);
|
||||
|
||||
if (auto_flip) {
|
||||
if (!isFlipped(_from)) {
|
||||
static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
|
||||
_from *= flip; // flip "from"
|
||||
}
|
||||
if (flip_vertical || (auto_flip && !isFlipped(_orientation))) {
|
||||
static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
|
||||
_vertical *= flip; // flip vertical
|
||||
}
|
||||
|
||||
auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
|
||||
auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical));
|
||||
double _, yaw;
|
||||
diff.getRPY(_, _, yaw);
|
||||
auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
|
||||
_from = _from * q; // set yaw from "to" to "from"
|
||||
tf::quaternionTFToMsg(_from, to); // set "from" to "to"
|
||||
_vertical = _vertical * q; // set yaw from orientation to vertical
|
||||
tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation
|
||||
}
|
||||
|
||||
inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
|
||||
|
||||
@@ -151,6 +151,9 @@ catkin_make run_tests #&& catkin_test_results
|
||||
echo_stamp "Change permissions for catkin_ws"
|
||||
chown -Rf pi:pi /home/pi/catkin_ws
|
||||
|
||||
echo_stamp "Update www"
|
||||
sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
|
||||
|
||||
echo_stamp "Make \$HOME/examples symlink"
|
||||
ln -s "$(catkin_find clover examples --first-only)" /home/pi
|
||||
chown -Rf pi:pi /home/pi/examples
|
||||
|
||||
@@ -35,6 +35,7 @@ import pymavlink
|
||||
from pymavlink import mavutil
|
||||
# from espeak import espeak
|
||||
from pyzbar import pyzbar
|
||||
import docopt
|
||||
|
||||
print(cv2.getBuildInformation())
|
||||
|
||||
|
||||
@@ -71,7 +71,25 @@ if [ -z $VM ]; then
|
||||
[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
|
||||
fi
|
||||
|
||||
# determine user home directory
|
||||
[ $VM ] && H="/home/clover" || H="/home/pi"
|
||||
|
||||
# test basic ros tool work
|
||||
source $H/catkin_ws/devel/setup.bash
|
||||
roscd
|
||||
rosrun
|
||||
rosmsg
|
||||
rossrv
|
||||
rosnode || [ $? -eq 64 ] # usage output code is 64
|
||||
rostopic || [ $? -eq 64 ]
|
||||
rosservice || [ $? -eq 64 ]
|
||||
rosparam
|
||||
roslaunch -h
|
||||
|
||||
# validate examples are present
|
||||
[[ $(ls $H/examples/*) ]]
|
||||
|
||||
# validate web tools present
|
||||
[ -d $H/.ros/www ]
|
||||
[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ]
|
||||
[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ]
|
||||
|
||||
@@ -21,7 +21,7 @@ center_pub = rospy.Publisher('~center', Image, queue_size=1)
|
||||
|
||||
def get_color_name(h):
|
||||
if h < 15: return 'red'
|
||||
if h < 30: return 'orange'
|
||||
elif h < 30: return 'orange'
|
||||
elif h < 60: return 'yellow'
|
||||
elif h < 90: return 'green'
|
||||
elif h < 120: return 'cyan'
|
||||
|
||||
@@ -19,8 +19,8 @@
|
||||
<param name="estimate_poses" value="true"/>
|
||||
<param name="send_tf" value="true"/>
|
||||
<param name="use_map_markers" value="true"/>
|
||||
<param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
|
||||
<param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
|
||||
<param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="length" value="$(arg length)"/>
|
||||
<param name="transform_timeout" value="0.1"/>
|
||||
<!-- aruco detector parameters -->
|
||||
@@ -36,8 +36,8 @@
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<remap from="markers" to="aruco_detect/markers"/>
|
||||
<param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
|
||||
<param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
|
||||
<param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
|
||||
<param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="image_axis" value="true"/>
|
||||
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
|
||||
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
|
||||
@@ -53,8 +53,4 @@
|
||||
<param name="force_init" value="$(arg force_init)"/>
|
||||
<param name="offset_frame_id" value="aruco_map"/>
|
||||
</node>
|
||||
|
||||
<!-- run map_flipped frame if placement is ceiling -->
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame"
|
||||
args="0 0 0 3.1415926 3.1415926 0 map map_flipped" if="$(eval placement == 'ceiling')"/>
|
||||
</launch>
|
||||
|
||||
@@ -85,8 +85,4 @@
|
||||
<param name="use_fake_gcs" value="false"/>
|
||||
</node>
|
||||
|
||||
<!-- Update static directory -->
|
||||
<node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
|
||||
<param name="default_package" value="clover"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -43,8 +43,7 @@
|
||||
<depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<exec_depend>python-pymavlink</exec_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<test_depend>ros_pytest</test_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
flask==1.1.1
|
||||
docopt==0.6.2
|
||||
geopy==1.11.0
|
||||
smbus2==0.3.0
|
||||
VL53L1X==0.0.5
|
||||
|
||||
@@ -13,7 +13,12 @@ from util import handle_response
|
||||
|
||||
rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
|
||||
|
||||
flow_client = dynamic_reconfigure.client.Client('optical_flow')
|
||||
try:
|
||||
flow_client = dynamic_reconfigure.client.Client('optical_flow', timeout=2)
|
||||
except rospy.ROSException:
|
||||
flow_client = None
|
||||
print('Cannot configure optical flow, skip')
|
||||
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
|
||||
land = handle_response(rospy.ServiceProxy('land', Trigger))
|
||||
@@ -67,12 +72,13 @@ input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
|
||||
navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
|
||||
print_current_map_position()
|
||||
|
||||
input('Disable optical flow and keep hovering [enter] ')
|
||||
flow_client.update_configuration({'enabled': False})
|
||||
rospy.sleep(5)
|
||||
if flow_client:
|
||||
input('Disable optical flow and keep hovering [enter] ')
|
||||
flow_client.update_configuration({'enabled': False})
|
||||
rospy.sleep(5)
|
||||
|
||||
input('Enable optical flow back [enter] ')
|
||||
flow_client.update_configuration({'enabled': True})
|
||||
input('Enable optical flow back [enter] ')
|
||||
flow_client.update_configuration({'enabled': True})
|
||||
|
||||
input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
|
||||
navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
# The above copyright notice and this permission notice shall be included in all
|
||||
# copies or substantial portions of the Software.
|
||||
|
||||
import os
|
||||
import os, sys
|
||||
import math
|
||||
import subprocess
|
||||
import re
|
||||
@@ -50,6 +50,16 @@ thread_local = threading.local()
|
||||
reports_lock = Lock()
|
||||
|
||||
|
||||
# formatting colors
|
||||
if sys.stdout.isatty():
|
||||
GREY = '\033[90m'
|
||||
GREEN = '\033[92m'
|
||||
RED = '\033[31m'
|
||||
END = '\033[0m'
|
||||
else:
|
||||
GREY = GREEN = RED = END = ''
|
||||
|
||||
|
||||
def failure(text, *args):
|
||||
msg = text % args
|
||||
thread_local.reports += [{'failure': msg}]
|
||||
@@ -75,9 +85,9 @@ def check(name):
|
||||
if 'failure' in report:
|
||||
rospy.logerr('%s: %s', name, report['failure'])
|
||||
elif 'info' in report:
|
||||
rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info'])
|
||||
rospy.loginfo(GREY + name + END + ': ' + report['info'])
|
||||
if not thread_local.reports:
|
||||
rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name)
|
||||
rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
|
||||
if rospy.get_param('~time', False):
|
||||
rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
|
||||
return wrapper
|
||||
@@ -87,7 +97,7 @@ def check(name):
|
||||
def ff(value, precision=2):
|
||||
# safely format float or int
|
||||
if value is None:
|
||||
return '\033[31m???\033[0m'
|
||||
return RED + '???' + END
|
||||
if isinstance(value, float):
|
||||
return ('{:.' + str(precision + 1) + '}').format(value)
|
||||
elif isinstance(value, int):
|
||||
@@ -224,6 +234,7 @@ def check_fcu():
|
||||
state = rospy.wait_for_message('mavros/state', State, timeout=3)
|
||||
if not state.connected:
|
||||
failure('no connection to the FCU (check wiring)')
|
||||
info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
|
||||
return
|
||||
|
||||
if not is_process_running('px4', exact=True): # can't use px4 console in SITL
|
||||
@@ -306,6 +317,7 @@ def check_fcu():
|
||||
|
||||
except rospy.ROSException:
|
||||
failure('no MAVROS state (check wiring)')
|
||||
info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
|
||||
|
||||
|
||||
def describe_direction(v):
|
||||
@@ -386,15 +398,18 @@ def check_aruco():
|
||||
|
||||
if is_process_running('aruco_detect', full=True):
|
||||
try:
|
||||
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
|
||||
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length', '?'))
|
||||
except KeyError:
|
||||
failure('aruco_detect/length parameter is not set')
|
||||
known_tilt = rospy.get_param('aruco_detect/known_tilt', '')
|
||||
if known_tilt == 'map':
|
||||
known_tilt += ' (ALL markers are on the floor)'
|
||||
elif known_tilt == 'map_flipped':
|
||||
known_tilt += ' (ALL markers are on the ceiling)'
|
||||
info('aruco_detect/known_tilt = %s', known_tilt)
|
||||
known_vertical = rospy.get_param('aruco_detect/known_vertical', '')
|
||||
flip_vertical = rospy.get_param('aruco_detect/flip_vertical', False)
|
||||
description = ''
|
||||
if known_vertical == 'map' and not flip_vertical:
|
||||
description = ' (all markers are on the floor)'
|
||||
elif known_vertical == 'map' and flip_vertical:
|
||||
description = ' (all markers are on the ceiling)'
|
||||
info('aruco_detect/known_vertical = %s', known_vertical)
|
||||
info('aruco_detect/flip_vertical = %s%s', flip_vertical, description)
|
||||
try:
|
||||
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
|
||||
except rospy.ROSException:
|
||||
@@ -405,12 +420,15 @@ def check_aruco():
|
||||
return
|
||||
|
||||
if is_process_running('aruco_map', full=True):
|
||||
known_tilt = rospy.get_param('aruco_map/known_tilt', '')
|
||||
if known_tilt == 'map':
|
||||
known_tilt += ' (marker\'s map is on the floor)'
|
||||
elif known_tilt == 'map_flipped':
|
||||
known_tilt += ' (marker\'s map is on the ceiling)'
|
||||
info('aruco_map/known_tilt = %s', known_tilt)
|
||||
known_vertical = rospy.get_param('aruco_map/known_vertical', '')
|
||||
flip_vertical = rospy.get_param('aruco_map/flip_vertical', False)
|
||||
description = ''
|
||||
if known_vertical == 'map' and not flip_vertical:
|
||||
description += ' (markers map is on the floor)'
|
||||
elif known_vertical == 'map' and flip_vertical:
|
||||
description += ' (markers map is on the ceiling)'
|
||||
info('aruco_map/known_vertical = %s', known_vertical)
|
||||
info('aruco_map/flip_vertical = %s%s', flip_vertical, description)
|
||||
|
||||
try:
|
||||
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
|
||||
@@ -450,7 +468,8 @@ def check_vpe():
|
||||
except rospy.ROSException:
|
||||
if not is_process_running('vpe_publisher', full=True):
|
||||
info('no vision position estimate, vpe_publisher is not running')
|
||||
elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
|
||||
elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \
|
||||
and rospy.get_param('aruco_map/flip_vertical', False):
|
||||
failure('no vision position estimate, markers are on the ceiling')
|
||||
elif is_on_the_floor():
|
||||
info('no vision position estimate, the drone is on the floor')
|
||||
@@ -593,6 +612,10 @@ def check_global_position():
|
||||
|
||||
@check('Optical flow')
|
||||
def check_optical_flow():
|
||||
if not is_process_running('optical_flow', full=True):
|
||||
info('optical_flow is not running')
|
||||
return
|
||||
|
||||
# TODO:check FPS!
|
||||
try:
|
||||
rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
using std::string;
|
||||
using namespace geometry_msgs;
|
||||
|
||||
bool reset_flag = false;
|
||||
bool reset_flag = true; // offset should be reset on the start
|
||||
string local_frame_id, frame_id, child_frame_id, offset_frame_id;
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
ros::Publisher vpe_pub;
|
||||
|
||||
4
clover/src/www
Executable file
4
clover/src/www
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
export ROSWWW_DEFAULT=clover
|
||||
rosrun roswww_static update
|
||||
@@ -1,4 +1,4 @@
|
||||
<package format="2">
|
||||
<package format="3">
|
||||
<name>clover_simulation</name>
|
||||
<version>0.23.0</version>
|
||||
<description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
|
||||
@@ -22,6 +22,8 @@
|
||||
<depend>gazebo_ros</depend>
|
||||
<depend>gazebo_plugins</depend>
|
||||
<depend>rospy</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
|
||||
|
||||
<export>
|
||||
<gazebo_ros gazebo_media_path="${prefix}"/>
|
||||
|
||||
@@ -147,6 +147,8 @@ sudo systemctl enable roscore
|
||||
sudo systemctl start roscore
|
||||
```
|
||||
|
||||
### Web tools setup
|
||||
|
||||
Install any web server to serve Clover's web tools (`~/.ros/www` directory), e. g. Monkey:
|
||||
|
||||
```bash
|
||||
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
|
||||
sudo systemctl enable monkey
|
||||
sudo systemctl start monkey
|
||||
```
|
||||
|
||||
Create `~/.ros/www` using the following command:
|
||||
|
||||
```bash
|
||||
rosrun clover www
|
||||
```
|
||||
|
||||
If the set of packages containing a web part (through `www` directory) is changed, the above command also must be run.
|
||||
|
||||
@@ -49,10 +49,10 @@ If you are using the marker map, where the markers have equal distances along th
|
||||
|
||||
After you fill out the map, you need to apply it. To do it, edit the file `aruco.launch`, located in `~/catkin_ws/src/clover/clover/launch/`. Change the line `<param name="map" value="$(find aruco_pose)/map/map_name.txt"/>`, where `map_name.txt` is the name of your map file.
|
||||
|
||||
If you are using markers that are not linked to horizontal surfaces (floor, ceiling), you must disable the parameter `known_tilt` both in the module `aruco_detect` and `aruco_map` in the same file. To do it automatically, enter:
|
||||
If you are using markers that are not linked to horizontal surfaces (floor, ceiling), you must blank the `placement` argument in the same file:
|
||||
|
||||
```bash
|
||||
sed -i "/known_tilt/s/value=\".*\"/value=\"\"/" /home/pi/catkin_ws/src/clover/clover/launch/aruco.launch
|
||||
```xml
|
||||
<arg name="placement" default=""/>
|
||||
```
|
||||
|
||||
After all the settings, call `sudo systemctl restart clover` to restart the `clover` service.
|
||||
|
||||
@@ -147,6 +147,8 @@ sudo systemctl enable roscore
|
||||
sudo systemctl start roscore
|
||||
```
|
||||
|
||||
### Конфигурация веб-инструментов
|
||||
|
||||
Установите любой веб-сервер, чтобы раздавать веб-инструменты Клевера (директория `~/.ros/www`), например, Monkey:
|
||||
|
||||
```bash
|
||||
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
|
||||
sudo systemctl enable monkey
|
||||
sudo systemctl start monkey
|
||||
```
|
||||
|
||||
Создайте директорию `~/.ros/www` следующей командой:
|
||||
|
||||
```bash
|
||||
rosrun clover www
|
||||
```
|
||||
|
||||
При обновлении набора пакетов, содержащих веб-часть (через каталог `www`), также необходимо выполнение данной команды.
|
||||
|
||||
@@ -51,10 +51,10 @@ sed -i "/direction_y/s/default=\".*\"/default=\"\"/" /home/pi/catkin_ws/src/clov
|
||||
|
||||
После того, как вы заполните карту, необходимо применить ее — для этого отредактируйте файл `aruco.launch`, расположенный в `~/catkin_ws/src/clover/clover/launch/`. Измените в нем строку `<param name="map" value="$(find aruco_pose)/map/map_name.txt"/>`, где `map_name.txt` название вашего файла с картой.
|
||||
|
||||
При использовании маркеров, не привязанных к горизонтальным плоскостям(пол, потолок), необходимо отключить параметр `known_tilt` как в модуле `aruco_detect`, так и в модуле `aruco_map` в том же файле. Для того, чтобы сделать это автоматически, введите:
|
||||
При использовании маркеров, не привязанных к горизонтальным плоскостям (пол, потолок), необходимо также сделать пустым значение аргумента `placement` в том же файле:
|
||||
|
||||
```bash
|
||||
sed -i "/known_tilt/s/value=\".*\"/value=\"\"/" /home/pi/catkin_ws/src/clover/clover/launch/aruco.launch
|
||||
```xml
|
||||
<arg name="placement" default=""/>
|
||||
```
|
||||
|
||||
После всех настроек вызовите `sudo systemctl restart clover` для перезагрузки сервиса `clover`.
|
||||
|
||||
@@ -5,8 +5,6 @@ find_package(catkin REQUIRED)
|
||||
|
||||
catkin_package()
|
||||
|
||||
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
catkin_install_python(PROGRAMS main.py
|
||||
catkin_install_python(PROGRAMS src/update
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
@@ -6,12 +6,14 @@ Note: you should configure your web server to make it follow symlinks.
|
||||
|
||||
## Instructions
|
||||
|
||||
* Run `main.py` node and it will generate the symlinks and index file.
|
||||
* Run `update` script and it will generate the symlinks and index file: `rosrun roswww_static update`.
|
||||
* Point your static web server path to `~/.ros/www`.
|
||||
|
||||
You can rerun `main.py` if the list of installed packages changes.
|
||||
You can rerun `update` if the list of installed packages changes.
|
||||
|
||||
## Parameters
|
||||
|
||||
* `index` – path for index page, otherwise packages list would be generated.
|
||||
* `default_package` – if set then the index page would redirect to this package's page.
|
||||
Parameters are passed through environment variables:
|
||||
|
||||
* `ROSWWW_INDEX` – path for index page, otherwise packages list would be generated.
|
||||
* `ROSWWW_DEFAULT` – if set then the index page would redirect to this package's page.
|
||||
|
||||
@@ -1,6 +0,0 @@
|
||||
<launch>
|
||||
<node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true" output="screen">
|
||||
<!-- <param name="index" value="$(find my_package)/www/index.html"/> -->
|
||||
<!-- <param name="default_package" value="my_package"/> -->
|
||||
</node>
|
||||
</launch>
|
||||
@@ -13,17 +13,15 @@
|
||||
|
||||
import os
|
||||
import shutil
|
||||
import rospy
|
||||
import rospkg
|
||||
|
||||
rospy.init_node('roswww_static')
|
||||
|
||||
rospack = rospkg.RosPack()
|
||||
|
||||
www = rospkg.get_ros_home() + '/www'
|
||||
index_file = rospy.get_param('~index_file', None)
|
||||
default_package = rospy.get_param('~default_package', None)
|
||||
index_file = os.environ.get('ROSWWW_INDEX')
|
||||
default_package = os.environ.get('ROSWWW_DEFAULT')
|
||||
|
||||
print('using www dir: ' + www)
|
||||
shutil.rmtree(www, ignore_errors=True) # reset www directory content
|
||||
os.mkdir(www)
|
||||
|
||||
@@ -34,7 +32,7 @@ index = '<h1>Packages list</h1>\n<ul>\n'
|
||||
for name in packages:
|
||||
path = rospack.get_path(name)
|
||||
if os.path.exists(path + '/www'):
|
||||
rospy.loginfo('found www path for %s package', name)
|
||||
print('found www path for %s package' % name)
|
||||
os.symlink(path + '/www', www + '/' + name)
|
||||
index += '<li><a href="{name}/">{name}</a></li>'.format(name=name)
|
||||
|
||||
@@ -42,7 +40,7 @@ if default_package is not None:
|
||||
redirect_html = '<meta http-equiv=refresh content="0; url={name}/">'.format(name=default_package)
|
||||
open(www + '/index.html', 'w').write(redirect_html)
|
||||
elif index_file is not None:
|
||||
rospy.loginfo('symlinking index file')
|
||||
print('symlinking index file')
|
||||
os.symlink(index_file, www + '/index.html')
|
||||
else:
|
||||
open(www + '/index.html', 'w').write(index)
|
||||
Reference in New Issue
Block a user