reenabled motor handicap and made parameters configureable with

dyn_reconf

diff --git a/config/wt_node.cfg b/config/wt_node.cfg
index 3b97339ec3b01acab196210e9f0655c39102c2f2..dbd9c1b285ef9922372e9f0ae2ab75624e8d5e6e 100755 (executable)
--- a/config/wt_node.cfg
+++ b/config/wt_node.cfg
@@ -8,5 +8,8 @@ gen.add("range_sensor_clip",    bool_t,         0, "Clip range sensor values to max range"
 gen.add("range_sensor_max",    double_t,       0, "Range sensor max range", 0.5, 0.4, 4)
 gen.add("odom_covar_xy",       double_t,       0, "Odometry covariance: translation", 1e-2, 1e-6, 1)
 gen.add("odom_covar_angle",    double_t,       0, "Odometry covariance: rotation", 0.25, 1e-6, 6.2832)
+gen.add("pitch_handicap_enable",bool_t,                0, "Enable motor handicap on pitch", True)
+gen.add("pitch_handicap_limit",        double_t,       0, "Pitch handicap limit (degree)", 30, 0, 90)
+gen.add("pitch_handicap_strength",double_t,    0, "Pitch handicap limit strength", 60, 0, 100)
 
 exit(gen.generate("wild_thumper", "wild_thumper", "WildThumper"))

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 2aa98a24b78d416da53ef8616dde5463eaf31628..5fc59d7dbf5dad1dfd63639fa267534032073aa4 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -71,8 +71,8 @@ class MoveBase:
                self.handicap_last = (-1, -1)
                self.pStripe = LPD8806(1, 0, 12)
                rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
-               #rospy.Subscriber("imu", Imu, self.imuReceived)
                rospy.Subscriber("led_stripe", LedStripe, self.led_stripe_received)
+               rospy.Subscriber("imu", Imu, self.imuReceived)
                self.run()
        
        def run(self):
@@ -104,6 +104,13 @@ class MoveBase:
                self.range_sensor_max = config["range_sensor_max"]
                self.odom_covar_xy = config["odom_covar_xy"]
                self.odom_covar_angle = config["odom_covar_angle"]
+               self.pitch_handicap_enable = config["pitch_handicap_enable"]
+               self.pitch_handicap_limit = config["pitch_handicap_limit"]
+               self.pitch_handicap_strength = config["pitch_handicap_strength"]
+
+               if not self.pitch_handicap_enable:
+                       self.set_motor_handicap(0, 0)
+
                return config
 
        def set_motor_handicap(self, front, aft): # percent
@@ -114,15 +121,16 @@ class MoveBase:
                        self.handicap_last = (front, aft)
 
        def imuReceived(self, msg):
-               (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
-               if pitch > 30*pi/180:
-                       val = (100.0/60)*abs(pitch)*180/pi
-                       self.set_motor_handicap(int(val), 0)
-               elif pitch < -30*pi/180:
-                       val = (100.0/60)*abs(pitch)*180/pi
-                       self.set_motor_handicap(0, int(val))
-               else:
-                       self.set_motor_handicap(0, 0)
+               if self.pitch_handicap_enable:
+                       (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
+                       if pitch > self.pitch_handicap_limit*pi/180:
+                               val = (100.0/self.pitch_handicap_strength)*abs(pitch)*180/pi
+                               self.set_motor_handicap(int(val), 0)
+                       elif pitch < -self.pitch_handicap_limit*pi/180:
+                               val = (100.0/self.pitch_handicap_strength)*abs(pitch)*180/pi
+                               self.set_motor_handicap(0, int(val))
+                       else:
+                               self.set_motor_handicap(0, 0)
 
        def get_reset(self):
                reset = struct.unpack(">B", i2c_read_reg(0x50, 0xA0, 1))[0]