minor fixes

diff --git a/cfg/razor.yaml b/cfg/razor.yaml
index 0f588317e926b02112b1ece2fe0d6f6014e1f271..228ff74951301f8189cd73025c710ccd267a92b3 100644 (file)
--- a/cfg/razor.yaml
+++ b/cfg/razor.yaml
@@ -23,8 +23,9 @@ magn_z_max: 596
 
 # extended calibration
 calibration_magn_use_extended: true
 
 # 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
 
 # AHRS to robot calibration
 imu_yaw_calibration: 0.0

diff --git a/launch/teleop.launch b/launch/teleop.launch
index 18f083cdec64a81705874873b8c573afdb731153..371ba016a9ca8b63943aaf1ea75a883d0c7282c0 100644 (file)
--- a/launch/teleop.launch
+++ b/launch/teleop.launch
@@ -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"/>
 <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"/>
                <param name="axis_deadman" value="3"/>
                <param name="axis_linear" value="1"/>
                <param name="axis_angular" value="0"/>

diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index bf7217d71a2bb8946dd0e25f00c30938e3c27f42..c2a5d0128744fe93858fdfa7ea2350b1f00a003f 100644 (file)
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -15,7 +15,7 @@
                <rosparam command="load" file="$(find wild_thumper)/cfg/analyzers.yaml"/>
        </node>
 
                <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>
                <param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>

diff --git a/scripts/christmas.py b/scripts/christmas.py
index 6c53dce82ef11482fada2c3f5ab618485b5f392c..bdfd688272e44fa9df32594ce369fd8edd2ebb29 100755 (executable)
--- a/scripts/christmas.py
+++ b/scripts/christmas.py
@@ -4,7 +4,6 @@
 import rospy
 from random import *
 from wild_thumper.msg import LedStripe, Led
 import rospy
 from random import *
 from wild_thumper.msg import LedStripe, Led

-from time import sleep

 
 max_val = 10
 
 
 max_val = 10
 

diff --git a/scripts/forward_tty.sh b/scripts/forward_tty.sh
index daf60405748327bba67ce2b1d2cd5ea69b58752b..61d6cf9f741edbc7adda13e51e40e54770ff9470 100755 (executable)
--- a/scripts/forward_tty.sh
+++ b/scripts/forward_tty.sh
@@ -1,3 +1,6 @@
 #!/bin/sh
 #!/bin/sh

+#
+# Client:
+# socat PTY,link=/dev/ttyS4,mode=777 TCP:wildthumper:10001

 
 socat /dev/ttyUSB0,b57600 TCP4-LISTEN:10001,reuseaddr
 
 socat /dev/ttyUSB0,b57600 TCP4-LISTEN:10001,reuseaddr

diff --git a/scripts/i2c.py b/scripts/i2c.py
index c597c85b150e042d33d5cd51d24a69044a30c1b3..2bb9f94026849106394d92e9f3210d7feff14de6 100755 (executable)
--- a/scripts/i2c.py
+++ b/scripts/i2c.py
@@ -6,10 +6,12 @@ import inspect
 import os
 import logging
 import struct
 import os
 import logging
 import struct

+import fcntl

 from ctypes import *
 from time import sleep
 
 DEBUG=0
 from ctypes import *
 from time import sleep
 
 DEBUG=0

+I2C_FILENAME = "/dev/i2c-2"

 logger = logging.getLogger(__name__)
 
 class i2c:
 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
                                        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")
                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")
                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 96d0db6f46532b78e7ed8ce16e458c279485e8bb..2672fed1d2bc636bb4ca3b7f40bed22e3e2f0b54 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -6,7 +6,8 @@ import tf
 import struct
 import prctl
 import spidev
 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
 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.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)
 
                msg.status.append(stat)
                self.pub_diag.publish(msg)


@@ -236,16 +245,7 @@ class MoveBase:
                dev.close()
 
        def read_dist_srf(self, num):
                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()
 
        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:
 
        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:
 
        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:
 
        def get_dist_forward(self):
                if self.pub_range_fwd.get_num_connections() > 0: