class TrajectoryScheduler:
"""Scheduler that sends sends the wanted trajectories to the topic listened
to by the exoskeleton/simulation."""
def __init__(self, node):
self._failed = False
self._node = node
self._goals: List[TrajectoryCommand] = []
self.logger = Logger(self._node, __class__.__name__)
# Temporary solution to communicate with ros1 action server, should
# be updated to use ros2 action implementation when simulation is
# migrated to ros2
self._trajectory_goal_pub = self._node.create_publisher(
msg_type=FollowJointTrajectoryActionGoal,
topic="/march/controller/trajectory/follow_joint_trajectory/goal",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
self._cancel_pub = self._node.create_publisher(
msg_type=GoalID,
topic="/march/controller/trajectory/follow_joint_trajectory/cancel",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
self._trajectory_goal_result_sub = self._node.create_subscription(
msg_type=FollowJointTrajectoryActionResult,
topic="/march/controller/trajectory/follow_joint_trajectory/result",
callback=self._done_cb,
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
# Publisher for sending hold position mode
self._trajectory_command_pub = self._node.create_publisher(
msg_type=JointTrajectory,
topic="/march/controller/trajectory/command",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
def schedule(self, command: TrajectoryCommand):
"""Schedules a new trajectory.
:param TrajectoryCommand command: The trajectory command to schedule
"""
self._failed = False
stamp = command.start_time.to_msg()
command.trajectory.header.stamp = stamp
goal = FollowJointTrajectoryGoal(trajectory=command.trajectory)
self._trajectory_goal_pub.publish(
FollowJointTrajectoryActionGoal(
header=Header(stamp=stamp),
goal_id=GoalID(stamp=stamp, id=str(command)),
goal=goal,
)
)
info_log_message = f"Scheduling {command.name}"
debug_log_message = f"Subgait {command.name} starts "
if self._node.get_clock().now() < command.start_time:
time_difference = Duration.from_ros_duration(
command.start_time - self._node.get_clock().now()
)
debug_log_message += f"in {round(time_difference.seconds, 3)}s"
else:
debug_log_message += "now"
self._goals.append(command)
self.logger.info(info_log_message)
self.logger.debug(debug_log_message)
def cancel_active_goals(self):
now = self._node.get_clock().now()
for goal in self._goals:
if goal.start_time + goal.duration > now:
self._cancel_pub.publish(
GoalID(stamp=goal.start_time.to_msg(), id=str(goal))
)
def send_position_hold(self):
self._trajectory_command_pub.publish(JointTrajectory())
def failed(self) -> bool:
return self._failed
def reset(self):
self._failed = False
self._goals = []
def _done_cb(self, result):
if result.result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
self.logger.error(
f"Failed to execute trajectory. {result.result.error_string} ({result.result.error_code})"
)
self._failed = True
class GaitSelection(Node):
"""Base class for the gait selection module."""
def __init__(
self,
gait_package=None,
directory=None,
robot=None,
balance=None,
dynamic_gait=None,
):
super().__init__(NODE_NAME,
automatically_declare_parameters_from_overrides=True)
self.logger = Logger(self, __class__.__name__)
self._balance_used = False
self._dynamic_gait = False
try:
# Initialize all parameters once, and set up a callback for dynamically
# reconfiguring
if gait_package is None:
gait_package = (self.get_parameter(
"gait_package").get_parameter_value().string_value)
if directory is None:
directory = (self.get_parameter(
"gait_directory").get_parameter_value().string_value)
if balance is None:
self._balance_used = (self.get_parameter(
"balance").get_parameter_value().bool_value)
if dynamic_gait is None:
self._dynamic_gait = (self.get_parameter(
"dynamic_gait").get_parameter_value().bool_value)
self._early_schedule_duration = self._parse_duration_parameter(
"early_schedule_duration")
self._first_subgait_delay = self._parse_duration_parameter(
"first_subgait_delay")
# Setting dynamic gait parameters
self.dynamic_subgait_duration = (self.get_parameter(
"dynamic_subgait_duration").get_parameter_value().double_value)
self.middle_point_fraction = (self.get_parameter(
"middle_point_fraction").get_parameter_value().double_value)
self.middle_point_height = (self.get_parameter(
"middle_point_height").get_parameter_value().double_value)
self.minimum_stair_height = (self.get_parameter(
"minimum_stair_height").get_parameter_value().double_value)
self.push_off_fraction = (self.get_parameter(
"push_off_fraction").get_parameter_value().double_value)
self.push_off_position = (self.get_parameter(
"push_off_position").get_parameter_value().double_value)
except ParameterNotDeclaredException:
self.logger.error(
"Gait selection node started without required parameters "
"gait_package, gait_directory and balance")
self._directory_name = directory
self._gait_package = gait_package
self._gait_directory, self._default_yaml = self._initialize_gaits()
if not os.path.isdir(self._gait_directory):
self.logger.error(f"Gait directory does not exist: {directory}")
raise FileNotFoundError(directory)
if not os.path.isfile(self._default_yaml):
self.logger.error(
f"Gait default yaml file does not exist: {directory}/default.yaml"
)
self._robot = get_robot_urdf_from_service(
self) if robot is None else robot
self._joint_names = sorted(get_joint_names_from_robot(self._robot))
self._realsense_yaml = os.path.join(self._gait_directory,
"realsense_gaits.yaml")
self._realsense_gait_version_map = self._load_realsense_configuration()
(
self._gait_version_map,
self._positions,
self._dynamic_edge_version_map,
) = self._load_configuration()
self._robot_description_sub = self.create_subscription(
msg_type=String,
topic="/march/robot_description",
callback=self._update_robot_description_cb,
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self._create_services()
self._gaits = self._load_gaits()
self._early_schedule_duration = self._parse_duration_parameter(
"early_schedule_duration")
self._first_subgait_delay = self._parse_duration_parameter(
"first_subgait_delay")
if not self._validate_inverse_kinematics_is_possible():
self.logger.warning(
"The currently available joints are unsuitable for "
"using inverse kinematics.\n"
"Any interpolation on foot_location will return "
"the base subgait instead. Realsense gaits will "
"not be loaded.")
self.logger.info("Successfully initialized gait selection node.")
@property
def joint_names(self):
return self._joint_names
@property
def gaits(self):
return self._gaits
def _validate_inverse_kinematics_is_possible(self):
return (validate_and_get_joint_names_for_inverse_kinematics(
self.logger) is not None)
def _initialize_gaits(self):
package_path = get_package_share_directory(self._gait_package)
gait_directory = os.path.join(package_path, self._directory_name)
default_yaml = os.path.join(gait_directory, "default.yaml")
if not os.path.isdir(gait_directory):
self.logger.error(f"Gait directory does not exist: "
f"{gait_directory}")
if not os.path.isfile(default_yaml):
self.logger.error(f"Gait default yaml file does not exist: "
f"{gait_directory}/default.yaml")
return gait_directory, default_yaml
def update_gaits(self):
"""
Update the gaits after one of the gait attributes has been changed.
"""
self._gait_directory, self._default_yaml = self._initialize_gaits()
self._realsense_yaml = os.path.join(self._gait_directory,
"realsense_gaits.yaml")
self._realsense_gait_version_map = self._load_realsense_configuration()
(
self._gait_version_map,
self._positions,
self._semi_dynamic_gait_version_map,
) = self._load_configuration()
self._loaded_gaits = self._load_gaits()
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 _parse_duration_parameter(self, name: str) -> Duration:
"""Get a duration parameter from the parameter server.
Returns 0 if the parameter does not exist.
Clamps the duration to 0 if it is negative.
"""
if self.has_parameter(name):
value = self.get_parameter(name).value
if value < 0:
value = 0
return Duration(seconds=value)
else:
return Duration(0)
def shortest_subgait(self) -> Subgait:
"""Get the subgait with the smallest duration of all subgaits in the loaded gaits."""
shortest_subgait = None
for gait in self._gaits.values():
for subgait in gait.subgaits.values():
if (shortest_subgait is None
or subgait.duration < shortest_subgait.duration):
shortest_subgait = subgait
return shortest_subgait
@property
def robot(self):
"""Return the robot obtained from the robot state publisher."""
return self._robot
@property
def gait_version_map(self):
"""Returns the mapping from gaits and subgaits to versions."""
return self._gait_version_map
@property
def positions(self):
"""Returns the named idle positions."""
return self._positions
def _update_robot_description_cb(self, msg):
"""
Callback that is used to update the robot description when
robot_state_publisher sends out an update.
"""
self._robot = urdf.Robot.from_xml_string(msg.data)
def set_gait_versions(self, gait_name, version_map):
"""Sets the subgait versions of given gait.
:param str gait_name: Name of the gait to change versions
:param dict version_map: Mapping subgait names to versions
"""
if gait_name not in self._gaits:
raise GaitNameNotFoundError(gait_name)
# Only update versions that are different
version_map = {
name: version
for name, version in version_map.items()
if version != self._gait_version_map[gait_name][name]
}
self._gaits[gait_name].set_subgait_versions(self._robot,
self._gait_directory,
version_map)
self._gait_version_map[gait_name].update(version_map)
self.logger.info(
f"Setting gait versions successful: {self._gaits[gait_name]}")
def set_gait_versions_cb(self, request, response):
"""Sets a new gait version to the gait selection instance.
:type msg: march_shared_resources.srv.SetGaitVersionRequest
:rtype march_shared_resources.srv.SetGaitVersionResponse
"""
if len(request.subgaits) != len(request.versions):
return [
False,
"`subgaits` and `versions` array are not of equal length"
]
version_map = dict(zip(request.subgaits, request.versions))
try:
self.logger.info(f"Setting gait versions from {request}")
self.set_gait_versions(request.gait, version_map)
response.success = True
response.message = ""
return response
except Exception as e: # noqa: PIE786
response.success = False
response.message = str(e)
return response
def contains_gait_cb(self, request, response):
"""
Checks whether a gait and subgait are loaded.
:type request: ContainsGaitRequest
:param request: service request
:return: True when the gait and subgait are loaded
"""
gait = self._gaits.get(request.gait)
if gait is None:
response.contains = False
return response
response.contains = True
for subgait in request.subgaits:
if gait[subgait] is None:
response.contains = False
return response
def scan_directory(self):
"""Scans the gait_directory recursively and create a dictionary of all
subgait files.
:returns:
dictionary of the maps and files within the directory
"""
gaits = {}
for gait in os.listdir(self._gait_directory):
gait_path = os.path.join(self._gait_directory, gait)
if os.path.isdir(gait_path):
subgaits = {}
for subgait in os.listdir(gait_path):
subgait_path = os.path.join(gait_path, subgait)
if os.path.isdir(subgait_path):
versions = sorted([
v.replace(".subgait", "")
for v in os.listdir(os.path.join(subgait_path))
if v.endswith(".subgait")
])
subgaits[subgait] = versions
gaits[gait] = subgaits
return gaits
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 add_gait(self, gait):
"""Adds a gait to the loaded gaits if it does not already exist.
The to be added gait should implement `GaitInterface`.
"""
if gait.name in self._gaits:
self.logger.warning(
"Gait `{gait}` already exists in gait selection".format(
gait=gait.name))
else:
self._gaits[gait.name] = gait
def _load_gaits(self):
"""Loads the gaits in the specified gait directory.
:returns dict: A dictionary mapping gait name to gait instance
"""
gaits = {}
for gait in self._gait_version_map:
gaits[gait] = SetpointsGait.from_file(gait, self._gait_directory,
self._robot,
self._gait_version_map)
for gait in self._dynamic_edge_version_map:
self.logger.debug(f"Adding dynamic gait {gait}")
start_is_dynamic = self._dynamic_edge_version_map[gait].pop(
"start_is_dynamic", True)
final_is_dynamic = self._dynamic_edge_version_map[gait].pop(
"final_is_dynamic", True)
gaits[gait] = DynamicEdgeSetpointsGait.dynamic_from_file(
gait,
self._gait_directory,
self._robot,
self._dynamic_edge_version_map,
start_is_dynamic,
final_is_dynamic,
)
self._gait_version_map[gait] = self._dynamic_edge_version_map[gait]
self._load_realsense_gaits(gaits)
if self._balance_used and "balance_walk" in gaits:
balance_gait = BalanceGait(node=self,
default_walk=gaits["balance_walk"])
if balance_gait is not None:
self.logger.info("Successfully created a balance gait")
gaits["balanced_walk"] = balance_gait
if self._dynamic_gait:
# We pass along the gait_selection_node to be able to listen
# to the CoViD topic wihtin the DynamicSetpointGait class.
self.dynamic_setpoint_gait = DynamicSetpointGait(
gait_selection_node=self)
gaits["dynamic_walk"] = self.dynamic_setpoint_gait
self.logger.info("Added dynamic_walk to gaits")
return gaits
def _load_realsense_gaits(self, gaits):
"""
Load all gaits from the realsense gait version map.
Also create a service with a separate callback group that can be used by the
realsense gaits to get parameters from the realsense_reader. A new callback
group is necessary to prevent a deadlock.
:param gaits: The dictionary where the loaded gaits will be added to.
"""
if not self._validate_inverse_kinematics_is_possible():
return
get_gait_parameters_service = self.create_client(
srv_type=GetGaitParameters,
srv_name="/camera/process_pointcloud",
callback_group=MutuallyExclusiveCallbackGroup(),
)
for gait_name in self._realsense_gait_version_map:
gait_folder = gait_name
gait_path = os.path.join(self._gait_directory, gait_folder,
gait_name + ".gait")
with open(gait_path, "r") as gait_file:
gait_graph = yaml.load(gait_file,
Loader=yaml.SafeLoader)["subgaits"]
gait = RealsenseGait.from_yaml(
gait_selection=self,
robot=self._robot,
gait_name=gait_name,
gait_config=self._realsense_gait_version_map[gait_name],
gait_graph=gait_graph,
gait_directory=self._gait_directory,
process_service=get_gait_parameters_service,
)
gaits[gait_name] = gait
def _load_realsense_configuration(self):
if not os.path.isfile(self._realsense_yaml):
self.logger.info(
"No realsense_yaml present, no realsense gaits will be created."
)
return {}
with open(self._realsense_yaml, "r") as realsense_config_file:
return yaml.load(realsense_config_file, Loader=yaml.SafeLoader)
def _load_configuration(self):
"""Loads and verifies the gaits configuration."""
with open(self._default_yaml, "r") as default_yaml_file:
default_config = yaml.load(default_yaml_file,
Loader=yaml.SafeLoader)
version_map = default_config["gaits"]
dynamic_edge_version_map = {}
if "dynamic_edge_gaits" in default_config:
dynamic_edge_version_map = default_config["dynamic_edge_gaits"]
if not isinstance(version_map, dict):
raise TypeError("Gait version map should be of type; dictionary")
if not self._validate_version_map(version_map):
raise GaitError(msg="Gait version map: {gm}, is not valid".format(
gm=version_map))
positions = {}
for position_name, position_values in default_config[
"positions"].items():
positions[position_name] = {
"gait_type": position_values["gait_type"],
"joints": {},
}
for joint, joint_value in position_values["joints"].items():
if joint in self._joint_names:
positions[position_name]["joints"][joint] = joint_value
if set(positions[position_name]["joints"].keys()) != set(
self._joint_names):
raise NonValidGaitContentError(
f"The position {position_name} does not "
f"have a position for all required joints: it "
f"has {positions[position_name]['joints'].keys()}, "
f"required: {self._joint_names}")
return version_map, positions, dynamic_edge_version_map
def _validate_version_map(self, version_map):
"""Validates if the current versions exist.
:param dict version_map: Version map to verify
:returns bool: True when all versions exist, False otherwise
"""
for gait_name in version_map:
gait_path = os.path.join(self._gait_directory, gait_name)
if not os.path.isfile(os.path.join(gait_path,
gait_name + ".gait")):
self.logger.warning(
"gait {gn} does not exist".format(gn=gait_name))
return False
for subgait_name in version_map[gait_name]:
version = version_map[gait_name][subgait_name]
if not Subgait.validate_version(gait_path, subgait_name,
version):
self.logger.warning("{0}, {1} does not exist".format(
subgait_name, version))
return False
return True
def __getitem__(self, name):
"""Returns a gait from the loaded gaits."""
return self._gaits.get(name)
def __iter__(self):
"""Returns an iterator over all loaded gaits."""
return iter(self._gaits.values())
class RealsenseGait(SetpointsGait):
"""
The RealsenseGait class is used for creating gaits based on the parameters given
by the realsense reader. From these parameters the subgaits to interpolate are
interpolated after a realsense call during the start of the gait. It is based on the
setpoints gait, and it uses the interpolation over 1 or 2 dimensions with 2 or 4
subgaits respectively.
"""
SERVICE_TIMEOUT = Duration(seconds=2.0)
INITIAL_START_DELAY_TIME = Duration(seconds=10.0)
CAMERA_NAME_MAP = {
"front": GetGaitParameters.Request.CAMERA_FRONT,
"back": GetGaitParameters.Request.CAMERA_BACK,
}
REALSENSE_CATEGORY_MAP = {
"stairs_up": GetGaitParameters.Request.STAIRS_UP,
"stairs_down": GetGaitParameters.Request.STAIRS_DOWN,
"ramp_up": GetGaitParameters.Request.RAMP_UP,
"ramp_down": GetGaitParameters.Request.RAMP_DOWN,
"sit": GetGaitParameters.Request.SIT,
"curb_up": GetGaitParameters.Request.CURB_UP,
"curb_down": GetGaitParameters.Request.CURB_DOWN,
}
def __init__(
self,
gait_name: str,
subgaits: dict,
graph: SubgaitGraph,
gait_selection: GaitSelection,
realsense_category: str,
camera_to_use: str,
subgaits_to_interpolate: dict,
dimensions: InterpolationDimensions,
process_service: Client,
starting_position: EdgePosition,
final_position: EdgePosition,
parameters: List[float],
dependent_on: List[str],
responsible_for: List[str],
):
super(RealsenseGait, self).__init__(gait_name, subgaits, graph)
self._gait_selection = gait_selection
self.logger = Logger(self._gait_selection, __class__.__name__)
self.parameters = parameters
self.dimensions = dimensions
self.dimensions = dimensions
self.realsense_category = self.realsense_category_from_string(
realsense_category)
self.camera_to_use = self.camera_msg_from_string(camera_to_use)
self.subgaits_to_interpolate = subgaits_to_interpolate
# Set up service and event for asynchronous
self._get_gait_parameters_service = process_service
self.realsense_service_event = Event()
self.realsense_service_result = None
self._starting_position = starting_position
self._final_position = final_position
self._dependent_on = dependent_on
self._responsible_for = responsible_for
@property
def dependent_on(self):
return self._dependent_on
@property
def responsible_for(self):
return self._responsible_for
@property
def subsequent_subgaits_can_be_scheduled_early(self) -> bool:
"""
Whether a subgait can be scheduled early, this is not possible for the realsense
gait, since this will later have a service call to determine the next subgait.
"""
return True
@property
def first_subgait_can_be_scheduled_early(self) -> bool:
"""
Whether the first subgait can be started with a delay, this is possible for
the realsense gait.
"""
return True
@property
def starting_position(self) -> EdgePosition:
return self._starting_position
@property
def final_position(self) -> EdgePosition:
return self._final_position
@classmethod
def from_yaml(
cls,
gait_selection: GaitSelection,
robot: urdf.Robot,
gait_name: str,
gait_config: dict,
gait_graph: dict,
gait_directory: str,
process_service: Client,
):
"""
Construct a realsense gait from the gait_config from the realsense_gaits.yaml.
:param gait_selection: The GaitSelection node that will be used for making the
service calls to the
realsense reader.
:param robot: The urdf robot that can be used to verify the limits of the
subgaits.
:param gait_name: The name of the gait.
:param gait_config: The yaml node with the needed configurations.
:param gait_graph: The graph from the .gait file with the subgait transitions.
:param gait_directory: The gait_directory that is being used.
:param process_service: The service from which to get the gait parameters
:return: The constructed RealsenseGait
"""
graph = SubgaitGraph(gait_graph)
subgaits_to_interpolate = {}
try:
dimensions = InterpolationDimensions.from_integer(
gait_config["dimensions"])
dependent_on = gait_config.get("dependent_on", [])
responsible_for = gait_config.get("responsible_for", [])
parameters = [0.0 for _ in range(amount_of_parameters(dimensions))]
realsense_category = gait_config["realsense_category"]
camera_to_use = gait_config["camera_to_use"]
subgait_version_map = gait_config["subgaits"]
# Create subgaits to interpolate with
for subgait_name in subgait_version_map:
subgaits_to_interpolate[subgait_name] = [
Subgait.from_name_and_version(robot, gait_directory,
gait_name, subgait_name,
version)
for version in subgait_version_map[subgait_name]
]
if len(subgaits_to_interpolate[subgait_name]
) != amount_of_subgaits(dimensions):
raise WrongRealSenseConfigurationError(
f"The amount of subgaits in the realsense version map "
f"({len(subgaits_to_interpolate[subgait_name])}) doesn't match "
f"the amount of dimensions for subgait {subgait_name}")
subgaits = {}
for subgait_name in subgait_version_map:
if subgait_name not in ("start", "end"):
subgaits[subgait_name] = Subgait.interpolate_n_subgaits(
dimensions=dimensions,
subgaits=subgaits_to_interpolate[subgait_name],
parameters=parameters,
use_foot_position=True,
)
starting_position = cls.parse_edge_position(
gait_config["starting_position"],
subgaits[graph.start_subgaits()[0]].starting_position,
)
final_position = cls.parse_edge_position(
gait_config["final_position"],
subgaits[graph.end_subgaits()[0]].final_position,
)
except KeyError as e:
raise WrongRealSenseConfigurationError(
f"There was a missing key to create realsense gait in gait {gait_name}:"
f" {e}")
except ValueError as e:
raise WrongRealSenseConfigurationError(
f"There was a wrong value in the config for the realsense gait"
f" {gait_name}: {e}")
return cls(
gait_name=gait_name,
subgaits=subgaits,
graph=graph,
gait_selection=gait_selection,
realsense_category=realsense_category,
camera_to_use=camera_to_use,
subgaits_to_interpolate=subgaits_to_interpolate,
dimensions=dimensions,
process_service=process_service,
starting_position=starting_position,
final_position=final_position,
parameters=parameters,
dependent_on=dependent_on,
responsible_for=responsible_for,
)
@classmethod
def parse_edge_position(cls, config_value: str,
position_values: Dict[str, float]):
"""
Parse the edge position based on the string in the realsense_gaits.yaml.
:param config_value: The value in the yaml file.
:param position_values: The actual joint positions at the edge of the gait.
:return: The edge position to use.
"""
if config_value == "static":
return StaticEdgePosition(position_values)
elif config_value == "dynamic":
return DynamicEdgePosition(position_values)
else:
raise WrongRealSenseConfigurationError(
"The edge position did not have a "
"valid value, should be static or "
f"dynamic, but was `{config_value}`")
@classmethod
def realsense_category_from_string(cls, gait_name: str) -> int:
"""
Construct the realsense gait from the string in the realsense_gaits.yaml.
:param gait_name: The string from the config.
:return: The integer to send to the realsense reader to define the category.
"""
if gait_name not in cls.REALSENSE_CATEGORY_MAP:
raise WrongRealSenseConfigurationError(
f"Gait name {gait_name} from the config is not known as a possible "
f"realsense reader gait configuration")
return cls.REALSENSE_CATEGORY_MAP[gait_name]
@classmethod
def camera_msg_from_string(cls, camera_name: str) -> int:
"""
Construct the camera name msg from the string in the realsense_gaits.yaml.
:param camera_name: The string from the config.
:return: The integer to send to the realsense reader to define the camera.
"""
if camera_name not in cls.CAMERA_NAME_MAP:
raise WrongRealSenseConfigurationError(
f"The camera configuration {camera_name} from the realsense_gaits.yaml"
f"is not one of the known camera names: {cls.CAMERA_NAME_MAP.keys()}"
)
return cls.CAMERA_NAME_MAP[camera_name]
DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION = Duration(0)
def start(
self,
current_time: Time,
first_subgait_delay: Optional[
Duration] = DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION,
) -> GaitUpdate:
"""
This function is called to start the realsense gait, it does the following.
1) Make a service call to march_realsense_reader.
2) Update all subgaits to interpolated subgaits with the given parameters
(this will later become only some of the subgaits when the update function is
also used).
3) Update the gait parameters to prepare for start
4) Return the first subgait, if correct parameters were found.
:return: A gait update that tells the state machine what to do. Empty means
that that state machine should not start a gait.
"""
self._reset()
# Delay start until parameterization is done
self._start_is_delayed = True
# Start time will be set later, but to prevent updates during the service
# calls to think the gait start time has passed, set start time in the future.
self._start_time = current_time + self.INITIAL_START_DELAY_TIME
self._current_time = current_time
# If a gait is dependent on some other gait its subgaits are already
# interpolated from parameters so we can skip the realsense call
if not self._dependent_on:
realsense_update_successful = self.get_realsense_update()
if not realsense_update_successful:
return GaitUpdate.empty()
self._current_subgait = self.subgaits[self.graph.start_subgaits()[0]]
self._next_subgait = self._current_subgait
if first_subgait_delay is None:
first_subgait_delay = self.DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION
self._start_time = self._gait_selection.get_clock().now(
) + first_subgait_delay
self._end_time = self._start_time + self._current_subgait.duration
return GaitUpdate.should_schedule_early(
self._command_from_current_subgait())
def get_realsense_update(self):
"""
Makes a realsense service call and handles the result
:return: Whether the call was successful
"""
service_call_succesful = self.make_realsense_service_call()
if not service_call_succesful:
self.logger.warn("No service response received within timeout")
return False
gait_parameters_response = self.realsense_service_result
if gait_parameters_response is None or not gait_parameters_response.success:
self.logger.warn(
"No gait parameters were found, gait will not be started, "
f"{gait_parameters_response}")
return False
return self.update_gaits_from_realsense_call(
gait_parameters_response.gait_parameters)
def update_gaits_from_realsense_call(
self, gait_parameters: GaitParameters) -> bool:
"""
Update the gait parameters based on the message of the current gaits and its
responsibilities.
:param gait_parameters: The parameters to update to.
"""
success = True
self.set_parameters(gait_parameters)
success &= self.interpolate_subgaits_from_parameters()
if self._responsible_for and success:
for gait_name in self._responsible_for:
gait = self._gait_selection.gaits[gait_name]
# Make a recursive call to also handle the dependencies of the
# dependent gait
if isinstance(gait, RealsenseGait):
gait.update_gaits_from_realsense_call(gait_parameters)
return success
def make_realsense_service_call(self) -> bool:
"""
Make a call to the realsense service, if it is available
and returns the response.
:return: Whether the call was successful
"""
if self._current_subgait is not None:
subgait_name = self._current_subgait.subgait_name
else:
# Assume that the gait is starting and use the first subgait name
subgait_name = self.graph.start_subgaits()[0]
request = GetGaitParameters.Request(
realsense_category=self.realsense_category,
camera_to_use=self.camera_to_use,
subgait_name=subgait_name,
)
self.realsense_service_event.clear()
if self._get_gait_parameters_service.wait_for_service(
timeout_sec=self.SERVICE_TIMEOUT.seconds):
gait_parameters_response_future = (
self._get_gait_parameters_service.call_async(request))
gait_parameters_response_future.add_done_callback(
self._realsense_response_cb)
else:
self.logger.error(
f"The service took longer than {self.SERVICE_TIMEOUT} to become "
f"available, is the realsense reader running?")
return False
return self.realsense_service_event.wait(
timeout=self.SERVICE_TIMEOUT.seconds)
def _realsense_response_cb(self, future: Future):
"""Set capture point result when the capture point service returns."""
self.realsense_service_result = future.result()
self.realsense_service_event.set()
def interpolate_subgaits_from_parameters(self) -> bool:
"""Change all subgaits to one interpolated from the current parameters."""
self.logger.info(
f"Interpolating gait {self.gait_name} with parameters:"
f" {self.parameters}")
new_subgaits = {}
for subgait_name in self.subgaits.keys():
new_subgaits[subgait_name] = Subgait.interpolate_n_subgaits(
dimensions=self.dimensions,
subgaits=self.subgaits_to_interpolate[subgait_name],
parameters=self.parameters,
use_foot_position=True,
)
try:
self.set_subgaits(new_subgaits, self._gait_selection)
except NonValidGaitContentError:
return False
return True
def set_parameters(self, gait_parameters: GaitParameters) -> None:
"""
Set the gait parameters based on the message.
:param gait_parameters: The parameters to set.
"""
if self.dimensions == InterpolationDimensions.ONE_DIM:
self.parameters = [gait_parameters.first_parameter]
elif self.dimensions == InterpolationDimensions.TWO_DIM:
self.parameters = [
gait_parameters.first_parameter,
gait_parameters.second_parameter,
]
else:
raise UnknownDimensionsError(self.dimensions)
def set_edge_positions(self, starting_position: EdgePosition,
final_position: EdgePosition):
"""
Set the new edge positions. Overrides from the setpoints gait, which does not
store the starting or final position
:param starting_position: The new starting position
:param final_position: The new final position
"""
self._starting_position = starting_position
self._final_position = final_position