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 __init__(self, client=None):
"""Constructor of the class
Args:
client (WebSocket): Contains the connection with the ROS server
"""
self.set_position_service = roslibpy.Service(
client, '/gazebo/set_model_state', 'gazebo_msgs/SetModelState')
self.get_position_service = roslibpy.Service(
client, '/gazebo/get_model_state', 'gazebo_msgs/GetModelState')
def __init__(self, session_name=''):
self.client = roslibpy.Ros(host=settings.ROS_HOST,
port=settings.ROS_PORT)
self.client.run()
self.session_name = session_name
self.srv_status = roslibpy.Service(self.client, '/rosout/get_loggers',
'roscpp/GetLoggers')
self.srv_path = roslibpy.Service(self.client,
'/deepbuilder/robot/plan_path',
'/deepbuilder/ro_plan_path')
self.srv_update_state_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_state_mesh',
'/deepbuilder/ro_update_state_mesh')
self.srv_update_compressed_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_compressed_mesh',
'/deepbuilder/ro_update_compressed_mesh')
self.srv_get_tags = roslibpy.Service(self.client,
'/deepbuilder/sensing/get_tags',
'/deepbuilder/se_get_tags')
self.srv_collect_state = roslibpy.Service(
self.client, '/deepbuilder/sensing/collect_state',
'/deepbuilder/se_collect_state')
#self.srv_get_joint_states = roslibpy.Service(self.client, '/deepbuilder/robot/get_robot_state', '/deepbuilder/ro_get_robot_state')
self.srv_move_path = roslibpy.Service(self.client,
'/deepbuilder/robot/move_path',
'/deepbuilder/ro_move_path')
self.srv_print_path = roslibpy.Service(
self.client, '/deepbuilder/robot/print_path',
'/deepbuilder/ro_print_path')
def reset_client(self):
self.client.close()
time.sleep(0.5)
self.client.connect()
time.sleep(0.5)
self.srv_status = roslibpy.Service(self.client, '/rosout/get_loggers',
'roscpp/GetLoggers')
self.srv_path = roslibpy.Service(self.client,
'/deepbuilder/robot/plan_path',
'/deepbuilder/ro_plan_path')
self.srv_update_state_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_state_mesh',
'/deepbuilder/ro_update_state_mesh')
self.srv_update_compressed_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_compressed_mesh',
'/deepbuilder/ro_update_compressed_mesh')
self.srv_get_tags = roslibpy.Service(self.client,
'/deepbuilder/sensing/get_tags',
'/deepbuilder/se_get_tags')
self.srv_collect_state = roslibpy.Service(
self.client, '/deepbuilder/sensing/collect_state',
'/deepbuilder/se_collect_state')
#self.srv_get_joint_states = roslibpy.Service(self.client, '/deepbuilder/robot/get_robot_state', '/deepbuilder/ro_get_robot_state')
self.srv_move_path = roslibpy.Service(self.client,
'/deepbuilder/robot/move_path',
'/deepbuilder/ro_move_path')
self.srv_print_path = roslibpy.Service(
self.client, '/deepbuilder/robot/print_path',
'/deepbuilder/ro_print_path')
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']))
async def setup_rosbridge(self) : # connect to rosbridge self.rosbridge_client = roslibpy.Ros(host=self.rosbridge_address, port=self.rosbridge_port) await self.attempt(self.rosbridge_client.run, check=(lambda x : self.rosbridge_client.is_connected), sync=True, timeout=1) # create client for frame service self.frame_service = roslibpy.Service(self.rosbridge_client, self.frame_req_service_name, self.frame_req_msg_type) # subscribe to frame topic self.frame_subscriber = roslibpy.Topic(self.rosbridge_client, self.frame_resp_topic_name, self.frame_resp_msg_type) self.frame_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for storage goal topic self.storage_publisher = roslibpy.Topic(self.rosbridge_client, self.storage_req_topic_name, self.storage_req_msg_type) self.storage_publisher.advertise() # subscribe to storage result topic self.storage_subscriber = roslibpy.Topic(self.rosbridge_client, self.storage_resp_topic_name, self.storage_resp_msg_type) self.storage_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for registration goal topic self.registration_publisher = roslibpy.Topic(self.rosbridge_client, self.registration_req_topic_name, self.registration_req_msg_type) self.registration_publisher.advertise() # subscribe to registration result topic self.registration_subscriber = roslibpy.Topic(self.rosbridge_client, self.registration_resp_topic_name, self.registration_resp_msg_type) self.registration_subscriber.subscribe(lambda msg: self.subscriber_callback(msg))
def custom_service(handler):
"""
This method is designed to be used as a decorator (@custom_service)
to advertise the handler method as a service via the ROS_master_connection.
By default, the service can be found at `/isaacs_server/[handler_name]`
with a service type of `isaacs_server/[handler_name]`.
Exceptions for the handler name to service type mapping can be added
to the exceptions dictionary.
parameter: handler(request, response) handles an incoming service request.
returns: handler
"""
exceptions = {
'save_drone_topics': 'isaacs_server/TypeToTopic',
'save_sensor_topics': 'isaacs_server/TypeToTopic',
'shutdown_drone': 'isaacs_server/TypeToTopic',
'shutdown_sensor': 'isaacs_server/TypeToTopic'
}
if handler.__name__ in exceptions:
serv_type = exceptions[handler.__name__]
else:
serv_type = f'isaacs_server/{to_camel_case(handler.__name__)}'
service = roslibpy.Service(ROS_master_connection,
f'/isaacs_server/{handler.__name__}', serv_type)
print(service.name)
service.advertise(handler)
services.append(service)
return handler
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 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 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 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 __init__(self):
self.client = roslibpy.Ros(host='localhost', port=9090)
self.client.run()
self.service = roslibpy.Service(self.client, 'get_images', 'exp_demo/ObjectArray')
self.publisher = roslibpy.Topic(self.client, '/indicate', 'geometry_msgs/PointStamped')
self.publisher.advertise()
def register_service(obj, topic, msg_type, method_name):
service = roslibpy.Service(obj.ros_client, topic, msg_type)
def fn():
request = roslibpy.ServiceRequest()
return service.call(request)
setattr(obj, method_name, fn)
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 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 advertise_service(self,
service_name,
service_type,
handler,
include_namespace=False):
if include_namespace:
service_name = self.namespace + service_name
service = roslibpy.Service(self.connection, service_name, service_type)
service.advertise(handler)
self.services.append(service)
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_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 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 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 __init__(self, client):
# Connect with ROS
self.client = client
# ROS topic to control the system
self.camera_scan = roslibpy.Topic(self.client, 'detect_image',
'std_msgs/String')
self.robot_move = roslibpy.Topic(self.client, 'target_pose',
'geometry_msgs/Pose')
self.gripper_grasp = roslibpy.Topic(self.client, 'gripper_grasping',
'std_msgs/String')
self.update_planning = roslibpy.Topic(self.client, 'add_box_ur5',
'std_msgs/String')
self.camera_scan.advertise()
self.robot_move.advertise()
self.gripper_grasp.advertise()
self.update_planning.advertise()
# ROS services to control Gazebo environemnt
self.dir_path = rospack.get_path('semantic_web') + "/simulation/assembly_samples"
self.shape = ["Triangle", "Rectangle", "Pentagon"]
self.spawn_srv = roslibpy.Service(self.client,
'/gazebo/spawn_sdf_model',
'gazebo_msgs/SpawnModel')
self.delete_srv = roslibpy.Service(self.client,
'/gazebo/delete_model',
'gazebo_msgs/DeleteModel')
# Temporary data
self.task = OrderedDict()
self.gazebo_object_names = []
if KnowledgeBase.get_property("Kinect", "Data") != "":
initial_gazebo_names = eval(KnowledgeBase.get_property("Kinect",
"Data"))
self.gazebo_object_names = initial_gazebo_names
# Control Gazebo spawn/delete models
self.gazebo = GazeboSyncing()
def register_time_service(self, main_callback=None):
self.time_service = roslibpy.Service(self.cli, '/read_metrics', '/cartographer_ros_msgs/ReadMetrics')
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)
return call_time
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 register_trajectory_service(self, main_callback=None):
self.trajectory_query_service = roslibpy.Service(self.cli, '/trajectory_query',
'/cartographer_ros_msgs/TrajectoryQuery')
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}')
return call_trajectory
def get_service():
import roslibpy
client = roslibpy.Ros(host='192.168.1.189', port=9090)
client.run()
service = roslibpy.Service(client, 'get_telemetry', 'clever/GetTelemetry')
request = {'frame_id': 'aruco_map'}
print('Calling service...')
result = service.call(request)
client.terminate()
return result
