def __init__(self, config):
super().__init__()
node_name = config.get('node_name', 'GazeboEnvNode')
rospy.init_node(node_name, disable_signals=True)
self.gz_state = GazeboState.INIT
self.config = config
self.state_pub = Publisher('/gazebo/set_model_state',
ModelState,
queue_size=10)
self.pause = ServiceProxy("/gazebo/pause_physics", Empty)
self.unpause = ServiceProxy("/gazebo/unpause_physics", Empty)
self.reset_proxy = ServiceProxy("/gazebo/reset_simulation", Empty)
self.add_agents()
time.sleep(0.2)
self.action_space = Dict(
{k: v.action_space
for k, v in self.agents.items()})
self.observation_space = Dict(
{k: v.observation_space
for k, v, in self.agents.items()})
self.unpause_gazebo()
# check_for_ROS()
self.pause_gazebo()
print('=' * 100)
print(self)
print(rospy.get_name())
print(rospy.get_namespace())
print(rospy.get_node_uri())
print('=' * 100)
def record_cylinder(t, cylinder_recorder):
from rospy import ServiceProxy
from gazebo_msgs.srv import GetModelState, GetLinkState
from gazebo_msgs.msg import LinkState
model_name = 'cylinder'
state_proxy = ServiceProxy('/gazebo/get_model_state',
GetModelState,
persistent=False)
position_proxy = ServiceProxy('/gazebo/get_link_state',
GetLinkState,
persistent=False)
cylinder = state_proxy(model_name, "world")
hand = position_proxy('robot::hand_f2_link', 'world')
if cylinder.success and hand.success:
cylinder_position = np.array([
cylinder.pose.position.x, cylinder.pose.position.y,
cylinder.pose.position.z
])
hand_position = np.array([
hand.link_state.pose.position.x, hand.link_state.pose.position.y,
hand.link_state.pose.position.z
])
distance = np.linalg.norm(cylinder_position - hand_position)
cylinder_recorder.record_entry(t, cylinder_position[1], distance)
def __init__(self):
self.__leds = []
for _ in range(25):
self.__leds.append(ColorRGBA())
rospy.wait_for_service("geoscan/alive")
rospy.wait_for_service("geoscan/led/module/set")
self.__alive = ServiceProxy("geoscan/alive", Live)
self.__led_service = ServiceProxy("geoscan/led/module/set", Led)
def __init__(self, callback):
rospy.wait_for_service("geoscan/alive")
rospy.wait_for_service("geoscan/flight/set_event")
rospy.wait_for_service("geoscan/flight/set_yaw")
rospy.wait_for_service("geoscan/flight/set_local_position")
rospy.wait_for_service("geoscan/flight/set_global_position")
self.__alive = ServiceProxy("geoscan/alive", Live)
self.__event_service = ServiceProxy("geoscan/flight/set_event", Event)
self.__yaw_service = ServiceProxy("geoscan/flight/set_yaw", Yaw)
self.__local_position_service = ServiceProxy(
"geoscan/flight/set_local_position", Position)
self.__global_position_service = ServiceProxy(
"geoscan/flight/set_global_position", PositionGPS)
self.__callback_event = Subscriber("geoscan/flight/callback_event",
Int32, callback)
def clicked_point(msg):
# rospy.loginfo(msg)
goal_pub = rospy.Publisher("/goal", MoveBaseActionGoal, queue_size=1)
plan_client = ServiceProxy("/move_base/make_plan", GetPlan)
goalPos = PoseStamped()
goalPos.header.frame_id = "map"
goalPos.header.stamp = rospy.Time.now()
goalPos.pose.position.x = msg.point.x
goalPos.pose.position.y = msg.point.y
goalPos.pose.orientation.x = rot[0]
goalPos.pose.orientation.y = rot[1]
goalPos.pose.orientation.z = rot[2]
goalPos.pose.orientation.w = rot[3]
getplan = GetPlanRequest()
getplan.start.header.frame_id = "map"
getplan.start.header.stamp = rospy.Time.now()
getplan.start.pose.position.x = x
getplan.start.pose.position.y = y
getplan.start.pose.orientation.x = 0
getplan.start.pose.orientation.y = 0
getplan.start.pose.orientation.z = 0
getplan.start.pose.orientation.w = 1
getplan.goal = goalPos
getplan.tolerance = 0.5
goal = MoveBaseActionGoal()
goal.goal.target_pose = goalPos
goal.header.frame_id = "map"
goal.header.stamp = rospy.Time.now()
goal_pub.publish(goal)
res = plan_client.call(getplan)
rospy.loginfo(res.plan.header.frame_id)
path_pub = rospy.Publisher("trajectory", Path, queue_size=1)
if isinstance(res, GetPlanResponse):
res.plan.header.frame_id = "map"
# res.plan.header.stamp = rospy.Time.now()
path_pub.publish(res.plan)
while True:
plan_pub = rospy.Publisher("/move_base/GlobalPlanner/plan",
Path,
queue_size=1)
plan_pub.publish(res.plan)
def robot_speak(self, value):
wait_for_service('robot_speak')
try:
speak = ServiceProxy('robot_speak', Speak)
speak(value)
except ServiceException as e:
print(e)
def delete():
try:
srv = ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
req.model_name = "turtlebot3_burger"
resp = srv(req)
except ServiceException, e:
print("Service call failed: %s" % e)
return
def get_qrs(self):
log.info("Waiting for service %s." % topics.get_world_model)
wait_for_service(topics.get_world_model)
try:
res = ServiceProxy(topics.get_world_model, GetWorldModel)()
return res.worldModel.qrs
except ServiceProxy as e:
log.error(str(e))
return None
def __init__(self, name, service_class, persistent=True, headers=None):
"""
Creates a new asynchronous service proxy.
@param name: name of service to call
@param service_class: auto-generated service class
@param persistent: (optional) if True, proxy maintains a persistent
connection to service. For the asynchronous retrieval of the result the connection has to
be persistent.
@param headers: (optional) arbitrary headers
"""
self.__proxy = ServiceProxy(name, service_class, persistent, headers)
self.__await_finish_thread = Thread(
target=self.__retrieve_results_async)
self.__await_finish_thread.daemon = True
self.__retrieve_result = Event()
self.__call_future = Future()
self.__shutdown_result_thread = False
def call_service(service, args=None):
# Given the service name, fetch the type and class of the service,
# and a request instance
service_type = get_service_type(str(service))
if service_type is None:
raise InvalidServiceException(service)
service_class = get_service_class(service_type)
inst = get_service_request_instance(service_type)
# Populate the instance with the provided args
args_to_service_request_instance(service, inst, args)
# Call the service
proxy = ServiceProxy(service, service_class)
response = proxy.call(inst)
# Turn the response into JSON and pass to the callback
json_response = extract_values(response)
return json_response
def call_service(service, args=None):
# Given the service name, fetch the type and class of the service,
# and a request instance
service_type = get_service_type(str(service))
if service_type is None:
raise InvalidServiceException(service)
service_class = get_service_class(service_type)
inst = get_service_request_instance(service_type)
# Populate the instance with the provided args
args_to_service_request_instance(service, inst, args)
# Call the service
proxy = ServiceProxy(service, service_class)
response = proxy.call(inst)
# Turn the response into JSON and pass to the callback
json_response = extract_values(response)
return json_response
def save_mission_client(self, mission_name):
print('WAITING FOR SEVICE')
wait_for_service('geotiff/saveMission')
msg = String(mission_name)
try:
save_mission = ServiceProxy('geotiff/saveMission', SaveMission)
save_mission(msg)
except ServiceException as e:
print('Service call failed: ' + str(e))
print('Service call succeded')
return True
def save_mission_client(self, mission_name):
print('WAITING FOR SEVICE')
wait_for_service('qr_csv/createCSV')
msg = String(mission_name)
try:
save_mission = ServiceProxy('qr_csv/createCSV', createCSV)
save_mission(msg)
except ServiceException as e:
print('Service call failed: ' + str(e))
print('Service call succeded')
return True
def save_mission_client(self, mission_name):
print("WAITING FOR SEVICE")
wait_for_service(save_geotiff_service)
msg = String(mission_name)
try:
save_mission = ServiceProxy(save_geotiff_service, SaveMission)
resp = save_mission(msg)
except ServiceException as e:
print("Service call failed :%s" % e)
print("Service call succeded")
return True
def Sensor2Brain (t, cylinder_x, cylinder_y, cylinder_z):
from rospy import ServiceProxy
from gazebo_msgs.srv import GetModelState
model_name = 'cylinder'
state_proxy = ServiceProxy('/gazebo/get_model_state',
GetModelState, persistent=False)
cylinder_state = state_proxy(model_name, "world")
if cylinder_state.success:
current_position = cylinder_state.pose.position
# clientLogger.info('curr_pos: {}'.format(current_position))
cylinder_x.amplitude = np.abs(current_position.x) # * (np.random.rand() - .5) * 10
cylinder_y.amplitude = np.abs(current_position.y) # * (np.random.rand() - .5) * 10
cylinder_z.amplitude = np.abs(current_position.z) # * (np.random.rand() - .5) * 10
def record_ball_csv(t, ball_recorder):
from rospy import ServiceProxy
from gazebo_msgs.srv import GetModelState
model_name = 'ball'
state_proxy = ServiceProxy('/gazebo/get_model_state',
GetModelState,
persistent=False)
ball_state = state_proxy(model_name, "world")
if ball_state.success:
current_position = ball_state.pose.position
ball_recorder.record_entry(t, current_position.x, current_position.y,
current_position.z)
def robot_listen(self):
wait_for_service('robot_listen')
try:
listen = ServiceProxy('robot_listen', Listen)
response = listen()
replace_chars = ['u\'', '\'']
data = response.data
for char in replace_chars:
data = data.replace(char, '"')
return json.loads(data)
except (ServiceException, json.decoder.JSONDecodeError) as e:
print(e)
return {'_text': '', 'entities': {}}
def __init__(self,
ns='',
list_controllers_topic='/list_controllers',
switch_controller_topic='/switch_controller'):
""" Constructs a client to a ros_control ControllerManager.
@param ns: ROS namespace containing the ControllerManager
@type ns: str
@param list_controllers_topic: name of ListControllers topic
@type list_controllers_topic: str
@param switch_controller_topic: name of SwitchController topic
@type switch_controller_topic: str
"""
from controller_manager_msgs.srv import (ListControllers,
SwitchController)
from rospy import ServiceProxy
self._list_controllers_srv = ServiceProxy(ns + list_controllers_topic,
ListControllers,
persistent=True)
self._switch_controllers_srv = ServiceProxy(ns +
switch_controller_topic,
SwitchController,
persistent=True)
def delete_model(mav_sys_id, vehicle_type, ros_master_uri=None):
if ros_master_uri:
original_uri = os.environ[ROS_MASTER_URI]
os.environ[ROS_MASTER_URI] = ros_master_uri
try:
srv = ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
unique_name = 'uav' + str(mav_sys_id)
req.model_name = unique_name
resp = srv(req)
except ServiceException, e:
print_error("===================================================================================")
print_error(" Service call failed: %s" % e)
print_error("===================================================================================")
sys.exit(2)
def __init__(self, do_register=True, silent=True):
self._name = rospy.get_name()
self._silent = silent
self._acknowledge_publisher = Publisher('/robot/state/server',
RobotModeMsg,
latch=True,
queue_size=5)
self._state_subscriber = Subscriber('/robot/state/clients',
RobotModeMsg,
self.server_state_information,
queue_size=10)
self._state_info = ServiceProxy('/robot/state/info', GetStateInfo)
self._state_changer = ActionClient('/robot/state/change',
RobotModeAction)
if do_register:
self.client_register()
def __init__(self):
rospy.wait_for_service("geoscan/alive")
rospy.wait_for_service("geoscan/board/restart")
rospy.wait_for_service("geoscan/board/get_time")
rospy.wait_for_service("geoscan/board/get_uptime")
rospy.wait_for_service("geoscan/board/get_flight_time")
rospy.wait_for_service("geoscan/board/get_info")
rospy.wait_for_service("geoscan/board/get_parameters")
rospy.wait_for_service("geoscan/board/set_parameters")
self.__alive = ServiceProxy("geoscan/alive", Live)
self.__restart = ServiceProxy("geoscan/board/restart", Empty)
self.__time_service = ServiceProxy("geoscan/board/get_time", Time)
self.__uptime_service = ServiceProxy("geoscan/board/get_uptime", Time)
self.__flight_time_service = ServiceProxy(
"geoscan/board/get_flight_time", Time)
self.__info_service = ServiceProxy("geoscan/board/get_info", Info)
self.__get_parameters_service = ServiceProxy(
"geoscan/board/get_parameters", ParametersList)
self.__set_parameters_service = ServiceProxy(
"geoscan/board/set_parameters", SetParametersList)
def spawn():
req = SpawnModelRequest()
req.model_name = "model"
req.model_xml = "/home/rbe3002/catkin_ws/src/turtlebot3/turtlebot3_description/rviz/model.rviz"
req.robot_namespace = "model"
req.initial_pose.position.x = 0.0
req.initial_pose.position.y = 0.0
req.initial_pose.position.z = 0.0
req.initial_pose.orientation.x = 0.0
req.initial_pose.orientation.y = 0.0
req.initial_pose.orientation.z = 0.0
req.initial_pose.orientation.w = 1.0
req.reference_frame = ''
try:
srv = ServiceProxy('/gazebo/set_model_state', SpawnModel)
resp = srv(req)
except ServiceException, e:
print(" Service call failed: %s" % e)
return
def __init__(self):
self.error_number = -255.
self.__battery_state = SimpleBatteryState()
self.__gyro = Point()
self.__accel = Point()
self.__orientation = Orientation()
self.__altitude = 0.0
self.__mag = Point()
rospy.wait_for_service("geoscan/alive")
self.__alive = ServiceProxy("geoscan/alive", Live)
self.__gyro_subscriber = Subscriber("geoscan/sensors/gyro", Point, self.__gyro_callback)
self.__accel_subscriber = Subscriber("geoscan/sensors/accel", Point, self.__accel_callback)
self.__orientation_subscriber = Subscriber("geoscan/sensors/orientation", Orientation, self.__orientation_callback)
self.__altitude_subscriber = Subscriber("geoscan/sensors/altitude", Float32, self.__altitude_callback)
self.__mag_subscriber = Subscriber("geoscan/sensors/mag", Point, self.__mag_callback)
self.__power_subscriber = Subscriber("geoscan/battery_state", SimpleBatteryState, self.__power_callback)
rospy.wait_for_message("geoscan/sensors/gyro", Point)
rospy.wait_for_message("geoscan/sensors/accel", Point)
rospy.wait_for_message("geoscan/sensors/orientation", Orientation)
rospy.wait_for_message("geoscan/sensors/altitude", Float32)
rospy.wait_for_message("geoscan/battery_state", SimpleBatteryState)
def delete_model(mav_sys_id, vehicle_type, ros_master_uri=None):
if ros_master_uri:
original_uri = os.environ[ROS_MASTER_URI]
os.environ[ROS_MASTER_URI] = ros_master_uri
srv = ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
unique_name = vehicle_type + '_' + str(mav_sys_id)
req.model_name = unique_name
resp = srv(req)
if ros_master_uri:
os.environ[ROS_MASTER_URI] = original_uri
if resp.success:
print(resp.status_message, '(%s)' % unique_name)
return 0
else:
print("failed to delete model [%s]: %s" %
(unique_name, resp.status_message),
file=sys.stderr)
return 1
def __init__(self, mode_type_change_handler, traffic_light_change_handler, world_image_change_handler, color_peak_change_handler):
#
# Publishers
#
self.mode_change_pub = Publisher(
"/smart_home/mode_change_chatter",
ModeChange,
queue_size=1
)
self.device_activation_change_pub = Publisher(
"/smart_home/device_activation_change_chatter",
DeviceActivationChange,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.intensity_change_pub = Publisher(
"/smart_home/intensity_change_chatter",
Float32,
queue_size=1
)
self.countdown_state_pub = Publisher(
"/smart_home/countdown_state_chatter",
CountdownState,
queue_size=1
)
self.active_frequencies_pub = Publisher(
"/smart_home/active_frequencies_chatter",
Float32Arr,
queue_size=1
)
self.start_wave_mode_pub = Publisher(
"/smart_home/start_wave_mode_chatter",
Empty,
queue_size=1
)
self.wave_update_pub = Publisher(
"/smart_home/wave_update_chatter",
WaveUpdate,
queue_size=MAX_AUX_DEVICE_COUNT
)
#
# Subscribers
#
self.mode_change_request_sub = Subscriber(
"/smart_home/mode_change_request_chatter",
ModeChangeRequest,
self.mode_change_request_callback,
queue_size=1
)
self.countdown_state_sub = Subscriber(
"/smart_home/countdown_state_chatter",
CountdownState,
self.countdown_state_callback,
queue_size=1
)
self.participant_location_sub = Subscriber(
"/smart_home/wave_participant_location_chatter",
WaveParticipantLocation,
self.participant_location_callback,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.color_peak_left_sub = Subscriber(
"/smart_home/color_peak_left",
Color,
lambda msg: color_peak_change_handler(msg, 1),
queue_size=1
)
self.color_peak_right_sub = Subscriber(
"/smart_home/color_peak_right",
Color,
lambda msg: color_peak_change_handler(msg, 2),
queue_size=1
)
self.cap_peaks_telem_sub = Subscriber(
"/smart_home/color_peaks_telem",
ColorPeaksTelem,
self.color_peaks_telem_callback,
queue_size=1
)
#
# Service clients
#
self.screen_calibration_request_cli = ServiceProxy(
"/smart_home/screen_calibration_requests",
RequestScreenCalibration
)
self.screen_calibration_set_homography_points_cli = ServiceProxy(
"/smart_home/screen_calibration_homography_points",
SetScreenCalibrationPointsOfHomography
)
#
# Other initialization
#
self.mode_type_change_handler = mode_type_change_handler
self.traffic_light_change_handler = traffic_light_change_handler
self.world_image_change_handler = world_image_change_handler
self.current_mode = None
self.active_mode_sequence_characteristics = (-1,)
self.send_mode_type(ModeChange.FULL_OFF, False)
self.participant_locations = None
SmartHomeHubNodeInterface.log_info("Started.")
def spawn_model(
mav_sys_id,
vehicle_type,
baseport,
color,
pose,
ros_master_uri=None,
mavlink_address=None,
debug=False,
autopilot='ardupilot',
gazebo_ip='127.0.0.1',
local_ip='127.0.0.1',
use_radar=False,
):
x, y, yaw = pose
if ros_master_uri:
original_uri = os.environ[ROS_MASTER_URI]
os.environ[ROS_MASTER_URI] = ros_master_uri
srv = ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
model_file = ""
if vehicle_type == "iris":
if autopilot == 'px4':
model_directory = "rotors_description"
model_file = "urdf/iris_base.xacro"
elif autopilot == 'ardupilot':
model_directory = 'iris_with_standoffs_demo'
model_file = "model.sdf"
elif vehicle_type == "delta_wing":
if autopilot == 'px4':
model_directory = "delta_wing"
model_file = "delta_wing.sdf"
elif autopilot == 'ardupilot':
model_directory = 'zephyr_delta_wing_ardupilot_demo'
model_file = "delta_wing.sdf"
model_pathname = os.path.join(os.path.dirname(__file__), '..', '..', '..',
'..', 'share', 'uctf', 'models',
'%s' % model_directory)
model_filename = os.path.join(model_pathname, '%s' % model_file)
with open(model_filename, 'r') as h:
model_xml = h.read()
kwargs = {
'mappings': {
'mavlink_udp_port': str(baseport),
},
}
# some default args for iris (see sitl_gazebo cmake file for list)
kwargs['mappings']['enable_mavlink_interface'] = "true"
kwargs['mappings']['enable_ground_truth'] = "false"
kwargs['mappings']['enable_logging'] = "false"
kwargs['mappings']['enable_camera'] = "false"
# ros commandline arguments
kwargs['mappings']['rotors_description_dir'] = model_pathname
kwargs['mappings']['scripts_dir'] = os.path.join(os.path.dirname(__file__),
'..', '..', '..', '..',
'share', 'uctf',
'sitl_gazebo_scripts')
# additional xacro params for uctf
if color:
# material_name = 'Gazebo/%s' % color.capitalize()
kwargs['mappings']['visual_material'] = color.capitalize()
if mavlink_address:
kwargs['mappings']['mavlink_addr'] = mavlink_address
if autopilot == 'ardupilot':
kwargs['mappings']['fdm_addr'] = local_ip
kwargs['mappings']['listen_addr'] = gazebo_ip
# fdm in is gazebo out
kwargs['mappings']['fdm_port_in'] = str(baseport + 5)
# fdm out is gazebo in
kwargs['mappings']['fdm_port_out'] = str(baseport + 4)
kwargs['mappings']['use_radar'] = 'true' if use_radar else 'false'
model_xml = xacro(model_xml, **kwargs)
if debug:
print(model_xml)
req = SpawnModelRequest()
unique_name = vehicle_type + '_' + str(mav_sys_id)
req.model_name = unique_name
req.model_xml = model_xml
req.robot_namespace = unique_name
req.initial_pose.position.x = x
req.initial_pose.position.y = y
req.initial_pose.position.z = 50.0
req.initial_pose.orientation.x = 0.0
req.initial_pose.orientation.y = 0.0
req.initial_pose.orientation.z = math.sin(yaw / 2.0)
req.initial_pose.orientation.w = math.cos(yaw / 2.0)
req.reference_frame = ''
resp = srv(req)
if ros_master_uri:
os.environ[ROS_MASTER_URI] = original_uri
if resp.success:
print(resp.status_message, '(%s)' % unique_name)
return 0
else:
print(resp.status_message, file=sys.stderr)
return 1
def __init__(self):
self.debug = get_param('debug')
self.ogmpp_pp = ServiceProxy('/ogmpp_path_planners/plan', OgmppPathPlanningSrv)
self.ogmpp_map = ServiceProxy('/ogmpp_path_planners/set_map', OgmppSetMapSrv)
def __init__(self):
self.__msg = ""
rospy.wait_for_service("geoscan/get_log")
rospy.wait_for_message("geoscan/log", String)
self.__log_service = ServiceProxy("geoscan/get_log", Log)
self.__log_sub = Subscriber("geoscan/log", String, self.__callback)
pub_photos = Publisher("swarm_drones/white/work_completed",
Int32,
queue_size=10)
pub_low_battery = Publisher("swarm_drones/white/low_battery",
Bool,
queue_size=10)
pub_out_points_low_bat = Publisher("swarm_drones/white/out_points_low_bat",
Array,
queue_size=10)
sub_point = Subscriber("swarm_drones/white/points", Array, callback_point)
sub_stateRep = Subscriber("swarm_drones/state_repeater", Int32, callback_state)
sub_pos = Subscriber("geoscan/navigation/local/position", Point, callback_pos)
sub_in_points_low_bat = Subscriber("swarm_drones/white/in_points_low_bat",
Array, callback_in_point)
proxy_cam = ServiceProxy("swarm/take_photos", TakeScreen)
while not rospy.is_shutdown():
if board.runStatus() and not once:
rospy.loginfo("start programm")
lat_home = latitude
lon_home = longitude
ap.preflight()
once = True
if charge < 10.5:
low_battery = True
pub_out_points_low_bat.publish(coordinates[::-1])
else:
low_battery = False
pub_low_battery.publish(low_battery)
if repeat != 0:
sub_in_points_low_bat_oran = Subscriber("swarm_drones/orange/in_points", Array,
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)
def init_service_proxies(self):
self.unpause = ServiceProxy("/gazebo/unpause_physics", Empty)
self.pause = ServiceProxy("/gazebo/pause_physics", Empty)
self.reset_proxy = ServiceProxy("/gazebo/reset_simulation", Empty)
