From: Erik Andresen Date: Fri, 18 Dec 2015 19:42:18 +0000 (+0100) Subject: minor fixes X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=9349f21c7fb8f1629fe4154cc054fc894a9e600d;p=ros_wild_thumper.git minor fixes --- diff --git a/cfg/razor.yaml b/cfg/razor.yaml index 0f58831..228ff74 100644 --- a/cfg/razor.yaml +++ b/cfg/razor.yaml @@ -23,8 +23,9 @@ magn_z_max: 596 # extended calibration calibration_magn_use_extended: true -magn_ellipsoid_center: [253.780, 56.6661, -712.749] -magn_ellipsoid_transform: [[0.929568, 0.00880376, 0.0124014], [0.00880376, 0.949055, 0.00600124], [0.0124014, 0.00600124, 0.996672]] +magn_ellipsoid_center: [293.553, 46.4419, -692.900] +magn_ellipsoid_transform: [[0.939516, 0.0193393, 0.0340815], [0.0193393, 0.957723, 0.0120572], [0.0340815, 0.0120572, 0.966198]] + # AHRS to robot calibration imu_yaw_calibration: 0.0 diff --git a/launch/teleop.launch b/launch/teleop.launch index 18f083c..371ba01 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -2,7 +2,7 @@ - + diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch index bf7217d..c2a5d01 100644 --- a/launch/wild_thumper.launch +++ b/launch/wild_thumper.launch @@ -15,7 +15,7 @@ - + diff --git a/scripts/christmas.py b/scripts/christmas.py index 6c53dce..bdfd688 100755 --- a/scripts/christmas.py +++ b/scripts/christmas.py @@ -4,7 +4,6 @@ import rospy from random import * from wild_thumper.msg import LedStripe, Led -from time import sleep max_val = 10 diff --git a/scripts/forward_tty.sh b/scripts/forward_tty.sh index daf6040..61d6cf9 100755 --- a/scripts/forward_tty.sh +++ b/scripts/forward_tty.sh @@ -1,3 +1,6 @@ #!/bin/sh +# +# Client: +# socat PTY,link=/dev/ttyS4,mode=777 TCP:wildthumper:10001 socat /dev/ttyUSB0,b57600 TCP4-LISTEN:10001,reuseaddr diff --git a/scripts/i2c.py b/scripts/i2c.py index c597c85..2bb9f94 100755 --- a/scripts/i2c.py +++ b/scripts/i2c.py @@ -6,10 +6,12 @@ import inspect import os import logging import struct +import fcntl from ctypes import * from time import sleep DEBUG=0 +I2C_FILENAME = "/dev/i2c-2" logger = logging.getLogger(__name__) class i2c: @@ -32,9 +34,10 @@ class i2c: logger.warning("Error: (%s) I2C blocked %fs by %s!", parent, count*0.001, parent_owner) i2c.__parent_owner = inspect.stack()[1] i2c.__single = True - self.dev = i2c.libc.open("/dev/i2c-2", os.O_RDWR) + self.dev = i2c.libc.open(I2C_FILENAME, os.O_RDWR) if self.dev < 0: raise IOError("open") + fcntl.flock(self.dev, fcntl.LOCK_EX) err = i2c.libc.ioctl(self.dev, i2c.I2C_SLAVE, addr>>1) if err < 0: raise IOError("ioctl") diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 96d0db6..2672fed 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -6,7 +6,8 @@ import tf import struct import prctl import spidev -from i2c import * +from time import sleep +from i2c import i2c, i2c_write_reg, i2c_read_reg from math import * from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -115,10 +116,18 @@ class MoveBase: 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))))) + wdrf = bool(reset & (1 << 3)) + if wdrf: rospy.loginfo("Watchdog Reset") + borf = bool(reset & (1 << 2)) + if borf: rospy.loginfo("Brown-out Reset Flag") + extrf = bool(reset & (1 << 1)) + if extrf: rospy.loginfo("External Reset Flag") + porf = bool(reset & (1 << 0)) + if porf: rospy.loginfo("Power-on Reset Flag") + stat.values.append(KeyValue("Watchdog Reset Flag", str(wdrf))) + stat.values.append(KeyValue("Brown-out Reset Flag", str(borf))) + stat.values.append(KeyValue("External Reset Flag", str(extrf))) + stat.values.append(KeyValue("Power-on Reset Flag", str(porf))) msg.status.append(stat) self.pub_diag.publish(msg) @@ -236,16 +245,7 @@ class MoveBase: dev.close() def read_dist_srf(self, num): - dev = i2c(0x52) - s = struct.pack("B", num) - dev.write(s) - dev.close() - - dev = i2c(0x52) - s = dev.read(2) - dev.close() - - return struct.unpack(">H", s)[0]/1000.0 + 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): msg = Range() @@ -260,13 +260,15 @@ class MoveBase: def get_dist_left(self): if self.pub_range_left.get_num_connections() > 0: - dist = 30.553/(self.get_dist_ir(0x1) - -67.534) - self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 1) + dist = self.get_dist_ir(0x1) + if dist > -67: + self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, 30.553/(dist - -67.534), 0.04, 0.3, 1) def get_dist_right(self): if self.pub_range_right.get_num_connections() > 0: - dist = 17.4/(self.get_dist_ir(0x3) - 69) - self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 1) + dist = self.get_dist_ir(0x3) + if dist > 69: + self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, 17.4/(dist - 69), 0.04, 0.3, 1) def get_dist_forward(self): if self.pub_range_fwd.get_num_connections() > 0: