def control_gripper(command): """ Function to open the end effector aka the gripper or closing it according to the command given. If command == 1 opens gripper, if command == -1 closes gripper. """ # where is the gripper defined? grip = SetBool() if command == 1: # opens gripper grip = False rospy.loginfo(color.BOLD + color.YELLOW + '-- OPEN GRIPPER --' + color.END) #print('OK') else: # closes gripper grip = True rospy.loginfo(color.BOLD + color.YELLOW + '-- CLOSE GRIPPER --' + color.END) #print('NOK') rospy.wait_for_service('/gripper/grasp') try: grip_service = rospy.ServiceProxy('/gripper/grasp', SetBool) response = grip_service(grip) grip_service.close() return response except rospy.ServiceException: rospy.logerror('Service call failed')
def __init__(self): """Init manual_command node""" super().__init__("manual_command") self.max_vel_speed = 0.3 self.max_dir_speed = 1.0 serial_port = "/dev/ttyUSB0" self.deadzone = 15 self.middle_value = 126.5 self.msg = Twist() self.cmd_pb = self.create_publisher(Twist, "/asterix/cmd_vel", 10) self.get_enable_drivers_request = SetBool.Request() self.get_enable_drivers_request.data = True self.get_enable_drivers_client = self.create_client( SetBool, "/asterix/enable_drivers") self.get_logger().info( "manual_command node is calling drive for motor arming...") while not self.get_enable_drivers_client.wait_for_service( timeout_sec=1.0): self.get_logger().info("service not available, waiting again...") self.get_enable_drivers_client.call_async( self.get_enable_drivers_request) self.ser = Serial(port=serial_port, baudrate=115200) self.get_logger().info("manual_command node is ready") self.start_loop()
def ReqPontos(): """ Pede as informaçoes das bases que ainda nao foram processadas Retorno: @return: lista com as posiçoes de todos as bases nao processadas dadas em [x,y,z] """ rospy.wait_for_service('get_object') a = rospy.ServiceProxy('get_object', GetObject) requ = GetObject._request_class() requ.identifier.state.data = Identifier.STATE_NOPROCESSADO requ.identifier.type.data = Identifier.TYPE_BASE response = a(requ) lista = response.list rospy.wait_for_service('/ger_drone/set_atualiza_mapa') proxy = rospy.ServiceProxy('/ger_drone/set_atualiza_mapa', SetBool) req = SetBool._request_class() req.data = True proxy(req) listaPontos = [] for i in lista: x = i.pose.position.x y = i.pose.position.y z = i.pose.position.z listaPontos.append([x,y,z]) return(listaPontos)
def getSensor(): """! Requisita um objeto que contém quais foram os sensores vemelhos localizados e a sua posicao Adiciona as poses desses sensores em lista """ print('entrou') rospy.wait_for_service('get_object') a = rospy.ServiceProxy('get_object', GetObject) reqf = GetObject._request_class() reqf.identifier.type.data = Identifier.TYPE_SENSOR_VERMELHO response = a(reqf) lista = response.list print(len(lista)) rospy.wait_for_service('/ger_drone/set_atualiza_mapa') proxy = rospy.ServiceProxy('/ger_drone/set_atualiza_mapa', SetBool) req = SetBool._request_class() req.data = False proxy(req) voarSensor(lista)
def call_led_service(self): request = SetBool.Request() if self.radio_button_led_on.isChecked(): request.data = True elif self.radio_button_led_off.isChecked(): request.data = False wait_count = 1 while not self.service_client.wait_for_service(timeout_sec=0.5): if wait_count > 5: return self.node.get_logger().error( 'Service not available #{0}'.format(wait_count)) wait_count += 1 future = self.service_client.call_async(request) while rclpy.ok(): if future.done(): if future.result() is not None: response = future.result() self.node.get_logger().info( 'Result of service call: {0}'.format(response.message)) else: self.node.get_logger().error('Error calling service') break
def ReqPontos(): """! Requisita os pontos das bases ao mapa Retorno: @return Listas de Object com as bases """ rospy.wait_for_service('get_object') a = rospy.ServiceProxy('get_object', GetObject) requ = GetObject._request_class() requ.identifier.state.data = Identifier.STATE_NOPROCESSADO #mudar para processado quando visitar requ.identifier.type.data = Identifier.TYPE_BASE response = a(requ) lista = response.list #print(lista) #lista[i].identifier.data == "A" rospy.wait_for_service('/ger_drone/set_atualiza_mapa') proxy = rospy.ServiceProxy('/ger_drone/set_atualiza_mapa', SetBool) req = SetBool._request_class() req.data = True proxy(req) return(lista)
def client_call_cli1(self): # Wait for service while not self.client_cli1.wait_for_service(timeout_sec=1.0): self.get_logger().info('service not available, waiting again...') # Create request and fill it with data self.request_cli1 = SetBool.Request() # The service is type std_srvs/SetBool # Remember to store data in the attributes of self.request_cli1 self.future_cli1 = self.client_cli1.call_async(self.request_cli1)
class drone: frame = None event = Event() bridge = CvBridge() subscription_image = None subscritpion_control = None drone_id = None status = None node = None cli = None req = SetBool.Request() def __init__(self,drone_id,node): self.node = node self.cli = node.create_client(SetBool, drone_id + '_enc/quality_info') self.drone_id = drone_id self.status = False def get_frame(self): self.event.wait() self.event.clear() return self.frame def changeQuality(self,quality): self.subscritpion_control.publish(quality) def start(self): self.subscription = node.create_subscription(Image, self.drone_id + "_dec/decoded", self.on_image, 100) self.subscritpion_control = node.create_publisher(String, self.drone_id + "_enc/control", 10) self.status = True def stop(self): node.destroy_subscription(self.subscription) node.destroy_publisher(self.subscritpion_control) self.status = False def on_image(self,msg): try: #print(msg.header) image = self.bridge.imgmsg_to_cv2(msg, "bgr8") ret, jpeg = cv2.imencode('.jpg', image) self.frame = jpeg.tobytes() self.event.set() except: print("Lost Frames") def getQuality(self): future = self.cli.call(self.req) return future.message
def __init__(self, dvl_on_off_service_name, running, cooldown): super(A_SetDVLRunning, self).__init__(name="A_SetDVLRunning") self.switcher_service = rospy.ServiceProxy(dvl_on_off_service_name, SetBool) self.bb = pt.blackboard.Blackboard() self.sb = SetBool() self.sb.data = running self.running = running self.last_toggle = 0 self.cooldown = cooldown self.service_name = dvl_on_off_service_name
def __init__(self): super().__init__('ControlPinces') self.subscriber_postion_ok = self.create_subscription(Bool,'near_ball',self.callback_initialize,1) self.subscriber_relache_ok = self.create_subscription(Bool,'relache',self.callback_relache,1) self.publisher_pinces_commande = self.create_publisher(Twist, 'cmd_pince', 10) self.publisher_roues_commande = self.create_publisher(Twist, 'cmd_roues', 10) self.publisher_in_pince = self.create_publisher(Bool,'in_pince',10) self.publisher_out_pince = self.create_publisher(Bool,'out_pince',10) self.get_logger().info('Initialisation complete') self.take_ball = True self.relache_ball = True self.cli = self.create_client(SetBool, '/switch') self.req = SetBool.Request() self.req.data = False self.cli.call_async(self.req)
def getListObject(): """! Solicita para o Mapa as coordenadas de cada base encontrada e que nao foi visitada ainda """ rospy.wait_for_service('get_object') cinco = rospy.ServiceProxy('get_object', GetObject) reqc = GetObject._request_class() reqc.identifier.state.data = Identifier.STATE_NOPROCESSADO reqc.identifier.type.data = Identifier.TYPE_BASE response = cinco(reqc) lista = response.list rospy.wait_for_service('/ger_drone/set_atualiza_mapa') proxy = rospy.ServiceProxy('/ger_drone/set_atualiza_mapa', SetBool) req = SetBool._request_class() req.data = False proxy(req) getPose(lista)
def __init__(self): super().__init__(node_name="cetautomatix") # Detect simulation mode self.simulation = True if machine() != "aarch64" else False self.i = 0 self.stupid_actions = ["STUPID_1", "STUPID_2", "STUPID_3"] self._triggered = False self._position = None self._orientation = None self._start_time = None self._current_action = None self.robot = self.get_namespace().strip("/") # parameters interfaces self.side = self.declare_parameter("side", "blue") self.declare_parameter("length") self.declare_parameter("width") self.declare_parameter("strategy_mode") self.length_param = self.get_parameter("length") self.width_param = self.get_parameter("width") self.strategy_mode_param = self.get_parameter("strategy_mode") # Bind actuators self.actuators = import_module(f"actuators.{self.robot}").actuators # Do selftest self.selftest = Selftest(self) # Prechill engines self.actuators.setFansEnabled(True) # Stop ros service self._stop_ros_service = self.create_service(Trigger, "stop", self._stop_robot_callback) # strategix client interfaces self._get_available_request = GetAvailableActions.Request() self._get_available_request.sender = self.robot self._get_available_client = self.create_client( GetAvailableActions, "/strategix/available") self._change_action_status_request = ChangeActionStatus.Request() self._change_action_status_request.sender = self.robot self._change_action_status_client = self.create_client( ChangeActionStatus, "/strategix/action") # Phararon delploy client interfaces self._get_trigger_deploy_pharaon_request = Trigger.Request() self._get_trigger_deploy_pharaon_client = self.create_client( Trigger, "/pharaon/deploy") # Slider driver self._set_slider_position_request = Slider.Request() self._set_slider_position_client = self.create_client( Slider, "slider_position") # Odometry subscriber self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self) self._odom_pose_stamped = tf2_geometry_msgs.PoseStamped() self._odom_callback(self._odom_pose_stamped) self._odom_sub = self.create_subscription(Odometry, "odom", self._odom_callback, 1) # Py-Trees blackboard to send NavigateToPose actions self.blackboard = py_trees.blackboard.Client(name="NavigateToPose") self.blackboard.register_key(key="goal", access=py_trees.common.Access.WRITE) # LCD driver direct access self._lcd_driver_pub = self.create_publisher(Lcd, "/obelix/lcd", 1) # Wait for strategix as this can block the behavior Tree while not self._get_available_client.wait_for_service(timeout_sec=5): self.get_logger().warn( "Failed to contact strategix services ! Has it been started ?") # Enable stepper drivers if not self.simulation: self._get_enable_drivers_request = SetBool.Request() self._get_enable_drivers_request.data = True self._get_enable_drivers_client = self.create_client( SetBool, "enable_drivers") self._synchronous_call(self._get_enable_drivers_client, self._get_enable_drivers_request) # Robot trigger service self._trigger_start_robot_server = self.create_service( Trigger, "start", self._start_robot_callback) if self.robot == "obelix": self._trigger_start_asterix_request = Trigger.Request() self._trigger_start_asterix_client = self.create_client( Trigger, "/asterix/start") # Reached initialized state self.get_logger().info("Cetautomatix ROS node has been started")
def set_autonomous(req): is_autonomous = req.data rospy.loginfo("is_autonomous is set to %r" % is_autonomous) resp = SetBool() resp.success = True return resp
def __init__(self): super().__init__(node_name="cetautomatix") # Detect simulation mode self.simulation = True if machine() != "aarch64" else False self.name = self.get_namespace().strip("/") # Declare parameters self.triggered = False self.declare_parameter("length") self.declare_parameter("width") self.declare_parameter("strategy_mode") self.length = self.get_parameter("length") self.width = self.get_parameter("width") self.strategy_mode = self.get_parameter("strategy_mode") # Bind actuators self.actuators = import_module(f"actuators.{self.name}").actuators self.actuators.robot_node = self # Do selftest self.selftest = Selftest(self) # Prechill engines # self.actuators.setFansEnabled(True) # Create empty action queue self.action_list = [] self.current_action = None # Stop ROS service self.stop_ros_service = self.create_service(Trigger, "stop", self.stop_robot_callback) # Strategix client to get the side self.get_side_request = SetBool.Request() self.get_side_request.data = True self.get_side_client = self.create_client(SetBool, "/strategix/side") # Strategix client to get available actions self.get_available_actions_request = GetAvailableActions.Request() self.get_available_actions_request.sender = self.name self.get_available_actions_client = self.create_client( GetAvailableActions, "/strategix/available") # Strategix client to change the status of an action self.change_action_status_request = ChangeActionStatus.Request() self.change_action_status_request.sender = self.name self.change_action_status_client = self.create_client( ChangeActionStatus, "/strategix/action") # Odometry subscriber self.odom_sub = self.create_subscription( PoseStamped, "odom_map_relative", self.odom_callback, 10, ) # Py-Trees blackboard to send NavigateToPose actions self.blackboard = py_trees.blackboard.Client(name="NavigateToPose") self.blackboard.register_key(key="goal", access=py_trees.common.Access.WRITE) # LCD driver direct access self.lcd_driver_pub = self.create_publisher(Lcd, "/obelix/lcd", 1) # Wait for Strategix as this can block the whole Behaviour Tree while not self.get_available_actions_client.wait_for_service( timeout_sec=5): self.get_logger().warn( "Failed to contact strategix services ! Has it been started ?") # Enable stepper drivers if not self.simulation: self.get_enable_drivers_request = SetBool.Request() self.get_enable_drivers_request.data = True self.get_enable_drivers_client = self.create_client( SetBool, "enable_drivers") self.synchronous_call(self.get_enable_drivers_client, self.get_enable_drivers_request) # Pharaon trigger client self.trigger_deploy_pharaon_request = Trigger.Request() self.trigger_deploy_pharaon_client = self.create_client( Trigger, "/pharaon/deploy") # Robot start trigger service self.trigger_start_robot_server = self.create_service( Trigger, "start", self.start_robot_callback) if self.name == "obelix": self.trigger_start_asterix_request = Trigger.Request() self.trigger_start_asterix_client = self.create_client( Trigger, "/asterix/start") # Reached initialized state self.get_logger().info("Cetautomatix ROS node has been started")
srv.goal = [drone[0],drone[1], drone[2], yaw] yaw_srv(srv) if __name__ == "__main__": rospy.init_node('experiment_preparation', anonymous=True) if 'UAV_NAME' in os.environ: uav_id =os.environ['UAV_NAME'] else: uav_id = "uav" init_planning = SetBool() planning_2 = rospy.ServiceProxy('/uav2/formation_church_planning/toggle_state', SetBool) planning_3 = rospy.ServiceProxy('/uav3/formation_church_planning/toggle_state', SetBool) start_vision = rospy.ServiceProxy('/'+uav_id+'/balloon_filter/start_estimation',StartEstimation) desired_pose = rospy.ServiceProxy('/'+uav_id+'/action',ShootingAction) rospy.Subscriber("/gazebo/dynamic_model/jeff_electrician/odometry", Odometry, callback) rospy.Subscriber('/'+uav_id+'/odometry/odom_main', Odometry, callback_drone_pose) yaw_srv = rospy.ServiceProxy('/'+uav_id+'/control_manager/goto',Vec4) key = raw_input("press a key to start estimation") start_estimation_srv = StartEstimationRequest() start_estimation_srv.radius = 50 start_vision(start_estimation_srv) raw_input("Establish -30 0 2")
def _motor_request(self, request_data=False): request = SetBool.Request() request.data = request_data self._client_motor_power.call_async(request)
def __init__(self): super().__init__('minimal_client_async') self.cli = self.create_client(SetBool, 'open_gripper') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('service not available, waiting again...') self.req = SetBool.Request()
def set_autonomous(req): is_autonomous = req.data rospy.loginfo("is_autonomous is set to %r" % is_autonomous) resp = SetBool() resp.success = True return resp
def perform(self, reevaluate=False): if not self.blackboard.visualization_active and not self.blackboard.simulation_active: req = SetBool.Request() req.data = False self.blackboard.motor_switch_service.call(req) return self.pop()