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

aruco_pose: add flip_vertical parameter and get rid of map_flipped

Browse Source
This commit is contained in:
Oleg Kalachev
2022-11-11 05:41:46 +06:00
parent c7bf964ea5
commit cf62364ac7
4 changed files with 13 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
aruco_pose/src/aruco_detect.cpp
View File

@@ -71,7 +71,7 @@ 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_;
@@ -106,6 +106,7 @@ public:
transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
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_");
@@ -207,7 +208,7 @@ private:
// 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, auto_flip_);
applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
}
if (send_tf_) {

2
aruco_pose/src/aruco_map.cpp
View File

@@ -206,7 +206,7 @@ public:
try {
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, auto_flip_);
applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
} catch (const tf2::TransformException& e) {
NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
}

11
aruco_pose/src/utils.h
View File

@@ -107,17 +107,16 @@ inline bool isFlipped(tf::Quaternion& q)
}
/* Apply a vertical to an orientation */
inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical, bool auto_flip = false)
inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
bool flip_vertical = false, bool auto_flip = false)
{
tf::Quaternion _vertical, _orientation;
tf::quaternionMsgToTF(vertical, _vertical);
tf::quaternionMsgToTF(orientation, _orientation);
if (auto_flip) {
if (!isFlipped(_vertical)) {
static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
_vertical *= flip; // flip vertical
}
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(_orientation).transposeTimes(tf::Matrix3x3(_vertical));

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_vertical" value="map" if="$(eval placement == 'floor')"/>
<param name="known_vertical" 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_vertical" value="map" if="$(eval placement == 'floor')"/>
<param name="known_vertical" 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>