Leveraging a functional
approach for easier testing andmaintenance of ROS 2 code
October 2023

Bilal Gillbilal.gill@picknik.ai

Outline
●
●
●
●
●

2

Introduction
ROS 2 Conventional Approach
Introduction to Functional
Programming Principles
Refactoring using Functional
Programming Principles
Conclusion

Introduction

About Me
●

●
●

4

Robotics Engineer on the services team at PickNik Robotics○ Contributed to a wide variety of client projects: remotely operated
underwater inspection vehicles, autonomous mobile base for agriculture
applications, and more
Have worked at General Dynamics Electric Boat, MIT Lincoln Laboratory
Interested in robotics since high school

Why give this talk?
●5

Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend

Why give this talk?
●●6

Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the clientWhy give this talk?
●
●
●

7

Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the client
How can the client expect proper operation of the software once they start developing on
top of it?

Why give this talk?
●●●Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the client
How can the client expect proper operation of the software once they start developing on
top of it?
Answer: Tests and documentation! Lots and lots of documentation!

8

Why give this talk?
●●●Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the client
How can the client expect proper operation of the software once they start developing on
top of it?
Answer: Tests and documentation! Lots and lots of documentation!

●

The tests written for the software should not be ﬂaky!
○

9

Flaky tests: tests that return both passes and failures despite no changes to the code or the test
itselfWhy give this talk?
●●●Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the client
How can the client expect proper operation of the software once they start developing on
top of it?
Answer: Tests and documentation! Lots and lots of documentation!

●

The tests written for the software should not be ﬂaky!
○

●

10

Flaky tests: tests that return both passes and failures despite no changes to the code or the test
itself

ROS 2 documentation encourages an object-oriented paradigm that can lead to trouble
writing code that achieves the goalWhy give this talk?
●●●Engineers at PickNik experiment with different ways to architect code that uses ROS 2 –creating code that is easy to understand, test, maintain, and extend
At the end of a services project, code is handed over to the client
How can the client expect proper operation of the software once they start developing on
top of it?
Answer: Tests and documentation! Lots and lots of documentation!

●

The tests written for the software should not be ﬂaky!
○

●

●
11

Flaky tests: tests that return both passes and failures despite no changes to the code or the test
itself

ROS 2 documentation encourages an object-oriented paradigm that can lead to trouble
writing code that achieves the goal
Adopting functional programming techniques into our code has made it easier to test,
maintain, and extend code!

Motivating Example●

Problem: A robot wants to navigate from its
current location to some goal

12

Clip from https://www.youtube.com/watch?v=VTeY-l-Xh6c

Motivating Example●

Problem: A robot wants to navigate from its
current location to some goal
The robot needs to know where obstacles arelocated in its environment●

13

Clip from https://www.youtube.com/watch?v=VTeY-l-Xh6c

Motivating Example●

Problem: A robot wants to navigate from its
current location to some goal
The robot needs to know where obstacles arelocated in its environment
Let’s use an occupancy map to represent theenvironment

●
●

14

Clip from https://www.youtube.com/watch?v=VTeY-l-Xh6c

Motivating Example●

Problem: A robot wants to navigate from its
current location to some goal
The robot needs to know where obstacles arelocated in its environment
Let’s use an occupancy map to represent theenvironment
Assumption: The robot knows its location in
the occupancy map at all times

●
●
●

15

Clip from https://www.youtube.com/watch?v=VTeY-l-Xh6c

Motivating Example●

Problem: A robot wants to navigate from its
current location to some goal
The robot needs to know where obstacles arelocated in its environment
Let’s use an occupancy map to represent theenvironment
Assumption: The robot knows its location in
the occupancy map at all times
Solution: The robot will send a request to a
ROS 2 service that generates a path from therobot’s current location and goal location,
given an occupancy map

●
●
●
●

16

Clip from https://www.youtube.com/watch?v=VTeY-l-Xh6c

ROS 2 Conventional
ApproachConventional Approachclass PathGenerator : public rclcpp::Node {public:explicit PathGenerator(
rclcpp::NodeOptions const& options = rclcpp::NodeOptions{}): Node("path_generator", options);●

private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);●void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);
Map<unsigned char> map_;
int robot_size_;
std::unique_ptr<CollisionChecker<unsigned char>> is_occupied_;rclcpp::Service<SetMap>::SharedPtr map_setter_service_;
rclcpp::Service<GetPath>::SharedPtr path_generator_service_;
};

18

●

PathGenerator will be used to generate
the path for our robot
This code was written using example
code available from the ROS 2
documentation
Testing this class requires spinning up
clients to send requests to the services
and inspecting the responses

