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 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_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 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 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 __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 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 __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 trigger(self):
_req = Trigger.Request()
future = self._client.call_async(_req)
return future
def disable(self):
request = Trigger.Request()
return self.disable_service.call_async(request)