From d44edcac9dd285abbd1a6b2f9f82b6d15be00ff6 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 24 Mar 2017 22:55:37 +0100 Subject: [PATCH] reenabled motor handicap and made parameters configureable with dyn_reconf --- config/wt_node.cfg | 3 +++ scripts/wt_node.py | 28 ++++++++++++++++++---------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/config/wt_node.cfg b/config/wt_node.cfg index 3b97339..dbd9c1b 100755 --- 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 2aa98a2..5fc59d7 100755 --- 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] -- 2.39.5