Conventional Approachclass PathGenerator : public rclcpp::Node {public:explicit PathGenerator(
rclcpp::NodeOptions const& options = rclcpp::NodeOptions{}): Node("path_generator", options);●

private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);●void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);
Map<unsigned char> map_;
int robot_size_;
std::unique_ptr<CollisionChecker<unsigned char>> is_occupied_;rclcpp::Service<SetMap>::SharedPtr map_setter_service_;
rclcpp::Service<GetPath>::SharedPtr path_generator_service_;
};

19

●

●

PathGenerator will be used to generate
the path for our robot
This code was written using example
code available from the ROS 2
documentation
Testing this class requires spinning up
clients to send requests to the services
and inspecting the responses
Involving inter-process communicationin unit tests introduces ﬂakinessConventional Approachclass PathGenerator : public rclcpp::Node {public:explicit PathGenerator(
rclcpp::NodeOptions const& options = rclcpp::NodeOptions{}): Node("path_generator", options);●

private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);●void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);
Map<unsigned char> map_;
int robot_size_;
std::unique_ptr<CollisionChecker<unsigned char>> is_occupied_;rclcpp::Service<SetMap>::SharedPtr map_setter_service_;
rclcpp::Service<GetPath>::SharedPtr path_generator_service_;
};

20

●

●
●

PathGenerator will be used to generate
the path for our robot
This code was written using example
code available from the ROS 2
documentation
Testing this class requires spinning up
clients to send requests to the services
and inspecting the responses
Involving inter-process communicationin unit tests introduces ﬂakinessIs there a way to refactor this code suchthat invoking the ROS 2 API is avoided?

Conventional Approachclass PathGenerator : public rclcpp::Node {public:explicit PathGenerator(
rclcpp::NodeOptions const& options = rclcpp::NodeOptions{}): Node("path_generator", options);●

private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);●void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);
Map<unsigned char> map_;
int robot_size_;
std::unique_ptr<CollisionChecker<unsigned char>> is_occupied_;rclcpp::Service<SetMap>::SharedPtr map_setter_service_;
rclcpp::Service<GetPath>::SharedPtr path_generator_service_;
};

21

●

●
●
●

PathGenerator will be used to generate
the path for our robot
This code was written using example
code available from the ROS 2
documentation
Testing this class requires spinning up
clients to send requests to the services
and inspecting the responses
Involving inter-process communicationin unit tests introduces ﬂakinessIs there a way to refactor this code suchthat invoking the ROS 2 API is avoided?
Yes, by using functional programming
principles

Introduction to Functional
Programming Principles

What is Functional Programming?
●

23

A programming paradigm characterized by the use of pure functions and the
avoidance of side effects

What is Functional Programming?
●●

24

A programming paradigm characterized by the use of pure functions and the
avoidance of side effects
Functional programming is identiﬁed by the use of higher order functions, purefunctions, monads, declarative syntax

What is Functional Programming?
●●

A programming paradigm characterized by the use of pure functions and the
avoidance of side effects
Functional programming is identiﬁed by the use of higher order functions, purefunctions, monads, declarative syntax○

25

C++ has all the tools to implement functional programming, including lambda
functions, std::function, std::optional, std::expected, and moreWhat is Functional Programming?
●●

A programming paradigm characterized by the use of pure functions and the
avoidance of side effects
Functional programming is identiﬁed by the use of higher order functions, purefunctions, monads, declarative syntax○○

26

C++ has all the tools to implement functional programming, including lambda
functions, std::function, std::optional, std::expected, and more
Want to maximize use of these features to write code with a minimal number of side
effects

What is Functional Programming?
●●

A programming paradigm characterized by the use of pure functions and the
avoidance of side effects
Functional programming is identiﬁed by the use of higher order functions, purefunctions, monads, declarative syntax
○
○

●

27

C++ has all the tools to implement functional programming, including lambda
functions, std::function, std::optional, std::expected, and more
Want to maximize use of these features to write code with a minimal number of side
effects

Let’s go over some principles and see how we can use them in refactoring
PathGenerator

Pure Functions●

A pure function is a function that○28

is deterministic: They always return the same output for the same set of inputs

Pure Functions●

A pure function is a function that○○29

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output

Pure Functions●

A pure function is a function that
○
○

●

30

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output

Why is this desirable?

Pure Functions●

A pure function is a function that○○●

Why is this desirable?
○

31

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output
local reasoning: The code can be understood just by looking at that portion and a limited scopearound itPure Functions●

A pure function is a function that○○●

Why is this desirable?
○○

32

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output
local reasoning: The code can be understood just by looking at that portion and a limited scopearound it
testability: Testing pure functions is trivial

Pure Functions●

A pure function is a function that○○●

Why is this desirable?
○○●

