def __init__(self, context):
self.pwd = os.environ['PWD']
self.filelist = os.listdir(self.pwd + '/src/kampala/scenarios/launch')
self.simulation = rospy.get_param('/simulation', 'false')
self.land_permission = Permission()
self.lander_channel = []
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._widget.ConnectButton.clicked.connect(self.Connect)
self._widget.LANDButton.clicked.connect(self.Land)
self._widget.ArmButton.clicked.connect(self.Arm)
self._widget.StartButton.clicked.connect(self.Start)
self._widget.ParamButton.clicked.connect(self.Param)
self._widget.StartInputField.returnPressed.connect(self.Autocomplete)
self._widget.IrisInputBox.insertItems(0, ['iris1', 'iris2', 'iris3'])
result.y_acc=obj.y_acc
result.z_acc=obj.z_acc
result.yaw_acc=obj.yaw_acc
result.pitch_acc=obj.pitch_acc
result.roll_acc=obj.roll_acc
result.time_diff=obj.time_secs
return(result)
if __name__=='__main__':
rospy.init_node('security_guard_topic')
loop_rate=rospy.Rate(30)
current_point=Point()
controller_on=True
lander_permission=Permission()
controller_permission=Permission()
trajectory_done=Trajectory()
#Get the body ID as a parameter
body_id=sml_setup.Get_Parameter(NODE_NAME,'body_id',8)
mocap_topic='/body_data/id_'+str(body_id)
#Publish topics
lander_channel=rospy.Publisher('security_guard/lander',Permission,queue_size=10)
controller_channel=rospy.Publisher('security_guard/controller',Permission,queue_size=10)
data_forward=rospy.Publisher('security_guard/data_forward',QuadPositionDerived,queue_size=10)
#Subscribe topics
rospy.Subscriber(mocap_topic,QuadPositionDerived,New_Point,current_point)
rospy.Subscriber('trajectory_gen/done',Permission,Trajectory_Done,trajectory_done)
result.y_acc = obj.y_acc
result.z_acc = obj.z_acc
result.yaw_acc = obj.yaw_acc
result.pitch_acc = obj.pitch_acc
result.roll_acc = obj.roll_acc
result.time_diff = obj.time_secs
return (result)
if __name__ == '__main__':
rospy.init_node('security_guard_topic')
loop_rate = rospy.Rate(30)
current_point = Point()
controller_on = True
lander_permission = Permission()
controller_permission = Permission()
trajectory_done = Trajectory()
#Get the body ID as a parameter
body_id = sml_setup.Get_Parameter(NODE_NAME, 'body_id', 8)
mocap_topic = '/body_data/id_' + str(body_id)
#Publish topics
lander_channel = rospy.Publisher('security_guard/lander',
Permission,
queue_size=10)
controller_channel = rospy.Publisher('security_guard/controller',
Permission,
queue_size=10)
data_forward = rospy.Publisher('security_guard/data_forward',
def Arm(self):
self.land_pub.publish(Permission(False))
inputstring = "roslaunch scenarios iris_nodes.launch ns:=%s simulation:=%s" % (
self.name, self.simulation)
self.execute(inputstring)
def Land(self):
#self.lander_channel.publish(Permission(True))
self.land_pub.publish(Permission(True))
def send_permission(self, boolean): self.security_pub.publish(Permission(boolean))
def get_tilted_circle(self):
tilted_midpoint = self.inverse_transform(self.midpoint, self.theta)
a_max = 0.6**2.0 / 0.8
#v_max = (self.radius*a_max)**(0.5)
#if self.velo >= v_max:
# self.velo = 0.9*v_max
rospy.init_node('TG', anonymous=True)
pub = rospy.Publisher('trajectory_gen/target',
QuadPositionDerived,
queue_size=10)
start_point = self.go_to_start(tilted_midpoint, pub)
sec_pub = rospy.Publisher('trajectory_gen/done',
Permission,
queue_size=10)
rospy.sleep(4.)
r = 10.0
rate = rospy.Rate(r)
t_f = self.velo / math.sqrt(a_max**2.0 -
self.velo**4.0 / self.radius**2.0)
time = self.accelerate(pub, a_max, r, tilted_midpoint) / 2.0 - 1 / r
period = 2 * math.pi * self.radius / self.velo
points_in_period = int(period * r)
counter = 0
while not rospy.is_shutdown() and not self.done:
out_pos = self.transform_coordinates(
self.get_outpos(self.radius, self.velo, tilted_midpoint, time),
self.theta)
out_velo = self.transform_coordinates(
self.get_outvelo(self.radius, self.velo, time), self.theta)
out_acc = self.transform_coordinates(
self.get_outacc(self.radius, self.velo, time), self.theta)
out_msg = QuadPositionDerived()
out_msg.x = out_pos[0]
out_msg.y = out_pos[1]
out_msg.z = out_pos[2]
out_msg.yaw = 0
out_msg.x_vel = out_velo[0]
out_msg.y_vel = out_velo[1]
out_msg.z_vel = out_velo[2]
out_msg.yaw_vel = 0
out_msg.x_acc = out_acc[0]
out_msg.y_acc = out_acc[1]
out_msg.z_acc = out_acc[2]
out_msg.yaw_acc = 0
pub.publish(out_msg)
sec_pub.publish(Permission(False))
time += 1.0 / r
counter += 1
rate.sleep()
if time >= 3 * period - t_f / 2:
end_pos = self.decallerate(pub, a_max, self.radius, self.velo,
tilted_midpoint, time)
rospy.set_param("trajectory_generator/start_point", end_pos)
rospy.set_param("trajectory_generator/end_point",
[0.0, 0.0, 0.6])
sl_gen = StraightLineGen()
sl_gen.generate()
rospy.sleep(4.)
#self.turn(pub, [0.0,0.0,0.6],0)
self.done = True
sec_pub.publish(Permission(True))