Nonlinear mapping implemented

.gitignore

@@ -0,0 +1 @@
+.vscode

launch/hrp_gb_controller.launch

@@ -1,16 +1,29 @@
 <launch>
 
-  <!--Coefficients to map linear accelerations into linear and angular velocities-->
-  <!--husqvarna = [0.05,0.05]| miro = [50, 0.25]-->
+  <!-- Flag to decide which method must be applied -->
+
+  <param name = "nonlinear" value="1" />
+
+  <!-- Coefficients to map linear accelerations into linear and angular velocities -->
+  <!-- husqvarna = [0.05,0.05]| miro = [50, 0.25] -->
 
   <param name = "linear_coefficient" value="0.05" />
   <param name = "angular_coefficient" value="0.05" />
 
-  <arg name="inertial_sub_topic"  default="/inertial"/>
+  <!-- Coefficients used to perform nonlinear mapping -->
+
+  <param name = "halt_threshold" value="1" />
+  <param name = "max_linear_velocity" value="0.4" />
+  <param name = "max_angular_velocity" value="0.3" />
+  <param name = "x_acceleration_saturation" value="8" />
+  <param name = "y_acceleration_saturation" value="6" />
+
+  <!-- <arg name="inertial_sub_topic"  default="/inertial"/> -->
+  <arg name="inertial_sub_topic"  default="/G_Watch_R_5567/imu_data"/>
   <arg name="mode_pub_topic"  default="/cmd_mode"/>
   <arg name="vel_pub_topic"  default="/cmd_vel"/>
 
-  <node pkg="gesture_based_controller" type="gb_controller.py" name="gb_controller" output="screen">
+  <node pkg="gesture_based_controller" type="gb_controller.py" name="gb_controller" output="screen" required="true">
     <remap from="/inertial" to="$(arg inertial_sub_topic)"/>
     <remap from="/cmd_mode" to="$(arg mode_pub_topic)"/>
     <remap from="/cmd_vel" to="$(arg vel_pub_topic)"/>

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
@@ -12,13 +14,48 @@ 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)
+        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)
         ## 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)
+
+        ## 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)
+
+        ## 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
+        if self.nonlinear_flag == 1:
+            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]
         ## Time instant in which the last acceleration message arrived
@@ -38,13 +75,16 @@ 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
     #
     #  @param self The object pointer
     def run(self):
-        self.init()
+        #self.init()
         r = rospy.Rate(self.update_rate)
         while True:
             try:
@@ -76,15 +116,45 @@ 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):
+        x_acc = self.last_acc[0]
+        if abs(x_acc) < self.halt_threshold:
+            return 0
+        elif abs(x_acc) >= self.x_acc_saturation:
+            return self.lin_max_velocity * 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):
+        y_acc = self.last_acc[1]
+        if abs(y_acc) < self.halt_threshold:
+            return 0
+        elif abs(y_acc) >= self.y_acc_saturation:
+            return self.ang_max_velocity * 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
     #
     #  @param self The object pointer
     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()
         self.pub_twist.publish(twist)