local reasoning: The code can be understood just by looking at that portion and a limited scopearound it
testability: Testing pure functions is trivial

Practically, pure functions do not:○33

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output

contain state (static variables or class member variables)Pure Functions●

A pure function is a function that○○●

Why is this desirable?
○○●

local reasoning: The code can be understood just by looking at that portion and a limited scopearound it
testability: Testing pure functions is trivial

Practically, pure functions do not:○○34

is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output

contain state (static variables or class member variables)
mutate input parametersPure Functions●

A pure function is a function that○○●

Why is this desirable?
○
○

●

35

local reasoning: The code can be understood just by looking at that portion and a limited scopearound it
testability: Testing pure functions is trivial

Practically, pure functions do not:○○●is deterministic: They always return the same output for the same set of inputs
has no side effects: They don’t alter the state of the program, global variables, or produce any
observable interactions with the outside world, such as writing to ﬁles or displaying output

contain state (static variables or class member variables)
mutate input parameters

A function is pure if and only if it could be replaced by a lookup table (potentially inﬁnitely
large!)

Higher Order Functions●

A higher order function is a function that can:○36

accept other functions as arguments

Higher Order Functions●

A higher order function is a function that can:○○37

accept other functions as arguments
return functions as a result

Higher Order Functions●

A higher order function is a function that can:
○
○

●

38

accept other functions as arguments
return functions as a result

Why is this desirable?

Higher Order Functions●

A higher order function is a function that can:○○●Why is this desirable?
○

39

accept other functions as arguments
return functions as a result
Modularity and reusability: The behavior of a higher order function is conﬁgurable
and they can be used in different contexts

Higher Order Functions●

A higher order function is a function that can:○○●Why is this desirable?
○○

40

accept other functions as arguments
return functions as a result
Modularity and reusability: The behavior of a higher order function is conﬁgurable
and they can be used in different contexts
Control ﬂow abstraction: control ﬂows, like looping and conditional execution, canbe abstracted in a readable and reusable manner

Higher Order Functions●

A higher order function is a function that can:○○●Why is this desirable?
○
○
○

41

accept other functions as arguments
return functions as a result
Modularity and reusability: The behavior of a higher order function is conﬁgurable
and they can be used in different contexts
Control ﬂow abstraction: control ﬂows, like looping and conditional execution, canbe abstracted in a readable and reusable manner
Testability: Smaller well-deﬁned functions are easier to test

Higher Order Functions●

A higher order function is a function that can:○○●Why is this desirable?
○
○
○

●

42

accept other functions as arguments
return functions as a result
Modularity and reusability: The behavior of a higher order function is conﬁgurable
and they can be used in different contexts
Control ﬂow abstraction: control ﬂows, like looping and conditional execution, canbe abstracted in a readable and reusable manner
Testability: Smaller well-deﬁned functions are easier to test

The Standard Template Library contains many higher order functions!

Higher Order Functions●

A higher order function is a function that can:○○●Why is this desirable?
○
○
○

●

Modularity and reusability: The behavior of a higher order function is conﬁgurable
and they can be used in different contexts
Control ﬂow abstraction: control ﬂows, like looping and conditional execution, canbe abstracted in a readable and reusable manner
Testability: Smaller well-deﬁned functions are easier to test

The Standard Template Library contains many higher order functions!
○43

accept other functions as arguments
return functions as a result

std::transform, std::ﬁnd_if, std::copy, and more

Monadic Error Handling●

44

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way

Monadic Error Handling●●

45

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way
This approach encapsulates error handling into types and operations on these types

Monadic Error Handling
●
●
●

46

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way
This approach encapsulates error handling into types and operations on these types
Why is this desirable?

Monadic Error Handling●●●

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way
This approach encapsulates error handling into types and operations on these types
Why is this desirable?
○

47

Type encapsulation: Monadic error handling encapsulates the result of computations along withpossible errors within a single type

Monadic Error Handling●●●

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way
This approach encapsulates error handling into types and operations on these types
Why is this desirable?
○○

48

Type encapsulation: Monadic error handling encapsulates the result of computations along withpossible errors within a single type
Compositional error handling: Monadic error handling allows composition of operations thatmight fail, in a way that if any operation fails, the whole computation fails

Monadic Error Handling●●●

Monadic error handling is a functional programming technique for dealing with errors in aclean, compositional, and type-safe way
This approach encapsulates error handling into types and operations on these types
Why is this desirable?
○
○
○

49

Type encapsulation: Monadic error handling encapsulates the result of computations along withpossible errors within a single type
Compositional error handling: Monadic error handling allows composition of operations thatmight fail, in a way that if any operation fails, the whole computation fails
Error Propagation: Errors can be automatically propagated through a sequence of computations
until they are explicitly handled

