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