CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-27 05:29:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Stop publishing zero vision positions when local position is obtained

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-28 04:01:52 +03:00
parent 6769e8b86f
commit ecddf122bf
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
clever/src/aruco_vpe.cpp
View File

@@ -41,7 +41,8 @@ private:
bool use_mocap;
nh_priv.param<bool>("use_mocap", use_mocap, false);
static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
static ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &ArucoVPE::handlePose, this);
static ros::Subscriber aruco_pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
vision_position_pub_ = nh.advertise<PoseStamped>(use_mocap ? "mavros/mocap/pose" : "mavros/vision_pose/pose", 1);
@@ -61,6 +62,13 @@ private:
vision_position_pub_.publish(ps);
}
void handlePose(const geometry_msgs::PoseStampedConstPtr& pose)
{
// local position is inited, stop posting dummy position
ROS_INFO_ONCE("Got local position, stop publishing zeroes");
dummy_vision_timer_.stop();
}
void handleArucoPose(const geometry_msgs::PoseStampedConstPtr& pose)
{
static TransformBroadcaster br;