#!/usr/bin/env python

import rospy
import random
from geometry_msgs.msg import Twist, Pose
from kobuki_msgs.msg import CliffEvent, BumperEvent

MULT = 1

def beware_cliffs(data):
	rospy.loginfo("Cliff!")
	rospy.loginfo(data)
	if data.state == CliffEvent.CLIFF:
		pub = rospy.Publisher('/cmd_vel_mux/input/safety_controller', Twist, queue_size=10)
		safe_move = Twist()

		# Si vide devant alors on recule
		#if data.sensor == CliffEvent.CENTER:
		safe_move.linear.x = 0

		pub.publish(safe_move)


def beware_walls(data):
	rospy.loginfo("Wall!")
	rospy.loginfo(data)
	if data.state == BumperEvent.PRESSED:
		pub = rospy.Publisher('/cmd_vel_mux/input/safety_controller', Twist, queue_size=10)
		safe_move = Twist()

		#if data.sensor == BumperEvent.CENTER:
		safe_move.linear.x = -1

		pub.publish(safe_move)


def listen():
	rospy.init_node('bodyguard_cliff', anonymous=True)
	rospy.Subscriber('events/cliff', CliffEvent, beware_cliffs)
	rospy.Subscriber('events/bumper', BumperEvent, beware_walls)
	rospy.spin()


if __name__ == '__main__':
	try:
		listen()
	except rospy.ROSInterruptException:
		pass