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)