From be5b4ef00bbf3ef1eedded2b7bfeef40bbed321b Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Fri, 8 Sep 2017 14:47:59 +0200
Subject: [PATCH] added parameter distance threshold to yaml file
---
.../src/d_fall_pps/param/SafeController.yaml | 3 +++
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 23 ++++++++++---------
2 files changed, 15 insertions(+), 11 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index 62302025..ad5423c8 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -22,3 +22,6 @@ takeOffDistance : 0.7
landingDistance : 0.1
durationTakeOff : 3
durationLanding : 3
+
+# Liner Trayectory following parameters
+distanceThreshold : 0.5
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 65ac933b..890ff921 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -62,11 +62,6 @@
#define DURATION_LANDING 3 // seconds
-// line trayectory defines
-
-#define DISTANCE_THRESHOLD 0.4
-
-
#define PI 3.141592653589
using namespace d_fall_pps;
@@ -91,6 +86,7 @@ Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
bool was_in_threshold = false;
+double distance_threshold; //to be loaded from yaml
ros::ServiceClient centralManager;
@@ -402,16 +398,16 @@ void viconCallback(const ViconData& viconData) {
else
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
- ROS_INFO_STREAM("distance: " << distance);
+ ROS_INFO_STREAM("distance: " << distance);
// here, detect if euclidean distance between setpoint and current position is higher than a threshold
- if(distance > DISTANCE_THRESHOLD)
+ if(distance > distance_threshold)
{
- ROS_INFO("inside 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];
- setpoint_msg.y = local.y + DISTANCE_THRESHOLD * unit_vector[1];
- setpoint_msg.z = local.z + DISTANCE_THRESHOLD * unit_vector[2];
+ setpoint_msg.x = local.x + distance_threshold * unit_vector[0];
+ setpoint_msg.y = local.y + distance_threshold * unit_vector[1];
+ setpoint_msg.z = local.z + distance_threshold * unit_vector[2];
// yaw is better divided by the number of steps?
setpoint_msg.yaw = current_safe_setpoint.yaw;
@@ -490,6 +486,11 @@ void loadSafeControllerParameters()
{
ROS_ERROR("Failed to get duration_landing");
}
+
+ if(!nh_safeControllerService.getParam("distanceThreshold", distance_threshold))
+ {
+ ROS_ERROR("Failed to get distance_threshold");
+ }
}
void loadCrazyflieContext() {
--
GitLab