def spawnGenEnvironment(req):
initial_pose = Pose()
initial_pose.position.x = 0
initial_pose.position.y = 0
initial_pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
initial_pose.orientation.x = quaternion[0]
initial_pose.orientation.y = quaternion[1]
initial_pose.orientation.z = quaternion[2]
initial_pose.orientation.w = quaternion[3]
f = open(
'/home/knmcguire/Software/catkin_ws/src/indoor_environment_generator/models/random_generated_environment/test_model.sdf',
'r')
sdff = f.read()
rospy.wait_for_service('gazebo/spawn_sdf_model')
spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
spawn_model_prox("some_robo_name", sdff, "robotos_name_space",
initial_pose, "world")
msg = Trigger()
msg.succes = True
msg.message = "jeej"
return []
def Run(self):
try:
xPosRate = self._faceDetection.Control(self._face)
if self._on:
if not self._isOn:
self._future = self._srvClientOn.call_async(
Trigger.Request())
self._isOn = True
if self._run:
if not self._isRun:
self._wallAround.Start()
self._isRun = True
else:
self._wallAround.Run()
elif self._face:
self._faceToFace.Rotate(xPosRate)
self._isRun = False
else:
self._wallAround.SetVelocity(self._forward, self._rotation)
self._isRun = False
else:
if self._isOn:
self._wallAround.SetVelocity(0.0, 0.0)
self._future = self._srvClientOff.call_async(
Trigger.Request())
self._isOn = False
self._isRun = False
except Exception as e:
print(e)
def _toggle(req):
global _ctrl_on
_ctrl_on = not _ctrl_on
srv = Trigger()
srv.success = True
srv.message = "Controller is {}".format(_ctrl_on)
return TriggerResponse(True, "Controller is {}".format(_ctrl_on))
def _toggle(req):
global _ctrl_on
_ctrl_on = not _ctrl_on
srv = Trigger()
srv.success = True
srv.message = "Controller is {}".format(_ctrl_on)
return TriggerResponse(True, "Controller is {}".format(_ctrl_on))
def callback(self, message):
self.latest_message = message
self.detection_dict = self.four_sector_detection()
crashed, direction = self.has_crashed():
if crashed:
trigger = Trigger()
trigger.success = False
trigger.message = direction
self.crash_publisher.publish(trigger)
def reset(self):
print("reset simulation")
# future = self.reset_srv.call_async(Empty.Request())
self.ball_hit_ground = False
self.ball_hit_location = None
self.ball_xyz = self.prev_ball_xyz = None
self.upper_link_xyz = None
req = Trigger.Request()
future = self.atach_srv.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# import pdb; pdb.set_trace()
self.ball_detached = False
req = Empty.Request()
future = self.unpause_srv.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# rclpy.spin_until_future_complete(self.node, future)
# self.spawn_srv = self.node.create_client(SpawnModel, '/gazebo/spawn_sdf_model')
# self.delete_srv = self.node.create_client(DeleteModel, '/gazebo/delete_model')
# self.reset_srv = self.node.create_client(Empty, "/gazebo/reset_simulation")
# self.com_srv = self.node.create_client(GetLinkState, "/gazebo/get_link_state")
# self.link_states_sub = self.node.create_subscription(LinkStates, "/gazebo/link_states", self.link_states_cb)
# self.joint_states_sub = self.node.create_subscription(JointState, "/joint_states", self.joint_states_cb)
# self.step_srv = self.node.create_client(Trigger, "/roboy/simulation/step")
# self.detach_srv = self.node.create_client(Trigger, "/roboy/simulation/joint/detach")
# self.command_pub = self.node.create_publisher(MotorCommand, "/roboy/middleware/MotorCommand")
# self.step_pub = self.node.create_publisher(Int32, "/roboy/simulation/step")
#
# self.detach_pub = self.node.create_publisher(Bool, "/roboy/simulation/joint/detach")
# rclpy.spin_until_future_complete(self.node, future)
print("done")
def get_version_map(self):
"""Get the gait version map used in the gait selection node."""
try:
self.wait_for_service(self._get_version_map)
return dict(
ast.literal_eval(
self._get_version_map.call(Trigger.Request()).message))
except ValueError:
raise InvalidResponseError
def get_current_gait_directory(self):
"""Get the gait directory used by the gait selection node,
to allow updating the default version."""
gait_dir_client = self._node.create_client(
srv_type=Trigger,
srv_name="/march/gait_selection/get_gait_directory")
self.wait_for_service(gait_dir_client)
gait_directory = gait_dir_client.call(Trigger.Request()).message
gait_dir_client.destroy()
return gait_directory # noqa: R504
def get_directory_structure(self):
"""Get the gait directory of the selected gait_directory in the gait selection node."""
try:
self.wait_for_service(self._get_directory_structure)
return dict(
ast.literal_eval(
self._get_directory_structure.call(
Trigger.Request()).message))
except ValueError:
raise InvalidResponseError
def pousar():
"""!
Pousa o drone na posicao atual
"""
print('P')
rospy.wait_for_service('/uav1/uav_manager/land')
um = rospy.ServiceProxy('/uav1/uav_manager/land', Trigger)
reqa = Trigger._request_class()
um(reqa)
rospy.sleep(1)
def do_simulation(self, action):
# print("Action: ")
# print(action)
self.motor_command.set_points = [(x + 2.5) * 100.0 for x in action[:4]]
self.command_pub.publish(self.motor_command)
if (not self.ball_detached and action[-1] > 0.9):
# import pdb; pdb.set_trace()
print("RELEASED")
# msg = Bool()
# msg.data = True
# self.detach_pub.publish(msg)
future = self.detach_srv.call_async(Trigger.Request())
rclpy.spin_until_future_complete(self.node, future)
self.ball_detached = True
# msg = Int32()
# msg.data = 100
# self.step_pub.publish(msg)
future = self.step_srv.call_async(Trigger.Request())
rclpy.spin_until_future_complete(self.node, future)
def _create_services(self) -> None:
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_version_map",
callback=lambda req, res: Trigger.Response(
success=True, message=str(self.gait_version_map)),
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_gait_directory",
callback=lambda req, res: Trigger.Response(
success=True, message=self._directory_name),
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_default_dict",
callback=self.get_default_dict_cb,
)
self.create_service(
srv_type=SetGaitVersion,
srv_name="/march/gait_selection/set_gait_version",
callback=self.set_gait_versions_cb,
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_directory_structure",
callback=lambda req, res: Trigger.Response(
success=True, message=str(self.scan_directory())),
)
self.create_service(
srv_type=ContainsGait,
srv_name="/march/gait_selection/contains_gait",
callback=self.contains_gait_cb,
)
def decolar():
"""!
O drone decola a partir desse momento
Não é recomendado efetuar outros comandos enquanto ele decola
"""
print('D')
rospy.wait_for_service('/uav1/uav_manager/takeoff')
dois = rospy.ServiceProxy('/uav1/uav_manager/takeoff', Trigger)
reqb = Trigger._request_class()
dois(reqb)
rospy.sleep(1)
while(chegou == False):
pass
def __init__(self, args=None):
super().__init__("game_manager", args)
self._trigger_start_asterix_request = Trigger.Request()
self._trigger_start_asterix_client = self.create_client(
Trigger, "/asterix/start"
)
makedirs(path.dirname(animation_path), exist_ok=True)
while not self._trigger_start_asterix_client.wait_for_service(timeout_sec=60.0):
self.get_logger().info("Waiting for asterix")
self.get_logger().info("Starting match")
self.robot.animationStartRecording(animation_path)
self._synchronous_call(
self._trigger_start_asterix_client, self._trigger_start_asterix_request
)
self._startup_time = self.robot.getTime()
self._timer = self.create_timer(2.0, self._timer_callback)
def __init__(self, context):
"""Initialize the NotesPLugin."""
super(NotesPlugin, self).__init__(context)
ui_file = os.path.join(
get_package_share_directory("march_rqt_note_taker"),
"note_taker.ui")
self._node: Node = context.node
self._model = EntryModel()
self._widget = NotesWidget(self._model, ui_file, self._node)
context.add_widget(self._widget)
self._use_current_time = self._should_use_current_time()
# Log a message when it is an error,
# or when the content is 'March is fully operational'
self._filter_map = FilterMap()
self._filter_map.add_filter_on_minimal_level(Log.ERROR)
self._filter_map.add_filter_on_level(
level=Log.INFO, msg_filter=self.filter_useful_text)
self._node.create_subscription(Log,
"/rosout_agg",
self._rosout_cb,
qos_profile=10)
self._node.create_subscription(
CurrentState,
"/march/gait_selection/current_state",
self._current_state_cb,
qos_profile=10,
)
self._get_gait_version_map_client = self._node.create_client(
Trigger, "/march/gait_selection/get_version_map")
config_client = self._node.create_client(
Trigger, "/march/gain_scheduling/get_configuration")
if not config_client.service_is_ready():
while config_client.wait_for_service(timeout_sec=1):
self._node.get_logger().warn(
"Failed to contact gain scheduling config "
"service")
future = config_client.call_async(Trigger.Request())
future.add_done_callback(lambda res: self._model.insert_row(
Entry(f"Configuration is {future.result().message}")))
def command_callback(self, msg):
try:
if msg.isValid():
if msg.getReceiver() == self.command_proxy_server_name:
command = msg.getCommand()
try:
data = msg.getData("data")
except:
data = None
Trigger(data)
Logger.log(self.command_proxy_server_name.replace("_supervisor", "") + ": " + command)
else:
self.command_server_last_message = None
else:
self.command_server_last_message = None
Logger.error(self.command_proxy_server_name.replace("_supervisor", "") + ": invalid message!!")
self.sendResponse(False)
except Exception as e:
self.sendResponse(False)
def set_default_versions(self):
"""Save the current gait version map in the gait selection node as a default."""
self.wait_for_service(self._get_default_dict)
file_path = os.path.join(GAIT_SOURCE_DIR, self._gait_directory,
"default.yaml")
new_default_dict = dict(
ast.literal_eval(
self._get_default_dict.call(Trigger.Request()).message))
try:
with open(file_path, "w") as default_yaml_content:
yaml_content = yaml.dump(new_default_dict,
default_flow_style=False)
default_yaml_content.write(yaml_content)
return True, f"Successfully updated default to file: {file_path}"
except IOError:
warning = f"Error occurred when writing to file path: {file_path}"
self._node.get_logger().warn(warning)
return False, warning
def test_1_switch_on(self):
"""Test power on a robot."""
empty_req = Trigger.Request()
get_robot_mode_req = GetRobotMode.Request()
self.call_service(self.power_on_client, empty_req)
end_time = time.time() + 10
mode = RobotMode.DISCONNECTED
while mode not in (RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time:
result = self.call_service(self.get_robot_mode_client, get_robot_mode_req)
mode = result.robot_mode.mode
self.assertIn(mode, (RobotMode.IDLE, RobotMode.RUNNING))
self.call_service(self.brake_release_client, empty_req)
end_time = time.time() + 10
while mode != RobotMode.RUNNING and time.time() < end_time:
result = self.call_service(self.get_robot_mode_client, get_robot_mode_req)
mode = result.robot_mode.mode
self.assertEqual(mode, RobotMode.RUNNING)
def _current_state_cb(self, current_state):
"""Callback for when the current state changes.
After the current state is changed this callback does either
1) Log that march is in idle state, or
2) Log the gait that is selected, by getting the version_map and
creating a new entry.
:param current_state: Current state being executed
"""
if current_state.state_type == CurrentState.IDLE:
message = f"March is idle in {current_state.state}"
self._model.insert_row(Entry(message))
elif current_state.state_type == CurrentState.GAIT:
try:
future = self._get_gait_version_map_client.call_async(
Trigger.Request())
future.add_done_callback(
lambda future_done: self._get_version_map_callback(
future_done, current_state))
except InvalidServiceNameException as error:
self._node.get_logger().warn(
f"Failed to contact gait selection node "
f"for gait versions: {error}")
def disable(self):
request = Trigger.Request()
return self.disable_service.call_async(request)
def handle(req):
arming(True)
rospy.sleep(0.5)
arming(False)
r = Trigger()
return (True, 'ok')
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 get_default_dict_cb(self, req, res):
defaults = {
"gaits": self._gait_version_map,
"positions": self._positions
}
return Trigger.Response(success=True, message=str(defaults))
def decolar():
print('D')
rospy.wait_for_service('/uav1/uav_manager/takeoff')
dois = rospy.ServiceProxy('/uav1/uav_manager/takeoff', Trigger)
reqb = Trigger._request_class()
dois(reqb)
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")
rospy.sleep(tempo)
msg = LedColor()
msg.r = 0
msg.g = 0
msg.b = 0
pubLed.publish(msg)
def pousar():
"""!
Pousa o drone na posicao atual
"""
print('P')
rospy.wait_for_service('/uav1/uav_manager/land')
um = rospy.ServiceProxy('/uav1/uav_manager/land', Trigger)
reqa = Trigger._request_class()
um(reqa)
def Sinal(gas,ajuste):
"""
Define a cor do led se o valor do gas estiver entre 45 e 55 para verde, senao vermelho
Define a cor do led se o valor do ajuste estiver entre -5 e 5 para verde, senao vermelho
@param gas: valor do gas
@param ajuste: valor do ajuste
"""
ativaLed([255,0,255], 10)
if (gas>=45) and (gas<=55):
ativaLed([0,255,0], 10)
else:
def timeout(self, event):
"""Stop recording when the timeout is reached, callback function"""
if self.recording:
rospy.logwarn("Reached recording limit, stopping recording!")
self.stop_recording(Trigger())
def pousar():
print('P')
rospy.wait_for_service('/uav1/uav_manager/land')
um = rospy.ServiceProxy('/uav1/uav_manager/land', Trigger)
reqa = Trigger._request_class()
um(reqa)
#!/usr/bin/env python
import rospy
from std_srvs.srv import Trigger
"""!
Passos necessários o drone decolar
Pode ser chamado de qualquer lugar do código
"""
rospy.wait_for_service('/uav1/uav_manager/takeoff')
dois = rospy.ServiceProxy('/uav1/uav_manager/takeoff', Trigger)
reqb = Trigger._request_class()
dois(reqb)
# Create timer event and publish.
period = float(rospy.get_param("~period"))
# Create lock for msgs.
lock = Lock()
# Setup services and publish mission.
rospy.Service("get_active_mission", Trigger, get_active_mission)
rospy.Service("get_mission_by_id", GetMissionByID, get_mission_by_id)
# Get mission to begin publishing. This is the first mission published.
mission_id = rospy.get_param("~id")
msgs = None
retry_rate = rospy.Rate(1)
while msgs is None and not rospy.is_shutdown():
# If id is negative then it is default and non existent.
if mission_id >= 0:
req = GetMissionByID()
req.id = mission_id
get_mission_by_id(req)
else:
get_active_mission(Trigger())
retry_rate.sleep()
# Publish message on timer.
timer = rospy.Timer(rospy.Duration(period), publish_mission)
rospy.spin()
def trigger(self):
_req = Trigger.Request()
future = self._client.call_async(_req)
return future
