Thoughts on the Software Design of MoveIt

[tylerjw]

Software design and MoveIt

Currently, in MoveIt the software design revolves around collecting data and pointers to other classes in the private member variables of a class and then offering methods to mutate that data or perform operations involving the other classes that it has access to.

This pattern of software is common in projects created by those who have not studied software design patterns and has some distinct disadvantages to a design that makes types and functions first class.

To make the point let us use RobotTrajectory from MoveIt as an example. Currently, this is the design of the type:

class RobotTrajectory
{
private:
  moveit::core::RobotModelConstPtr robot_model_;
  const moveit::core::JointModelGroup* group_;
  std::deque<moveit::core::RobotStatePtr> waypoints_;
  std::deque<double> duration_from_previous_;
  rclcpp::Clock clock_ros_;
public:
  // methods
};

There are many things to be upset about here from a software design perspective but let us first focus on the type RobotTrajectory and what it contains. Drawing a graph of types to dependent types results in this graph:

graph TD

RobotTrajectory-->RobotModel
RobotModel-->LinkModel
JointModelGroup-->RobotModel
RobotState-->RobotModel
LinkModel-->JointModel
KinematicsSolver-->SolverAllocatorFn
AttachedBody-->LinkModel
KinematicsBase-->rclcpp::Node
SolverAllocatorFn-->JointModelGroup
RobotTrajectory-->JointModelGroup
RobotModel-->JointModel
JointModelGroup-->JointModel
RobotState-->JointModel
LinkModel-->LinkModel
KinematicsSolver-->KinematicsBase
AttachedBody-->Eigen::Isometry3d
SolverAllocatorFn-->KinematicsBase
KinematicsBase-->RobotModel
RobotTrajectory-->RobotState
RobotModel-->JointModelGroup
JointModelGroup-->JointModel::Bounds
RobotState-->Eigen::Isometry3d
LinkModel-->Eigen::Isometry3d
AttachedBody-->shapes::Shape
AttachedBody-->EigenSTL::vector_Isometry3d
KinematicsBase-->DiscretizationMethod
RobotTrajectory-->rclcpp::Clock
JointModelGroup-->LinkModel
RobotState-->AttachedBody
LinkModel-->EigenSTL::vector_Isometry3d
AttachedBody-->trajectory_msgs::msg::JointTrajectory
JointModelGroup-->KinematicsSolver
RobotState-->AttachedBodyCallback
JointModelGroup-->GroupMimicUpdate
LinkModel-->Eigen::Vector3d
JointModelGroup-->srdf::Model::Group
RobotState-->random_numbers::RandomNumberGenerator
LinkModel-->shapes::Shape
AttachedBodyCallback-->AttachedBody

Loading

This diagram shows the relationships of types to their member variables. By contrast here is the DescreteTimeTrajectory type from Drake (T is a template parameter for the time type that is used):

graph TD

DiscreteTimeTrajectory-->Eigen::Matrix
DiscreteTimeTrajectory-->T

Loading

Rot

Looking at this diagram you can begin to see that the design of MoveIt has the signs of what Robert Martin would call rot. ¹ Software design rot can be observed by the following properties.

Rigidity. Changes that should be small result in a cascade of changes in other parts of the code. When a type in your software depends on many other things through an interconnected web of dependencies and pointers changes become really hard.

Fragility. The tendance of the software to break in many places each time there is a change. Recently we have seen this when trying to deal with our use of executors and nodes from ROS 2.

Immobility. The inability to reuse code from one part of the project or from another project. This results in copy-paste and is caused by an excess of dependencies.

Viscosity. There are many ways to make a change. Viscosity is when it is very hard to make changes without increasing technical debt.

Decoupling Dependencies with SOLID

Creating dependent relationships between types in your design is the root of Rot. You can combat this by creating firewalls between your types. There are various principles of software design you can employ to achieve firewalls between dependencies. A collection of them is commonly referred to as SOLID:

• Single-Responsibility Principal
• Open Closed Principal
• Liskov Substitution Principal
• Interface Segregation Principal
• Dependency Inversion Principal

Single-Responsibility Principal

There should never be more than one reason for a class to change

Classes in C++ are the way we collect behavior and data. A problem arises when we collect too many disconnected things and make them depend on each other. The SRP² is the idea that you should only collect things together that change for the same reason. If there are separate reasons for something to change, those should be separate types.

Considering our RobotTrajectory, here are some things that it does:

• Represent a trajectory for a set of joints (joint model group)
  • Every operation has to consider only joints in the group from the RobotStates
• Store a collection of RobotStates and Durations between those states
• Present a container (Inserting, Indexing, Iterator) interface to the RobotStates and Durations
• Calculate the sum of durations (duration of the trajectory)
• Calculate the average durations
• Convert to and from a RobotTrajectory message
• Unwrap continuous joint states contained in the collection of RobotState
• Calculate index (and blend) of RobotState given duration
• Format RobotTrajectory for printing / logging

Here are some unrelated reasons a RobotTrajectory might change are:

• Robot Trajectory message changes so converting to/from a robot trajectory needs to change.
• Duration from start is needed instead of duration since last waypoint.
• Resample a robot trajectory (more or fewer states per unit of time)
• Represent a trajectory that cannot be resampled (discrete states)
• Calculate metrics about a trajectory for comparison other than total and average duration
• Change formatting of printed trajectory
• Change type of state, or attributes of the RobotState
• Change unit of time

For a concrete example, you should look at the amount of code that has to change to convert from using a double to represent durations to rclcpp::Duration. Here is a PR where I first attempted to do this.

