trajectory: add time_horizon field

L4ncelot · May 7, 2018 · e10bd62 · e10bd62
1 parent f179f80
commit e10bd62
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/mavros_extras/src/plugins/trajectory.cpp b/mavros_extras/src/plugins/trajectory.cpp
@@ -561,7 +561,15 @@ class TrajectoryPlugin : public plugin::PluginBase {
 			return a;
 		}
 
-		return fmod(a + M_PI, 2.0f * M_PI) - M_PI;
+		while (a >= M_PI) {
+			a -= (M_PI * 2.0f);
+		}
+
+		while (a < -M_PI) {
+			a += (M_PI * 2.0f);
+		}
+
+		return a;
 	}
 };
 }					// namespace extra_plugins

diff --git a/mavros_msgs/msg/Trajectory.msg b/mavros_msgs/msg/Trajectory.msg
@@ -14,3 +14,5 @@ mavros_msgs/PositionTarget point_4
 mavros_msgs/PositionTarget point_5
 
 uint8[5] point_valid # States if respective point is valid.
+
+float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN
Original file line number	Diff line number	Diff line change
Expand Up		@@ -14,3 +14,5 @@ mavros_msgs/PositionTarget point_4
		mavros_msgs/PositionTarget point_5

		uint8[5] point_valid # States if respective point is valid.

		float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN