X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fwt_node.py;h=7254c1d38ad4573961862f0af7850e87872d417b;hp=2672fed1d2bc636bb4ca3b7f40bed22e3e2f0b54;hb=c3307ab04ec14a48e15833c0894d9518d49613cf;hpb=9349f21c7fb8f1629fe4154cc054fc894a9e600d diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 2672fed..7254c1d 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -14,6 +14,8 @@ from nav_msgs.msg import Odometry from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from sensor_msgs.msg import Imu, Range from wild_thumper.msg import LedStripe +from dynamic_reconfigure.server import Server +from wild_thumper.cfg import WildThumperConfig WHEEL_DIST = 0.248 @@ -55,30 +57,34 @@ class MoveBase: self.tf_broadcaster = tf.broadcaster.TransformBroadcaster() else: self.tf_broadcaster = None + self.dyn_conf = Server(WildThumperConfig, self.execute_dyn_reconf) self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16) self.pub_diag = rospy.Publisher("diagnostics", DiagnosticArray, queue_size=16) self.pub_range_fwd = rospy.Publisher("range_forward", Range, queue_size=16) self.pub_range_bwd = rospy.Publisher("range_backward", Range, queue_size=16) self.pub_range_left = rospy.Publisher("range_left", Range, queue_size=16) self.pub_range_right = rospy.Publisher("range_right", Range, queue_size=16) + self.cmd_vel = None 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.pStripe = LPD8806(1, 0, 12) rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived) - rospy.Subscriber("imu", Imu, self.imuReceived) + #rospy.Subscriber("imu", Imu, self.imuReceived) rospy.Subscriber("led_stripe", LedStripe, self.led_stripe_received) self.run() def run(self): rate = rospy.Rate(20.0) + sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag reset_val = self.get_reset() rospy.loginfo("Reset Status: 0x%x" % reset_val) i = 0 while not rospy.is_shutdown(): + rospy.logdebug("Loop alive") #print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test - self.get_tle_err() + self.get_motor_err() self.get_odom() self.get_voltage() if i % 2: @@ -88,9 +94,19 @@ class MoveBase: self.get_dist_backward() self.get_dist_right() i+=1 + if self.cmd_vel != None: + self.set_speed(self.cmd_vel[0], self.cmd_vel[1]) + self.cmd_vel = None rate.sleep() + def execute_dyn_reconf(self, config, level): + self.bClipRangeSensor = config["range_sensor_clip"] + self.range_sensor_max = config["range_sensor_max"] + return config + def set_motor_handicap(self, front, aft): # percent + if front > 100: front = 100 + if aft > 100: aft = 100 if self.handicap_last != (front, aft): i2c_write_reg(0x50, 0x94, struct.pack(">bb", front, aft)) self.handicap_last = (front, aft) @@ -98,10 +114,10 @@ class MoveBase: 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 + val = (100.0/60)*abs(pitch)*180/pi self.set_motor_handicap(int(val), 0) elif pitch < -30*pi/180: - val = (100.0/65)*abs(pitch)*180/pi + val = (100.0/60)*abs(pitch)*180/pi self.set_motor_handicap(0, int(val)) else: self.set_motor_handicap(0, 0) @@ -134,7 +150,7 @@ class MoveBase: return reset - def get_tle_err(self): + def get_motor_err(self): err = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1))[0] msg = DiagnosticArray() @@ -144,10 +160,16 @@ class MoveBase: 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))))) + # Diag + stat.values.append(KeyValue("aft left diag", str(bool(err & (1 << 0))))) + stat.values.append(KeyValue("front left diag", str(bool(err & (1 << 1))))) + stat.values.append(KeyValue("aft right diag", str(bool(err & (1 << 2))))) + stat.values.append(KeyValue("front right diag", str(bool(err & (1 << 3))))) + # Stall + stat.values.append(KeyValue("aft left stall", str(bool(err & (1 << 4))))) + stat.values.append(KeyValue("front left stall", str(bool(err & (1 << 5))))) + stat.values.append(KeyValue("aft right stall", str(bool(err & (1 << 6))))) + stat.values.append(KeyValue("front right stall", str(bool(err & (1 << 7))))) msg.status.append(stat) self.pub_diag.publish(msg) @@ -167,8 +189,7 @@ class MoveBase: def get_odom(self): - posx, posy, angle = struct.unpack(">fff", i2c_read_reg(0x50, 0x40, 12)) - speed_trans, speed_rot = struct.unpack(">ff", i2c_read_reg(0x50, 0x38, 8)) + speed_trans, speed_rot, posx, posy, angle = struct.unpack(">fffff", i2c_read_reg(0x50, 0x38, 20)) current_time = rospy.Time.now() # since all odometry is 6DOF we'll need a quaternion created from yaw @@ -218,9 +239,9 @@ class MoveBase: i2c_write_reg(0x50, 0x50, struct.pack(">ff", trans, rot)) def cmdVelReceived(self, msg): - trans = msg.linear.x - rot = msg.angular.z # rad/s - self.set_speed(trans, rot) + rospy.logdebug("Set new cmd_vel:", msg.linear.x, msg.angular.z) + self.cmd_vel = (msg.linear.x, msg.angular.z) # commit speed on next update cycle + rospy.logdebug("Set new cmd_vel done") # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12 def get_dist_ir(self, num): @@ -248,6 +269,8 @@ class MoveBase: return struct.unpack(">H", i2c_read_reg(0x52, num, 2))[0]/1000.0 def send_range(self, pub, frame_id, typ, dist, min_range, max_range, fov_deg): + if self.bClipRangeSensor and dist > max_range: + dist = max_range msg = Range() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id @@ -273,13 +296,13 @@ class MoveBase: def get_dist_forward(self): if self.pub_range_fwd.get_num_connections() > 0: dist = self.read_dist_srf(0x15) - self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 40) + self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30) self.start_dist_srf(0x5) # get next value def get_dist_backward(self): if self.pub_range_bwd.get_num_connections() > 0: dist = self.read_dist_srf(0x17) - self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 40) + self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30) self.start_dist_srf(0x7) # get next value def led_stripe_received(self, msg):