Beispiel #1
0
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
Beispiel #2
0
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