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()
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
#print(listening.action,'while loop')
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)
# Compute ang_vel considering singularities
angular_vel_0 = (np.arctan2(V[0][1], V[0][0]) - orientation[0])