class WatcherTrigger(Watcher):
'''
name: the name of the Watcher used in logging
service_name: the name of the service to be used
'''
def __init__(self, name, service_name):
super(WatcherTrigger, self).__init__(name)
if service_name == None or service_name == "":
self.logfatal("service_name not set")
self.__service_name = service_name
self.__service = None
### Method Overrides ###
def start(self):
super(WatcherTrigger, self).start()
self.__service = Service(self.__service_name, Trigger, self.handler)
def stop(self):
super(WatcherTrigger, self).stop()
self.__service.shutdown()
### / Method Overrides ###
### Overridable methods ###
'''
Handles incoming service requests.
By default, the watcher is triggered and True is returned.
'''
def handler(self, req):
with self.event.get_lock():
self.event.value = True
return TriggerResponse(True, "")
def __init__(self):
init_node('iiwa_sunrise', log_level = INFO)
hardware_interface = get_param('~hardware_interface', 'PositionJointInterface')
self.robot_name = get_param('~robot_name', 'iiwa')
model = get_param('~model', 'iiwa14')
tool_length = get_param('~tool_length', 0.0)
if model == 'iiwa7':
self.l02 = 0.34
self.l24 = 0.4
elif model == 'iiwa14':
self.l02 = 0.36
self.l24 = 0.42
else:
logerr('unknown robot model')
return
self.l46 = 0.4
self.l6E = 0.126 + tool_length
self.tr = 0.0
self.v = 1.0
self.joint_names = ['{}_joint_1'.format(self.robot_name),
'{}_joint_2'.format(self.robot_name),
'{}_joint_3'.format(self.robot_name),
'{}_joint_4'.format(self.robot_name),
'{}_joint_5'.format(self.robot_name),
'{}_joint_6'.format(self.robot_name),
'{}_joint_7'.format(self.robot_name)]
joint_states_sub = Subscriber('joint_states', JointState, self.jointStatesCb, queue_size = 1)
command_pose_sub = Subscriber('command/CartesianPose', PoseStamped, self.commandPoseCb, queue_size = 1)
command_pose_lin_sub = Subscriber('command/CartesianPoseLin', PoseStamped, self.commandPoseLinCb, queue_size = 1)
redundancy_sub = Subscriber('command/redundancy', Float64, self.redundancyCb, queue_size = 1)
joint_position_sub = Subscriber('command/JointPosition', JointPosition, self.jointPositionCb, queue_size = 1)
self.state_pose_pub = Publisher('state/CartesianPose', PoseStamped, queue_size = 1)
self.joint_trajectory_pub = Publisher(
'{}_trajectory_controller/command'.format(hardware_interface), JointTrajectory, queue_size = 1)
path_parameters_configuration_srv = Service(
'configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits, self.handlePathParametersConfiguration)
path_parameters_lin_configuration_srv = Service(
'configuration/setSmartServoLinLimits', SetSmartServoLinSpeedLimits, self.handlePathParametersLinConfiguration)
smart_servo_configuration_srv = Service(
'configuration/ConfigureControlMode', ConfigureControlMode, self.handleSmartServoConfiguration)
spin()
def __init__(self, devs, params):
statusDevs = []
self._devs = devs
for dev in devs['motorsCl']:
motor = DevStatus()
motor.devName = dev.getName()
motor.type = EngineCL
motor.values = [dev.getKp(), dev.getKi(), dev.getKd()]
statusDevs.append(motor)
self._monitor = MonitorDevs()
self._monitor.devs = statusDevs
Service('/devsOnline', get_devs, self.getDevsOnline)
Service('/devsSetParam', setParam, self.setParam)
def __init__(self):
# Read Parameters From YAML File
self.turtlebot_odom_topic = get_param('turtlebot_odom_topic')
self.turtlebot_base_footprint_frame = get_param(
'turtlebot_base_footprint_frame')
self.map_frame = get_param('map_frame')
self.drone_odom_frame = get_param('drone_odom_frame')
self.join_tree_srv_name = get_param('join_tree_srv')
self.debug = get_param('debug')
# TF Broadcaster
self.tf_broadcast = TransformBroadcaster()
# TF Listener
self.tf_listen = Buffer()
self.tf_listener = TransformListener(self.tf_listen)
# Subscribers
Subscriber(self.turtlebot_odom_topic, Odometry, self.turtlebot_odom_cb)
# Services for Joining the Drone TF Tree to Main Tree (Localization)
self.join_tree_service = Service(self.join_tree_srv_name, JointTrees,
self.join_tree_srv_function)
# Parameters
self.drone_tf = TransformStamped()
self.turtlebot_tf = TransformStamped()
self.turtlebot_tf.header.frame_id = self.map_frame
self.turtlebot_tf.child_frame_id = self.turtlebot_base_footprint_frame
self.drone_tf.header.frame_id = self.map_frame
self.drone_tf.child_frame_id = self.drone_odom_frame
self.drone_tf.transform.rotation.w = 1.0
self.turtlebot_tf.transform.rotation.w = 1.0
# Flags
self.join_trees = False # Set True When The Two TF Trees are Joint in One
def __init__(self):
self.authority = False
control_authority_srv = Service('/dji_sdk/sdk_control_authority',
SDKControlAuthority, self.handler)
self.control_pub = Publisher('/dji_sdk/matlab_rpvy', Joy, queue_size=1)
self.wind_pub = Publisher('/dji_sdk/matlab_wind', Joy, queue_size=1)
joy_sub = Subscriber('/dji_sdk/xbox_joy', Joy, self.joy_callback)
control_sub = Subscriber(
'/dji_sdk/flight_control_setpoint_rollpitch_yawrate_zvelocity',
Joy, self.control_callback)
def __init__(self):
init_node(NODE_NAME, anonymous=True)
Service(SERVICE_NAME, Planning, self.callback)
rospy.Subscriber("pose", Float32MultiArray, self.pose_callback)
self.planner = Planner()
self.navigator = Navigator()
self.position = Position()
self.status = IDLE
self.publisher = Publisher(PUBLISH_TO,
Float32MultiArray,
queue_size=10)
self.rate = Rate(10)
self.goal = None
self.calculating = False
def __init__(self, name: str):
super().__init__(name, "action")
self.name = name
# Service for action point computation
self._action_point_server = Service(get_action_point_service(name),
ComputeActionPoint,
self._compute_action_point)
# Action server for this action
self._action_server = SimpleActionServer(get_action_server(name),
PerformAction,
auto_start=False)
self._action_server.register_goal_callback(self._on_goal)
self._action_server.register_preempt_callback(self._on_preempt)
self._action_server.start()
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name,
Odometry,
queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name,
Twist,
self.diffServiceCallback,
queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
self._haveRightToPublish = False
self._prevOdom = None
self._firstTimePub = True
def __init__(self, param, output):
Device.__init__(self, param.getIMUName(), output)
self._frameId = param.getIMUFrameId()
self._angle = param.getIMUOrientation()
self._pub = Publisher('%s_AGQ' % self._name,
Imu,
queue_size=param.getIMUPubHz())
self._pubMag = Publisher('%s_M' % self._name,
MagneticField,
queue_size=param.getIMUPubHz())
self._pubCalib = Publisher('/imu_calib_publisher',
imuCalib,
queue_size=param.getIMUPubHz())
Service('/imu_Calibration', calibIMU, self.serviceCallBack)
self._haveRightToPublish = False
self._calib = False
self._xMax = 0.0
self._yMax = 0.0
self._zMax = 0.0
self._xMin = 0.0
self._yMin = 0.0
self._zMin = 0.0
callback_in_points_orange)
pub_stream = Publisher("swarm_drones/white/stream", Image, queue_size=85)
sub_comp = Subscriber("swarm_drones/white/work_completed", Int32,
callback_comp)
sub_oran = Subscriber("swarm_drones/orange/telemetry", Telemetry,
callback_orange)
sub_blue = Subscriber("swarm_drones/blue/telemetry", Telemetry, callback_blue)
pub_tel = Publisher("swarm_drones/white/telemetry", Telemetry, queue_size=10)
pub_points = Publisher("swarm_drones/white/points", Array, queue_size=85)
pub_in_points_low_bat = Publisher("swarm_drones/white/in_points_low_bat",
Array,
queue_size=85)
proxy_voit_res = ServiceProxy("swarm_drones/white/voit_res", Voti)
service = Service("swarm_drones/take_points", PointCloud, handler_take_points)
while not rospy.is_shutdown():
voit = proxy_voit_res()
tel = Telemetry()
stream = Image()
tel.completed = complet
tel.charge = power
tel.position = position
tel.sat = satel
pub_tel.publish(tel)
if len(in_coordb) > 0:
pub_in_points_low_bat.publish(in_coordb)
elif len(in_coordo) > 0:
pub_in_points_low_bat.publish(in_coordo)
pub_stream.publish(stream)
def wifiConnect(req):
if req.command == 1:
connectToWifi(req.id)
elif req.command == 2:
enableHotspot()
return 1
# return 192 when complited
def callback_low_battery(data):
global low_battery
low_battery = data
"""def callback_req(data):
global com
com = data"""
pub_stateRep = Publisher("swarm_drones/state_repeater", Int32, queue_size=4)
proxy = Service("swarm_drones/to_repeater", Repeater, wifiConnect)
sub_low_battery = Subscriber("swarm_drones/white/low_battery", Bool,
callback_low_battery)
while not rospy.is_shutdown():
if low_battery == True:
pub_stateRep.publish(-1)
pub_stateRep.publish(0)
pass
def start(self):
super(WatcherTrigger, self).start()
self.__service = Service(self.__service_name, Trigger, self.handler)
def __init__(self):
init_node('iiwa_sunrise', log_level=INFO)
hardware_interface = get_param('~hardware_interface',
'PositionJointInterface')
self.robot_name = get_param('~robot_name', 'iiwa')
model = get_param('~model', 'iiwa14')
tool_length = get_param('~tool_length', 0.0)
flange_type = get_param('~flange_type', 'basic')
self.current_joint_positions = [0, 0, 0, 0, 0, 0, 0]
self.current_cartesian_pose = Pose()
if model == 'iiwa7':
self.l02 = 0.34
self.l24 = 0.4
self.max_joint_velocities = [
np.deg2rad(98),
np.deg2rad(98),
np.deg2rad(100),
np.deg2rad(130),
np.deg2rad(140),
np.deg2rad(180),
np.deg2rad(180)
]
elif model == 'iiwa14':
self.l02 = 0.36
self.l24 = 0.42
self.max_joint_velocities = [
np.deg2rad(85),
np.deg2rad(85),
np.deg2rad(100),
np.deg2rad(75),
np.deg2rad(130),
np.deg2rad(135),
np.deg2rad(135)
]
else:
logerr('unknown robot model')
return
if flange_type in [
'basic', 'electrical', 'pneumatic', 'IO_pneumatic',
'IO_electrical', 'inside_electrical'
]:
flange_offset = 0.0
elif flange_type in [
'touch_pneumatic', 'touch_electrical', 'IO_valve_pneumatic'
]:
flange_offset = 0.026
else:
logerr('unkown flange type')
return
self.l46 = 0.4
self.l6E = 0.126 + tool_length + flange_offset
self.tr = 0.0
self.rs = 2.0
self.v = 1.0
self.relative_joint_velocity = 1.0
self.joint_names = [
'{}_joint_1'.format(self.robot_name), '{}_joint_2'.format(
self.robot_name), '{}_joint_3'.format(self.robot_name),
'{}_joint_4'.format(self.robot_name),
'{}_joint_5'.format(self.robot_name),
'{}_joint_6'.format(self.robot_name),
'{}_joint_7'.format(self.robot_name)
]
joint_states_sub = Subscriber('joint_states',
JointState,
self.jointStatesCb,
queue_size=1)
command_pose_sub = Subscriber('command/CartesianPose',
CartesianPose,
self.commandPoseCb,
queue_size=1)
command_pose_lin_sub = Subscriber('command/CartesianPoseLin',
CartesianPose,
self.commandPoseLinCb,
queue_size=1)
redundancy_sub = Subscriber('command/redundancy',
RedundancyInformation,
self.redundancyCb,
queue_size=1)
joint_position_sub = Subscriber('command/JointPosition',
JointPosition,
self.jointPositionCb,
queue_size=1)
self.state_pose_pub = Publisher('state/CartesianPose',
CartesianPose,
queue_size=1)
self.joint_position_pub = Publisher('state/JointPosition',
JointPosition,
queue_size=1)
self.joint_trajectory_pub = Publisher(
'{}_trajectory_controller/command'.format(hardware_interface),
JointTrajectory,
queue_size=1)
path_parameters_configuration_srv = Service(
'configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits,
self.handlePathParametersConfiguration)
path_parameters_lin_configuration_srv = Service(
'configuration/setSmartServoLinLimits',
SetSmartServoLinSpeedLimits,
self.handlePathParametersLinConfiguration)
smart_servo_configuration_srv = Service(
'configuration/ConfigureControlMode', ConfigureControlMode,
self.handleSmartServoConfiguration)
spin()
if par_route == 1: # выбор нужных точек из облака
route = n_route / 2 + 1
dots = n_s * route
i = 0
while i < dots:
coordinate.append(coord[i])
i = i + 1
elif par_route == 0:
route = round(n_route / 2)
dots = n_s * route
i = 0
while i < dots:
coordinate.append(coord[i])
i = i + 1
server_otherVoit = Service("white/voite", Voting, handle_otherVoit)
while not rospy.is_shutdown():
voited = VotingRequest()
comRep = RepeaterRequest()
voited.my_voit = voit
if com == -1:
if voit_blue(voit) == voit and voit_orange(
voit) == voit and i_repeater:
sub_stateRep.unregister()
comRep.commande = 1
comRep.id = voit
change_rep(comRep)
tel = Telemetry()
stream = Image()
tel.charg = power
def __init__(self, uart="/dev/ttyS0", rate = None):
self.navSystemParam = { # параметры необходимые для изменения систем позиционирования
"GPS": [
Parameter("BoardPioneer_modules_gnss", 1.0),
Parameter("Imu_magEnabled", 1),
Parameter("SensorMux_gnss", 255.0),
Parameter("Copter_man_velScale", 5),
Parameter("Copter_man_vzScale", 5),
Parameter("Copter_pos_vDesc", 0.7),
Parameter("Copter_pos_vDown", 1),
Parameter("Copter_pos_vLanding", 0.5),
Parameter("Copter_pos_vMax", 2),
Parameter("Copter_pos_vTakeoff", 0.5),
Parameter("Copter_pos_vUp", 1),
Parameter("Flight_com_homeAlt", 1.5),
Parameter("Flight_com_landingAlt", 1),
Parameter("Flight_com_navSystem", 0),
Parameter("State_startCount", 92)
],
"LPS": [
Parameter("BoardPioneer_modules_gnss", 0),
Parameter("Imu_magEnabled", 0.0),
Parameter("SensorMux_gnss", 0.0),
Parameter("Copter_man_velScale", 0.5),
Parameter("Copter_man_vzScale", 0.5),
Parameter("Copter_pos_vDesc", 0.4),
Parameter("Copter_pos_vDown", 0.7),
Parameter("Copter_pos_vLanding", 0.3),
Parameter("Copter_pos_vMax", 1),
Parameter("Copter_pos_vTakeoff", 0.3),
Parameter("Copter_pos_vUp", 0.5),
Parameter("Flight_com_homeAlt", 0.5),
Parameter("Flight_com_landingAlt", 0),
Parameter("Flight_com_navSystem", 1),
Parameter("State_startCount", 90)
],
"OPT": [
Parameter("BoardPioneer_modules_gnss", 0.0),
Parameter("Imu_magEnabled", 0),
Parameter("SensorMux_gnss", 0.0),
Parameter("Copter_man_velScale", 0.5),
Parameter("Copter_man_vzScale", 0.5),
Parameter("Copter_pos_vDesc", 0.4),
Parameter("Copter_pos_vDown", 0.5),
Parameter("Copter_pos_vLanding", 0.3),
Parameter("Copter_pos_vMax", 0.4),
Parameter("Copter_pos_vTakeoff", 0.3),
Parameter("Copter_pos_vUp", 0.5),
Parameter("Flight_com_homeAlt", 0.5),
Parameter("Flight_com_landingAlt", 0),
Parameter("Flight_com_navSystem", 2),
Parameter("State_startCount", 93)
]
}
self.uart = uart # название порта
self.log = [] # массив, хранящий все сообщения лога
self.live = False # состояние подключение к базовой платы АП
self.restart = False # состояние перезапуска
self.navSystem = 0 # текущая система позиционирования
self.event_messages = (10, 12, 23, 2) # доступные события(команды) АП
self.callback_event_messages = (255, 26, 31, 32, 42, 43, 51, 56, 65) # события, возвращаемые АП
self.navSystemName = {0:"GPS", 1:"LPS", 2:"OPT"} # доступные системы позиционирования
self.state_event = -1 # последнеее событие, отправленное в АП
self.state_callback_event = 0 # полседнее событие пришедшее от АП
self.state_position = [0., 0., 0., 0.] # последняя точка, на которую был отправлен коптер (в локальных координатах)
self.state_gps_position = [0., 0., 0.] # последняя координата, нак оторую был отправлен коптер (в глобальных координатах)
self.state_board_led = [] # текущее состояние светодиодов на базовой плате
self.state_module_led = [] # текущее состояние светодиодов на Led-модуле
self.global_point_seq = 0 # номер точки в глобальной системе
self.autopilot_params = [] # выгруженные параметры АП
self.messenger = None # основной объект класса Messenger, отвечающий за коммуникацию между RPi и базовой платы
self.rate = rate # таймер
self.logger = Service("geoscan/get_log", Log, self.handle_log) # сервис логов
self.alive = Service("geoscan/alive", Live, self.handle_live) # сервис, показывающий состояние подключения
self.info_service = Service("geoscan/board/get_info", Info, self.handle_info) # сервис, возвращающий бортовую информация
self.time_service = Service("geoscan/board/get_time", Time, self.handle_time) # сервис, возвращающий время с момента включения коптера
self.uptime_service = Service("geoscan/board/get_uptime", Time, self.handle_uptime) # сервис, возвращающий время запуска для системы навигации
self.flight_time_service = Service("geoscan/board/get_flight_time", Time, self.handle_flight_time) # сервис, возвращающий время с начала полета
self.get_autopilot_params_service = Service("geoscan/board/get_parameters", ParametersList, self.handle_get_autopilot_params) # сервис, возвращающий параметры АП
self.set_autopilot_params_service = Service("geoscan/board/set_parameters", SetParametersList, self.handle_set_autopilot_params) # сервис, устанавливающий параметры АП
self.restart_service = Service("geoscan/board/restart", Empty, self.handle_restart) # сервси перезапуска базововй платы
self.get_navigation_service = Service("geoscan/navigation/get_system", NavigationSystem, self.handle_get_navigation_system) # сервис, возвращающий текущую систему позиционирования
self.set_navigation_service = Service("geoscan/navigation/set_system", SetNavigationSystem, self.handle_set_navigation_system) # сервис, устанавливающий текущую систему позиционирования
self.local_position_service = Service("geoscan/flight/set_local_position", Position, self.handle_local_pos) # сервис полета в локальную точку
self.global_position_service = Service("geoscan/flight/set_global_position", PositionGPS, self.handle_gps_pos) # сервси полета в глобальную точку
self.yaw_service = Service("geoscan/flight/set_yaw", Yaw, self.handle_yaw) # сервис управления рысканьем
self.event_service = Service("geoscan/flight/set_event", Event, self.handle_event) # севрис управления событиями АП
self.board_led_service = Service("geoscan/led/board/set", Led, self.handle_board_led) # сервис управления светодиодами на базовой плате
self.module_led_service = Service("geoscan/led/module/set", Led, self.handle_board_led) # сервис управления светодиодами на LED-модуле
self.logger_publisher = Publisher("geoscan/log", String, queue_size=10) # издатель темы логов
self.battery_publisher = Publisher("geoscan/battery_state", SimpleBatteryState, queue_size = 10) # издатель темы состояния АКБ
self.local_position_publisher = Publisher("geoscan/navigation/local/position", Point, queue_size = 10) # издатель темы позиции в LPS
self.local_yaw_publisher = Publisher("geoscan/navigation/local/yaw", Float32, queue_size = 10) # издаетель темы рысканья в LPS
self.local_velocity_publisher = Publisher("geoscan/navigation/local/velocity", Point, queue_size = 10) # издатель темы ускорения в LPS
self.global_position_publisher = Publisher("geoscan/navigation/global/position", PointGPS, queue_size = 10) # издатель темы позиции в GPS
self.global_status_publisher = Publisher("geoscan/navigation/global/status", Int8, queue_size = 10) # издатель темы статуса GPS модуля
self.satellites_publisher = Publisher("geoscan/navigation/satellites", SatellitesGPS, queue_size = 10) # издатель темы состояния спутников
self.opt_velocity_publisher = Publisher("geoscan/navigation/opt/velocity", OptVelocity, queue_size = 10) # издатель темы ускорения в OPT
self.callback_event_publisher = Publisher("geoscan/flight/callback_event", Int32, queue_size = 10) # издатель темы событий, возвращаемых АП
self.gyro_publisher = Publisher("geoscan/sensors/gyro", Point, queue_size = 10) # издатель темы данных гироскопа
self.accel_publisher = Publisher("geoscan/sensors/accel", Point, queue_size = 10) # издатель темы данных акселерометра
self.orientation_publisher = Publisher("geoscan/sensors/orientation", Orientation, queue_size = 10) # издатель темы данных о положении
self.altitude_publisher = Publisher("geoscan/sensors/altitude", Float32, queue_size = 10) # издатель темы данных о высоте по барометру
self.mag_publisher = Publisher("geoscan/sensors/mag", Point, queue_size = 10) # издатель темы данных магнитометра
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from rospy import Service, ServiceProxy
from swarm_drones.srv import VotiResponse, VotiRequest, Voti
def handle_voti_res(req):
global voit
voit = 1
return voit # вставить id коптера
rospy.init_node("samopal_node")
service_voit_res = Service("swarm_drones/voit_res", Voti, handle_voti_res)
while not rospy.is_shutdown():
pass
piexif.GPSIFD.GPSLatitudeRef: "N",
piexif.GPSIFD.GPSLatitude: ((lat_1, 1), (lat_2, 1), (lat_3, 1)),
piexif.GPSIFD.GPSLongitudeRef: "E",
piexif.GPSIFD.GPSLongitude: ((lon_1, 1), (lon_2, 1), (lon_3, 1)),
piexif.GPSIFD.GPSAltitudeRef: (0),
piexif.GPSIFD.GPSAltitude: (int(position.altitude), 1)
}
zero = {
piexif.ImageIFD.Make: u'PiCamera v2',
piexif.ImageIFD.Software: u'piexif'
}
exif_dict = {"0th": zero, "GPS": gps_ifd}
piexif.insert(
piexif.dump(exif_dict),
"/home/ubuntu/geoscan_ws/photo/geoscan_{}.jpg".format(
currentPhotoCnt))
currentPhotoCnt = currentPhotoCnt + 1
return TakeScreenResponse(1)
except Exception as e:
print(str(e))
return TakeScreenResponse(0)
sub_pos = Subscriber("geoscan/navigation/global/position", PointGPS,
callback_pos)
sub_cam = Subscriber("pioneer_max_camera/image_raw", Image, callback_img)
camera_service = Service("swarm/take_photos", TakeScreen, handle_photo)
while not rospy.is_shutdown():
pass
rospy.init_node("voting_node")
sub_pos = Subscriber("geoscan/navigation/local/position", Point, callback_pos)
sub_bat = Subscriber("geoscan/battery_state", SimpleBatteryState, callback_bat)
sub_oran = Subscriber("swarm_drones/orange/telemetry", Telemetry,
callback_orange)
sub_blue = Subscriber("swarm_drones/blue/telemetry", Telemetry, callback_blue)
sub_stateRep = Subscriber("swarm_drones/state_repeater", Int32, callback_req)
pub_stateRep = Publisher("swarm_drones/state_repeater", Int32, queue_size=4)
pun_repWhite = Subscriber("swarm_drones/white/repeater", Int32,
callback_repWhite)
sub_repBlue = Subscriber("swarm_drones/repeater", Int32, callback_repBlue)
sub_repBlue = Subscriber("swarm_drones/repeater", Int32, callback_repBlue)
service_voit_res = Service("swarm_drones/white/voit_res", Voti,
handle_voti_res)
server_otherVoit = Service("swarm_drones/white/voite", Voting,
handle_otherVoit)
voit_orange = ServiceProxy("swarm_drones/orange/voit", Voting)
voit_blue = ServiceProxy("swarm_drones/blue/voit", Voting)
change_rep = ServiceProxy("swarm_drones/to_repeater", Repeater)
powerDrones = [0, 0, 0]
timeNeed = [0, 0, 0]
while not rospy.is_shutdown():
#x = position.latitude
#y = position.longitude
#xo = telOrange.position.latitude
#yo = telOrange.position.longitude
def __init__(self, config='strategies.json', strategy='normal',
name='Pandora', testing=False, verbose=False):
""" Initializes the agent.
:param :name The name of the agent. Defaults to Pandora.
:param :strategy Defines the configuration that will be loaded from
the Agent.
:param :config A yaml/json file that contains the agent strategies.
The file should be located in the config folder of this
package.
"""
# Configuration folder
config_dir = RosPack().get_path(PKG) + '/config/'
self.name = name
self.verbose = verbose
self.testing = testing
self.strategy = strategy
self.config = config_dir + config
# Dispatcher for event based communication.
self.dispatcher = EventEmitter()
# SUBSCRIBERS.
self.world_model_sub = Subscriber(topics.world_model, WorldModel,
self.receive_world_model)
# ACTION CLIENTS.
self.explorer = clients.Explorer(self.dispatcher)
self.data_fusion = clients.DataFusion()
self.navigator = clients.Navigator(self.dispatcher)
self.gui_client = clients.GUI()
self.effector = clients.Effector()
if not self.testing:
Service('/gui/kill_agent', Empty, self.destroy_agent)
self.transform_listener = tf.TransformListener()
# State client
if not self.testing:
log.debug('Connecting to state manager.')
self.state_changer = StateClient()
self.state_changer.client_initialize()
log.debug('Connection established.')
self.state_changer.change_state_and_wait(RobotModeMsg.MODE_OFF)
# General information.
self.current_pose = Pose()
self.exploration_mode = DoExplorationGoal.TYPE_DEEP
# Victim information.
self.available_targets = []
self.gui_result = ValidateVictimGUIResult()
# Between-transition information.
self.base_converged = threading.Event()
self.poi_found_lock = threading.Lock()
# Utility Variables
self.MOVE_BASE_RETRIES = 0
self.target = Target(self.dispatcher)
# Expose client methods to class
setattr(self, 'test_end_effector', self.effector.test)
setattr(self, 'park_end_effector', self.effector.park)
setattr(self, 'preempt_end_effector', self.effector.cancel_all_goals)
setattr(self, 'preempt_explorer', self.explorer.cancel_all_goals)
setattr(self, 'scan', self.effector.scan)
self.generate_global_state_transitions()
self.load()
log.info('Agent initialized...')
def __init__(self, param, relayNum, output):
Device.__init__(self, param.getRelayName(relayNum), output)
self._relayNum = relayNum
Service('%s/setRelay' % self._name, Relay, self.setRelayCallBack)
