def kobuki_move(self):
if self.status.power == False:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
return
else:
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
if self.status.bumped == True:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0
self.status.bumped = False
self.status.power = False
elif self.status.direction == 90:
self.vel_cmd.linear.x = 0.1
self.vel_cmd.angular.z = 0
elif self.status.direction == 0:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = -0.4
elif self.status.direction == 270:
self.vel_cmd.linear.x = -0.1
self.vel_cmd.angular.z = 0
elif self.status.direction == 180:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0.4
else:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0
self.vel_cmd_pub.publish(self.vel_cmd)
def spin(self):
key = ord(getch())
message = ""
if key in {83, 115}: # S or s
message = "START"
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
elif key in {81, 113}: #Q or q
message = "QUIT"
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
elif key == 65: #UP
message = "UP"
self.vel_cmd.linear.x += 0.05
elif key == 66: #DOWN
message = "DOWN"
self.vel_cmd.linear.x -= 0.05
elif key == 67: #RIGHT
message = "RIGHT"
self.vel_cmd.angular.z -= 0.4
elif key == 68: #LEFT
message = "LEFT"
self.vel_cmd.angular.z += 0.4
elif key == 3: #Ctr+C
raise KeyboardInterrupt
else:
pass
if message:
message = message + " is pressed "
self.vel_cmd_pub.publish(self.vel_cmd)
sys.stdout.write("\r\033[K" + message + "linear.x = " +
str(self.vel_cmd.linear.x) + " angular.z = " +
str(self.vel_cmd.angular.z) + "\r")
def spin(self):
key = self.scr.getch()
message = ""
if key in {83,115}: # S or s
message = "START"
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
elif key in {81,113}: #Q or q
message = "QUIT"
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
elif key == 65: #UP
message = "UP"
self.vel_cmd.linear.x = 0.1
self.vel_cmd.angular.z = 0
elif key == 66: #DOWN
message = "DOWN"
self.vel_cmd.linear.x = -0.1
self.vel_cmd.angular.z = 0
elif key == 67: #RIGHT
message = "RIGHT"
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = -0.4
elif key == 68: #LEFT
message = "LEFT"
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0.4
elif key == 3:#Ctr+C
raise KeyboardInterrupt
if message:
message = message + " is pressed "
self.scr.move(3,0)
self.scr.clrtoeol()
self.scr.addstr(3,0,message + "linear.x = "+str(self.vel_cmd.linear.x)+" angular.z = "+str(self.vel_cmd.angular.z))
self.vel_cmd_pub.publish(self.vel_cmd)
def kobuki_move(self):
if self.status.power == False:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
return
else:
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
if self.status.blueball == True:
#spin and kobuki_move are called for 20 times per second(20Hz).
if self.status.blueballcount > 100:
self.status.power = False
self.status.blueballcount = 0
elif self.status.blueballx < 220:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0.4
elif self.status.blueballx > 420:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = -0.4
else:
self.vel_cmd.linear.x = 0.1
self.vel_cmd.angular.z = 0
self.status.blueballcount += 1
elif self.status.blueball == False:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = -0.4
self.vel_cmd_pub.publish(self.vel_cmd)
def MotorTalker():
MotorON = MotorPower()
MotorON.state = Motor.ON
MotorOFF = MotorPower()
Motor.state = Motor.OFF
pub = rospy.Publisher('mobile_base/commands/motor_power',
MotorPower,
queue_size=10)
rospy.init_node('MotorTalker', anonymous=True)
BumperListener()
WheelDropListener()
while not rospy.is_shutdown():
MotorON = MotorPower()
MotorON.state = Motor.ON
MotorOFF = MotorPower()
Motor.state = Motor.OFF
if bump or wheel:
rospy.loginfo("motor off")
pub.publish(MotorOFF)
else:
rospy.loginfo("motor on")
pub.publish(MotorON)
BumperListener()
WheelDropListener()
def kobuki_move(self):
if self.status.power == False:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
return
else:
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
self.vel_cmd_pub.publish(self.vel_cmd)
def toggleMotor(self, on_off):
if on_off:
self.pub['motor_power'].publish(MotorPower(MotorPower.ON))
self.publish_cmd_vel = True
else:
self.pub['motor_power'].publish(MotorPower(MotorPower.OFF))
self.publish_cmd_vel = False
self.cmd_vel = Twist()
def kobuki_move(self):
if self.status.power == False:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
return
else:
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
if self.status.blueball == True:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0
self.status.blueball = False
elif self.status.blueball == False:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = -0.4
self.vel_cmd_pub.publish(self.vel_cmd)
def kobuki_move(self):
if self.status.power == False:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
return
else:
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))
#spin and kobuki_move are called for 20 times per second(20Hz)
if self.status.hits > 40:
self.vel_cmd.linear.x = 0
self.vel_cmd.angular.z = 0
self.status.hits = 0
elif self.status.hits > 0:
self.status.hits = self.status.hits + 1
self.vel_cmd.linear.x = -0.1
self.vel_cmd.angular.z = 0
self.vel_cmd_pub.publish(self.vel_cmd)
def bumper_cb(self, data):
self.scr.move(1, 0)
self.scr.clrtoeol()
if data.state == BumperEvent.PRESSED:
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
self.scr.addstr(1, 0, "PRESSED")
elif data.state == BumperEvent.RELEASED:
self.scr.addstr(1, 0, "RELEASED")
else:
self.scr.addstr(1, 0, "Bumper Unknown event")
def bumper_cb(self, bumper):
self.scr.move(1, 0) #1st line
self.scr.clrtoeol() #Clear to End of Line
if bumper.state == BumperEvent.PRESSED:
self.scr.addstr(1, 0, "PRESSED")
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
elif bumper.state == BumperEvent.RELEASED:
self.scr.addstr(1, 0, "RELEASED")
else:
self.scr.addstr(1, 0, "Bumper Unknown event")
def __init__(self):
#ATRIBUTOS PARA PUBLICADORES
self.vel = Twist()
self.mot = MotorPower()
#ATRIBUTOS PARA PARAMETROS DESDE EL SERVER
self.angle = None
#ATRIBUTOS PARA PROGRAMA EN PYTHON
self.angle_meas = 0.0
self.aa = [0, 0]
self.angle_to_py = 0.0
self.error_angle = 1
key_timeout = rospy.get_param("~key_timeout", 0.0)
if key_timeout == 0.0:
key_timeout = None
self.getParameters()
if (self.angle is None):
rospy.signal_shutdown("Parameters not declared")
else:
rospy.loginfo("Parameters found")
#DEFINICION DE LOS PUBLICADORES
self.nameTopic_mot = "/mobile_base/commands/motor_power"
self.nameTopic_vel = "/mobile_base/commands/velocity"
self.nameTopic_pose = "/odom"
self.pubvel = rospy.Publisher(self.nameTopic_vel,
numpy_msg(Twist),
queue_size=10)
self.pubmot = rospy.Publisher(self.nameTopic_mot,
MotorPower,
queue_size=10)
#SE DEFINE EL SUSCRIPTOR PARA RECIBIR LA POSE (ODOMETRÍA)
self.subpose = rospy.Subscriber(self.nameTopic_pose, Odometry,
self.callback)
#INICIAMOS SERVIDOR PARA PARAMETROS
srv = Server(Kobuki_Exer2Config, self.DynConfCB)
#INICIAMOS EL MOTOR
self.turn_on_mot()
#control para el angulo
while True:
self.angle_control()
pub_vel_r0 = rospy.Publisher("robot0/mobile_base/commands/velocity",
Twist,
queue_size=10)
pub_vel_r1 = rospy.Publisher("robot1/mobile_base/commands/velocity",
Twist,
queue_size=10)
pub_mot_r0 = rospy.Publisher("robot0/mobile_base/commands/motor_power",
MotorPower,
queue_size=10)
pub_mot_r1 = rospy.Publisher("robot1/mobile_base/commands/motor_power",
MotorPower,
queue_size=10)
rospy.Subscriber('/gazebo/model_states', ModelStates, updateWorld)
motor0 = MotorPower()
motor1 = MotorPower()
motor0.state = motor0.ON
motor1.state = motor1.ON
pub_mot_r0.publish(motor0)
pub_mot_r1.publish(motor1)
t = 0
while not rospy.is_shutdown():
# compute desired vel to goal
V_des = compute_V_des(X, goal, V_max)
# compute the optimal vel to avoid collision
V = RVO_update(X, V_des, V, scenario)
def turnOff(self):
self.power_cmd_pub.publish(MotorPower(MotorPower.OFF))
def toggle(self):
if super(MotorWidget, self).state is 1:
self._pub.publish(MotorPower(0))
else:
self._pub.publish(MotorPower(1))
pub_j = rospy.Publisher('/arm_2_joint/command', Float64, queue_size=10)
pub_j1 = rospy.Publisher('/arm_4_joint/command', Float64, queue_size=10)
i = 0
while i < 20:
pub_grip.publish(cmd_grip)
pub_j.publish(0)
pub_j1.publish(0)
i = i + 1
rate.sleep()
pub2 = rospy.Publisher('/mobile_base/commands/motor_power',
MotorPower,
queue_size=10)
turn_on_motors = MotorPower()
turn_on_motors.state = 1 # or "ON"
pub2.publish(turn_on_motors)
listening = Listener_Action()
listenToModelstate = Listener_state()
listen_object = Listener_object()
joint_2 = Listener_Joint_2()
joint_4 = Listener_Joint_4()
joint_grip = Listener_Joint_grip()
sleep = 20
i = 0
kp = 5
kd = 2
while not rospy.is_shutdown(): # main program
def turnOn(self):
self.power_cmd_pub.publish(MotorPower(MotorPower.ON))