vpe_publisher: allow taking pose data from topic

book.json

@@ -2,7 +2,7 @@
     "title": "Clever",
     "description": "Конструктор квадрокоптера «Клевер»",
     "author": "Copter Express",
-    "language": "ru",
+    "language": "en",
     "root": "docs/",
     "gitbook": "^3.2.2",
     "plugins": [

builder/test/tests.py

@@ -15,7 +15,7 @@ from mavros_msgs.srv import CommandBool, CommandLong, SetMode
 
 from std_srvs.srv import Trigger
 from clever.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
-    SetAttitude, SetRates
+    SetAttitude, SetRates, SetLEDEffect
 
 import tf2_ros
 import tf2_geometry_msgs

clever/src/selfcheck.py

@@ -477,7 +477,7 @@ def check_local_position():
 @check('Velocity estimation')
 def check_velocity():
     try:
-        velocity = rospy.wait_for_message('mavros/local_position/velocity', TwistStamped, timeout=1)
+        velocity = rospy.wait_for_message('mavros/local_position/velocity_local', TwistStamped, timeout=1)
         horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
         vert = velocity.twist.linear.z
         if abs(horiz) > 0.1:
@@ -485,6 +485,7 @@ def check_velocity():
         if abs(vert) > 0.1:
             failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
+        velocity = rospy.wait_for_message('mavros/local_position/velocity_body', TwistStamped, timeout=1)
         angular = velocity.twist.angular
         ANGULAR_VELOCITY_LIMIT = 0.1
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:

clever/src/vpe_publisher.cpp

@@ -14,16 +14,21 @@
 #include <tf2/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <std_srvs/Trigger.h>
 // #include <aruco_pose/MarkerArray.h>
 
 using std::string;
 using namespace geometry_msgs;
 
+bool use_tf;
+bool reset_flag = false;
 string local_frame_id, frame_id, child_frame_id, offset_frame_id;
 tf2_ros::Buffer tf_buffer;
 ros::Publisher vpe_pub;
@@ -34,6 +39,28 @@ ros::Time got_local_pos(0);
 ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
 TransformStamped offset;
 
+inline geometry_msgs::TransformStamped poseToTransform(const geometry_msgs::PoseStamped& pose)
+{
+	geometry_msgs::TransformStamped result;
+	result.header.frame_id = pose.header.frame_id;
+	result.header.stamp = pose.header.stamp;
+	result.transform.translation.x = pose.pose.position.x;
+	result.transform.translation.y = pose.pose.position.y;
+	result.transform.translation.z = pose.pose.position.z;
+	result.transform.rotation = pose.pose.orientation;
+	return result;
+}
+
+inline geometry_msgs::Transform poseToTransform(const geometry_msgs::Pose& pose)
+{
+	geometry_msgs::Transform result;
+	result.translation.x = pose.position.x;
+	result.translation.y = pose.position.y;
+	result.translation.z = pose.position.z;
+	result.rotation = pose.orientation;
+	return result;
+}
+
 void publishZero(const ros::TimerEvent& e)
 {
 	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
@@ -64,33 +91,46 @@ inline Pose getPose(const PoseStampedConstPtr& pose) { return pose->pose; }
 
 inline Pose getPose(const PoseWithCovarianceStampedConstPtr& pose) { return pose->pose.pose; }
 
+inline Pose getPose(const nav_msgs::OdometryConstPtr& pose) { return pose->pose.pose; }
+
 template <typename T>
 void callback(const T& msg)
 {
 	static tf2_ros::StaticTransformBroadcaster br;
 
 	try {
-		if (!frame_id.empty()) {
-			// get VPE transform from TF
-			auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id,
-													msg->header.stamp, ros::Duration(0.02));
-			vpe.pose.position.x = transform.transform.translation.x;
-			vpe.pose.position.y = transform.transform.translation.y;
-			vpe.pose.position.z = transform.transform.translation.z;
-			vpe.pose.orientation = transform.transform.rotation;
-		} else {
-			vpe.pose = getPose(msg);
+		if (!use_tf) {
+			// republish transform from pose for convenience
+			static tf2_ros::TransformBroadcaster tr_br;
+			tf2::Transform pose;
+			tf2::fromMsg(poseToTransform(getPose(msg)), pose);
+			pose = pose.inverse();
+			TransformStamped transform;
+			transform.transform = tf2::toMsg(pose);
+			transform.header.stamp = msg->header.stamp;
+			transform.header.frame_id = child_frame_id;
+			transform.child_frame_id = frame_id;
+			tr_br.sendTransform(transform);
 		}
 
+		// get VPE transform from TF
+		auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id,
+												msg->header.stamp, ros::Duration(0.02));
+		vpe.pose.position.x = transform.transform.translation.x;
+		vpe.pose.position.y = transform.transform.translation.y;
+		vpe.pose.position.z = transform.transform.translation.z;
+		vpe.pose.orientation = transform.transform.rotation;
+
 		// offset
 		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
 				offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
 				                                   msg->header.stamp, ros::Duration(0.02));
 				// offset.header.frame_id = vpe.header.frame_id;
 				offset.child_frame_id = offset_frame_id;
 				br.sendTransform(offset);
+				reset_flag = false;
 				ROS_INFO("offset reset");
 			}
 			// apply the offset
@@ -98,7 +138,7 @@ void callback(const T& msg)
 		}
 
 		vpe.header.frame_id = local_frame_id;
-		vpe.header.stamp = msg->header.stamp;
+		vpe.header.stamp = ros::Time::now();//msg->header.stamp;
 		vpe_pub.publish(vpe);
 
 	} catch (const tf2::TransformException& e) {
@@ -106,19 +146,28 @@ 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) {
 	ros::init(argc, argv, "vpe_publisher");
 	ros::NodeHandle nh, nh_priv("~");
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
 
+	string base_link;
 	nh_priv.param<string>("frame_id", frame_id, "");
 	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
 	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
-	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
+	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", base_link, "base_link");
+	nh_priv.param<string>("child_frame_id", child_frame_id, base_link);
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
-	if (!frame_id.empty()) {
+	if (use_tf) {
 		ROS_INFO("using data from TF");
 	} else {
 		ROS_INFO("using data topic");
@@ -126,6 +175,7 @@ int main(int argc, char **argv) {
 
 	auto pose_sub = nh_priv.subscribe<PoseStamped>("pose", 1, &callback);
 	auto pose_cov_sub = nh_priv.subscribe<PoseWithCovarianceStamped>("pose_cov", 1, &callback);
+	auto odom_sub = nh_priv.subscribe<nav_msgs::Odometry>("odom", 1, &callback, ros::TransportHints().tcpNoDelay());
 	//auto markers_sub = nh_priv.subscribe<aruco_pose::MarkerArray>("markers", 1, &callback);
 
 	vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
@@ -139,6 +189,8 @@ int main(int argc, char **argv) {
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}
 
+	auto reset_serv = nh_priv.advertiseService("reset", &reset);
+
 	ROS_INFO("ready");
 	ros::spin();
 }

redirects.json

@@ -1,6 +1,5 @@
 {
     "redirects": [
-        { "from": "index.html", "to": "ru/index.html" },
         { "from": "assemble.html", "to": "ru/assemble_2.html" },
         { "from": "assemble_clever3_4in1.html", "to": "ru/assemble_3.html" },
         { "from": "glossary.html", "to": "ru/gloss.html" },