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 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 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): 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 __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 __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")
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 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()