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