remove ir sensors

[ros_wild_thumper.git] / scripts / wt_node.py

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index ee0aa41429f932d32b72884703840cca15913a10..2969d613c7f11a06e81ac6efe17d740f059cf80f 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -16,6 +16,7 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 from dynamic_reconfigure.server import Server
 from sensor_msgs.msg import Imu, Range, BatteryState
 from wild_thumper.cfg import WildThumperConfig
 from dynamic_reconfigure.server import Server
 from sensor_msgs.msg import Imu, Range, BatteryState
 from wild_thumper.cfg import WildThumperConfig

+from std_srvs.srv import SetBool, SetBoolResponse

 
 WHEEL_DIST = 0.248
 BATTERY_CAPACITY_FULL = 2.750 # Ah, NiMH=2.750, LiFePO4=3.100
 
 WHEEL_DIST = 0.248
 BATTERY_CAPACITY_FULL = 2.750 # Ah, NiMH=2.750, LiFePO4=3.100


@@ -36,8 +37,6 @@ class WTBase:
                self.pub_range_fwd_left = rospy.Publisher("range_forward_left", Range, queue_size=16)
                self.pub_range_fwd_right = rospy.Publisher("range_forward_right", Range, queue_size=16)
                self.pub_range_bwd = rospy.Publisher("range_backward", Range, queue_size=16)
                self.pub_range_fwd_left = rospy.Publisher("range_forward_left", Range, queue_size=16)
                self.pub_range_fwd_right = rospy.Publisher("range_forward_right", 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.pub_battery = rospy.Publisher("battery", BatteryState, queue_size=16)
                self.cmd_vel = None
                self.cur_vel = (0, 0)
                self.pub_battery = rospy.Publisher("battery", BatteryState, queue_size=16)
                self.cmd_vel = None
                self.cur_vel = (0, 0)


@@ -47,6 +46,7 @@ class WTBase:
                i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
                rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
                rospy.Subscriber("imu", Imu, self.imuReceived)
                i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
                rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
                rospy.Subscriber("imu", Imu, self.imuReceived)

+               rospy.Service('~manipulator_enable', SetBool, self.enableManipulator)

                self.bDocked = False
                self.bDocked_last = False
                self.battery_capacity = BATTERY_CAPACITY_FULL*3600 # As
                self.bDocked = False
                self.bDocked_last = False
                self.battery_capacity = BATTERY_CAPACITY_FULL*3600 # As


@@ -311,18 +311,6 @@ class WTBase:
                msg.range = dist
                pub.publish(msg)
 
                msg.range = dist
                pub.publish(msg)
 

-       def get_dist_left(self):
-               if self.pub_range_left.get_num_connections() > 0:
-                       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 = 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_left(self):
                if self.pub_range_fwd_left.get_num_connections() > 0:
                        dist = self.read_dist_srf(0x15)
        def get_dist_forward_left(self):
                if self.pub_range_fwd_left.get_num_connections() > 0:
                        dist = self.read_dist_srf(0x15)


@@ -373,7 +361,11 @@ class WTBase:
                        rospy.loginfo("Redocking done")
                else:
                        rospy.logerr("Redocking failed")
                        rospy.loginfo("Redocking done")
                else:
                        rospy.logerr("Redocking failed")

-               
+
+       def enableManipulator(self, msg):
+               rospy.loginfo("Set Manipulator: %d", msg.data)
+               i2c_write_reg(0x52, 0x0f, chr(msg.data))
+               return SetBoolResponse(True, "")

 
 if __name__ == "__main__":
        WTBase()
 
 if __name__ == "__main__":
        WTBase()