def __init__(self):
super(PatrolController, self).__init__('PatrolServer')
self.__keyListener = rospy.Subscriber('keys', String, self.__keysCallback)
self.__actionClient = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.__actionClient.wait_for_server()
self.__waypoints = self.__loadWaypointsFromCsv()
self.__homeGoal = self.__getGoal(self.__waypoints[0])
self.__marvin = Marvin()
self.__markerPublisher = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=100)
self.__markerArray = MarkerArray()
self.__costmapListener = rospy.Subscriber('/move_base/local_costmap/costmap', OccupancyGrid, self.__costmapCallback)
self.circlefinder = CircleFinder(1)
#Not used for exercise 3 but kept for further development
self.__scanner = rospy.Subscriber('scan', LaserScan, self.__scanCallback)
self.__laser_projector = LaserProjection()
class PatrolController(GenericNode):
__waypoints = []
__poser = None
__homeGoal = None
__actionClient = None
__keyListener = None
__isPatrolling = False
__stopFlag = False
__shuffleFlag = False
__awaitingGoalCompletion = False
__marvin = None
__indianaJones = Indy()
__markerPublisher = None
__markerArray = None
__scanner = None
def __init__(self):
super(PatrolController, self).__init__('PatrolServer')
self.__keyListener = rospy.Subscriber('keys', String, self.__keysCallback)
self.__actionClient = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.__actionClient.wait_for_server()
self.__waypoints = self.__loadWaypointsFromCsv()
self.__homeGoal = self.__getGoal(self.__waypoints[0])
self.__marvin = Marvin()
self.__markerPublisher = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=100)
self.__markerArray = MarkerArray()
self.__costmapListener = rospy.Subscriber('/move_base/local_costmap/costmap', OccupancyGrid, self.__costmapCallback)
self.circlefinder = CircleFinder(1)
#Not used for exercise 3 but kept for further development
self.__scanner = rospy.Subscriber('scan', LaserScan, self.__scanCallback)
self.__laser_projector = LaserProjection()
def startPatrol(self):
try:
rospy.loginfo('Starting new patrol')
self.__marvin = Marvin()
waypoints = self.__preparePatrol(self.__shuffleFlag)
self.__setMarkers(waypoints)
self.__stopFlag = False
self.__shuffleFlag = False
self.__isPatrolling = True
for n in range(len(waypoints)):
if not self.__stopFlag:
rospy.logwarn('PatrolNode.patrol - Patrolling to node {0}'.format(str(n)))
self.__actionClient.send_goal(self.__getGoal(waypoints[n]),
active_cb=self.__OnGoalStarted(),
done_cb=self.__OnGoalDone())
self.__awaitingGoalCompletion = True
self.__actionClient.wait_for_result()
rospy.logwarn('PatrolNode.patrol - Arrived at node {0}'.format(str(n)))
self.__isPatrolling = False
except Exception as ex:
self.__isPatrolling = False
rospy.logwarn('PatrolNode.patrol - ', ex.message)
def startExploring(self):
try:
rospy.loginfo('Starting Indiana Jones Mode')
self.__marvin = Indy()
waypoints = self.__preparePatrol(self.__shuffleFlag)
self.__setMarkers(waypoints)
self.__stopFlag = False
self.__isPatrolling = True
if not self.__stopFlag:
rospy.logwarn('PatrolNode.Explore - Exploring...')
self.__actionClient.send_goal(self.__getGoal(waypoints[0]),
active_cb=self.__OnGoalStarted(),
done_cb=self.__OnGoalDone())
self.__awaitingGoalCompletion = True
self.__actionClient.wait_for_result()
rospy.logwarn('PatrolNode.Explore - Found the Ark!!')
self.__isPatrolling = False
except Exception as ex:
self.__isPatrolling = False
rospy.logwarn('PatrolNode.Explore - ', ex.message)
def stopPatrol(self):
try:
rospy.logwarn('Requested Stop Patrol')
self.__actionClient.cancel_all_goals()
self.returnHome()
except Exception as ex:
rospy.logwarn('PatrolNode.endPatrolAndRestart - ', ex.message)
def returnHome(self):
try:
rospy.loginfo('Heading home')
_waypoint = self.__homeGoal
self.__actionClient.send_goal(_waypoint)
self.__actionClient.wait_for_result()
rospy.loginfo('Arrived safe and sound')
return 'succeeded'
except Exception as ex:
rospy.logwarn('PatrolNode.__goHome - ', ex.message)
return 'failed'
def isPatrolling(self):
return self.__isPatrolling
def __loadWaypointsFromCsv(self):
_retVal = []
try:
_filename = pkg_resources.resource_filename('resources', 'test.csv')
with open(_filename, 'rb') as f:
_reader = csv.reader(f)
for _row in _reader:
_buffer = []
for _col in range(len(_row)):
_buffer.append(float(_row[_col]))
_retVal.append(_buffer)
except Exception as ex:
import sys
print "System path= ", sys.path
print"Error! --> ", ex.message
return _retVal
def __preparePatrol(self, randomize = False):
try:
if randomize == True:
waypoints = copy(self.__waypoints)
random.shuffle(waypoints)
rospy.loginfo("Waypoints are now shuffled")
rospy.loginfo(waypoints[0])
return waypoints
else:
return self.__waypoints
except Exception as ex:
rospy.logwarn('PatrolNode.PreparePatrol - ', ex.message)
def __keysCallback(self, msg):
try:
rospy.logwarn('Received keypress "{0}"'.format(msg.data))
if len(msg.data) == 0:
rospy.logwarn('Received unmapped keypress')
return # unknown key.
if msg.data == 'h':
rospy.logwarn('Go home and restart patrol')
self.__stopFlag = True
self.stopPatrol()
self.__isPatrolling = False
elif msg.data == 'r':
rospy.logwarn('Start Randomized Patrol')
self.__stopFlag = True
self.__shuffleFlag = True
self.stopPatrol()
self.__isPatrolling = False
except Exception as ex:
rospy.logwarn('PatrolNode.__keysCallback - ', ex.message)
def __scanCallback(self, msg):
pass
def __costmapCallback(self, msg):
print msg.data
circle = self.circlefinder.findCircle(msg.data)
if circle is not None:
rospy.loginfo('Circle found :-)')
if self.__isNewCircle(circle) == True:
self.__addMarkerForCircle(circle)
self.__indianaJones.sayRandomSentence()
else:
rospy.loginfo('Circle was already found :-( ')
else:
rospy.loginfo('No circle found in this costmap :-/')
def __getGoal(self, data):
_waypoint = MoveBaseGoal()
_waypoint.target_pose.header.frame_id = 'map'
_waypoint.target_pose.pose.position.x = data[0]
_waypoint.target_pose.pose.position.y = data[1]
_waypoint.target_pose.pose.position.z = data[2]
_waypoint.target_pose.pose.orientation.x = data[3]
_waypoint.target_pose.pose.orientation.y = data[4]
_waypoint.target_pose.pose.orientation.z = data[5]
_waypoint.target_pose.pose.orientation.w = data[6]
return _waypoint
def __OnGoalDone(self):
self.__marvin.sayRandomSentence(False)
self.__awaitingGoalCompletion = False
def __OnGoalStarted(self):
self.__awaitingGoalCompletion = True
rospy.loginfo('Goal is Started')
def __OnGoalFeedback(self):
rospy.loginfo('Goal is giving feedback')
def __isNewCircle(self, circle):
return True
def __addMarkerForCircle(self, circle):
pass
def __setMarkers(self, waypoints):
try:
#self.__markerArray.markers
rospy.logwarn('Preparing to deploy Markers')
for n in range(len(waypoints)):
waypoint = self.__getGoal(waypoints[n])
marker = self.__setMarker(n, waypoint, [1.0, random.uniform(0.0, 1.0), random.uniform(0.0, 1.0), random.uniform(0.0, 1.0)])
markers = len(self.__markerArray.markers)
if markers > len(waypoints):
self.__markerArray.markers.pop(0)
self.__markerArray.markers.append(marker)
self.__markerPublisher.publish(self.__markerArray)
rospy.logwarn('Markers are set')
except Exception as ex:
rospy.logwarn('PatrolNode.__setMarkers - ', ex.message)
def __setMarker(self, id, waypoint, colors = [1,0,0,0]):
try:
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.ns = 'patrol'
marker.id = id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = colors[0]
marker.color.r = colors[1]
marker.color.b = colors[2]
marker.color.g = colors[3]
marker.pose.orientation.w = 1.0
marker.pose.position.x = waypoint.target_pose.pose.position.x
marker.pose.position.y = waypoint.target_pose.pose.position.y
marker.pose.position.z = waypoint.target_pose.pose.position.z
return marker
except Exception as ex:
rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
def __setTextMarker(self, id, waypoint, colors = [1,0,0,0]):
try:
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.ns = 'patrol'
marker.id = id + len(self.__waypoints)
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.text = str(id)
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = colors[0]
marker.color.r = colors[1]
marker.color.b = colors[2]
marker.color.g = colors[3]
marker.pose.orientation.w = 1.0
marker.pose.position.x = waypoint.target_pose.pose.position.x
marker.pose.position.y = waypoint.target_pose.pose.position.y
marker.pose.position.z = waypoint.target_pose.pose.position.z
return marker
except Exception as ex:
rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