How does functional programming help?
●

50Functional programming lends itself to the minimization of mutable stateHow does functional programming help?
●●

51

Functional programming lends itself to the minimization of mutable state
Testing is easier with pure functions because only the return value of thefunction needs to be evaluatedHow does functional programming help?
●
●
●

52

Functional programming lends itself to the minimization of mutable state
Testing is easier with pure functions because only the return value of thefunction needs to be evaluatedDifferent functions can be passed as arguments to higher order functions,lending itself to modularity

How does functional programming help?
●
●
●
●

53

Functional programming lends itself to the minimization of mutable state
Testing is easier with pure functions because only the return value of thefunction needs to be evaluatedDifferent functions can be passed as arguments to higher order functions,lending itself to modularity
Monadic error handling simpliﬁes error checking

How does functional programming help?
●
●
●
●
●

54

Functional programming lends itself to the minimization of mutable state
Testing is easier with pure functions because only the return value of thefunction needs to be evaluatedDifferent functions can be passed as arguments to higher order functions,lending itself to modularity
Monadic error handling simpliﬁes error checking
Let’s try and refactor PathGenerator

How does functional programming help?
●
●
●
●
●

Functional programming lends itself to the minimization of mutable state
Testing is easier with pure functions because only the return value of thefunction needs to be evaluatedDifferent functions can be passed as arguments to higher order functions,lending itself to modularity
Monadic error handling simpliﬁes error checking
Let’s try and refactor PathGenerator○

55

Claim: that the refactored PathGenerator has 100% coverageRefactoring using Functional
Programming Principles

Refactoring PathGenerator
class PathGenerator : public rclcpp::Node {public:explicit PathGenerator(rclcpp::NodeOptions const&
options = rclcpp::NodeOptions{}): Node("path_generator", options);
private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);/* Additional private members*/
};

57

●

How the current PathGenerator looksRefactoring PathGenerator
class PathGenerator : public rclcpp::Node {public:explicit PathGenerator(rclcpp::NodeOptions const&
options = rclcpp::NodeOptions{}): Node("path_generator", options);
private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);/* Additional private members*/
};

58

●
●

A rclcpp::Node object can be constructedin main and services can be assigned
No need to have a PathGenerator object
that inherits from rclcpp::NodeRefactoring PathGenerator
class PathGenerator : public rclcpp::Node {public:explicit PathGenerator(rclcpp::NodeOptions const&
options = rclcpp::NodeOptions{}): Node("path_generator", options);
private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);/* Additional private members*/
};

59

●
●
●
●

A rclcpp::Node object can be constructedin main and services can be assigned
No need to have a PathGenerator object
that inherits from rclcpp::NodeThe create_service method accepts classmethods, free functions, and lambdas as the
callback function
The private functions of PathGenerator canbe turned into free functions and lambda
functionsRefactoring PathGenerator
class PathGenerator : public rclcpp::Node {public:explicit PathGenerator(rclcpp::NodeOptions const&
options = rclcpp::NodeOptions{}): Node("path_generator", options);
private:void set_map_service(
const std::shared_ptr<SetMap::Request> request,
std::shared_ptr<SetMap::Response> response);void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response);

●
●
●
●

bool set_costmap(const std_msgs::msg::UInt8MultiArray& costmap);
Path generate_global_path(Position const& start,Position const& goal);/* Additional private members*/
};

60

●

A rclcpp::Node object can be constructedin main and services can be assigned
No need to have a PathGenerator object
that inherits from rclcpp::NodeThe create_service method accepts classmethods, free functions, and lambdas as the
callback function
The private functions of PathGenerator canbe turned into free functions and lambda
functionsLet’s refactor the callback function for thegenerate path serviceRefactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;}61

Refactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;
}

62

●

generate_path_service is:○ printing errors

Refactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;
}

63

●

generate_path_service is:○ printing errors○ generating the pathRefactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;
}

64

●

generate_path_service is:○ printing errors○ generating the path
○ setting an out parameterRefactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;
}

65

●

●

generate_path_service is:○ printing errors○ generating the path
○ setting an out parameter
Let’s isolate the error printing
functionality to another function○ The error printing functionneeds to be passed an error
typeRefactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */●●auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;
}

66

●

