#!/usr/bin/env python
import roslib; roslib.load_manifest('irobot_nav')
import rospy
import irobot_mudd
import sensor_msgs.msg as sm
from std_msgs.msg import String
from irobot_mudd.srv import *
from irobot_mudd.msg import *
import math
import time
import TheHive
import communication_client
#Get robot and data instances
D = TheHive.get_data_instance()
Robo = TheHive.get_robot_instance()
#Get hive status updates
hive = communication_client.get_hive_status()
#Robot status updates topic
status_updates = rospy.Publisher('status_updates', String)
#################### ROBOT SPECIFIC FUNCTIONS #####################
def activate():
"""Activates robot, or reactivates after a deactivation."""
if Robo.state != "active":
#!/usr/bin/env python
import roslib; roslib.load_manifest('irobot_nav')
import rospy
from std_msgs.msg import *
from irobot_nav.msg import *
import TheHive
robot = TheHive.get_robot_instance()
def publish_robot_status():
status_updates = rospy.Publisher('status_updates', String)
while not rospy.is_shutdown():
update = robot.status
rospy.loginfo(update)
status_updates.publish(String(update))
rospy.sleep(0.5)
def main():
"""Initializes node, creates services, and starts publishing topics."""
#Initialize master communication node
rospy.init_node('robot_status')
#Publish robot's status updates
#!/usr/bin/env python
import roslib
roslib.load_manifest('irobot_nav')
import rospy
import irobot_mudd
import cv_bridge
import cv
import sensor_msgs.msg as sm
from std_msgs.msg import String
from irobot_mudd.srv import *
from irobot_mudd.msg import *
import TheHive
#Get robot instance
R = TheHive.get_robot_instance()
######################### INITIALIZATION FUNCTIONS #########################
def initialize(D):
"""Creates all the images we'll need. Is separate from init_globals
since we need to know what size the images are before we can make
them.
"""
# Find the size of the image
# (we set D.image right before calling this function)
D.size = cv.GetSize(D.image)
# Create images for each color channel
D.red = cv.CreateImage(D.size, 8, 1)
#!/usr/bin/env python
import roslib
roslib.load_manifest('formation_assignment')
import rospy
import irobot_mudd
import cv_bridge
import cv
import sensor_msgs.msg as sm
from std_msgs.msg import String
from irobot_mudd.srv import *
from irobot_mudd.msg import *
import TheHive
import HandleData
#Get data and robot instances
D = TheHive.get_data_instance()
R = TheHive.get_robot_instance()
#Get hive status from the communication node
#hive = communication_client.get_hive_status()
def initialize_interface():
"""Initializes all windows needed by our program."""
#Create window to monitor robot status
cv.NamedWindow('Monitor')
cv.MoveWindow('Monitor', 0, 0)
D.size = (100, 100)
D.dummy = cv.CreateImage(D.size, 8, 1)
D.image = cv.Zero(D.dummy)
#!/usr/bin/env python
# coding: latin-1
import roslib; roslib.load_manifest('formation_assignment')
import rospy
import math
import cmath
import TheHive
#Get instance of the hive for getting all robot positions
H = TheHive.get_hive_instance()
################################################################################
###
### Calculates translation vector τ and rotation angle θ
### in order to minimize the below cost function:
###
### (τ[k], θ[k]) = arg min[τ,θ] L(Y[k], τ, θ)
###
### where Y[k] is the cost function for a given assignment set.
###
################################################################################
def find_minimum_cost():
"""Minimizes the cost function for translation vector and rotation angle."""
N = 4
calculate_optimal_rotation(N)
