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()