From c497d9461694d4d2b88c0bd83adc633777c66ff5 Mon Sep 17 00:00:00 2001
From: baumanta <tanja@auterion.com>
Date: Fri, 17 May 2019 10:32:48 +0200
Subject: [PATCH] publish a minimal message for logging/debugging

---
 msg/collision_constraints.msg                 |  4 ----
 .../CollisionPrevention.cpp                   | 24 +++++++++++++++++++
 .../CollisionPrevention.hpp                   |  3 +++
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/msg/collision_constraints.msg b/msg/collision_constraints.msg
index 4c08ac801d..535aab7e33 100644
--- a/msg/collision_constraints.msg
+++ b/msg/collision_constraints.msg
@@ -3,10 +3,6 @@
 
 uint64 timestamp	# time since system start (microseconds)
 
-#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement.
-float32[2] constraints_normalized_x   # constraints determined by range sensor measurements [x negative, x positive]
-float32[2] constraints_normalized_y   # constraintss determined by range sensor measurements [y negative, y positive]
-
 float32[2] original_setpoint   # velocities demanded
 float32[2] adapted_setpoint    # velocities allowed
 
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index dca2ec47e9..83ef4fb5b8 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -65,6 +65,29 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio
 	return true;
 }
 
+void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
+		const Vector2f &adapted_setpoint)
+{
+
+	collision_constraints_s	constraints;	/**< collision constraints message */
+
+	//fill in values
+	constraints.timestamp = hrt_absolute_time();
+
+	constraints.original_setpoint[0] = original_setpoint(0);
+	constraints.original_setpoint[1] = original_setpoint(1);
+	constraints.adapted_setpoint[0] = adapted_setpoint(0);
+	constraints.adapted_setpoint[1] = adapted_setpoint(1);
+
+	// publish constraints
+	if (_constraints_pub != nullptr) {
+		orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);
+
+	} else {
+		_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
+	}
+}
+
 void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float max_acc,
 		const Vector2f &curr_vel)
 {
@@ -165,5 +188,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
 	}
 
 	_interfering = currently_interfering;
+	publishConstrainedSetpoint(original_setpoint, new_setpoint);
 	original_setpoint = new_setpoint;
 }
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp
index c1d99b92ca..aff3faf157 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.hpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp
@@ -75,6 +75,7 @@ private:
 
 	bool _interfering{false};		/**< states if the collision prevention interferes with the user input */
 
+	orb_advert_t _constraints_pub{nullptr};  	/**< constraints publication */
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
 
 	uORB::SubscriptionPollable<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
@@ -93,4 +94,6 @@ private:
 
 	void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const float max_acc, const matrix::Vector2f &curr_vel);
 
+	void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
+
 };