From b4731549ab2b35296ebb7c8fc8c7b6fa92b47e92 Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Thu, 7 Sep 2017 18:30:16 +0200
Subject: [PATCH] solved small problems
---
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 14 +++++++++++---
1 file changed, 11 insertions(+), 3 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index c3e3b6c7..244a0725 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -64,7 +64,7 @@
// line trayectory defines
-#define DISTANCE_THRESHOLD 0.5
+#define DISTANCE_THRESHOLD 0.4
#define PI 3.141592653589
@@ -90,6 +90,7 @@ Setpoint controller_setpoint;
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
+bool was_in_threshold = false;
ros::ServiceClient centralManager;
@@ -401,10 +402,11 @@ void viconCallback(const ViconData& viconData) {
else
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
-
+ ROS_INFO_STREAM("distance: " << distance);
// here, detect if euclidean distance between setpoint and current position is higher than a threshold
if(distance > DISTANCE_THRESHOLD)
{
+ ROS_INFO("inside threshold");
Setpoint setpoint_msg;
// here, where we are now, or where we were in the beginning?
setpoint_msg.x = local.x + DISTANCE_THRESHOLD * unit_vector[0];
@@ -414,10 +416,16 @@ void viconCallback(const ViconData& viconData) {
// yaw is better divided by the number of steps?
setpoint_msg.yaw = current_safe_setpoint.yaw;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+ was_in_threshold = true;
}
else
{
- goToControllerSetpoint(); //maybe this is a bit repetitive?
+ if(was_in_threshold)
+ {
+ was_in_threshold = false;
+ safeControllerServiceSetpointPublisher.publish(current_safe_setpoint);
+ // goToControllerSetpoint(); //maybe this is a bit repetitive?
+ }
}
bool success = safeController.call(controllerCall);
--
GitLab