generate_path_service is:○ printing errors○ generating the path
○ setting an out parameter
Let’s isolate the error printing
functionality to another function○ The error printing functionneeds to be passed an error
type
The object held by the shared pointercan be assigned by another functionRefactoring PathGenerator
void generate_path_service(
const std::shared_ptr<GetPath::Request> request,
std::shared_ptr<GetPath::Response> response) {if (map_.get_data().size() == 0) {RCLCPP_ERROR_STREAM(this->get_logger(), "MAP IS EMPTY!!");
response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
response->path = std_msgs::msg::UInt8MultiArray();
return;}/* More error pre-checks */●●auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path
auto const path = generate_global_path(start, goal);●// Start populating the response message
auto response_path = std_msgs::msg::UInt8MultiArray();●/* Code about populating the message here */response->code.code = !path.empty() ? example_srvs::msg::GetPathCodes::SUCCESS :example_srvs::msg::GetPathCodes::NO_VALID_PATH;
response->path = response_path;}67

generate_path_service is:○ printing errors○ generating the path
○ setting an out parameter
Let’s isolate the error printing
functionality to another function○ The error printing functionneeds to be passed an error
type
The object held by the shared pointercan be assigned by another function
The generate_global_path
function and associated pre-checks
can be extracted to another functionRefactoring PathGenerator
std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

68

●

Here is the refactored core functionalityof the generate path callbackRefactoring PathGenerator
std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

69

●
●

Here is the refactored core functionalityof the generate path callback
This function returns a type which canbe used for monadic error handlingRefactoring PathGenerator
std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

70

●
●
●

Here is the refactored core functionalityof the generate path callback
This function returns a type which canbe used for monadic error handlingIf there is an error, the function canhandle the error in a compile timecheckable wayRefactoring PathGenerator
using PathingGenerator = std::function<std::optional<Path>(
Position const&, Position const&, Map<unsigned char> const&)>;●std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */●auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

71

●
●

Here is the refactored core functionalityof the generate path callback
This function returns a type which canbe used for monadic error handlingIf there is an error, the function canhandle the error in a compile timecheckable way
The function that generates the path can
now be passed in, making this function ahigher order functionRefactoring PathGenerator
using PathingGenerator = std::function<std::optional<Path>(
Position const&, Position const&, Map<unsigned char> const&)>;●std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */●auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

72

●
●
●

Here is the refactored core functionalityof the generate path callback
This function returns a type which canbe used for monadic error handlingIf there is an error, the function canhandle the error in a compile timecheckable way
The function that generates the path can
now be passed in, making this function ahigher order function
This function is deterministic and has noside effects, so it is a pure functionRefactoring PathGenerator
using PathingGenerator = std::function<std::optional<Path>(
Position const&, Position const&, Map<unsigned char> const&)>;●std::expected<GetPath::Response, error> generate_path(
std::shared_ptr<GetPath::Request> const request,
Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {if (occupancy_map.get_data().size() == 0) {return std::unexpected(error::EMPTY_OCCUPANCY_MAP);}/* More error pre-checks */●auto const start = Position{request->start.data[0], request->start.data[1]};auto const goal = Position{request->goal.data[0], request->goal.data[1]};// Generate the path using the path generator function that was input
auto const path = path_generator(start, goal, occupancy_map);
if (!path.has_value()) {return std::unexpected(error::NO_VALID_PATH);}
auto response = GetPath::Response{};/* More implementation code */return response;
}

73

●
●
●
●

Here is the refactored core functionalityof the generate path callback
This function returns a type which canbe used for monadic error handlingIf there is an error, the function canhandle the error in a compile timecheckable way
The function that generates the path can
now be passed in, making this function ahigher order function
This function is deterministic and has noside effects, so it is a pure function
Let’s test this function

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();
auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);}

74

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();
auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);
}

75

●

Testing the refactored functionality istrivialTesting the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();
auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);
}

76

●

Testing the refactored functionality istrivial○ Create required parameters

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();
auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);
}

77

●

Testing the refactored functionality istrivial○ Create required parameters○ Pass the parameters into thefunction under test

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();
auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);
}

78

●

Testing the refactored functionality istrivial○ Create required parameters○ Pass the parameters into thefunction under test
○ Check the return

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();●

auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);
}

79

●

Testing the refactored functionality istrivial○ Create required parameters○ Pass the parameters into thefunction under test
○ Check the return
All the functions that have been
refactored so far can be tested this way

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();●

auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);●// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);

●

}

80

Testing the refactored functionality istrivial○ Create required parameters○ Pass the parameters into thefunction under test
○ Check the return
All the functions that have been
refactored so far can be tested this way
Everything can now be put together forthe callback being executed by thegenerate path service

Testing the Refactored PathGenerator
TEST(GeneratePath, NoValidPath) {// GIVEN a GetPath request and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();●

auto const request = std::make_shared<GetPath::Request>();request->start.data = {2, 2};request->goal.data = {5, 5};// WHEN the path is requested
auto const response = pathing::generate_path::generate_path(request, sample_occupancy_map, pathing::generate_global_path);●// THEN there should be an error with the error::NO_VALID_PATH type
EXPECT_EQ(response.error(), pathing::generate_path::error::NO_VALID_PATH);

●

}

