def __init__(self):
# Atributos
self.topicSubs = "/cmd_vel"
self.topicPubFront = "/front_wheel_ctrl/command"
self.topicPubLeft = "/left_wheel_ctrl/command"
self.topicPubRight = "/right_wheel_ctrl/command"
self.vel_X = 0.0
self.vel_Y = 0.0
self.vel_ang = 0.0
self.flagCmd = False
self.modelo = Model_robot()
# Publicador
self.pubVelFront = rospy.Publisher(self.topicPubFront,
numpy_msg(Float64),
queue_size=10)
self.pubVelLeft = rospy.Publisher(self.topicPubLeft,
numpy_msg(Float64),
queue_size=10)
self.pubVelRight = rospy.Publisher(self.topicPubRight,
numpy_msg(Float64),
queue_size=10)
# Subscriptor
rospy.Subscriber(self.topicSubs,
numpy_msg(Twist),
self.cmd_vel_CB,
queue_size=10)
# Polling con callbacks
rate = rospy.Rate(50)
while (not rospy.is_shutdown()):
if (self.flagCmd):
self.flagCmd = False
vel2Send = self.modelo.calcVelWheels(self.vel_X, self.vel_Y,
self.vel_ang)
# Para rueda front
msg = Float64()
msg.data = vel2Send[0]
self.pubVelFront.publish(msg)
# Para rueda left
msg = Float64()
msg.data = vel2Send[1]
self.pubVelLeft.publish(msg)
# Para rueda right
msg = Float64()
msg.data = vel2Send[2]
self.pubVelRight.publish(msg)
rate.sleep()
def __init__(self):
self.vel = Twist()
key_timeout = rospy.get_param("~key_timeout", 0.0)
if key_timeout == 0.0:
key_timeout = None
self.modelo = Model_robot()
self.nameTopicPub1 = "/right_wheel_ctrl/command"
self.nameTopicPub2 = "/left_wheel_ctrl/command"
self.nameTopicPub3 = "/vel_ddrobot"
self.pub1 = rospy.Publisher(self.nameTopicPub1,Float64,queue_size=10)
self.pub2 = rospy.Publisher(self.nameTopicPub2,Float64,queue_size=10)
self.pub3 = rospy.Publisher(self.nameTopicPub3,numpy_msg(Twist),queue_size=10)
rate = rospy.Rate(10)
vel_L = 0
vel_R = 0
while (not rospy.is_shutdown()):
key = getKey(self,key_timeout)
if key in moveBindings.keys():
if ((key == 'q') | (key == 'Q')):
break
elif (key == ' '):
vel_R = moveBindings[key][0]
vel_L = moveBindings[key][1]
else:
vel_R = vel_R + moveBindings[key][0]
vel_L = vel_L + moveBindings[key][1]
if (vel_R >= 29):
vel_R = 29
elif (vel_R <= -29):
vel_R = -29
if (vel_L >= 29):
vel_L = 29
elif (vel_L <= -29):
vel_L = -29
self.pub1.publish(vel_R)
self.pub2.publish(vel_L)
vel2send = self.modelo.calcVel(vel_R, vel_L)
self.vel.linear.x = vel2send[0]
self.vel.linear.y = vel2send[1]
self.vel.angular.z = vel2send[2]
self.pub3.publish(self.vel)
rate.sleep()
def __init__(self):
self.vel2send = Twist()
self.velWheel2send = Float64MultiArray()
self.vel_y = 0
self.w = 0
self.modelo = Model_robot()
self.nameTopicSub1 = "/vel_order"
self.nameTopicPub1 = "/right_motor_1/command"
self.nameTopicPub2 = "/right_motor_2/command"
self.nameTopicPub3 = "/right_motor_3/command"
self.nameTopicPub4 = "/left_motor_1/command"
self.nameTopicPub5 = "/left_motor_2/command"
self.nameTopicPub6 = "/left_motor_3/command"
self.nameTopicPub7 = "/vel_robot"
self.nameTopicPub8 = "/vel_wheel"
rospy.Subscriber(self.nameTopicSub1, Float64MultiArray, self.callback)
self.pub1 = rospy.Publisher(self.nameTopicPub1, Float64, queue_size=10)
self.pub2 = rospy.Publisher(self.nameTopicPub2, Float64, queue_size=10)
self.pub3 = rospy.Publisher(self.nameTopicPub3, Float64, queue_size=10)
self.pub4 = rospy.Publisher(self.nameTopicPub4, Float64, queue_size=10)
self.pub5 = rospy.Publisher(self.nameTopicPub5, Float64, queue_size=10)
self.pub6 = rospy.Publisher(self.nameTopicPub6, Float64, queue_size=10)
self.pub7 = rospy.Publisher(self.nameTopicPub7,
numpy_msg(Twist),
queue_size=10)
self.pub8 = rospy.Publisher(self.nameTopicPub8,
Float64MultiArray,
queue_size=10)
rate = rospy.Rate(10)
self.vel_y = 0
self.w = 0
self.quit = False
self.key = ' '
while (not rospy.is_shutdown()):
self.modelo.actualizar_modelo()
self.control_ruedas()
self.enviar_odometria()
rate.sleep()
class CONTROL_MOTORES:
def __init__(self):
self.vel2send = Twist()
self.velWheel2send = Float64MultiArray()
self.vel_y = 0
self.w = 0
self.modelo = Model_robot()
self.nameTopicSub1 = "/vel_order"
self.nameTopicPub1 = "/right_motor_1/command"
self.nameTopicPub2 = "/right_motor_2/command"
self.nameTopicPub3 = "/right_motor_3/command"
self.nameTopicPub4 = "/left_motor_1/command"
self.nameTopicPub5 = "/left_motor_2/command"
self.nameTopicPub6 = "/left_motor_3/command"
self.nameTopicPub7 = "/vel_robot"
self.nameTopicPub8 = "/vel_wheel"
rospy.Subscriber(self.nameTopicSub1, Float64MultiArray, self.callback)
self.pub1 = rospy.Publisher(self.nameTopicPub1, Float64, queue_size=10)
self.pub2 = rospy.Publisher(self.nameTopicPub2, Float64, queue_size=10)
self.pub3 = rospy.Publisher(self.nameTopicPub3, Float64, queue_size=10)
self.pub4 = rospy.Publisher(self.nameTopicPub4, Float64, queue_size=10)
self.pub5 = rospy.Publisher(self.nameTopicPub5, Float64, queue_size=10)
self.pub6 = rospy.Publisher(self.nameTopicPub6, Float64, queue_size=10)
self.pub7 = rospy.Publisher(self.nameTopicPub7,
numpy_msg(Twist),
queue_size=10)
self.pub8 = rospy.Publisher(self.nameTopicPub8,
Float64MultiArray,
queue_size=10)
rate = rospy.Rate(10)
self.vel_y = 0
self.w = 0
self.quit = False
self.key = ' '
while (not rospy.is_shutdown()):
self.modelo.actualizar_modelo()
self.control_ruedas()
self.enviar_odometria()
rate.sleep()
def callback(self, order):
self.vel_y = order.data[0]
self.w = order.data[1]
def control_ruedas(self):
self.velWheel = self.modelo.calcVelWheels(self.vel_y, self.w)
self.velWheel2send.data = [
self.velWheel[0], self.velWheel[1], self.velWheel[2],
self.velWheel[3], self.velWheel[4], self.velWheel[5]
]
self.pub1.publish(self.velWheel[0])
self.pub2.publish(self.velWheel[1])
self.pub3.publish(self.velWheel[2])
self.pub4.publish(-self.velWheel[3])
self.pub5.publish(-self.velWheel[4])
self.pub6.publish(-self.velWheel[5])
self.pub8.publish(self.velWheel2send)
def enviar_odometria(self):
self.vel = self.modelo.calcVel(self.velWheel[0], self.velWheel[1],
self.velWheel[2], self.velWheel[3],
self.velWheel[4], self.velWheel[5])
self.vel2send.linear.x = self.vel[0]
self.vel2send.linear.y = self.vel[1]
self.vel2send.angular.z = self.vel[2]
self.pub7.publish(self.vel2send)
class Communication_ROS:
def __init__(self):
# Atributos
self.topicSubs = "/cmd_vel"
self.topicPubFront = "/front_wheel_ctrl/command"
self.topicPubLeft = "/left_wheel_ctrl/command"
self.topicPubRight = "/right_wheel_ctrl/command"
self.vel_X = 0.0
self.vel_Y = 0.0
self.vel_ang = 0.0
self.flagCmd = False
self.modelo = Model_robot()
# Publicador
self.pubVelFront = rospy.Publisher(self.topicPubFront,
numpy_msg(Float64),
queue_size=10)
self.pubVelLeft = rospy.Publisher(self.topicPubLeft,
numpy_msg(Float64),
queue_size=10)
self.pubVelRight = rospy.Publisher(self.topicPubRight,
numpy_msg(Float64),
queue_size=10)
# Subscriptor
rospy.Subscriber(self.topicSubs,
numpy_msg(Twist),
self.cmd_vel_CB,
queue_size=10)
# Polling con callbacks
rate = rospy.Rate(50)
while (not rospy.is_shutdown()):
if (self.flagCmd):
self.flagCmd = False
vel2Send = self.modelo.calcVelWheels(self.vel_X, self.vel_Y,
self.vel_ang)
# Para rueda front
msg = Float64()
msg.data = vel2Send[0]
self.pubVelFront.publish(msg)
# Para rueda left
msg = Float64()
msg.data = vel2Send[1]
self.pubVelLeft.publish(msg)
# Para rueda right
msg = Float64()
msg.data = vel2Send[2]
self.pubVelRight.publish(msg)
rate.sleep()
#---------------------------------------------------------------------------------#
#---------------------------------------------------------------------------------#
# Funcion de cb para el comando de velocidad
def cmd_vel_CB(self, cmd):
rospy.loginfo("received cmd message")
self.vel_X = cmd.linear.x
self.vel_Y = cmd.linear.y
self.vel_ang = cmd.angular.z
self.flagCmd = True