From b6eda8fce2ff7f76068f8cb1b033c59a7e081fb2 Mon Sep 17 00:00:00 2001 From: Alessio Bollea Date: Wed, 18 Sep 2019 11:02:02 +0200 Subject: [PATCH 1/5] Started nonlinear mapping implementation Added parameters in the launchfile and modified the "update" method --- .gitignore | 1 + launch/hrp_gb_controller.launch | 12 ++++++++++-- src/gb_controller.py | 31 ++++++++++++++++++++++++++++--- 3 files changed, 39 insertions(+), 5 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..600d2d3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode \ No newline at end of file diff --git a/launch/hrp_gb_controller.launch b/launch/hrp_gb_controller.launch index fdc4685..d39d6f4 100644 --- a/launch/hrp_gb_controller.launch +++ b/launch/hrp_gb_controller.launch @@ -1,11 +1,19 @@ - - + + + + + + + + + + diff --git a/src/gb_controller.py b/src/gb_controller.py index 4e140c2..08c4c55 100755 --- a/src/gb_controller.py +++ b/src/gb_controller.py @@ -12,13 +12,19 @@ class GestureController(object): # @param self The object pointer def __init__(self): ## Node frequency (Hz) - self.update_rate = 10 + self.update_rate = 10 + + ## Flag for the method to apply (nonlinear: 1, linear: any other value) + self.nonlinear_flag = rospy.get_param('nonlinear', 0) ## Mapping coefficient for linear velocity self.linear_coefficent = rospy.get_param ('linear_coefficient', 0.05) ## Mapping coefficient for angular velocity self.angular_coefficent = rospy.get_param ('angular_coefficient', 0.05) + ## Halt threshold: if the linear velocity is less than this value, the mower doesn't move + self.halt_threshold = rospy.get_param ('halt_threshold', 0) + ## Stores the last acceleration received by the node self.last_acc = [0,0,0] ## Time instant in which the last acceleration message arrived @@ -76,6 +82,18 @@ def reset(self): rospy.loginfo("RESETTING VELOCITY COMMANDS ON SHUTDOWN") self.update() + ## Nonlinear mapping function for the linear velocity + # + # @param self The object pointer + def v_nonlinear_mapping(self): + pass + + ## Nonlinear mapping function for the angular velocity + # + # @param self The object pointer + def w_nonlinear_mapping(self): + pass + ## Mapping of acceleration to robot angular and linear velocities # # @param self The object pointer @@ -83,8 +101,15 @@ def update(self): if rospy.is_shutdown(): return twist = Twist() - twist.linear.x = self.last_acc[0] * self.linear_coefficent - twist.angular.z = self.last_acc[1] * self.angular_coefficent + # LINEAR METHOD + if self.nonlinear_flag != 1: + twist.linear.x = self.last_acc[0] * self.linear_coefficent + twist.angular.z = self.last_acc[1] * self.angular_coefficent + # NONLINEAR METHOD + else: + twist.linear.x = self.v_nonlinear_mapping() + twist.angular.z = self.w_nonlinear_mapping() + pass self.pub_twist.publish(twist) From a2822326a5ef7d42a6bab3f2a443d81c77529fd5 Mon Sep 17 00:00:00 2001 From: BolleA7X Date: Wed, 18 Sep 2019 15:36:24 +0200 Subject: [PATCH 2/5] Implementation of nonlinear mapping --- launch/hrp_gb_controller.launch | 8 +++--- src/gb_controller.py | 47 ++++++++++++++++++++++++++++++--- 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/launch/hrp_gb_controller.launch b/launch/hrp_gb_controller.launch index d39d6f4..a2f6dfa 100644 --- a/launch/hrp_gb_controller.launch +++ b/launch/hrp_gb_controller.launch @@ -2,7 +2,7 @@ - + @@ -12,13 +12,15 @@ - + + + - + diff --git a/src/gb_controller.py b/src/gb_controller.py index 08c4c55..187f817 100755 --- a/src/gb_controller.py +++ b/src/gb_controller.py @@ -1,5 +1,7 @@ #! /usr/bin/env python import rospy +import sys +from numpy import sign from geometry_msgs.msg import Pose, Twist from sensor_msgs.msg import Imu from std_msgs.msg import UInt16 @@ -16,6 +18,10 @@ def __init__(self): ## Flag for the method to apply (nonlinear: 1, linear: any other value) self.nonlinear_flag = rospy.get_param('nonlinear', 0) + if self.nonlinear_flag == 1: + rospy.loginfo("USING NONLINEAR MAPPING") + else: + rospy.loginfo("USING LINEAR MAPPING") ## Mapping coefficient for linear velocity self.linear_coefficent = rospy.get_param ('linear_coefficient', 0.05) @@ -25,6 +31,28 @@ def __init__(self): ## Halt threshold: if the linear velocity is less than this value, the mower doesn't move self.halt_threshold = rospy.get_param ('halt_threshold', 0) + ## Saturation values for x and y acceleration from the smartwatch + self.x_acc_saturation = rospy.get_param ('x_acceleration_saturation', 8) + self.y_acc_saturation = rospy.get_param ('y_acceleration_saturation', 6) + + ## The two saturation and the threshold values must be positive. + # The halt threshold must be less than both values. + # This check if performed only if the nonlinear flag is high + if self.nonlinear_flag == 1 and ( \ + self.halt_threshold <= 0 or \ + self.x_acc_saturation <= 0 or \ + self.y_acc_saturation <= 0 or \ + self.halt_threshold >= self.x_acc_saturation or \ + self.halt_threshold >= self.y_acc_saturation): + rospy.logfatal("INVALID LAUNCHFILE PARAMETERS VALUES!") + sys.exit(1) + + ## If the nonlinear flag is high, the two mapping coefficients are updated + # 0.4 and 0.3 are taken as the maximum linear and angular velocities possible + if self.nonlinear_flag == 1: + self.linear_coefficent = 0.4 / (self.x_acc_saturation - self.halt_threshold) + self.angular_coefficent = 0.3 / (self.y_acc_saturation - self.halt_threshold) + ## Stores the last acceleration received by the node self.last_acc = [0,0,0] ## Time instant in which the last acceleration message arrived @@ -50,7 +78,7 @@ def callback_continuos_control(self,data): # # @param self The object pointer def run(self): - self.init() + #self.init() r = rospy.Rate(self.update_rate) while True: try: @@ -86,13 +114,25 @@ def reset(self): # # @param self The object pointer def v_nonlinear_mapping(self): - pass + x_acc = self.last_acc[0] + if abs(x_acc) < self.halt_threshold: + return 0 + elif abs(x_acc) >= self.x_acc_saturation: + return 0.4 * sign(x_acc) + else: + return self.linear_coefficent * (x_acc - self.halt_threshold * sign(x_acc)) ## Nonlinear mapping function for the angular velocity # # @param self The object pointer def w_nonlinear_mapping(self): - pass + y_acc = self.last_acc[1] + if abs(y_acc) < self.halt_threshold: + return 0 + elif abs(y_acc) >= self.y_acc_saturation: + return 0.3 * sign(y_acc) + else: + return self.angular_coefficent * (y_acc - self.halt_threshold * sign(y_acc)) ## Mapping of acceleration to robot angular and linear velocities # @@ -109,7 +149,6 @@ def update(self): else: twist.linear.x = self.v_nonlinear_mapping() twist.angular.z = self.w_nonlinear_mapping() - pass self.pub_twist.publish(twist) From 75819b88e1f18bbdce2f458249f9fbe79228f27f Mon Sep 17 00:00:00 2001 From: BolleA7X Date: Thu, 19 Sep 2019 11:28:16 +0200 Subject: [PATCH 3/5] Bugfixing --- launch/hrp_gb_controller.launch | 3 ++- src/gb_controller.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/hrp_gb_controller.launch b/launch/hrp_gb_controller.launch index a2f6dfa..4e28631 100644 --- a/launch/hrp_gb_controller.launch +++ b/launch/hrp_gb_controller.launch @@ -16,7 +16,8 @@ - + + diff --git a/src/gb_controller.py b/src/gb_controller.py index 187f817..8a5f7fa 100755 --- a/src/gb_controller.py +++ b/src/gb_controller.py @@ -39,7 +39,7 @@ def __init__(self): # The halt threshold must be less than both values. # This check if performed only if the nonlinear flag is high if self.nonlinear_flag == 1 and ( \ - self.halt_threshold <= 0 or \ + self.halt_threshold < 0 or \ self.x_acc_saturation <= 0 or \ self.y_acc_saturation <= 0 or \ self.halt_threshold >= self.x_acc_saturation or \ From e230b8670aa633e1525218c710f67f89f51947b6 Mon Sep 17 00:00:00 2001 From: BolleA7X Date: Thu, 19 Sep 2019 11:57:48 +0200 Subject: [PATCH 4/5] Minor improvements --- launch/hrp_gb_controller.launch | 2 ++ src/gb_controller.py | 13 ++++++++----- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/launch/hrp_gb_controller.launch b/launch/hrp_gb_controller.launch index 4e28631..8fd0660 100644 --- a/launch/hrp_gb_controller.launch +++ b/launch/hrp_gb_controller.launch @@ -13,6 +13,8 @@ + + diff --git a/src/gb_controller.py b/src/gb_controller.py index 8a5f7fa..9304140 100755 --- a/src/gb_controller.py +++ b/src/gb_controller.py @@ -31,6 +31,10 @@ def __init__(self): ## Halt threshold: if the linear velocity is less than this value, the mower doesn't move self.halt_threshold = rospy.get_param ('halt_threshold', 0) + ## Maximum velocities: index 0 is the linear velocity, index 1 + self.lin_max_velocity = rospy.get_param('max_linear_velocity', 0.4) + self.ang_max_velocity = rospy.get_param('max_angular_velocity', 0.3) + ## Saturation values for x and y acceleration from the smartwatch self.x_acc_saturation = rospy.get_param ('x_acceleration_saturation', 8) self.y_acc_saturation = rospy.get_param ('y_acceleration_saturation', 6) @@ -48,10 +52,9 @@ def __init__(self): sys.exit(1) ## If the nonlinear flag is high, the two mapping coefficients are updated - # 0.4 and 0.3 are taken as the maximum linear and angular velocities possible if self.nonlinear_flag == 1: - self.linear_coefficent = 0.4 / (self.x_acc_saturation - self.halt_threshold) - self.angular_coefficent = 0.3 / (self.y_acc_saturation - self.halt_threshold) + self.linear_coefficent = self.lin_max_velocity / (self.x_acc_saturation - self.halt_threshold) + self.angular_coefficent = self.ang_max_velocity / (self.y_acc_saturation - self.halt_threshold) ## Stores the last acceleration received by the node self.last_acc = [0,0,0] @@ -118,7 +121,7 @@ def v_nonlinear_mapping(self): if abs(x_acc) < self.halt_threshold: return 0 elif abs(x_acc) >= self.x_acc_saturation: - return 0.4 * sign(x_acc) + return self.lin_max_velocity * sign(x_acc) else: return self.linear_coefficent * (x_acc - self.halt_threshold * sign(x_acc)) @@ -130,7 +133,7 @@ def w_nonlinear_mapping(self): if abs(y_acc) < self.halt_threshold: return 0 elif abs(y_acc) >= self.y_acc_saturation: - return 0.3 * sign(y_acc) + return self.ang_max_velocity * sign(y_acc) else: return self.angular_coefficent * (y_acc - self.halt_threshold * sign(y_acc)) From 9e70e90a972d306e4fbd4933923ae92255a9ee98 Mon Sep 17 00:00:00 2001 From: BolleA7X Date: Thu, 19 Sep 2019 13:38:22 +0200 Subject: [PATCH 5/5] Correction on timeout check --- src/gb_controller.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/gb_controller.py b/src/gb_controller.py index 9304140..3935f70 100755 --- a/src/gb_controller.py +++ b/src/gb_controller.py @@ -75,7 +75,10 @@ def callback_continuos_control(self,data): self.last_acc[0] = data.linear_acceleration.x self.last_acc[1] = data.linear_acceleration.y self.last_acc[2] = data.linear_acceleration.z - self.last_time = data.header.stamp.secs + + now = rospy.get_rostime() + self.last_time = now.secs + #self.last_time = data.header.stamp.secs ## Controller starter #