●

81

Testing the refactored functionality istrivial○ Create required parameters○ Pass the parameters into thefunction under test
○ Check the return
All the functions that have been
refactored so far can be tested this way
Everything can now be put together forthe callback being executed by thegenerate path service
All of this has been done without
invoking the ROS 2 API!

Putting it all together[this](auto const request, auto response) {auto const print_error = [this](std::string_view error)-> std::expected<GetPath::Response, std::string> {...};
auto const return_empty_response = []([[maybe_unused]] auto const)-> std::expected<GetPath::Response, std::string> {...};
auto const stringify_error = [](auto const error) {...};*response = generate_path::generate_path(request, this->map_,
generate_global_path).map_error(stringify_error).or_else(print_error).or_else(return_empty_response).value();}82●

The generate path callback function
has been replaced by a lambda
functionPutting it all together[this](auto const request, auto response) {auto const print_error = [this](std::string_view error)-> std::expected<GetPath::Response, std::string> {...};
auto const return_empty_response = []([[maybe_unused]] auto const)-> std::expected<GetPath::Response, std::string> {...};
auto const stringify_error = [](auto const error) {...};*response = generate_path::generate_path(request, this->map_,
generate_global_path).map_error(stringify_error).or_else(print_error).or_else(return_empty_response).value();
}

83

●
●

The generate path callback function
has been replaced by a lambda
function
If generate_path returns the
expected value, it is directly assigned
to response

Putting it all together[this](auto const request, auto response) {auto const print_error = [this](std::string_view error)-> std::expected<GetPath::Response, std::string> {...};
auto const return_empty_response = []([[maybe_unused]] auto const)-> std::expected<GetPath::Response, std::string> {...};●●

auto const stringify_error = [](auto const error) {...};*response = generate_path::generate_path(request, this->map_,
generate_global_path).map_error(stringify_error).or_else(print_error).or_else(return_empty_response).value();}84●

The generate path callback function
has been replaced by a lambda
function
If generate_path returns the
expected value, it is directly assigned
to response
If generate_path returns an error,
the error is handled by chaining
functions together○ This is the result of returning amonadic type and performing
monadic error handlingPutting it all together[this](auto const request, auto response) {auto const print_error = [this](std::string_view error)-> std::expected<GetPath::Response, std::string> {...};
auto const return_empty_response = []([[maybe_unused]] auto const)-> std::expected<GetPath::Response, std::string> {...};
auto const stringify_error = [](auto const error) {...};*response = generate_path::generate_path(request, this->map_,
generate_global_path).map_error(stringify_error).or_else(print_error).or_else(return_empty_response).value();
}

85

●
●
●

●

The generate path callback function
has been replaced by a lambda
function
If generate_path returns the
expected value, it is directly assigned
to response
If generate_path returns an error,
the error is handled by chaining
functions together○ This is the result of returning amonadic type and performing
monadic error handlingIf needed, more functions can be
added to manipulate the expected
type or error type, increasing
modularity

Putting it all together[this](auto const request, auto response) {auto const print_error = [this](std::string_view error)-> std::expected<GetPath::Response, std::string> {...};
auto const return_empty_response = []([[maybe_unused]] auto const)-> std::expected<GetPath::Response, std::string> {...};
auto const stringify_error = [](auto const error) {...};*response = generate_path::generate_path(request, this->map_,
generate_global_path).map_error(stringify_error).or_else(print_error).or_else(return_empty_response).value();
}

●
●
●

●

●
86

The generate path callback function
has been replaced by a lambda
function
If generate_path returns the
expected value, it is directly assigned
to response
If generate_path returns an error,
the error is handled by chaining
functions together○ This is the result of returning amonadic type and performing
monadic error handlingIf needed, more functions can be
added to manipulate the expected
type or error type, increasing
modularity
How can this lambda be tested?

DI and Functional Programming
template <typename ServiceType>using ServiceCallback = std::function<void(
std::shared_ptr<typename ServiceType::Request>const ,std::shared_ptr<typename ServiceType::Response>)>;struct Manager {struct MiddlewareHandle {// Define map service callback typeusing SetMapCallback = ServiceCallback<SetMap>;// Define path generation service callback typeusing GeneratePathCallback = ServiceCallback<GetPath>;
virtual ~MiddlewareHandle() = default;
virtual void register_set_map_service(SetMapCallback callback) = 0;virtual void register_generate_path_service(GeneratePathCallback callback) = 0;virtual void log_error(std::string const& msg) = 0;virtual void log_info(std::string const& msg) = 0;};
Manager(std::unique_ptr<MiddlewareHandle> mw);
private:std::unique_ptr<MiddlewareHandle> mw_;
Map<unsigned char> map_;
};
87