Open Closed Principal

A module should be open for extension but closed for modification.

We should be able to extend what our module does without changing the source code of the module. There are several techniques we can use in C++ to achieve this.

Dynamic Polymophism

Many of us are familiar with the concept of pure virtual interface classes in C++ that can be inherited from. This is the simplest way to implement a polymorphic interface. Here is a straightforward example:

struct ClockInterface {
  virtual rclcpp::Time now() = 0;
};

struct RosClock : public ClockInterface {
  rclcpp::Clock clock;
  rclcpp::Time now() override { return clock.now(); }
};

bool is_even_second(ClockInterface& clock) {
  return int64_t(clock.now().seconds()) % 2 == 0;
}

This may seem like a trivial example but the idea here is that if we wanted to change the way the now function behaves we can without changing any of our code that depends on calling that function. This means we can add support for different types of clocks without having to change all the code that uses the clock interface.

Type Errasure

Another more advanced version of implementing polymorphic interfaces is called "Type Erasure". Sean Parent gave an excellent talk that explains that technique.³

Templates

A third way to provide these sorts of interfaces is with Templates in C++. Due to the fact that templates will match anything, in C++20 it is useful to use constraints and concepts to restrict the template to types that have the interface you would like to use. Prior to C++20 you can use static asserts with std::is_invocable_r to enforce template parameters have the interface you depend on.

Liskov Substitution Principal

Subclasses should be substitutable for their base class

Barbara Liskov coined this idea as part of her work on type theory. In practice, this means that when you write a behavior that depends on a base type you should be careful to only depend on the interface of the base type and not any knowledge you have of the implementation of that base type. If you depend on the implementation of the base type you will ruin the ability for users to pass you subtypes and have the code continue to work.

In practice, this means that you can not define subclasses that reduce the valid operations that can be done on a base class. A derived class's methods are only substitutable for a base class if and only if:

1. Its preconditions are no stronger than the base class method.
2. Its postconditions are no weaker than the base class method.

Interface Segregation Principal

No code should be forced to depend on methods it does not use

ISP is one of the primary goals of microservices architectures. The idea is that any one thing may present multiple "role interfaces" and software can depend on the role interface. This is a type of dependency inversion. Users of your behavior then provide you restricted interfaces to other types that you depend on that represent only the interface you need and not the whole interface of that dependent type.

When writing ROS packages we are already fairly familiar with this concept as we use ROS message, service, and action types to define interfaces between our nodes. We can also templates or types with virtual functions to achieve this type of abstraction within C++.

Dependency Inversion Principal

Depend on Abstractions, do not depend on concretions

Dependency inversion is the primary method for decoupling dependencies. This is the tool we use to achieve the [[#Open Closed Principal]]. The basic idea is that any behavior or state should depend on behavior or state that is abstracted to an interface that can be replaced by the caller of the function (or owner of objects of the type). By using dependency injection you can now extend code without editing it and without affecting other users of that code. This is good because it enables you to test behaviors in your modules in an isolated way and it enables specific users to extend the behavior of your modules without affecting all other users of your modules.

Functions and Types

Designing software with Classes and OO is really hard. Even when it is done really well C++ developers need to keep track of contracts between classes and design interfaces that abstract away each module from each other module simply to test those objects. There are language features that existed before Classes and are the basis for many powerful, composable languages. Those features are functions and structs (aka the user-defined type).

Functions > Methods

When you combine your behavior with your representation of state by putting that behavior in a method when you could have written a function you are adding unnecessary coupling.

TL;DR: Go watch "Free Your Functions!" from Klaus Iglberger⁴.

Disadvantages of member functions

1. less encapsulation than the free function because a member function has access to the private interfaces of the class. (ISP)
2. increase coupling because all state of the Class is coupled with the method. This means that even if you don't use some state to perform some behavior it still needs to exist. (SRP)
3. less flexible and extensible because users cannot add them to your type without editing the source. (OCP)
4. less re-usable because only code with an object of the type can use them. (DRY)

Unique super powers of free functions

1. A powerful type of static polymorphism - function overloads. You cannot do that with methods. Overloads combined with templates make for composable compact logic that does the same thing on many different types. This is also all done without the performance hit.
2. testing is a free function that means only needs the exact dependencies (the parameters) of that function. Tests become easy to write and debug.
3. performance. Go watch the talk linked above to learn why free functions get optimized where member functions do not!

Conclusion

As the only constant in software is that the requirements change. If MoveIt is to meet the needs of developers and projects now and into the future it must be changeable. The current design of MoveIt makes changes (including bug fixes) very difficult. This is primarily due to a lack of attention to software design.

Hope is not lost though, we can incrementally convert methods locked into strongly coupled types to free functions and structs. Doing this will achieve the principles and benefits of SOLID and can be done one piece at a time.

Footnotes

1. Robert C. Martin. "Design Principles and Design Patterns" https://web.archive.org/web/20150906155800/http://www.objectmentor.com/resources/articles/Principles_and_Patterns.pdf ↩

2. Robert C. Martin. "The Single Responsibility Principle" https://blog.cleancoder.com/uncle-bob/2014/05/08/SingleReponsibilityPrinciple.html ↩

3. Sean Parent. "GoingNative 2013 Inheritance Is The Base Class of Evil" https://www.youtube.com/watch?v=2bLkxj6EVoM ↩

4. Klaus Iglberger. "CppCon 2017 Free Your Functions!" https://www.youtube.com/watch?v=WLDT1lDOsb4 ↩