minor fixes
authorErik Andresen <erik@vontaene.de>
Fri, 18 Dec 2015 19:42:18 +0000 (20:42 +0100)
committerErik Andresen <erik@vontaene.de>
Fri, 18 Dec 2015 19:42:18 +0000 (20:42 +0100)
cfg/razor.yaml
launch/teleop.launch
launch/wild_thumper.launch
scripts/christmas.py
scripts/forward_tty.sh
scripts/i2c.py
scripts/wt_node.py

index 0f58831..228ff74 100644 (file)
@@ -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
index 18f083c..371ba01 100644 (file)
@@ -2,7 +2,7 @@
 <launch>
        <node pkg="turtlebot_teleop" type="turtlebot_teleop_joy" name="turtlebot_teleop_joystick" respawn="true">
                <param name="scale_linear" value="0.2"/>
-               <param name="scale_angular" value="-0.2"/>
+               <param name="scale_angular" value="-0.1"/>
                <param name="axis_deadman" value="3"/>
                <param name="axis_linear" value="1"/>
                <param name="axis_angular" value="0"/>
index bf7217d..c2a5d01 100644 (file)
@@ -15,7 +15,7 @@
                <rosparam command="load" file="$(find wild_thumper)/cfg/analyzers.yaml"/>
        </node>
 
-       <node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="true">
+       <node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false">
                <param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>
index 6c53dce..bdfd688 100755 (executable)
@@ -4,7 +4,6 @@
 import rospy
 from random import *
 from wild_thumper.msg import LedStripe, Led
-from time import sleep
 
 max_val = 10
 
index daf6040..61d6cf9 100755 (executable)
@@ -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
index c597c85..2bb9f94 100755 (executable)
@@ -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")
index 96d0db6..2672fed 100755 (executable)
@@ -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: