Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/windows build #82

Open
wants to merge 14 commits into
base: master
Choose a base branch
from
2 changes: 2 additions & 0 deletions pytest.ini
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
[pytest]
junit_family=xunit2
11 changes: 6 additions & 5 deletions system_modes/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,12 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rcl_lifecycle REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(rosidl_typesupport_cpp REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(system_modes_msgs REQUIRED)

add_library(mode SHARED
Expand All @@ -40,12 +41,12 @@ ament_target_dependencies(mode
"rclcpp_lifecycle"
"rosidl_typesupport_cpp"
"lifecycle_msgs"
"std_msgs"
"system_modes_msgs"
)

"builtin_interfaces")
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(mode PRIVATE "SYSTEMMODES_BUILDING_LIBRARY")
target_compile_definitions(mode PRIVATE "SYSTEM_MODES_BUILDING_LIBRARY")

install(
DIRECTORY include/
Expand Down
94 changes: 75 additions & 19 deletions system_modes/include/system_modes/mode.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
#include <utility>
#include <iostream>

#include "system_modes/visibility_control.hpp"

namespace system_modes
{

Expand All @@ -45,27 +47,57 @@ using ModeMap = std::map<std::string, ModeConstPtr>;
class ModeBase
{
public:
SYSTEM_MODES_PUBLIC
explicit ModeBase(const std::string & mode_name);

SYSTEM_MODES_PUBLIC
virtual ~ModeBase() = default;

// cppcheck-suppress unknownMacro
RCLCPP_DISABLE_COPY(ModeBase)
SYSTEM_MODES_PUBLIC
RCLCPP_DISABLE_COPY(ModeBase)

std::string get_name() const;
SYSTEM_MODES_PUBLIC
std::string
get_name() const;

virtual void set_parameter(const rclcpp::Parameter & parameter) = 0;
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
virtual void set_part_mode(
SYSTEM_MODES_PUBLIC
virtual void
set_parameter(const rclcpp::Parameter & parameter) = 0;

SYSTEM_MODES_PUBLIC
virtual void
set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;

SYSTEM_MODES_PUBLIC
virtual void
set_part_mode(
const std::string & part,
const StateAndMode stateAndMode) = 0;

virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
virtual std::vector<std::string> get_parameter_names() const;
virtual const std::vector<rclcpp::Parameter> get_parameters() const;
SYSTEM_MODES_PUBLIC
virtual rclcpp::Parameter
get_parameter(const std::string & param_name) const;

SYSTEM_MODES_PUBLIC
virtual std::vector<std::string>
get_parameter_names() const;

virtual const std::vector<std::string> get_parts() const;
virtual const StateAndMode get_part_mode(const std::string & part) const;
SYSTEM_MODES_PUBLIC
virtual const std::vector<rclcpp::Parameter>
get_parameters() const;

virtual std::string print() const;
SYSTEM_MODES_PUBLIC
virtual const std::vector<std::string>
get_parts() const;

SYSTEM_MODES_PUBLIC
virtual const StateAndMode
get_part_mode(const std::string & part) const;

SYSTEM_MODES_PUBLIC
virtual std::string
print() const;

protected:
ModeImpl mode_impl_;
Expand All @@ -74,38 +106,62 @@ class ModeBase
class DefaultMode : public ModeBase
{
public:
SYSTEM_MODES_PUBLIC
DefaultMode();

SYSTEM_MODES_PUBLIC
explicit DefaultMode(const std::string & mode_name) = delete;
// cppcheck-suppress unknownMacro
RCLCPP_DISABLE_COPY(DefaultMode)

virtual void set_parameter(const rclcpp::Parameter & parameter);
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters);
SYSTEM_MODES_PUBLIC
virtual void
set_parameter(const rclcpp::Parameter & parameter);

virtual void set_part_mode(
SYSTEM_MODES_PUBLIC
virtual void
set_parameters(const std::vector<rclcpp::Parameter> & parameters);

SYSTEM_MODES_PUBLIC
virtual void
set_part_mode(
const std::string & part,
const StateAndMode stateAndMode);
};

class Mode : public ModeBase
{
public:
SYSTEM_MODES_PUBLIC
explicit Mode(const std::string & mode_name) = delete;

SYSTEM_MODES_PUBLIC
Mode(const std::string & mode_name, const DefaultModePtr default_mode);
// cppcheck-suppress unknownMacro
RCLCPP_DISABLE_COPY(Mode)

virtual ~Mode() = default;
SYSTEM_MODES_PUBLIC
virtual
~Mode() = default;

SYSTEM_MODES_PUBLIC
virtual void
set_parameter(const rclcpp::Parameter & parameter);

virtual void set_parameter(const rclcpp::Parameter & parameter);
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters);
SYSTEM_MODES_PUBLIC
virtual void
set_parameters(const std::vector<rclcpp::Parameter> & parameters);

virtual void set_part_mode(
SYSTEM_MODES_PUBLIC
virtual void
set_part_mode(
const std::string & part,
const StateAndMode stateAndMode);
};

inline std::ostream & operator<<(std::ostream & os, const Mode & mode)
SYSTEM_MODES_PUBLIC
inline std::ostream &
operator<<(std::ostream & os, const Mode & mode)
{
os.precision(3);
os << mode.print();
Expand Down
7 changes: 7 additions & 0 deletions system_modes/include/system_modes/mode_handling.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,11 @@

#include <stdint.h>
#include <shared_mutex>

#include <rclcpp/parameter.hpp>
#include <rclcpp/parameter_map.hpp>


#include <map>
#include <mutex>
#include <string>
Expand All @@ -28,6 +30,7 @@

#include "system_modes/mode.hpp"
#include "system_modes/mode_impl.hpp"
#include "system_modes/visibility_control.hpp"

namespace system_modes
{
Expand All @@ -50,11 +53,15 @@ using RulesMap = std::map<std::string, ModeRule>;
class ModeHandling
{
public:
SYSTEM_MODES_PUBLIC
explicit ModeHandling(const std::string & model_path);
// cppcheck-suppress unknownMacro
RCLCPP_DISABLE_COPY(ModeHandling)

SYSTEM_MODES_PUBLIC
virtual ~ModeHandling() = default;

SYSTEM_MODES_PUBLIC
virtual const std::vector<ModeRule> get_rules_for(
const std::string & system,
const StateAndMode & target);
Expand Down
109 changes: 86 additions & 23 deletions system_modes/include/system_modes/mode_impl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@
#include <memory>
#include <utility>

#include "system_modes/visibility_control.hpp"

using lifecycle_msgs::msg::State;

namespace system_modes
Expand All @@ -34,26 +36,41 @@ namespace system_modes
static const char DEFAULT_MODE[] = "__DEFAULT__";


unsigned int state_id_(const std::string & state_label);
const std::string state_label_(unsigned int state_id);
SYSTEM_MODES_PUBLIC
unsigned int
state_id_(const std::string & state_label);

SYSTEM_MODES_PUBLIC
const std::string
state_label_(unsigned int state_id);

unsigned int transition_id_(const std::string & transition_label);
const std::string transition_label_(unsigned int transition_id);
SYSTEM_MODES_PUBLIC
unsigned int
transition_id_(const std::string & transition_label);

unsigned int goal_state_(unsigned int transition_id);
SYSTEM_MODES_PUBLIC
const std::string
transition_label_(unsigned int transition_id);

SYSTEM_MODES_PUBLIC
unsigned int
goal_state_(unsigned int transition_id);

struct StateAndMode
{
unsigned int state;
std::string mode;

SYSTEM_MODES_PUBLIC
explicit StateAndMode(unsigned int newstate = 0, const std::string & newmode = "")
{
state = newstate;
mode = newmode;
}

bool operator==(const StateAndMode & cmp) const
SYSTEM_MODES_PUBLIC
bool
operator==(const StateAndMode & cmp) const
{
if (cmp.state != state) {
return false;
Expand All @@ -66,12 +83,16 @@ struct StateAndMode
(mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()); // DEFAULT_MODE the same
}

bool operator!=(const StateAndMode & cmp) const
SYSTEM_MODES_PUBLIC
bool
operator!=(const StateAndMode & cmp) const
{
return !(*this == cmp);
}

void from_string(const std::string & sam)
SYSTEM_MODES_PUBLIC
void
from_string(const std::string & sam)
{
auto dot = sam.find(".");
if (dot != std::string::npos) {
Expand All @@ -83,7 +104,9 @@ struct StateAndMode
}
}

std::string as_string() const
SYSTEM_MODES_PUBLIC
std::string
as_string() const
{
if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) {
return state_label_(state);
Expand All @@ -100,31 +123,71 @@ struct StateAndMode
class ModeImpl
{
public:
SYSTEM_MODES_PUBLIC
explicit ModeImpl(const std::string & mode_name);
virtual ~ModeImpl() = default;

SYSTEM_MODES_PUBLIC
virtual
~ModeImpl() = default;

SYSTEM_MODES_PUBLIC
ModeImpl(const ModeImpl & copy) = delete;

virtual std::string get_name() const;
SYSTEM_MODES_PUBLIC
virtual std::string
get_name() const;

SYSTEM_MODES_PUBLIC
virtual void
add_parameter(const rclcpp::Parameter & parameter);

SYSTEM_MODES_PUBLIC
virtual void
add_parameters(const std::vector<rclcpp::Parameter> & parameters);

virtual void add_parameter(const rclcpp::Parameter & parameter);
virtual void add_parameters(const std::vector<rclcpp::Parameter> & parameters);
virtual void add_part_mode(
SYSTEM_MODES_PUBLIC
virtual void
add_part_mode(
const std::string & part,
const StateAndMode stateAndMode);

virtual void set_parameter(const rclcpp::Parameter & parameter);
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters);
virtual void set_part_mode(
SYSTEM_MODES_PUBLIC
virtual void
set_parameter(const rclcpp::Parameter & parameter);

SYSTEM_MODES_PUBLIC
virtual void
set_parameters(const std::vector<rclcpp::Parameter> & parameters);

SYSTEM_MODES_PUBLIC
virtual void
set_part_mode(
const std::string & part,
const StateAndMode stateAndMode);

virtual std::vector<std::string> get_parameter_names() const;
virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
virtual bool get_parameter(const std::string & param_name, rclcpp::Parameter & parameter) const;
virtual const std::vector<rclcpp::Parameter> get_parameters() const;
SYSTEM_MODES_PUBLIC
virtual std::vector<std::string>
get_parameter_names() const;

SYSTEM_MODES_PUBLIC
virtual rclcpp::Parameter
get_parameter(const std::string & param_name) const;

SYSTEM_MODES_PUBLIC
virtual bool
get_parameter(const std::string & param_name, rclcpp::Parameter & parameter) const;

SYSTEM_MODES_PUBLIC
virtual const std::vector<rclcpp::Parameter>
get_parameters() const;

SYSTEM_MODES_PUBLIC
virtual const std::vector<std::string>
get_parts() const;

virtual const std::vector<std::string> get_parts() const;
virtual const StateAndMode get_part_mode(const std::string & part) const;
SYSTEM_MODES_PUBLIC
virtual const StateAndMode
get_part_mode(const std::string & part) const;

protected:
std::string name_;
Expand Down
Loading