def test_shutdown_drone(self):
# Register Drone
if not client.is_connected:
client.run()
serverReset()
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': "shutdown_mavros",
"drone_type": "Mavros"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
uid = result["id"]
# Save Topics
publishes = [{"name": "topicNameMavros", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Shutdown Drone
service = roslibpy.Service(client, 'isaacs_server/shutdown_drone',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
def pull_topics_n_services():
global ROS_TOPICS, ROS_SERVICES
service_topics = roslibpy.Service(ROS_CLIENT, '/rosapi/topics', 'roscpp/GetLoggers')
service_services = roslibpy.Service(ROS_CLIENT, '/rosapi/services', 'roscpp/GetLoggers')
ROS_TOPICS = service_topics.call(roslibpy.ServiceRequest())
ROS_SERVICES = service_services.call(roslibpy.ServiceRequest())
def test_all_drones_available_dji(self):
# Register Dji Drone
if not client.is_connected:
client.run()
serverReset()
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': "all_drones_dji",
"drone_type": "DjiMatrice"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
uid = result["id"]
# Dji Save Topics
publishes = [{"name": "topicNameDji", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# All Drones Available
service = roslibpy.Service(client,
'isaacs_server/all_drones_available',
'isaacs_server/AllDronesAvailable')
request = roslibpy.ServiceRequest({})
result = wrapped_service_call(service, request)
test_drone = {
"id": uid,
"name": "all_drones_dji",
"type": "DjiMatrice",
"topics": [{
"name": "topicNameDji",
"type": "topicType"
}],
"services": []
}
self.assertTrue(result["success"])
self.assertIn(test_drone, result['drones_available'])
# Shutdown Drone
service = roslibpy.Service(client, 'isaacs_server/shutdown_drone',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# All Drones Available
service = roslibpy.Service(client,
'isaacs_server/all_drones_available',
'isaacs_server/AllDronesAvailable')
request = roslibpy.ServiceRequest({})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
self.assertNotIn(test_drone, result['drones_available'])
def set_speed(self, speed):
try:
print("Attempting to set speed...")
service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/cmd/command',
'mavros_msgs/CommandLong')
request = roslibpy.ServiceRequest({
"command":
MavrosDrone.MAV_CMD.SET_SPEED.value,
"param1":
0,
"param2":
speed,
"param3":
-1,
"param4":
0
})
print('Calling mission_waypoint_setSpeed service...')
result = service.call(request)
print('Service response: {}'.format(result))
except:
result = {
"success": False,
"message": "Failed to set new drone speed"
}
return result
def send_element_trajectories(client, assembly, element_key):
element = assembly.element(element_key)
if len(element.trajectory) != 3:
raise Exception(
'Element {} does not have all 3 trajectories planned'.format(
element_key))
pick_trajectory, move_trajectory, place_trajectory = element.trajectory
print('[ ] Invoking ROS service...', end='\r')
service = roslibpy.Service(client, '/store_assembly_task',
'workshop_tum_msgs/AssemblyTask')
# service = roslibpy.Service(client, '/execute_assembly_task', 'workshop_tum_msgs/AssemblyTask')
request = roslibpy.ServiceRequest(
dict(node_id=str(element_key),
username=username,
pick_trajectory=client._convert_to_ros_trajectory(
pick_trajectory).msg,
move_trajectory=client._convert_to_ros_trajectory(
move_trajectory).msg,
place_trajectory=client._convert_to_ros_trajectory(
place_trajectory).msg))
response = service.call(request)
print('[{}] Received service response: {}'.format(
'OK' if response['success'] else '!!', response['message']))
def test_land_drone(self):
# Register Drone
uid = self.register_mavros_drone("land_mavros")
# Save Topics
publishes = [{"name": "topicNameMavros", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Land Drone
action_client = roslibpy.actionlib.ActionClient(
client, "isaacs_server/control_drone",
'isaacs_server/ControlDroneAction')
goal = roslibpy.actionlib.Goal(
action_client,
roslibpy.Message({
'id': uid,
"control_task": "land_drone"
}))
goal.on('feedback', lambda f: print(f['progress']))
goal.send()
result = goal.wait(10)
self.assertTrue(result["success"])
# Shutdown Drone
self.shutdown_mavros_drone(publishes, uid)
def reset_simulation(self):
"""
This is only for the simulated environment. It is to reset
the gazebo simulation.
"""
request = roslibpy.ServiceRequest()
self.reset_service.call(request)
def test_upload_mission(self):
# Register Drone
uid = self.register_mavros_drone("upload_mavros")
# Save Topics
publishes = [{"name": "topicNameMavros", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Upload_mission
action_client = roslibpy.actionlib.ActionClient(
client, "isaacs_server/upload_mission",
'isaacs_server/UploadMissionAction')
waypoints = [
navsatfix(-35.362881, 149.165222, 0),
navsatfix(-35.362881, 149.163501, 40)
]
goal = roslibpy.actionlib.Goal(
action_client, roslibpy.Message({
'id': uid,
"waypoints": waypoints
}))
goal.on('feedback', lambda f: print(f['progress']))
goal.send()
result = goal.wait(10)
self.assertTrue(result["success"])
# Shutdown Drone
self.shutdown_mavros_drone(publishes, uid)
def run_service(rosbridge_url: str, service_name: str, service_type: str):
client = roslibpy.Ros(rosbridge_url)
client.run()
service = roslibpy.Service(client, service_name, service_type)
request = roslibpy.ServiceRequest()
_ = service.call(request)
client.terminate()
def get_images(self):
request = roslibpy.ServiceRequest()
#print('Calling service...')
result = self.service.call(request)
#print('Received %d objects' % len(result['resp']['objects']))
i = 0
images = []
for obj in result['resp']['objects']:
#print(i)
point = obj['loc']
rgb_w = obj['rgb']['width']
rgb_h = obj['rgb']['height']
rgb_encoding = obj['rgb']['encoding']
rgb_step = obj['rgb']['step']
rgb_data = obj['rgb']['data'].encode('ascii')
rgb_bytes = base64.b64decode(rgb_data)
rgb_im = np.ndarray(shape=(rgb_h, rgb_w, 3),
dtype=np.uint8,
buffer=rgb_bytes)
rgb = cv2.cvtColor(rgb_im, cv2.COLOR_BGR2RGB)
depth_w = obj['depth']['width']
depth_h = obj['depth']['height']
depth_encoding = obj['depth']['encoding']
depth_step = obj['depth']['step']
depth_data = obj['depth']['data'].encode('ascii')
depth_bytes = base64.b64decode(depth_data)
depth = np.ndarray(shape=(depth_h, depth_w),
dtype=np.float32,
buffer=depth_bytes)
images.append((point, rgb, depth))
i += 1
return images
def test_query_topics_dji(self):
# Register Dji Drone
if not client.is_connected:
client.run()
serverReset()
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': "query_dji",
"drone_type": "DjiMatrice"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
uid = result["id"]
# Save Dji Topics
publishes = [{"name": "topicNameDji", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Query Topics
service = roslibpy.Service(client, 'isaacs_server/query_topics',
'isaacs_server/QueryTopics')
request = roslibpy.ServiceRequest({"id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
self.assertIn({
"name": "topicNameDji",
"type": "topicType"
}, result['all_topics'])
# Shutdown Drone
service = roslibpy.Service(client, 'isaacs_server/shutdown_drone',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Query Topics
service = roslibpy.Service(client, 'isaacs_server/query_topics',
'isaacs_server/QueryTopics')
request = roslibpy.ServiceRequest({"id": uid})
result = wrapped_service_call(service, request)
self.assertFalse(result["success"])
def get_topics(self):
self.ensure_is_connected()
service = roslibpy.Service(self.client, '/rosapi/topics',
'rosapi/Topics')
request = roslibpy.ServiceRequest()
result = service.call(request)
return result
def start_mission(self):
try:
print("Attempting to set to loiter...")
guided_service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/set_mode',
'mavros_msgs/SetMode')
guided_request = roslibpy.ServiceRequest({"custom_mode": "LOITER"})
guided_service.call(guided_request)
print("Attempting to arm...")
arm_service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/cmd/arming',
'mavros_msgs/CommandBool')
arm_request = roslibpy.ServiceRequest({'value': True})
arm_service.call(arm_request)
print("Attempting to takeoff...")
takeoff_service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/cmd/takeoff',
'mavros_msgs/CommandTOL')
takeoff_request = roslibpy.ServiceRequest({'altitude': 3})
takeoff_service.call(takeoff_request)
mission_start_service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/set_mode',
'mavros_msgs/SetMode')
mission_start_request = roslibpy.ServiceRequest(
{"custom_mode": "AUTO"})
print('Calling mission_waypoint_action start service...')
result = mission_start_service.call(mission_start_request)
print('Service response: {}'.format(result))
if result['mode_sent']:
self.prev_flight_status = Drone.Flight_Status.FLYING
result = {"success": True, "message": "Mission starting"}
else:
result = {
"success": False,
"message": "Mission failed to start"
}
except:
result = {"success": False, "message": "Mission failed to start"}
return result
def handler(request, response, done):
cloud_request = roslibpy.ServiceRequest()
def cloud_cb(result):
for key, value in result.items():
response[key] = value
done(True)
cloud_service.call(cloud_request, callback=cloud_cb)
def __call__(self, client, userdata, message):
def callback(message):
result_json = json.dumps(message.data)
self.iot_client.publish(self.topic + "/result", result_json, 1)
message_json = json.loads(message.payload)
rospy.logdebug("Received: " + json.dumps(message_json) + " from AWS")
request = roslibpy.ServiceRequest(message_json)
result = self.service.call(request, callback)
def call_trajectory(exact_callback=None, msg=None):
callback = exact_callback if exact_callback else main_callback
request = roslibpy.ServiceRequest({"trajectory_id": 0})
# self.trajectory_query_service.call(request, self._handle_trajectory(callback), self.error)
if msg:
print(f'before {msg}')
self.trajectory_query_service.call(request, callback, self.error)
if msg:
print(f'after {msg}')
def call_time(exact_callback=None):
callback = exact_callback if exact_callback else main_callback
def time_wrapper(msg):
resp = msg['timestamp']
callback(resp)
request = roslibpy.ServiceRequest(dict())
self.time_service.call(request, time_wrapper, self.error)
def get_position(self):
"""Get the actual position of the model (This case is hard wired to the person).
Returns:
Dictionary: with coordinates of the object in the simulation
"""
body = {'model_name': self.PERSON_MODEL}
request = roslibpy.ServiceRequest(body)
return self.get_position_service.call(request)['pose']
def led(self):
self.ensure_is_connected()
service = roslibpy.Service(
self.client, f'/{self.rover_id}/led_emitter_node/set_pattern',
'std_msgs/String')
#request = roslibpy.ServiceRequest("pattern_name: {data: RED}")
request = roslibpy.ServiceRequest({'data': 'RED'})
result = service.call(request)
return result
def register_DJI_drone(self, drone_name):
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': drone_name,
"drone_type": "DjiMatrice"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
uid = result["id"]
return uid
def register_drone(self, client, drone_name):
service = roslibpy.Service(client, '/isaacs_server/register_drone',
'isaacs_server/register_drone')
request = roslibpy.ServiceRequest({
'drone_name': drone_name,
'drone_type': 'Mavros'
})
result = service.call(request)
rospy.loginfo(result)
return result
def receive_resource_file(self, local_filename, resource_file_uri):
def write_binary_response_to_file(response):
LOGGER.info("Saving %s to %s" % (resource_file_uri, local_filename))
self.write_file(local_filename, binascii.a2b_base64(response.data['value']), 'wb')
#self.write_file(local_filename, response.data['value'])
self.update_file_request_status(resource_file_uri)
service = roslibpy.Service(self.ros, "/file_server/get_file",
"file_server/GetBinaryFile")
service.call(roslibpy.ServiceRequest({'name': resource_file_uri}),
write_binary_response_to_file, errback=None)
def test_register_drone(self):
# Register Drone
if not client.is_connected:
client.run()
serverReset()
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': "register_mavros",
"drone_type": "Mavros"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
def register_mavros_drone(self, drone_name):
if not client.is_connected:
client.run()
serverReset()
service = roslibpy.Service(client, 'isaacs_server/register_drone',
'isaacs_server/RegisterDrone')
request = roslibpy.ServiceRequest({
'drone_name': drone_name,
"drone_type": "Mavros"
})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
uid = result["id"]
return uid
def save_topics_to_drone(client, drone_id, drone_topics):
"""Saves the drone's topics with the server.
"""
save_topics_service = roslibpy.Service(client,
'isaacs_server/save_drone_topics',
'isaacs_server/SaveDroneTopics')
save_topics_request = roslibpy.ServiceRequest({
'id': drone_id,
'publishes': drone_topics
})
print('Calling topics_service...')
result = save_topics_service.call(save_topics_request)
print(f'Topics_service response{result}')
print()
def upload_mission(self, waypoints):
self.waypoints = waypoints
# Converts all the NavSatFix messages to Waypoint so that
# they're MAVROS compatible
converted_waypoint_objects = []
for navsatfix in waypoints:
converted_waypoint_objects.append(
self.convert_navsatfix_mavroswaypoint(navsatfix))
# Two takeoff commands prepended to the waypoint list for safety
# converted_waypoint_objects = 2 * [
# {'frame': MavrosDrone.FRAME_REFERENCE.RELATIVE_ALT.value,
# 'command': MavrosDrone.MAV_CMD.TAKEOFF.value, 'is_current': False,
# 'autocontinue': True, 'param1': 0, 'param2': 0, 'param3': 0,
# 'x_lat': self.position['latitude'],
# 'y_long': self.position['longitude'],
# 'z_alt': 10}
# ] + converted_waypoint_objects
print(converted_waypoint_objects)
try:
print("Attempting to upload mission...")
service = roslibpy.Service(
self.ROS_master_connection,
self.drone_namespace + '/mavros/mission/push',
'mavros_msgs/WaypointPush')
request = roslibpy.ServiceRequest(
{'waypoints': converted_waypoint_objects})
print('Calling /mavros/mission/push service...')
result = service.call(request)
print('Service response: {}'.format(result))
if result['success']:
result = {"success": True, "message": "Mission uploaded"}
else:
result = {
"success": False,
"message": "Mission failed to uploaded"
}
except:
result = {
"success": False,
"message": "Failed to upload waypoints"
}
return result
def load_meshes(self, url):
"""Load meshes from the given URL in the ROS file server.
A single mesh url can contain multiple meshes depending on the format.
Parameters
----------
url : str
Mesh URL
Returns
-------
list[:class:`compas.datastructures.Mesh`]
List of meshes.
"""
use_local_file = False
file_extension = _get_file_format(url)
if self.local_cache_enabled:
local_filename = self._local_mesh_filename(url)
use_local_file = _cache_file_exists(local_filename)
else:
_, local_filename = tempfile.mkstemp(suffix='.' + file_extension, prefix='ros_fileserver_')
if not use_local_file:
service = roslibpy.Service(self.ros, '/file_server/get_file', 'file_server/GetBinaryFile')
request = roslibpy.ServiceRequest(dict(name=url))
response = service.call(request, timeout=TIMEOUT)
file_content = binascii.a2b_base64(response.data['value'])
# Just look away, we're about to do something nasty!
# namespaces are handled differently between the CLI and CPython
# XML parsers, so, we just get rid of it for DAE files
if file_extension == 'dae':
file_content = file_content.replace(b'xmlns="http://www.collada.org/2005/11/COLLADASchema"', b'')
file_content = file_content.replace(b'xmlns="https://www.collada.org/2005/11/COLLADASchema"', b'')
# compas.files does not support file-like objects so we need to
# save the file to disk always. If local caching is enabled,
# we store it in the cache folder, otherwise, as a temp file.
_write_file(local_filename, file_content, 'wb')
else:
# Nothing to do here, the file will be read by the mesh importer
LOGGER.debug('Loading mesh file %s from local cache dir', local_filename)
return _fileserver_mesh_import(url, local_filename, self.precision)
def upload_waypoint_task(self, task):
try:
print("Attempting to upload waypoint task...")
# fake_mission_waypoint_upload found in srv folder. Copied directly from DJI SDK for local testing.
# service = roslibpy.Service(self.ROS_master_connection, 'dji_sdk/mission_waypoint_upload', 'dji_sdk/MissionWpUpload')
service = roslibpy.Service(self.ROS_master_connection, 'isaacs_server/fake_mission_waypoint_upload',
'isaacs_server/FakeWaypointMissionUpload')
request = roslibpy.ServiceRequest({"waypoint_task": task})
print('Calling mission_waypoint_upload service...')
result = service.call(request)
if result["result"]:
result = {"success":True, "message":"Upload mission successful"}
print('Service response: {}'.format(result))
except:
result = {"success":False, "message":"Upload mission failed"}
return result
def get_drone_id(self, drone_name):
"""Call the all_drones_available service to find the drone ID.
The all_drones_available service's callback asynchronously calls
set_drone_id.
Arguments:
drone_name (str): The name of the drone of interest.
"""
service = roslibpy.Service(self.client,
'isaacs_server/all_drones_available',
'isaacs_server/AllDronesAvailable')
request = roslibpy.ServiceRequest({})
print('Calling all_drones_available service...')
result = service.call(
request, callback=lambda r: self.set_drone_id(r, drone_name))
print('Service response: {}'.format(result))
def set_position(self, y_amount_move):
"""Set the new position for a model in the simulation (This case is hard wired to the person).
Args:
y_amount_move (Float): The amount to move from the original position in the y-axis
NOTE: We can expand this to the other axis
"""
body = {'model_state': {'model_name': self.PERSON_MODEL}}
actual_position = self.get_position()
actual_position['position']['y'] += y_amount_move
# actual_position['position']['x'] += y_amount_move
body['model_state']['pose'] = {
'position': actual_position['position'],
'orientation': actual_position['orientation']
}
request = roslibpy.ServiceRequest(body)
self.set_position_service.call(request)