●

With Dependency Injection (DI)!

DI and Functional Programming
template <typename ServiceType>using ServiceCallback = std::function<void(
std::shared_ptr<typename ServiceType::Request>const ,std::shared_ptr<typename ServiceType::Response>)>;struct Manager {struct MiddlewareHandle {// Define map service callback typeusing SetMapCallback = ServiceCallback<SetMap>;// Define path generation service callback typeusing GeneratePathCallback = ServiceCallback<GetPath>;
virtual ~MiddlewareHandle() = default;
virtual void register_set_map_service(SetMapCallback callback) = 0;virtual void register_generate_path_service(GeneratePathCallback callback) = 0;virtual void log_error(std::string const& msg) = 0;virtual void log_info(std::string const& msg) = 0;};
Manager(std::unique_ptr<MiddlewareHandle> mw);
private:std::unique_ptr<MiddlewareHandle> mw_;
Map<unsigned char> map_;
};
88

●

With Dependency Injection (DI)!
○ DI is used to move or “inject” objects
into another object

DI and Functional Programming
template <typename ServiceType>using ServiceCallback = std::function<void(
std::shared_ptr<typename ServiceType::Request>const ,std::shared_ptr<typename ServiceType::Response>)>;struct Manager {struct MiddlewareHandle {// Define map service callback typeusing SetMapCallback = ServiceCallback<SetMap>;// Define path generation service callback typeusing GeneratePathCallback = ServiceCallback<GetPath>;
virtual ~MiddlewareHandle() = default;
virtual void register_set_map_service(SetMapCallback callback) = 0;virtual void register_generate_path_service(GeneratePathCallback callback) = 0;virtual void log_error(std::string const& msg) = 0;virtual void log_info(std::string const& msg) = 0;};
Manager(std::unique_ptr<MiddlewareHandle> mw);
private:std::unique_ptr<MiddlewareHandle> mw_;
Map<unsigned char> map_;
};
89

●

With Dependency Injection (DI)!
○ DI is used to move or “inject” objects
into another object●

There still needs to be mutable state, tokeep track of the occupancy map between
service calls, thus the map_ member
variableDI and Functional Programming
template <typename ServiceType>using ServiceCallback = std::function<void(
std::shared_ptr<typename ServiceType::Request>const ,std::shared_ptr<typename ServiceType::Response>)>;struct Manager {struct MiddlewareHandle {// Define map service callback typeusing SetMapCallback = ServiceCallback<SetMap>;●

With Dependency Injection (DI)!
○ DI is used to move or “inject” objects
into another object●

There still needs to be mutable state, tokeep track of the occupancy map between
service calls, thus the map_ member
variable
For the Manager object, aMiddlewareHandle struct is deﬁned thatis the interface for the injected
dependency
This abstract interface can be used to
implement each function using the ROS
API// Define path generation service callback typeusing GeneratePathCallback = ServiceCallback<GetPath>;
virtual ~MiddlewareHandle() = default;●

virtual void register_set_map_service(SetMapCallback callback) = 0;virtual void register_generate_path_service(GeneratePathCallback callback) = 0;virtual void log_error(std::string const& msg) = 0;virtual void log_info(std::string const& msg) = 0;};
Manager(std::unique_ptr<MiddlewareHandle> mw);
private:std::unique_ptr<MiddlewareHandle> mw_;
Map<unsigned char> map_;
};
90

●

DI and Functional Programming
template <typename ServiceType>using ServiceCallback = std::function<void(
std::shared_ptr<typename ServiceType::Request>const ,std::shared_ptr<typename ServiceType::Response>)>;●struct Manager {struct MiddlewareHandle {// Define map service callback typeusing SetMapCallback = ServiceCallback<SetMap>;●// Define path generation service callback typeusing GeneratePathCallback = ServiceCallback<GetPath>;●

virtual ~MiddlewareHandle() = default;
virtual void register_set_map_service(SetMapCallback callback) = 0;virtual void register_generate_path_service(GeneratePathCallback callback) = 0;virtual void log_error(std::string const& msg) = 0;●virtual void log_info(std::string const& msg) = 0;};
Manager(std::unique_ptr<MiddlewareHandle> mw);
private:std::unique_ptr<MiddlewareHandle> mw_;
Map<unsigned char> map_;
};
91

●

With Dependency Injection (DI)!
○ DI is used to move or “inject” objects
into another object
There still needs to be mutable state, tokeep track of the occupancy map between
service calls, thus the map_ member
variable
For the Manager object, aMiddlewareHandle struct is deﬁned thatis the interface for the injected
dependency
This abstract interface can be used to
implement each function using the ROS
API
The lambda function that is used for thegenerate path service can be captured via
mocking and tested

