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
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)
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
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)
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()