CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-08 18:44:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

vpe_publisher: implement ~reset service

Browse Source
This commit is contained in:
Oleg Kalachev
2019-09-25 23:59:48 +03:00
parent 3ba4ebf3a2
commit a4fa53ba1b
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
clever/src/vpe_publisher.cpp
View File

@@ -24,6 +24,7 @@
using std::string; using std::string;
using namespace geometry_msgs; using namespace geometry_msgs;
bool reset_flag = false;
string local_frame_id, frame_id, child_frame_id, offset_frame_id; string local_frame_id, frame_id, child_frame_id, offset_frame_id;
tf2_ros::Buffer tf_buffer; tf2_ros::Buffer tf_buffer;
ros::Publisher vpe_pub; ros::Publisher vpe_pub;
@@ -84,13 +85,14 @@ void callback(const T& msg)
// offset // offset
if (!offset_frame_id.empty()) { if (!offset_frame_id.empty()) {
if (msg->header.stamp - vpe.header.stamp > offset_timeout) { if (reset_flag || msg->header.stamp - vpe.header.stamp > offset_timeout) {
// calculate the offset // calculate the offset
offset = tf_buffer.lookupTransform(local_frame_id, frame_id, offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
msg->header.stamp, ros::Duration(0.02)); msg->header.stamp, ros::Duration(0.02));
// offset.header.frame_id = vpe.header.frame_id; // offset.header.frame_id = vpe.header.frame_id;
offset.child_frame_id = offset_frame_id; offset.child_frame_id = offset_frame_id;
br.sendTransform(offset); br.sendTransform(offset);
reset_flag = false;
ROS_INFO("offset reset"); ROS_INFO("offset reset");
} }
// apply the offset // apply the offset
@@ -106,6 +108,13 @@ void callback(const T& msg)
} }
} }
bool reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
{
reset_flag = true;
res.success = true;
return true;
}
int main(int argc, char **argv) { int main(int argc, char **argv) {
ros::init(argc, argv, "vpe_publisher"); ros::init(argc, argv, "vpe_publisher");
ros::NodeHandle nh, nh_priv("~"); ros::NodeHandle nh, nh_priv("~");
@@ -139,6 +148,8 @@ int main(int argc, char **argv) {
local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback); local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
} }
auto reset_serv = nh_priv.advertiseService("reset", &reset);
ROS_INFO("ready"); ROS_INFO("ready");
ros::spin(); ros::spin();
} }