Testing with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
92
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);}●

Here is the code testing the generate pathlambda function

Testing with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
93
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);}●●

Here is the code testing the generate pathlambda function
This test ﬁxture calls the callback function
for the set occupancy map service when amock function is executedTesting with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
94
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);
}

●
●

●

Here is the code testing the generate pathlambda function
This test ﬁxture calls the callback function
for the set occupancy map service when amock function is executedThis text ﬁxture also captures the callback
function for the generate path service so it
can be executed later

Testing with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
95
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);
}

●
●

●

●

Here is the code testing the generate pathlambda function
This test ﬁxture calls the callback function
for the set occupancy map service when amock function is executedThis text ﬁxture also captures the callback
function for the generate path service so it
can be executed later
For this test the occupancy map has alreadybeen set via the test ﬁxture

Testing with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
96
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);
}

●
●

●

●
●

Here is the code testing the generate pathlambda function
This test ﬁxture calls the callback function
for the set occupancy map service when amock function is executedThis text ﬁxture also captures the callback
function for the generate path service so it
can be executed later
For this test the occupancy map has alreadybeen set via the test ﬁxture
The generate path callback can now be
tested by executing the callback function
directly

Testing with DI
struct PathManagerFixture : public testing::Test {PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {// When the map callback is called, set the costmap
ON_CALL(*mw_, register_set_map_service(testing::_)).WillByDefault([&](auto const& map_callback) {auto const map_request = make_occupancy_map();
auto map_response = std::make_shared<SetMap::Response>();map_callback(map_request, map_response);});// Capture the path callback so it can be called later
ON_CALL(*mw_, register_generate_path_service(testing::_)).WillByDefault(testing::SaveArg<0>(&path_callback_));}std::unique_ptr<MockMiddlewareHandle> mw_;
pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;};
TEST_F(PathManagerFixture, NoPath) {// GIVEN a path generator with a costmap
auto const path_generator = pathing::Manager{std::move(mw_)};// WHEN the generate path service is called with an unreachable goal
auto path_request = std::make_shared<GetPath::Request>();path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};auto path_response = std::make_shared<GetPath::Response>();path_callback_(path_request, path_response);// THEN the path generator should succeed
EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
auto const expected = pathing::Path{};// AND the path should be empty
97
EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
expected);
}

●
●
●
●
●
●

Here is the code testing the generate pathlambda function
This test ﬁxture calls the callback function
for the set occupancy map service when amock function is executedThis text ﬁxture also captures the callback
function for the generate path service so it
can be executed later
For this test the occupancy map has alreadybeen set via the test ﬁxture
The generate path callback can now be
tested by executing the callback function
directly
There was no invocation of the middlewareusing DI and all code is testable without
invoking the ROS 2 API!

Conclusion

Conclusion●

99

The refactored tests are deterministic - they cannot be ﬂaky

Conclusion
●
●

100

The refactored tests are deterministic - they cannot be ﬂaky
Break down code into discrete components that can be tested

Conclusion
●
●
●

101

The refactored tests are deterministic - they cannot be ﬂaky
Break down code into discrete components that can be tested
Prioritize using pure functions - easier to test and reason about

Conclusion
●
●
●
●

102

The refactored tests are deterministic - they cannot be ﬂaky
Break down code into discrete components that can be tested
Prioritize using pure functions - easier to test and reason about
Using higher order functions increased the modularity of the code, in this caseallowing for different path generating algorithms to be usedConclusion
●
●
●
●
●

103

The refactored tests are deterministic - they cannot be ﬂaky
Break down code into discrete components that can be tested
Prioritize using pure functions - easier to test and reason about
Using higher order functions increased the modularity of the code, in this caseallowing for different path generating algorithms to be used
Monadic error handling led to easier error checking

Conclusion●●●●

The refactored tests are deterministic - they cannot be ﬂaky
Break down code into discrete components that can be tested
Prioritize using pure functions - easier to test and reason about
Using higher order functions increased the modularity of the code, in this caseallowing for different path generating algorithms to be used
● Monadic error handling led to easier error checking● Refactoring PathGenerator using DI in conjunction with the functional
programming paradigm led to code that has 100% coverage

104Thanks to:
●
●
●
●
●
●
●
●

105

Mariyum Gill
Griswald Brooks
Davide Faconti
Kyle Kirves
Tyler Weaver
Chris Thrasher
Brian Cairl
Everyone else at PickNik

Bilal Gill
Leveraging a Functional Approach for More Testable and Maintainable
ROS 2 CodeThank you!
All code and the full presentation are available at:106