X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fmove_base.py;h=09f823b576dd790073b949f52c784ed85ad5dc3f;hp=0f0bc190fcd7030a6cbdf1b9824a93f309e7e689;hb=ba4ed192fc7db30bcd3115dbeee6e473a2dd5b30;hpb=6a8af73159db6b0af7382a22dae0b3b18295120f diff --git a/scripts/move_base.py b/scripts/move_base.py index 0f0bc19..09f823b 100755 --- a/scripts/move_base.py +++ b/scripts/move_base.py @@ -9,6 +9,7 @@ from math import * from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from sensor_msgs.msg import Imu WHEEL_DIST = 0.248 @@ -16,39 +17,98 @@ class MoveBase: def __init__(self): rospy.init_node('wild_thumper_move_base') rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived) - self.tf_broadcaster = tf.broadcaster.TransformBroadcaster() + rospy.Subscriber("imu", Imu, self.imuReceived) + enable_odom_tf = rospy.get_param("~enable_odom_tf", True) + if enable_odom_tf: + self.tf_broadcaster = tf.broadcaster.TransformBroadcaster() + else: + self.tf_broadcaster = None self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16) self.pub_diag = rospy.Publisher("diagnostics", DiagnosticArray, queue_size=16) self.set_speed(0, 0) rospy.loginfo("Init done") i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction + self.handicap_last = (-1, -1) self.run() def run(self): rate = rospy.Rate(20.0) + reset_val = self.get_reset() + rospy.loginfo("Reset Status: 0x%x" % reset_val) while not rospy.is_shutdown(): + #print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test self.get_tle_err() - #self.get_odom() + self.get_odom() + self.get_voltage() #self.get_dist_forward() #self.get_dist_backward() #self.get_dist_left() #self.get_dist_right() rate.sleep() + def set_motor_handicap(self, front, aft): # percent + if self.handicap_last != (front, aft): + i2c_write_reg(0x50, 0x94, struct.pack(">bb", front, aft)) + 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/65)*abs(pitch)*180/pi + self.set_motor_handicap(0, int(val)) + elif pitch < -30*pi/180: + val = (100.0/65)*abs(pitch)*180/pi + self.set_motor_handicap(int(val), 0) + else: + self.set_motor_handicap(0, 0) + + def get_reset(self): + reset = struct.unpack(">B", i2c_read_reg(0x50, 0xA0, 1))[0] + + msg = DiagnosticArray() + msg.header.stamp = rospy.Time.now() + stat = DiagnosticStatus() + stat.name = "Reset reason" + stat.level = DiagnosticStatus.ERROR if reset & 0x0c else DiagnosticStatus.OK + stat.message = "0x%02x" % reset + + stat.values.append(KeyValue("Watchdog Reset Flag", str(bool(reset & (1 << 3))))) + stat.values.append(KeyValue("Brown-out Reset Flag", str(bool(reset & (1 << 2))))) + stat.values.append(KeyValue("External Reset Flag", str(bool(reset & (1 << 1))))) + stat.values.append(KeyValue("Power-on Reset Flag", str(bool(reset & (1 << 0))))) + + msg.status.append(stat) + self.pub_diag.publish(msg) + return reset + + def get_tle_err(self): - err = struct.unpack(">B", i2c_read_reg(0x50, 0x94, 1))[0] + err = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1))[0] msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() stat = DiagnosticStatus() stat.name = "Motor: Error Status" - stat.level = DiagnosticStatus.OK if not err else DiagnosticStatus.ERROR - stat.message = "OK" if not err else "Error" + stat.level = DiagnosticStatus.ERROR if err else DiagnosticStatus.OK + stat.message = "0x%02x" % err + + stat.values.append(KeyValue("aft left", str(bool(err & (1 << 0))))) + stat.values.append(KeyValue("front left", str(bool(err & (1 << 1))))) + stat.values.append(KeyValue("front right", str(bool(err & (1 << 2))))) + stat.values.append(KeyValue("aft right", str(bool(err & (1 << 3))))) - stat.values.append(KeyValue("Motor 1", str(bool(err & (1 << 0))))) - stat.values.append(KeyValue("Motor 2", str(bool(err & (1 << 1))))) - stat.values.append(KeyValue("Motor 3", str(bool(err & (1 << 2))))) - stat.values.append(KeyValue("Motor 4", str(bool(err & (1 << 3))))) + msg.status.append(stat) + self.pub_diag.publish(msg) + + def get_voltage(self): + volt = struct.unpack(">h", i2c_read_reg(0x52, 0x09, 2))[0]/100.0 + + msg = DiagnosticArray() + msg.header.stamp = rospy.Time.now() + stat = DiagnosticStatus() + stat.name = "Voltage" + stat.level = DiagnosticStatus.ERROR if volt < 7 else DiagnosticStatus.OK + stat.message = "%.2fV" % volt msg.status.append(stat) self.pub_diag.publish(msg) @@ -63,7 +123,8 @@ class MoveBase: odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle) # first, we'll publish the transform over tf - self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_link", "odom") + if self.tf_broadcaster is not None: + self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_footprint", "odom") # next, we'll publish the odometry message over ROS odom = Odometry() @@ -78,12 +139,24 @@ class MoveBase: odom.pose.pose.orientation.y = odom_quat[1] odom.pose.pose.orientation.z = odom_quat[2] odom.pose.pose.orientation.w = odom_quat[3] + odom.pose.covariance[0] = 1e-3 # x + odom.pose.covariance[7] = 1e-3 # y + odom.pose.covariance[14] = 1e6 # z + odom.pose.covariance[21] = 1e6 # rotation about X axis + odom.pose.covariance[28] = 1e6 # rotation about Y axis + odom.pose.covariance[35] = 0.1 # rotation about Z axis # set the velocity - odom.child_frame_id = "base_link" + odom.child_frame_id = "base_footprint" odom.twist.twist.linear.x = speed_trans odom.twist.twist.linear.y = 0.0 odom.twist.twist.angular.z = speed_rot + odom.twist.covariance[0] = 1e-3 # x + odom.twist.covariance[7] = 1e-3 # y + odom.twist.covariance[14] = 1e6 # z + odom.twist.covariance[21] = 1e6 # rotation about X axis + odom.twist.covariance[28] = 1e6 # rotation about Y axis + odom.twist.covariance[35] = 0.1 # rotation about Z axis # publish the message self.pub_odom.publish(odom)