CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-28 14:09:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use C++ version of aruco_vpe

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-21 23:46:20 +03:00
parent 1ff46dc6b0
commit 7be12fcbe4
5 changed files with 28 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
clever/src/aruco_vpe.cpp
View File

@@ -15,6 +15,7 @@
using namespace tf2_ros;
using geometry_msgs::PoseStamped;
using geometry_msgs::TransformStamped;
using std::string;
class ArucoVPE : public nodelet::Nodelet
{
@@ -29,12 +30,18 @@ private:
ros::Duration lookup_timeout_;
ros::Publisher vision_position_pub_;
ros::Timer dummy_vision_timer_;
string aruco_orientation_;
void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& nh_priv = getPrivateNodeHandle();
static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
vision_position_pub_ = nh.advertise<PoseStamped>("mavros/vision_pose/pose", 1);
nh_priv.param<std::string>("aruco_orientation", aruco_orientation_, "local_origin");
ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str());
dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this);
@@ -66,7 +73,7 @@ private:
{
// Refine aruco map pose
// Reference in local origin
t = tf_buffer.lookupTransform("local_origin", "aruco_map_reference", stamp, lookup_timeout_);
t = tf_buffer.lookupTransform(aruco_orientation_, "aruco_map_reference", stamp, lookup_timeout_);
quaternionToEuler(t.transform.rotation, roll, pitch, yaw);
eulerToQuaternion(t.transform.rotation, 0, 0, yaw);
t.child_frame_id = "aruco_map_reference_horiz";

106
clever/src/aruco_vpe.py
View File

@@ -1,106 +0,0 @@
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseStamped, PointStamped, Quaternion
import tf2_ros
from tf2_geometry_msgs import do_transform_pose
import tf.transformations
from util import orientation_from_euler, euler_from_orientation
rospy.init_node('aruco_vpe')
LOOKUP_TIMEOUT = rospy.Duration(.05)
CAMERA_FRAME_ID = rospy.get_param('~camera_frame_id', 'bottom_camera_optical')
# TF2 stuff
tf_broadcaster = tf2_ros.TransformBroadcaster()
static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
vision_position_pub = rospy.Publisher('mavros/vision_pose/pose', PoseStamped, queue_size=1)
_vision_position_pub = rospy.Publisher('fake_vision_pose', PoseStamped, queue_size=1)
last_published = None
q = Quaternion()
q.w = 1
ps = PoseStamped()
ps.pose.orientation = q
def send_transform(transform, child_frame_id):
transform.child_frame_id = child_frame_id
tf_broadcaster.sendTransform(transform)
vpe_posted = False
def publish_vpe(pose):
stamp = pose.header.stamp
global last_published, vpe_posted
vpe_posted = True
def lookup_transform(target_frame, source_frame):
return tf_buffer.lookup_transform(target_frame, source_frame, stamp, LOOKUP_TIMEOUT)
# Refine aruco_map
reference_in_local_origin = lookup_transform('local_origin', 'aruco_map_reference')
roll, pitch, yaw = euler_from_orientation(reference_in_local_origin.transform.rotation)
reference_in_local_origin.transform.rotation = orientation_from_euler(0, 0, yaw)
send_transform(reference_in_local_origin, 'aruco_map_reference_horiz')
aruco_map_in_reference = lookup_transform('aruco_map_reference', 'aruco_map_raw')
aruco_map_in_reference.header.frame_id = 'aruco_map_reference_horiz'
send_transform(aruco_map_in_reference, 'aruco_map_vision')
# Reset VPE
if last_published is None or stamp - last_published > rospy.Duration(2):
rospy.loginfo('Reset VPE')
aruco_map_in_local_origin = lookup_transform('local_origin', 'aruco_map_vision')
aruco_map_in_local_origin.child_frame_id = 'aruco_map'
static_tf_broadcaster.sendTransform(aruco_map_in_local_origin)
# Calculate VPE
ps.header.frame_id = 'fcu_horiz'
ps.header.stamp = stamp
vpe_raw = tf_buffer.transform(ps, 'aruco_map_vision', LOOKUP_TIMEOUT)
vpe_raw.header.frame_id = 'aruco_map'
vpe = tf_buffer.transform(vpe_raw, 'local_origin', LOOKUP_TIMEOUT)
_vision_position_pub.publish(vpe_raw)
vision_position_pub.publish(vpe)
last_published = stamp
rospy.Subscriber('aruco_pose/pose', PoseStamped, publish_vpe, queue_size=1)
local_pose = None
def handle_pose(data):
global local_pose
local_pose = data
rospy.Subscriber('mavros/local_position/pose', PoseStamped, handle_pose, queue_size=1)
rospy.loginfo('aruco_vpe inited')
r = rospy.Rate(5)
while not rospy.is_shutdown():
if not vpe_posted:
ps.header.stamp = rospy.get_rostime()
vision_position_pub.publish(ps)
r.sleep()