def __init__(self):
rospy.init_node('cmd_vel_translator')
topic_out = rospy.get_param('~topic_out','/cwru/cmd_vel')
topic_in = rospy.get_param('~topic_in','/cmd_vel')
rospy.loginfo('Publishing to: '+topic_out)
rospy.loginfo('Subscribing to: '+topic_in)
self.pub = rospy.Publisher(topic_out,VelCmd)
rospy.Subscriber(topic_in, Twist, self.callback)
rospy.Subscriber("cwru/switches", Switches, self.callerback)
self.switchA = True
self.driveEnable = False
self.vel_called = False
while not rospy.is_shutdown():
if not self.vel_called:
cmd_vel = VelCmd()
cmd_vel.linear = 0
cmd_vel.angular = 0
self.pub.publish(cmd_vel)
self.vel_called = False
rospy.sleep(.5)
def callback(self,data):
self.vel_called = True
cmd_vel = VelCmd()
cmd_vel.linear = data.linear.x*1000.0
cmd_vel.angular = data.angular.z*1000.0
if cmd_vel.linear > 2000:
cmd_vel.linear = 2000
if cmd_vel.linear < -2000:
cmd_vel.linear = -2000
if not self.switchA and self.driveEnable:
self.pub.publish(cmd_vel)
else:
self.vel_called = False
rospy.logwarn("robot cannot accept vel comands now")