CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into v0.24-release

Browse Source
This commit is contained in:
Oleg Kalachev
2022-11-12 01:35:11 +06:00
parent 3870e62be7 95db8ba1b1
commit ac1ac33a1a
9 changed files with 80 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
aruco_pose/README.md
View File

@@ -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)

23
aruco_pose/src/aruco_detect.cpp
View File

@@ -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_) {

19
aruco_pose/src/aruco_map.cpp
View File

@@ -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;

25
aruco_pose/src/utils.h
View File

@@ -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)

12
clover/launch/aruco.launch
View File

@@ -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>

2
clover/launch/simulator.launch
View File

@@ -1,4 +1,4 @@
<launch>
<!-- shurtcut for running the simulation (`roslaunch clover simulator.launch`) -->
<!-- shortcut for running the simulation (`roslaunch clover simulator.launch`) -->
<include file="$(find clover_simulation)/launch/simulator.launch"/>
</launch>

18
clover/src/autotest/autotest_aruco.py
View File

@@ -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')

37
clover/src/selfcheck.py
View File

@@ -401,12 +401,15 @@ def check_aruco():
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:
@@ -417,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)
@@ -462,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')
@@ -605,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)

2
clover/src/vpe_publisher.cpp
View File

@@ -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;